#include #include #include #include #include "xc.h" #include "math.h" #include "UART.h" #include "config.h" #include "extern.h" #include "types.h" #include "conv.h" #include "isr.h" #include "CalibrationConstants.h" #include "GainScheduling.h" #include "libq.h" #include "RPM_Calculation.h" #include "WindingCurrentTarget.h" #include "park.h" #include "svgen.h" #include "SendCANPackets.h" #include "limiter.h" #include "PIDGainScaling.h" #include "DAC.h" #include "WindingCurrentControl.h" #include "WindingCurrentLimits.h" #include "BattSwitchControl.h" unsigned int *Buff; //Pointer to appropriate DMA Buffer, switched on PingPong //This DMA interrupt occurs once at the end of each sampling period in which the specified //channels are cycled through the specified number of times. The channel reading is done //at a rate specified by timer 3 (1.5uS) and that timer interrupt keeps count of how many //samples have been made. Example. If four channels are sampled once each at a rate of 1.5uS // then this DMA interrupt will occur 6 uS after the sampling was started by the PWM interrupt. void __attribute__((interrupt, no_auto_psv)) _DMA0Interrupt(void) //void __attribute__((__interrupt__(__save__(CORCON)))) _DMA0Interrupt(void) { int j; static int first_time = 1; static fractional VBus_Offset; static fractional IBatt_Offset; int outSamples[INNERFILTERSAMPLES]; __asm__ volatile ("push CORCON"); // T3CON = 0x0000; //Stop Timer b/c the correct number of samples have been converted // AD1CON1bits.DONE = 0; //clear flag // TMR3 = 0; //Reset to zero so it's ready for re-start if (DMACS1 & 0x0001) //Point to one buffer or the other depending on which one caused interrupt Buff = BufferB; else Buff = BufferA; if (first_time) //Compute analog counts corresponding to zero's of engineering units. { VBus_Offset = ConvertEngUnitsToInt(0.0, Cal.VBus_cnt, Cal.VBus_eng); IBatt_Offset = ConvertEngUnitsToInt(0.0, Cal.IBatt_cnt, Cal.IBatt_eng); first_time = 0; } #if defined(DEBUG) && defined(ALLOW_CALIBRATION) if (raw) //Remove offset if in calibration mode { VBus_Offset = 0; IBatt_Offset = 0; first_time = 1; } #endif // BufferA/BufferB and Buff are unsigned integers, because the ADC is set up to represent data as unsigned // integers with the lowest four bits equal to zero (since it's a 12 bit converter). // Below all variables that get set from the ADC values are shifted to the right one bit and then have the counts // representing the zero point subtracted to make signed integers. // This doesn't introduce a lack of precision for variables that are always positive (like VBus) because it's a 12-bit ADC, // so representing positive only values in 15 bits instead is OK. // The ADC FORMAT setting could be used to produce variables as signed integers, but this isn't used to // give some control over the zero-point, which may not fall exactly half-way between 0 and Vref in every case. //If the analog inputs used (or their ordering) are changed, the next lines must be changed to match //Additionally the analog input pin specification must be changed in init.c for (j = 0; j < INNERFILTERSAMPLES; j++) { VBus_ADC[j] = _Q15sub(Buff[0 + NUMAIO * j] >> 1, VBus_Offset); //AN16 Vbus = 0->400V is represented by 0->3.3V input, and as a positive unsigned integer here because the right-shift of an unsigned variable is a logical shift and a zero gets moved into the most significant byte. IBatt_ADC[j] = _Q15sub(Buff[1 + NUMAIO * j] >> 1, IBatt_Offset); //AN17 For test setup that doesn't have an offset built into the input channel. } if (INNERFILTERSAMPLES > 1) { asm volatile ("disi #0x3FFF"); FIR(INNERFILTERSAMPLES, &outSamples[0], (fractional *) & DiffPress_ADC[0], &Inner_Filter); asm volatile ("disi #0x0000"); DiffPressSamples[ADC_OuterSampleCtr] = outSamples[INNERFILTERSAMPLES - 1]; //Store current value for use later. asm volatile ("disi #0x3FFF"); FIR(INNERFILTERSAMPLES, &outSamples[0], (fractional *) & AuxTorque_ADC[0], &Inner_Filter); asm volatile ("disi #0x0000"); AuxTorqueSamples[ADC_OuterSampleCtr] = outSamples[INNERFILTERSAMPLES - 1]; //Store current value for use later asm volatile ("disi #0x3FFF"); FIR(INNERFILTERSAMPLES, &outSamples[0], (fractional *) & VBus_ADC[0], &Inner_Filter); asm volatile ("disi #0x0000"); VBusSamples[ADC_OuterSampleCtr] = outSamples[INNERFILTERSAMPLES - 1]; //Store current value for use later. asm volatile ("disi #0x3FFF"); FIR(INNERFILTERSAMPLES, &outSamples[0], (fractional *) & IBatt_ADC[0], &Inner_Filter); asm volatile ("disi #0x0000"); IBattSamples[ADC_OuterSampleCtr] = outSamples[INNERFILTERSAMPLES - 1]; //Store current value for use later } else { VBusSamples[ADC_OuterSampleCtr] = VBus_ADC[0]; IBattSamples[ADC_OuterSampleCtr] = IBatt_ADC[0]; } asm volatile ("disi #0x3FFF"); //This increment/check/reset needs to be atomic in case the SPI interrupts happen after it's incremented, and before it's reset. if (++ADC_OuterSampleCtr == OUTERFILTERSAMPLES) ADC_OuterSampleCtr = 0; asm volatile ("disi #0x0000"); IFS0bits.DMA0IF = 0; //Clear the DMA interrupt flag bit __asm__ volatile ("pop CORCON"); } unsigned int *Buff2; //Pointer to appropriate DMA Buffer, switched on PingPong //This DMA interrupt occurs once at the end of each sampling period in which the specified //channels are cycled through the specified number of times. The channel reading is done //at a rate specified by timer 3 (1.5uS) and that timer interrupt keeps count of how many //samples have been made. Example. If four channels are sampled once each at a rate of 1.5uS // then this DMA interrupt will occur 6 uS after the sampling was started by the PWM interrupt. void __attribute__((interrupt, no_auto_psv)) _DMA3Interrupt(void) //void __attribute__((__interrupt__(__save__(CORCON)))) _DMA0Interrupt(void) { int j; static int first_time = 1; static fractional DiffPress_Offset; static fractional AuxTorque_Offset; int outSamples[INNERFILTERSAMPLES]; __asm__ volatile ("push CORCON"); if (DMACS1 & 0x0008) //Point to one buffer or the other depending on which one caused interrupt Buff2 = Buffer2B; else Buff2 = Buffer2A; if (first_time) //Compute analog counts corresponding to zero's of engineering units. { DiffPress_Offset = ConvertEngUnitsToInt(0.0, Cal.DiffPress_cnt, Cal.DiffPress_eng); AuxTorque_Offset = ConvertEngUnitsToInt(0.0, Cal.AuxTorque_cnt, Cal.AuxTorque_eng); first_time = 0; } #if defined(DEBUG) && defined(ALLOW_CALIBRATION) if (raw) //Remove offset if in calibration mode { DiffPress_Offset = 0; AuxTorque_Offset = 0; //first_time = 1; } #endif // BufferA/BufferB and Buff are unsigned integers, because the ADC is set up to represent data as unsigned // integers with the lowest four bits equal to zero (since it's a 12 bit converter). // Below all variables that get set from the ADC values are shifted to the right one bit and then have the counts // representing the zero point subtracted to make signed integers. // This doesn't introduce a lack of precision for variables that are always positive (like VBus) because it's a 12-bit ADC, // so representing positive only values in 15 bits instead is OK. // The ADC FORMAT setting could be used to produce variables as signed integers, but this isn't used to // give some control over the zero-point, which may not fall exactly half-way between 0 and Vref in every case. //If the analog inputs used (or their ordering) are changed, the next lines must be changed to match //Additionally the analog input pin specification must be changed in init.c for (j = 0; j < INNERFILTERSAMPLES; j++) { DiffPress_ADC[j] = _Q15sub(Buff2[0 + NUMAIO * j] >> 1, DiffPress_Offset); //AN10 AuxTorque_ADC[j] = _Q15sub(Buff2[1 + NUMAIO * j] >> 1, AuxTorque_Offset); //AN11 } if (INNERFILTERSAMPLES > 1) { asm volatile ("disi #0x3FFF"); FIR(INNERFILTERSAMPLES, &outSamples[0], (fractional *) & DiffPress_ADC[0], &Inner_Filter); asm volatile ("disi #0x0000"); DiffPressSamples[ADC2_OuterSampleCtr] = outSamples[INNERFILTERSAMPLES - 1]; //Store current value for use later. asm volatile ("disi #0x3FFF"); FIR(INNERFILTERSAMPLES, &outSamples[0], (fractional *) & AuxTorque_ADC[0], &Inner_Filter); asm volatile ("disi #0x0000"); AuxTorqueSamples[ADC2_OuterSampleCtr] = outSamples[INNERFILTERSAMPLES - 1]; //Store current value for use later } else { DiffPressSamples[ADC2_OuterSampleCtr] = DiffPress_ADC[0]; AuxTorqueSamples[ADC2_OuterSampleCtr] = AuxTorque_ADC[0]; } asm volatile ("disi #0x3FFF"); //This increment/check/reset needs to be atomic in case the SPI interrupts happen after it's incremented, and before it's reset. if (++ADC2_OuterSampleCtr == OUTERFILTERSAMPLES) ADC2_OuterSampleCtr = 0; asm volatile ("disi #0x0000"); IFS2bits.DMA3IF = 0; //Clear the DMA interrupt flag bit __asm__ volatile ("pop CORCON"); } #ifdef DEBUG //This ISR is not needed but is here for debugging void __attribute__((interrupt, no_auto_psv)) _ADC1Interrupt(void) //void __attribute__((__interrupt__(__save__(CORCON)))) _ADC1Interrupt(void) { __asm__ volatile ("push CORCON"); _AD1IF = 0; //Clear Interrupt __asm__ volatile ("pop CORCON"); } #endif #ifdef DEBUG //This interrupt occurs when TMR3 Rolls over. //This ISR is not needed but is here for debugging void __attribute__((interrupt, no_auto_psv)) _T3Interrupt(void) //void __attribute__((__interrupt__(__save__(CORCON)))) _T3Interrupt(void) { __asm__ volatile ("push CORCON"); _T3IF = 0; // Clear interrupt __asm__ volatile ("pop CORCON"); } #endif void __attribute__((interrupt, no_auto_psv)) _T1Interrupt(void) //PR1 is set to achieve desired CAN Packet output rate ////void __attribute__((__interrupt__(__save__(CORCON)))) _T1Interrupt(void) { __asm__ volatile ("push CORCON"); #ifdef DEBUG f_time = f_time + 1.0 / CAN_packet_rate.Value; #endif //Form data for sending out over CAN by further filtering data output from 1kHz filter #ifdef FOO fractional outSamples[REPORTINGFILTERSAMPLES]; asm volatile ("disi #0x3FFF"); FIR(REPORTINGFILTERSAMPLES, &outSamples[0], (fractional *) & QuadratureCurrent[0], &Reporting_Filter); asm volatile ("disi #0"); R_QuadratureCurrent = outSamples[REPORTINGFILTERSAMPLES - 1]; asm volatile ("disi #0x3FFF"); FIR(REPORTINGFILTERSAMPLES, &outSamples[0], (fractional *) & VBus[0], &Reporting_Filter); asm volatile ("disi #0"); R_VBus = outSamples[REPORTINGFILTERSAMPLES - 1]; asm volatile ("disi #0x3FFF"); FIR(REPORTINGFILTERSAMPLES, &outSamples[0], (fractional *) & IBatt[0], &Reporting_Filter); asm volatile ("disi #0"); R_IBatt = outSamples[REPORTINGFILTERSAMPLES - 1]; asm volatile ("disi #0x3FFF"); FIR(REPORTINGFILTERSAMPLES, &outSamples[0], (fractional *) & Load_DC[0], &Reporting_Filter); asm volatile ("disi #0"); R_Load_DC = outSamples[REPORTINGFILTERSAMPLES - 1]; asm volatile ("disi #0x3FFF"); FIR(REPORTINGFILTERSAMPLES, &outSamples[0], (fractional *) & LoadDumpCurrent[0], &Reporting_Filter); asm volatile ("disi #0"); R_LoadDumpCurrent = outSamples[REPORTINGFILTERSAMPLES - 1]; asm volatile ("disi #0x3FFF"); FIR(REPORTINGFILTERSAMPLES, &outSamples[0], (fractional *) & RPM[0], &Reporting_Filter); asm volatile ("disi #0"); R_RPM = outSamples[REPORTINGFILTERSAMPLES - 1]; asm volatile ("disi #0x3FFF"); FIR(REPORTINGFILTERSAMPLES, &outSamples[0], (fractional *) & AuxTorque[0], &Reporting_Filter); asm volatile ("disi #0"); R_AuxTorque = outSamples[REPORTINGFILTERSAMPLES - 1]; #endif //Just use the most recent values. R_QuadratureCurrent = QuadratureCurrent[ReportingFilterCtr]; R_VBus = VBus[ReportingFilterCtr]; R_IBatt = IBatt[ReportingFilterCtr]; R_Load_DC = Load_DC[ReportingFilterCtr]; R_LoadDumpCurrent = LoadDumpCurrent[ReportingFilterCtr]; R_RPM = RPM[ReportingFilterCtr]; R_AuxTorque = AuxTorque[ReportingFilterCtr]; RPM_StdDev = AdjustScaleFactor(R_RPM); R_DiffPress = DiffPress[ReportingFilterCtr]; SendCANPackets(); StatusBitFields.TorqueCmdLimited = 0; //Reset status bit to assume that no torque limitations relative to request exist, this bit will be set any number of places if a torque command isn't realizable. _T1IF = 0; __asm__ volatile ("pop CORCON"); } // void __attribute__((interrupt, no_auto_psv)) _QEIInterrupt(void) { __asm__ volatile ("push CORCON"); if (EncoderStatus == INDEX_PULSE_SEEN) //Count Shaft Rotations after first index pulse seen. { if (QEICON & 0x0800) ShaftRotations++; else ShaftRotations--; } else EncoderStatus = INDEX_PULSE_SEEN; _QEIIF = 0; //Clear Interrupt __asm__ volatile ("pop CORCON"); } //Interrupt to handle incoming CAN messages void __attribute__((interrupt, no_auto_psv)) _C1Interrupt(void) { CANmsg RcvdMessage; int i; __asm__ volatile ("push CORCON"); if (C1INTFbits.RBIF) /* check to see if the interrupt is caused by receive */ { /*check to see if buffer 8 is full, indicating CmdID was 10 */ if (C1RXFUL1bits.RXFUL8) { DisAssembleCAN_Packet(8, &RcvdMessage); for (i = 0; i < NUMBER_OF_CMDS_TO_INTERPRET; i++) { if (CmdParams[i].id == RcvdMessage.data[0]) { //Cycle thru options until cooresponding cmd id is found and then call that function CmdParams[i].CmdFunc(RcvdMessage.data); // This could be done without the iteration if all the cmd ids are packed at low vales, i.e. 0,1,2,... break; // It's not that way at the moment so sticking with the legacy command ids. } } C1RXFUL1bits.RXFUL8 = 0; } if (C1RXFUL1bits.RXFUL9) { /*check to see if buffer 9 is full, indicating CmdID was 0 and the packet contains the "range" measurement from the spring controller*/ DisAssembleCAN_Packet(9, &RcvdMessage); if(RcvdMessage.data_length >= 8) //There should be 8 bytes in a valid message { unsigned int SC_Range_local; SC_Range_local = 256*RcvdMessage.data[3]+RcvdMessage.data[2]; if((SC_Range_local <= SC_Range_Max) && (SC_Range_local >= SC_Range_Min)) { SC_Range = SC_Range_local; SC_RangeCtr = SC_RANGE_VALID_TIMEOUT; //Reset counter on reception of valid range from string controller. } } C1RXFUL1bits.RXFUL9 = 0; } /* clear flag */ C1INTFbits.RBIF = 0; } else { if (C1INTFbits.TBIF) //If interrupt is caused by transmit. { /* clear flag */ C1INTFbits.TBIF = 0; } } /* clear interrupt flag */ IFS2bits.C1IF = 0; __asm__ volatile ("pop CORCON"); } /* - All of the control loops in the converter are synchronized to the PWM clock. Every time the PWM clock rolls over (~20kHz) * the _PWMInterrupt occurs. This ISR then starts the data collection from both the SPI Bus ADCs and the on-board dsPIC ADCs. * After about 18uS the SPI Bus transfers will complete and the resulting interrupts call the main routine that executes all of * the various control loops based upon the phase current measurements. * - Also within that 18uS the ADC samples and converts four channels, providing a single measurement of VBus and I_Batt for use in the * control loops. This ADC process takes about 16uS so when "WindingControl" is called from the SPI interrupts, all data is available. * Because the VBus and IBatt data aren't needed until near the end of the WindingControl process, a different approach would be to set * up the ADC to continue cycling through the channels a few more times until the VBus data is actually needed, this would allow some * filtering of several measurements. * - Another possibility is to move the ShuntRegulator code to the DMA interrupt and time it to finish a little bit before the WindingControl * code completes so that it runs either right after the WindingControl routine, or as soon as the ADC data becomes available. * There are a few ways to implement this to maximize the number of samples of VBUS and IBatt for optimum filtering * */ void __attribute__((interrupt, no_auto_psv)) _PWMInterrupt(void) //void __attribute__((__interrupt__(__save__(CORCON)))) _PWMInterrupt(void) { __asm__ volatile ("push CORCON"); static int ISR_Counter = 0; static int RPMctr = 0; if (ISR_Counter == 0) { //TMR3 = 0; //Reset timer before starting. // T3CON = 0x8000; //Start timer 3, an ADC sample and convert will then happen every ~4.04uSec until four are completed and DMA occurs. //The filtering circuitry driving the dsPic ADC requires 1.8uS to settle after the ADC selects that channel (LTC2055 driving the 18pF Sample //and Hold capacitor. The ADC conversion process is set up to take another 1.8uS (5*14*Tcy), so a total of 3.6uS is required per channel. //4uS selected here gives some margin. //The DMA interupt then stops the timer to keep more from being sampled and converted until next time it's turned on here. //There is only barely enough time to do this before the SPI bus reads complete which launches the current winding control, alternative is to start this at the end of winding control so everything is all set. //Start SPI Bus process to measure phase currents, SPIinterrupt will occur when done. if (CurrMeasStatus == 0x0008) //Only start SPI/ADC collection if last cycles data has been collected and processed { CurrMeasStatus = 0; //AUXIO_PIN2 = 1; //TMR3 = 0; //Reset to zero so it's ready for re-start // T3CON = 0x8000; //Start timer 3, an ADC sample and convert will then happen every ~4.04uSec until four are completed and DMA occurs. SPI1STATbits.SPIEN = 1; SPI2STATbits.SPIEN = 1; SPI1STATbits.SPIROV = 0; SPI2STATbits.SPIROV = 0; LATBbits.LATB2 = 0; SPI2BUF = *((unsigned int *) &SPI2_DataOut[0]); SPI1BUF = *((unsigned int *) &SPI1_DataOut[0]); } if (EncoderStatus == INDEX_PULSE_SEEN) //PRE not required in Cal Mode for convenience. Means RPM isn't computed. { if (RPMctr-- <= 0) { RPMs = ComputeRPM(); RPMctr = RPM_COMPUTATION_PERIOD - 1; } if(CalibrationMode == 0) // Don't run the auto batt-switch control in calibration mode. BattSwitchControl(); } else RPMs = 0; } if (ISR_Counter == WINDING_CONTROL_PERIOD - 1) ISR_Counter = 0; else ISR_Counter++; _PWMIF = 0; //Clear Interrupt __asm__ volatile ("pop CORCON"); } void __attribute__((interrupt, no_auto_psv)) _CNInterrupt(void) //void __attribute__((__interrupt__(__save__(CORCON)))) _CNInterrupt(void) { int TransitionValid = 0; __asm__ volatile ("push CORCON"); //This may need to be saved to keep the pid.s assembly stuff from generating an address error. static int last_halls = 0; last_halls = halls; halls = (PORTD & 0x0070) >> 4; // Shift RD4/5/6 to least significant bits of variable. if (EncoderStatus != INDEX_PULSE_SEEN) { switch (halls) { case 1: if (last_halls == 1) { POSCNT = ENC_CNTPERREV-1; TransitionValid = 1; } if (last_halls == 1) { POSCNT = ENC_CNTPERREV-1; TransitionValid = 1; } break; case 2: if (last_halls == 1) { POSCNT = ENC_CNTPERREV-1; TransitionValid = 1; } if (last_halls == 1) { POSCNT = ENC_CNTPERREV-1; TransitionValid = 1; } break; case 3: if (last_halls == 1) { POSCNT = ENC_CNTPERREV-1; TransitionValid = 1; } if (last_halls == 1) { POSCNT = ENC_CNTPERREV-1; TransitionValid = 1; } break; case 4: if (last_halls == 1) { POSCNT = ENC_CNTPERREV-1; TransitionValid = 1; } if (last_halls == 1) { POSCNT = ENC_CNTPERREV-1; TransitionValid = 1; } break; case 5: if (last_halls == 1) { POSCNT = ENC_CNTPERREV-1; TransitionValid = 1; } if (last_halls == 1) { POSCNT = ENC_CNTPERREV-1; TransitionValid = 1; } break; case 6: if (last_halls == 1) { POSCNT = ENC_CNTPERREV-1; TransitionValid = 1; } if (last_halls == 1) { POSCNT = ENC_CNTPERREV-1; TransitionValid = 1; } break; } if (TransitionValid) { EncoderStatus = ONLY_HALL_TRANSITION_HAS_OCCURED; IEC1bits.CNIE = 0; //Disable these interrupts, no longer needed. } } _CNIF = 0; // Clear interrupt __asm__ volatile ("pop CORCON"); } void __attribute__((interrupt, no_auto_psv)) _SPI1Interrupt(void) { static int CurrMeas = 0; __asm__ volatile ("push CORCON"); if (CurrMeasStatus & 0x0001) { //First word has already been read so this interrupt is due to the second word being read LATBbits.LATB2 = 1; CurrMeas |= (SPI1BUF >> 1); PhaseA_Curr = _Q15add(CurrMeas,Cal.PhaseA_Offset_cnts); //Current Measurement assembled, so copy to global variable. Sign swapped to make current flowing into motor positive. CurrMeasStatus &= ~0x0001; //Clear bit one indicating the second word has been read if (CurrMeasStatus == 0x0004) { SPI1STATbits.SPIEN = 0; SPI2STATbits.SPIEN = 0; WindingCurrentControl(); //Call Winding Current Control if both SPI channels have been read CurrMeasStatus = 0x0008; //Indicated processing is done. } } else { CurrMeas = (SPI1BUF << 15); CurrMeasStatus |= 0x0001; //Set bit one indicated the first word has been read SPI1STATbits.SPIROV = 0; if (CurrMeasStatus == 0x03) //Both First words have been read. { SPI2BUF = *((unsigned int *) &SPI2_DataOut[2]); SPI1BUF = *((unsigned int *) &SPI1_DataOut[2]); CurrMeasStatus |= 0x04; } } _SPI1IF = 0; // Clear the Interrupt flag __asm__ volatile ("pop CORCON"); } void __attribute__((interrupt, no_auto_psv)) _SPI2Interrupt(void) { static int CurrMeas = 0; __asm__ volatile ("push CORCON"); iTemp1 = 2; if (CurrMeasStatus & 0x0002) { //First word has already been read so this interrupt is due to the second word being read CurrMeas |= (SPI2BUF >> 1); PhaseB_Curr = _Q15add(_Q15neg(CurrMeas),Cal.PhaseB_Offset_cnts); //Current Measurement assembled, so copy to global variable. Sign swapped to make current flowing into motor positive. CurrMeasStatus &= ~0x0002; //Clear bit two indicating the second word has been read if (CurrMeasStatus == 0x0004) { SPI1STATbits.SPIEN = 0; SPI2STATbits.SPIEN = 0; WindingCurrentControl(); //Call Winding Current Control if both SPI channels have been read CurrMeasStatus = 0x0008; } } else { CurrMeas = (SPI2BUF << 15); CurrMeasStatus |= 0x0002; //Set bit two indicated the first word has been read if (CurrMeasStatus == 0x03) //Both First words have been read. { SPI2BUF = *((unsigned int *) &SPI2_DataOut[2]); SPI1BUF = *((unsigned int *) &SPI1_DataOut[2]); CurrMeasStatus |= 0x04; } } _SPI2IF = 0; // Clear the Interrupt flag __asm__ volatile ("pop CORCON"); } //This interrupt occurs when TMR5 Rolls over. ~1092Hz void __attribute__((interrupt, no_auto_psv)) _T5Interrupt(void) //void __attribute__((__interrupt__(__save__(CORCON)))) _T3Interrupt(void) //Set RPM to zero if the index pulse hasn't been seen yet. // Otherwise jump occurs in RPMs when the POSCNT jumps the first time. // Could do something fancier in "ComputeRPM()" to adjust for this jump, but it doesn't seem worth it since this is only an issue for the first fraction of a rev after power-up. { __asm__ volatile ("push CORCON"); if (SC_RangeCtr > 0) //Keep from rolling down below zero. { SC_RangeCtr--; StatusBitFields.SpringRangeValid = 1; //Indicate SC_Range is valid in mode word. } else { SC_RangeCtr = 0; StatusBitFields.SpringRangeValid = 0; //Indicate SC_Range has become invalid in mode word. } if((P1DC4 > 0x0600) && (SEVTCMP == 0x0400)) SEVTCMP = 0x0000; if((P1DC4 < 0x0200) && (SEVTCMP == 0x0000)) SEVTCMP = 0x0400; RPM[ReportingFilterCtr] = RPMs; #if 1 //Just use most recent measurement QuadratureCurrent[ReportingFilterCtr] = QuadratureCurrentSamples[SPI_OuterSampleCtr]; //This is the most filtered version computed and consists of OUTERFILTERSAMPLES*INNERFILTERSAMPLES samples. VBus[ReportingFilterCtr] = VBusSamples[ADC_OuterSampleCtr]; IBatt[ReportingFilterCtr] = IBattSamples[ADC_OuterSampleCtr]; Load_DC[ReportingFilterCtr] = LoadDCSamples[SPI_OuterSampleCtr]; LoadDumpCurrent[ReportingFilterCtr] =( (long) VBus[ReportingFilterCtr]) * LoadDumpCalibration(P1DC4) / (2 * P1TPER); //Compute Current from DC. DiffPress[ReportingFilterCtr] = DiffPressSamples[ADC2_OuterSampleCtr]; AuxTorque[ReportingFilterCtr] = AuxTorqueSamples[ADC2_OuterSampleCtr]; #else int outSamples[OUTERFILTERSAMPLES]; /***** Filter Measurements ***************************************/ //These DSP filterings seem to cause steppy noise in the VqVd magnitude, perhaps due to delay, or disabling interrupts, or corrupting the DSP. asm volatile ("disi #0x3FFF"); FIR(OUTERFILTERSAMPLES, &outSamples[0], (fractional *) & QuadratureCurrentSamples[0], &Outer_Filter); asm volatile ("disi #0"); QuadratureCurrent[ReportingFilterCtr] = outSamples[OUTERFILTERSAMPLES - 1]; //This is the most filtered version computed and consists of OUTERFILTERSAMPLES*INNERFILTERSAMPLES samples. asm volatile ("disi #0x3FFF"); FIR(OUTERFILTERSAMPLES, &outSamples[0], (fractional *) & VBusSamples[0], &Outer_Filter); asm volatile ("disi #0"); VBus[ReportingFilterCtr] = outSamples[OUTERFILTERSAMPLES - 1]; asm volatile ("disi #0x3FFF"); FIR(OUTERFILTERSAMPLES, &outSamples[0], (fractional *) & IBattSamples[0], &Outer_Filter); asm volatile ("disi #0"); IBatt[ReportingFilterCtr] = outSamples[OUTERFILTERSAMPLES - 1]; asm volatile ("disi #0x3FFF"); FIR(OUTERFILTERSAMPLES, &outSamples[0], (fractional *) & LoadDCSamples[0], &Outer_Filter); asm volatile ("disi #0"); Load_DC[ReportingFilterCtr] = outSamples[OUTERFILTERSAMPLES - 1]; asm volatile ("disi #0x3FFF"); FIR(OUTERFILTERSAMPLES, &outSamples[0], (fractional *) & LoadDumpCurrentSamples[0], &Outer_Filter); asm volatile ("disi #0"); LoadDumpCurrent[ReportingFilterCtr] = outSamples[OUTERFILTERSAMPLES - 1]; asm volatile ("disi #0x3FFF"); FIR(OUTERFILTERSAMPLES, &outSamples[0], (fractional *) & DiffPressSamples[0], &Outer_Filter); asm volatile ("disi #0"); DiffPress[ReportingFilterCtr] = outSamples[OUTERFILTERSAMPLES - 1]; asm volatile ("disi #0x3FFF"); FIR(OUTERFILTERSAMPLES, &outSamples[0], (fractional *) & AuxTorqueSamples[0], &Outer_Filter); asm volatile ("disi #0"); AuxTorque[ReportingFilterCtr] = outSamples[OUTERFILTERSAMPLES - 1]; #endif //Implement Mode Timeouts and lockouts if SC_Range is invalid. if (EncoderStatus == INDEX_PULSE_SEEN) { //Revert to Generator mode if no torque or velocity command has been received within timeout time if ((TorqueCmdTimeout_ctr > 0) ) { if(--TorqueCmdTimeout_ctr == 0) StatusBitFields.Mode = GENERATOR_MODE; } if (BiasCmdTimeout_ctr > 0) //Remove current bias if no bias command has been received within timeout time. { if(--BiasCmdTimeout_ctr == 0) BiasCurrent.Value = 0; } if ((StatusBitFields.PermissiveMode == 0) && (StatusBitFields.SpringRangeValid == 0)) //Could be logically combined above, but left separate for clarity. { StatusBitFields.Mode = GENERATOR_MODE; //Force to Generator Mode BiasCurrent.Value = 0; //Remove bias Current to prevent motoring. TorqueCmdTimeout_ctr = 0; //Reset timeouts to zero. Means a single invalid SC_Range instance causes Torque Cmd and BiasCommand to need to be restarted. BiasCmdTimeout_ctr = 0; } } /***** Set Target Torque Based on Mode **********************/ switch (StatusBitFields.Mode) { case PREINDEXPULSE_MODE: CommandedCurrent = SlowRotateCurrent; //Slow rotations until index pulse is seen if (++SlowRotatePosition == ENC_CNTPERREV) SlowRotatePosition = 0; if (EncoderStatus != INDEX_PULSE_SEEN) break; else StatusBitFields.Mode = GENERATOR_MODE; //Index pulse seen so set to Generator Mode for next time and and drop through for this time. case GENERATOR_MODE: //Generator only mode, follows torque speed curve modified by scale_factor and retract factor. CommandedCurrent = _Q15add(WindingCurrentTarget(RPMs), BiasCurrent.Value); //Sum in Bias Current. UserCommandedCurrent.Value = CommandedCurrent; //This keeps big jump from occurring when Torque mode is changed from keyboard when rate-limiter is de-activated. break; case TORQUE_MODE: //CommandedCurrent is specified by user command, so just need to add specified bias current CommandedCurrent = UserCommandedCurrent.Value; break; } if(P1OVDCON == 0x4000) { CommandedCurrent = 0; StatusBitFields.TorqueCmdLimited = 1; //Indicate commanded torque is not possible. } int locCommandedQuadratureCurrent; //Apply Rate and hard limits to the requested current. locCommandedQuadratureCurrent = CurrentMinMax(RPMs, CommandedCurrent); //Enforce Min/Max limits, a function of RPM. This effectively implements the hard limits since those hard limits are used to define the min/max curves in this function. locCommandedQuadratureCurrent = CurrentRateLimit(locCommandedQuadratureCurrent); //Rate limit how fast commanded current can change. if(locCommandedQuadratureCurrent != CommandedCurrent) StatusBitFields.TorqueCmdLimited = 1; //Indicate commanded torque is not possible. asm volatile ("disi #0x3FFF"); CommandedQuadratureCurrent = locCommandedQuadratureCurrent; asm volatile ("disi #0x0000"); asm volatile ("disi #0x3FFF"); //This increment/check/reset needs to be atomic in case an interrupts happen after it's incremented, and before it's reset. if (++ReportingFilterCtr == REPORTINGFILTERSAMPLES) ReportingFilterCtr = 0; asm volatile ("disi #0x0000"); _T5IF = 0; // Clear interrupt __asm__ volatile ("pop CORCON"); } //This interrupt occurs when TMR8 Rolls over. void __attribute__((interrupt, no_auto_psv)) _T8Interrupt(void) { __asm__ volatile ("push CORCON"); static int toggle = 0; if(eeprom_timer > 0) // Count down to zero and stop until restarted eeprom_timer--; if(toggle == 0) toggle = 1; else toggle = 0; AUXIO_PIN4 = toggle; _T8IF = 0; // Clear interrupt __asm__ volatile ("pop CORCON"); }