#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 "CalibrationConstants.h" #include "libq.h" #include "SendCANPackets.h" #include "ControlChargeTime.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) { int j; static int first_time = 1; static fractional TetherVoltage_Offset; static fractional BattVoltage_Offset; static fractional Pressure_Offset; static fractional Heading_Offset; static int TetherGoneVoltage; 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. { BattVoltage_Offset = ConvertEngUnitsToInt(0.0, BattVoltage_cnt, BattVoltage_eng); TetherVoltage_Offset = ConvertEngUnitsToInt(0.0, TetherVoltage_cnt, TetherVoltage_eng); Pressure_Offset = ConvertEngUnitsToInt(0.0, Pressure_cnt, Pressure_eng); Heading_Offset = ConvertDegreesToFract(COMPASS_OFFSET); TetherGoneVoltage = ConvertEngUnitsToFract(TETHER_GONE_VOLTAGE, TetherVoltage_cnt, TetherVoltage_eng); first_time = 0; } #ifdef ALLOW_CALIBRATION if (raw) //Remove offset if in calibration mode { BattVoltage_Offset = 0; TetherVoltage_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++) { Pressure_ADC[j] = _Q15sub(Buff[0 + NUMAIO * j] >> 1, Pressure_Offset); //AN11 TetherVoltage_ADC[j] = _Q15sub(Buff[1 + NUMAIO * j] >> 1, TetherVoltage_Offset); //AN17 BattVoltage_ADC[j] = _Q15sub(Buff[2 + NUMAIO * j] >> 1, BattVoltage_Offset); //AN18 } asm volatile ("disi #0x3FFF"); //Disable interrupts while most recent IMU_Data packet is copied to local structure. 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; asm volatile ("disi #0x0000"); 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 //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. asm volatile ("disi #0x3FFF"); FIR(FILTERSAMPLES, &outSamples[0], &BattVoltage_ADC[0], &ADC_Filter); asm volatile ("disi #0x0000"); BattVoltage_Buf = outSamples[FILTERSAMPLES - 1]; //Store current value for use later. asm volatile ("disi #0x3FFF"); FIR(FILTERSAMPLES, &outSamples[0], &TetherVoltage_ADC[0], &ADC_Filter); asm volatile ("disi #0x0000"); TetherVoltage_Buf = outSamples[FILTERSAMPLES - 1]; //Store current value for use later. asm volatile ("disi #0x3FFF"); FIR(FILTERSAMPLES, &outSamples[0], &Pressure_ADC[0], &ADC_Filter); asm volatile ("disi #0x0000"); Pressure_Buf = outSamples[FILTERSAMPLES - 1]; //Store current value for use later. time_ctr++; //Increment time counter. This is used only in main to know when time at CANPACKETRATE resolution has increased. //Send data over CAN SendCANPackets(&IMU_Data); // while(C1TR01CONbits.TXREQ0 && C1TR01CONbits.TXREQ1); //Wait for CAN messages to go. if((++CAN_PacketSend_ctr)%600 == 0) { if(comms_loss_timeout > 0) //Stop decrementing after timeout value is reached, eliminates counter rollover comms_loss_timeout--; //Count seconds, this counter is reset to zero when a CAN packet is recieved, reset in CmdInterpreter.c when watchdog reset command is recieved. if(power_loss_timeout > 0) //Stop decrementing after timeout value reaches zero, eliminates counter rollover power_loss_timeout--; //Count seconds, this counter is reset to zero when tether power is present, checked and reset below } if(TetherVoltage_Buf > TetherGoneVoltage) { power_loss_timeout = POWERLOSS_TIMEOUT; //Reset Power Loss Timeout counter if Tether Voltage is deemed valid } if(power_loss_timeout == 0) //Power has been gone for some time, { Status.bits.DoorPositionTarget = 0; //Request Doors to open } if(comms_loss_timeout == 0) //Comms have been gone for some time, { Status.bits.DoorPositionTarget = 0; //Request Doors to open } //If Power has been lost and doors are open, kill battery power to controller which will shut down controller until fresh re-boot when tether power returns. if((power_loss_timeout == 0) && (Status.bits.DoorPositionActual == 0)) { MAXON_POWER = 0; MAXON_TETHER_POWER = 0; unsigned int countdown = 65000; while(countdown-- > 0); CTRL_BATT_POWER = 0; //This removes battery power from controller, which will also turn off Maxon because active high output from controller causes power to flow to maxon. This is the end, my friend. } #ifndef KEYBOARD_TESTING ControlChargeTimes(); #endif 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, set here if timer expires. Timer is reset to zero when a data packet from the IMU comes in, which is the primary way ADC sampling starts, this is a backup. _T1IF = 0; __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 threw 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; } /* 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"); }