#include #include #include #include #include #include "xc.h" #include "math.h" #include "UART.h" #include "config.h" #include "extern.h" #include "conv.h" #include "isr.h" #include "config.h" #include "CalibrationConstants.h" #include "libq.h" #include "SendCANPackets.h" extern unsigned int pos; extern int step; extern unsigned int halls; int RetractedRudderPosition = -270; //Set as estimates initially, re-written after it finds limit switches. int ExtendedRudderPosition = 270; fractional fooQ15; float foo; unsigned int *Buff; //Pointer to appropriate DMA Buffer, switched on PingPong //This DMA interrupt occurs at the completion of the ADC reading, a process that is started after a valid AHRS data packet has been recieved. void __attribute__((interrupt, no_auto_psv)) _DMA0Interrupt(void) //void __attribute__((__interrupt__(__save__(CORCON)))) _DMA0Interrupt(void) { static int i = 0; int j; int Xvel = 0; int Yvel = 0; int Zvel = 0; char BeamAmp1, BeamAmp2, BeamAmp3; char BeamCorr1, BeamCorr2, BeamCorr3; static int first_time = 1; static fractional WingPos_Offset; static fractional Pressure_Offset; static fractional Heading_Offset; static fractional FilteredHeading; fractional HeadingError; static int StoppedRudderCmd = 0; static int LimitsNotFound = 3; //Variable that keeps state of limit-switch search at power-up. static fractional RudderCmd = 0; static unsigned int UpdateCounter = 0; static signed int deadband; IMU_Packet *IMU_Data; __asm__ volatile ("push CORCON"); AD1CON1bits.ASAM = 0; // Stop ADC, we have what we need. 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 zeros of engineering units. { WingPos_Offset = ConvertEngUnitsToInt(0.0, WingPos_cnt, WingPos_eng); Pressure_Offset = ConvertEngUnitsToInt(0.0, Pressure_cnt, Pressure_eng); Heading_Offset = ConvertDegreesToFract(COMPASS_OFFSET); FilteredHeading = ConvertDegreesToFract(IMU_DataA.yaw)-Heading_Offset; //OK to use DataA for this initialization since this won't be called until first IMU packet is recieved. first_time = 0; } #ifdef ALLOW_CALIBRATION if (raw) //Remove offset if in calibration mode { WingPos_Offset = 0; Pressure_Offset = 0; first_time = 1; } #endif // BuffferA/BufferB and Buff are unsigned integers, because the ADC is set up to represent data as unsigned fractionals (integers), // this is appropriate. Below all variables that get set from the ADC values are 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 it in 15 bits instead // of 16 doesn't change much. 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 < FILTERSAMPLES; j++) { WingPos_ADC[j] = _Q15sub(Buff[0 + NUMAIO * j] >> 1, WingPos_Offset); //AN4 Pressure_ADC[j] = _Q15sub(Buff[1 + NUMAIO * j] >> 1, Pressure_Offset); //AN11 } if (Nortek_Buff == 1) //writing int BuffA, so Buff B is valid { Xvel = Nortek_DataB.Vel_X; Yvel = Nortek_DataB.Vel_Y; Zvel = Nortek_DataB.Vel_Z; BeamAmp1 = Nortek_DataB.BeamAmp_1; BeamAmp2 = Nortek_DataB.BeamAmp_2; BeamAmp3 = Nortek_DataB.BeamAmp_3; BeamCorr1 = Nortek_DataB.BeamCorr_1; BeamCorr2 = Nortek_DataB.BeamCorr_2; BeamCorr3 = Nortek_DataB.BeamCorr_3; } else //writing int BuffB, so Buff A is valid { Xvel = Nortek_DataA.Vel_X; Yvel = Nortek_DataA.Vel_Y; Zvel = Nortek_DataA.Vel_Z; BeamAmp1 = Nortek_DataA.BeamAmp_1; BeamAmp2 = Nortek_DataA.BeamAmp_2; BeamAmp3 = Nortek_DataA.BeamAmp_3; BeamCorr1 = Nortek_DataA.BeamCorr_1; BeamCorr2 = Nortek_DataA.BeamCorr_2; BeamCorr3 = Nortek_DataA.BeamCorr_3; } if (IMU_Buff == 1) //writing int BuffA, so Buff B is valid { IMU_Data = &IMU_DataB; } else //writing int BuffB, so Buff A is valid { IMU_Data = &IMU_DataA; } MeasuredHeading_Buf = ConvertDegreesToFract(IMU_Data->yaw)-Heading_Offset; //Convert to fractional value for two's complement arithmetic, and adjust for compass orientation relative to platform #ifdef DEBUG // iTemp1 = Xvel; // iTemp2 = Yvel; // iTemp3 = Zvel; //iTemp1 = MeasuredHeading_Buf; //iTemp4 = (MeasuredHeading_Buf-FilteredHeading)/1024; // iTemp4 = HeadingFilterDivisorCounter; // iTemp1 = IMU_Data->groups; // iTemp2 = IMU_Data->group_field_1; // iTemp3 = IMU_Data->group_field_2; // iTemp4 = IMU_Data->group_field_3; #endif if (HeadingFilterDivisorCounter > 0) FilteredHeading = FilteredHeading + (int) (((((long) (MeasuredHeading_Buf - FilteredHeading)) << 16) / (((long) HeadingFilterDivisorCounter))) >> 16); else FilteredHeading = MeasuredHeading_Buf; #ifdef DEBUG // iTemp2 = FilteredHeading; #endif //Filter Analog Signals. //This uses the dsp library and changes the CORCON register. // Therefore interrupts that occur during this time must protect the CORCON and possibly other registers. FIR(FILTERSAMPLES, &outSamples[0], &WingPos_ADC[0], &ADC_Filter); WingPos_Buf = outSamples[FILTERSAMPLES - 1]; //Store current value for use later. FIR(FILTERSAMPLES, &outSamples[0], &Pressure_ADC[0], &ADC_Filter); Pressure_Buf = outSamples[FILTERSAMPLES - 1]; //Store current value for use later. // iTemp3 = WingPos_Buf; //iTemp4 = Pressure_Buf; Status.bits.RetractLimitSwitchState = EXTEND_LIMITSWITCH_INPUT; Status.bits.ExtendLimitSwitchState = RETRACT_LIMITSWITCH_INPUT; Status.bits.Mode = mode; time = time + 1.0 / CAN_PACKET_RATE; switch (LimitsNotFound) { case 3: if (Status.bits.RetractLimitSwitchState) { LATDbits.LATD6 = 0; //Enable Drive LATDbits.LATD5 = 1; //Set Direction to Retract OC5CONbits.OCM = 3; //Start Pulses (Toggle Pin on counter compare) } else { OC5CONbits.OCM = 0; //Stop Pulses LimitsNotFound = 2; f_RudderPosition = 0; LATDbits.LATD5 = 0; //Set Direction to Extend OC5CONbits.OCM = 3; //Start Pulses (Toggle Pin on counter compare) } break; case 2: if (Status.bits.ExtendLimitSwitchState) { f_RudderPosition += PosCtrIncr[speed]; } else { OC5CONbits.OCM = 0; //Stop Pulses LATDbits.LATD5 = 1; //Set Direction to Retract OC5CONbits.OCM = 3; //Start Pulses (Toggle Pin on counter compare) LimitsNotFound = 1; f_RudderPosition = f_RudderPosition / 2; RetractedRudderPosition = -f_RudderPosition; ExtendedRudderPosition = f_RudderPosition; } break; case 1: f_RudderPosition -= PosCtrIncr[speed]; deadband = PosCtrIncr[speed]/2; if ((f_RudderPosition <= deadband) && (f_RudderPosition >= -deadband)) { OC5CONbits.OCM = 0; //Stop Pulses LimitsNotFound = 0; } break; case 0: //Update Rudder Position if (OC5CONbits.OCM > 0) //Rudder has been moving for past isr cycle (.1 sec) { if (LATDbits.LATD5 == 1) // Retracting f_RudderPosition -= PosCtrIncr[speed]; //32767/((int)RetractedRudderPosition); else f_RudderPosition += PosCtrIncr[speed]; // 32767/((int) ExtendedRudderPosition); } if(!Status.bits.ExtendLimitSwitchState || !Status.bits.RetractLimitSwitchState) { OC5CONbits.OCM = 0; //Stop step pulses LATDbits.LATD6 = 1; //Disable Drive if(!Status.bits.ExtendLimitSwitchState) //Reset rudder position counter if limit swith hit. f_RudderPosition = ExtendedRudderPosition; if(!Status.bits.RetractLimitSwitchState) f_RudderPosition = RetractedRudderPosition; } speed = 2; PR2 = Speeds_PR2[speed]; deadband = PosCtrIncr[speed]/2; //iTemp2 = mode; HeadingError = HeadingTarget-FilteredHeading; #ifdef DEBUG // iTemp1 = HeadingTarget; // iTemp4 = HeadingError; #endif switch (mode) { case HEADING_CONTROL_MODE: if (HeadingTargetTimeout-- == 0) //Use default heading if there hasn't been a heading update within the timeout period set in config.h HeadingTarget = DefaultHeading; //These are signed int values corresponding to -180 -> 180. if (++UpdateCounter > HeadingControlUpdatePeriodCounter) { //Close heading control loop Heading_PID.controlReference = 0; Heading_PID.measuredOutput = _Q15neg(HeadingError); //Execute PID instructions //This uses the dsp library and changes the CORCON register. // Therefore interrupts that occur during this time must protect the CORCON and possibly other registers. PID(&Heading_PID); //Call the PID controller, result will be in VBus_PID.controlOutput as a Q15 //It would appear that interrupts that occur during the calling of the PID assembly routine tend //to cause an address error in PID.s after it gets back. I suspect this is because the CORCON register //is getting changed, but I'm not sure. The current solution is to not let interrupts occur while PID is running, //either by disabling them, or having no other interrupts present at a higher priority. // _U1RXIP = 7; //Set U1 Recieve interrupt to highest priority since this is occuring asyncronously to everything else. // __asm__ volatile ("disi #0x0000"); if(!Status.bits.ExtendLimitSwitchState && (Heading_PID.controlOutput >= ExtendedRudderPosition)) //Implement Wind-up limiters Heading_PID.controlOutput = ExtendedRudderPosition; if(!Status.bits.RetractLimitSwitchState && (Heading_PID.controlOutput <= RetractedRudderPosition)) //Implement Wind-up limiters Heading_PID.controlOutput = RetractedRudderPosition; // if(Heading_PID.controlOutput >= (int) (MaxRudderPosition*ExtendedRudderPosition)) //Implement Wind-up limiters // Heading_PID.controlOutput = (int) (MaxRudderPosition*ExtendedRudderPosition); // if(Heading_PID.controlOutput <= (int) (MaxRudderPosition*RetractedRudderPosition)) //Implement Wind-up limiters // Heading_PID.controlOutput = (int) (MaxRudderPosition*RetractedRudderPosition); RudderCmd = Heading_PID.controlOutput; //Heading_PID.controlOutput will always be between -32768 and +32767 UpdateCounter = 0; // iTemp3 = RudderCmd; } break; case RUDDERPOSITION_CONTROL_MODE: RudderCmd = SpecifiedRudderCmd; break; case BANGBANG_CONTROL_MODE: //iTemp3 = HeadingError; // iTemp2 = BangBang_Deadband; if (_Q15abs(HeadingError) > BangBang_Deadband) { if(HeadingError > 0) RudderCmd = (int) (MaxRudderPosition*ExtendedRudderPosition); else RudderCmd = (int) (MaxRudderPosition*RetractedRudderPosition); } else RudderCmd = 0; break; } //Implement Anti-Windup (Not needed if +32767/-32768 represents entire rudder range since PID() saturation will take care of anti-windup //Set direction as appropriate if (RudderCmd > f_RudderPosition) LATDbits.LATD5 = 0; else LATDbits.LATD5 = 1; if (_Q15abs(RudderCmd - f_RudderPosition) <= deadband) { OC5CONbits.OCM = 0; //Stop step pulses StoppedRudderCmd = RudderCmd; LATDbits.LATD6 = 1; //Disable Drive } else if ((_Q15abs(RudderCmd - StoppedRudderCmd) > RUDDER_DEADBAND*deadband) && (Status.bits.ExtendLimitSwitchState || (RudderCmd < f_RudderPosition)) && (Status.bits.RetractLimitSwitchState || (RudderCmd > f_RudderPosition))) //Only restart motion after Deadband if when RudderCmd has changed by more than deadband. This is neccessary because at 10Hz update, it overshoots a narrow deadband before turning off the steps. { LATDbits.LATD6 = 0; //Enable Drive OC5CONbits.OCM = 3; //Toggle Pin on counter compare) } break; } Status.bits.LimitSearchState = LimitsNotFound; #ifdef DEBUG // iTemp3 = RudderCmd; #endif //Send data over CAN SendCANPackets(Xvel,Yvel,Zvel,BeamAmp1,BeamAmp2,BeamAmp3,BeamCorr1,BeamCorr2,BeamCorr3,FilteredHeading,RudderCmd,IMU_Data); // while(C1TR01CONbits.TXREQ0 && C1TR01CONbits.TXREQ1); //Wait for CAN messages to go. i = 0; IFS0bits.DMA0IF = 0; //Clear the DMA interrupt flag bit (otherwise it seems to come right back from sleep) //Idle(); //This shuts down the main clock, but TMR1 is using the clock from SOSC so will continue to count and cause an interrupt // This interrupt, or a wake on bus activity interrupt for the can module will bring the device back from sleep. //PORTE = 0; //Turn both off; __asm__ volatile ("pop CORCON"); } // TMR1 Interrupt Runs at specified CAN packet rate, Starts ADC sampling, after enough samples are collected (specified by FILTERSAMPLES and REPORTINGSAMPLES), // the dma interrupt sends out the CAN packet and stops adc sampling until this isr starts it again. If sleep mode is being used, it is this // interrupt that wakes the processor from sleep generally, and the dma interrupt puts it back to sleep after the can packets have been sent. void __attribute__((interrupt, no_auto_psv)) _T1Interrupt(void) //void __attribute__((__interrupt__(__save__(CORCON)))) _T1Interrupt(void) { __asm__ volatile ("push CORCON"); //AD1CON1bits.ASAM = 1; //Auto-sample start bit _T1IF = 0; __asm__ volatile ("pop CORCON"); } void __attribute__((interrupt, no_auto_psv)) _C1Interrupt(void) { CANmsg RcvdMessage; char s[256]; signed int local_int; __asm__ volatile ("push CORCON"); if (C1INTFbits.RBIF) /* check to see if the interrupt is caused by receive */ { if (C1RXFUL1bits.RXFUL8) { /*check to see if buffer 8 is full, indicating CmdID was 10, and a change is being requested*/ DisAssembleCAN_Packet(8, &RcvdMessage); if (RcvdMessage.data_length >= 1) { switch (RcvdMessage.data[0]) { case 31: //Set Control Mode // sprintf(s, "Buf8 %d %d\r\n", RcvdMessage.data[0], RcvdMessage.data[1]); // putsU1(s); if (RcvdMessage.data[1] == RUDDERPOSITION_CONTROL_MODE) { SpecifiedRudderCmd = f_RudderPosition; mode = RUDDERPOSITION_CONTROL_MODE; } if (RcvdMessage.data[1] == HEADING_CONTROL_MODE) { mode = HEADING_CONTROL_MODE; } if (RcvdMessage.data[1] == BANGBANG_CONTROL_MODE) { mode = BANGBANG_CONTROL_MODE; } break; case 32: //Set User Specified Rudder Position local_int = ((signed int) ((RcvdMessage.data[2] << 8) + RcvdMessage.data[1])); //sprintf(s, "Buf8 %d %d %d %d\r\n", RcvdMessage.data[0], RcvdMessage.data[1], RcvdMessage.data[2], local_int); // putsU1(s); SpecifiedRudderCmd = (int) ExtendedRudderPosition * (((float) local_int) / 32767); break; case 33: //Set Heading Target local_int = ((signed int) ((RcvdMessage.data[2] << 8) + RcvdMessage.data[1])); // sprintf(s, "Buf8 %d %d %d %d\r\n", RcvdMessage.data[0], RcvdMessage.data[1], RcvdMessage.data[2], local_int); // putsU1(s); HeadingTarget = ConvertDegreesToFract((float) local_int); if (((long) HEADING_TIMEOUT) * CAN_PACKET_RATE < UINT_MAX) HeadingTargetTimeout = HEADING_TIMEOUT * CAN_PACKET_RATE; else HeadingTargetTimeout = UINT_MAX; //sprintf(s, "Buf8 %u \r\n", HeadingTargetTimeout); //putsU1(s); break; case 34: //Set Default Heading Value local_int = ((signed int) ((RcvdMessage.data[2] << 8) + RcvdMessage.data[1])); //sprintf(s, "Buf8 %d %d %d %d\r\n", RcvdMessage.data[0], RcvdMessage.data[1], RcvdMessage.data[2], local_int); // putsU1(s); DefaultHeading = ConvertDegreesToFract((float) local_int); //sprintf(s, "Buf8 %d \r\n", DefaultHeading); //putsU1(s); break; case 35: //Set Heading Filter Time Constant (passed in as seconds, converted to CAN update periods here for storage and use) local_int = ((signed int) ((RcvdMessage.data[2] << 8) + RcvdMessage.data[1])); // sprintf(s, "Buf8 %d %d %d %d\r\n", RcvdMessage.data[0], RcvdMessage.data[1], RcvdMessage.data[2], local_int); // putsU1(s); if ((local_int > 0) && (local_int <= 255)) HeadingFilterTimeConstant = local_int; else HeadingFilterTimeConstant = 0; //No filtering HeadingFilterDivisorCounter = HeadingFilterTimeConstant*CAN_PACKET_RATE; break; case 36: //Set Heading Control Loop Update Period ((passed in as seconds, converted to CAN update periods here for storage and use)) local_int = ((signed int) ((RcvdMessage.data[2] << 8) + RcvdMessage.data[1])); // sprintf(s, "Buf8 %d %d %d %d\r\n", RcvdMessage.data[0], RcvdMessage.data[1], RcvdMessage.data[2], local_int); // putsU1(s); if ((local_int > 0) && (local_int <= 255)) HeadingControlUpdatePeriod = local_int; else HeadingControlUpdatePeriod = 0; //Update at CAN Rate HeadingControlUpdatePeriodCounter = ((unsigned int) HeadingControlUpdatePeriod) * CAN_PACKET_RATE/10; break; case 37: //Set Heading Proportional Gain local_int = ((signed int) ((RcvdMessage.data[2] << 8) + RcvdMessage.data[1])); // sprintf(s, "Buf8 %d %d %d %d\r\n", RcvdMessage.data[0], RcvdMessage.data[1], RcvdMessage.data[2], local_int); // putsU1(s); if ((local_int >= 0) && (local_int <= 32767)) { kCoeffs_Heading[0] = local_int; //Prop Gain These gains must be between 0-32767 PIDCoeffCalc(&kCoeffs_Heading[0], &Heading_PID); } break; case 38: //Set Heading Integral Gain local_int = ((signed int) ((RcvdMessage.data[2] << 8) + RcvdMessage.data[1])); // sprintf(s, "Buf8 %d %d %d %d\r\n", RcvdMessage.data[0], RcvdMessage.data[1], RcvdMessage.data[2], local_int); // putsU1(s); if ((local_int >= 0) && (local_int <= 32767)) { kCoeffs_Heading[1] = local_int; //Integral Gain These gains must be between 0-32767 PIDCoeffCalc(&kCoeffs_Heading[0], &Heading_PID); } break; case 39: //Set Heading Derivative Gain local_int = ((signed int) ((RcvdMessage.data[2] << 8) + RcvdMessage.data[1])); // sprintf(s, "Buf8 %d %d %d %d\r\n", RcvdMessage.data[0], RcvdMessage.data[1], RcvdMessage.data[2], local_int); // putsU1(s); if ((local_int >= 0) && (local_int <= 32767)) { kCoeffs_Heading[2] = local_int; //Derivative Gain These gains must be between 0-32767 PIDCoeffCalc(&kCoeffs_Heading[0], &Heading_PID); } break; case 40: //Set Max Rudder Position local_int = ((signed int) ((RcvdMessage.data[2] << 8) + RcvdMessage.data[1])); // sprintf(s, "Buf8 %d %d %d %d\r\n", RcvdMessage.data[0], RcvdMessage.data[1], RcvdMessage.data[2], local_int); // putsU1(s); if ((local_int >= 0) && (local_int <= 32767)) { MaxRudderPosition = ((float) local_int)/32767; } break; case 41: //Set BangBand Deadband local_int = ((signed int) ((RcvdMessage.data[2] << 8) + RcvdMessage.data[1])); // sprintf(s, "Buf8 %d %d %d %d\r\n", RcvdMessage.data[0], RcvdMessage.data[1], RcvdMessage.data[2], local_int); // putsU1(s); if ((local_int >= 0) && (local_int <= 90)) { BangBang_Deadband = ConvertDegreesToFract(local_int); } break; case 0xff: asm ("RESET"); //Reset micro-controller. default: break; } } C1RXFUL1bits.RXFUL8 = 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"); }