int main(void) { //******************************************************************* Init(); TMRInit(2); // Initialize Timer interface with Priority=2 BLIInit(); // Initialize Signal interface ADCInit(3); // Initialize ADC UARTInitTX(6, 350); // Initialize UART1 for TX // This initialization routine accepts BaudRate in multiples // of 2400 bps; Thus: // BaudRate = 1 => 2400 bps // BaudRate = 2 => 4800 bps // ... // BaudRate = 48 => 115200 bps //------------------------------------------------------------ // High speed //------------------------------------------------------------ // BaudRate = 100 => 250,000 bps // BaudRate = 200 => 500,000 bps // BaudRate = 250 => 625,000 bps // BaudRate = 350 => 833,333 bps // BaudRate = 500 => 1,250,000 bps // BaudRate = 1000 => 2,500,000 bps //******************************************************************* struct { float V; uint Raw; uint Stat; } UData; int i = 0; int j = 0; BLISignalON(); while(j < 10) { TMRDelay(10); //-------------------------- UData.V = ADCGetBatteryVoltage(); UData.Raw = ADCGetRawSample(); UData.Stat = ADCGetBatteryStatus(); //-------------------------- UARTPostWhenReady((uchar*)&UData, sizeof(UData)); i++; //-------------------------- if (i >= 100) { UARTPostWhenReady(NULL, 0); BLISignalFlip(); //------------------------ i = 0; j++; } TMRDelay(10); } return 1; }
int main(void) { //******************************************************************* DBGInit(); // Initialize Debug interface DBG_1_ON(); Init(); DBG_1_Flip(); TMRInit(2); // Initialize Timer interface with Priority=2 BLIInit(); // Initialize Signal interface //******************************************************************* ulong i; DBG_1_ON(); BLIAsyncMorse("SSS", 3); while(1) { TMRDelay(50); // Delay 50 msec DBG_1_Flip(); DBG_2_Flip(); } /* BLIAsyncStart(30, 30); while(1) {TMRDelay(30);} */ BLISignalON(); while (1) { // TMRDelay(50); for (i=0; i < 1000001; i++); BLISignalFlip(); } while(1); { for (i=0; i < 1000001; i++); BLISignalFlip(); } while(1) { BLIAsyncStart(500, 1000); TMRDelay(5000); // Delay 5 sec //-------------------------------- BLIAsyncMorse("SOS", 3); TMRDelay(5000); // Delay 5 sec } //******************************************************************* return 1; }
int main(void) { //******************************************************************* Init(); TMRInit(2); // Initialize Timer interface with Priority=2 BLIInit(); // Initialize Signal interface //******************************************************************* //_T1IE = 0; // Temporarily disable Timer1 interrupt //******************************************************************* LogInit(); // Initialize Data Logger //******************************************************************* // Initialize UART for RX on IPL=6 at //------------------------------------------------------------------- // BaudRate = 48 => 115,200 bps //-------------------------------------- // BaudRate = 100 => 250,000 bps // BaudRate = 200 => 500,000 bps // BaudRate = 250 => 625,000 bps // BaudRate = 350 => 833,333 bps // BaudRate = 500 => 1,250,000 bps // BaudRate = 1000 => 2,500,000 bps //------------------------------------------------------------------- // Switch 1 controls UART speed - if the switch is in the "On" // position, UART initialized for high speed (833,333 bps), otherwise // UART is initialized at low speed (115,200 bps) compatible with // XBee. //------------------------------------------------------------------- if (_SW1) UARTInitRX(6, 350); // High speed: 833,333 bps else UARTInitRX(6, 48); // Low speed: 115,200 bps //******************************************************************* UARTDataBlock* DataBlock = NULL; UARTRXStat Stats; WORD RC = 0; int i; //----------------------------------- // Start Async read on UART in BLOCK mode while ( !(RC = UARTRXStartAsyncRead(64, TRUE, TRUE)) ); for (i = 0; i < 10; i++) { DataBlock = UARTReadBlock(); BLISignalON(); TMRDelay(50); UARTFreeBlock(DataBlock); BLISignalOFF(); } UARTRXStopAsyncRead(); UARTRXGetStat(&Stats); //------------------------------ i = sizeof(byte*); //------------------------------ while(1); return 0; }
//******************************************************************* // SR04ResetBase() function adjust altitude to 0 taking into account // distance from sensor to the ground when quad is on the ground. //==================================================================== uint SR04ResetBase() { if (0 == _SR04Init) return -1; // Component not initialized //---------------------------------------------------------------- SR04Data LocalSample; //---------------------------------------------------------------- _SR04Offset = 0.0; // Reset ground offset SR04ReadWhenReady(&LocalSample); // Clear "pipeline" TMRDelay(500); // Let at least 10 samples be captured SR04ReadWhenReady(&LocalSample); // Read sample averaged over 0.5 sec //---------------------------------------------------------------- // Capture current "altitude" as ground offset //---------------------------------------------------------------- _SR04Offset = LocalSample.Altitude; //---------------------------------------------------------------- return 0; }
//************************************************************* uint MPLAsyncReadWhenReady(MPLData* pSample) { if (0 == _MPL_Async) return MPL_NACT; // Asynchronous read not active... //-------------------------------------------------- while (0 == _MPL_Ready) // Wait until READY { if (TMRGetTS() - _MPL_DataTS > _MPL_MaxInt) // Looks like the Interrupt went missing... { MPL_IF = 1; // Trigger interrupt manually - emulate // rising edge // We did our best - now wait for Data Ready... while (0 == _MPL_Ready); } else // Introduce some delay... TMRDelay(2); } //------------------------ return _MPLAsyncRead(pSample); }
int main(void) { //******************************************************************* Init(); TMRInit(2); // Initialize Timer interface with Priority=2 BLIInit(); // Initialize Signal interface //******************************************************************* //_T1IE = 0; // Temporarily disable Timer1 interrupt //******************************************************************* LogInit(); // Initialize Data Logger UARTInitRX(6, 48); // Initialize UART RX at 115,200 baud //******************************************************************* UARTDataBlock* DataBlock = NULL; UARTRXStat Stats; WORD RC = 0; int i; //----------------------------------- // Start Async read on UART in BLOCK mode while ( !(RC = UARTRXStartAsyncRead(64, TRUE, TRUE)) ); for (i = 0; i < 10; i++) { DataBlock = UARTReadBlock(); BLISignalON(); TMRDelay(50); UARTFreeBlock(DataBlock); BLISignalOFF(); } UARTRXStopAsyncRead(); UARTRXGetStat(&Stats); //------------------------------ i = sizeof(byte*); //------------------------------ while(1); return 0; }
int main(void) { //******************************************************************* Init(); TMRInit(2); // Initialize Timer interface with Priority=2 BLIInit(); // Initialize Signal interface //******************************************************************* // Switch 1 controls the Serial Data Logger (SDL) communication speed //------------------------------------------------------------------- if (_SW1) // Switch 1 is ON - Configuring SDL for PIC-to-PIC // high-speed communication at 1 MBaud SDLInit(3, BAUD_1M); else // Switch 1 is OFF - Configuring SDL for ZigBEE // wireless communication at 115.2 KBaud SDLInit(3, BAUD_115200); //******************************************************************* I2CInit(5, 1); // First param: IL = 5 (interrupt request priority // Second param: I2C speed // 0 - lowest (123 kHz at Fcy = 64MHz) // 1 - 200 kHz // 2 - 400 kHz // 3 - 1 MHz //------------------------------------------------------------------- uint RC = 0; ulong Alarm = 0; //================================================================== BLIAsyncStart(100,100); TMRDelay(2000); BLIAsyncStop(); //================================================================== BLIAsyncStart(50,50); if (_SW2) // Switch 2 is ON - Configuring MPU fo Alt. sensitivity RC = MPUInit(0, 3, MPU_GYRO_1000ds, MPU_ACC_4g); // Initialize motion Sensor // 1 kHz/(0+1) = 1000 Hz (1ms) // DLPF=3 => Bandwidth 44 Hz (delay: 4.9 msec) else // Switch 2 is OFF - Configuring MPU fo normal sensitivity RC = MPUInit(0, 3, MPU_GYRO_2000ds, MPU_ACC_2g); // Initialize motion Sensor // 1 kHz/(0+1) = 1000 Hz (1ms) if (RC) BLIDeadStop("EG", 2); BLIAsyncStop(); //******************************************************************* BLISignalOFF(); //==================================================== // byte mpuID; // byte mpuDLPF; // byte mpuINT; // byte mpuPWRM1; // //--------------------------- // RC = MPUReadID(2, &mpuID); // RC = MPUGetPWRM1(2, &mpuPWRM1); // RC = MPUGetDLPF(2, &mpuDLPF); // RC = MPUGetINT(2, &mpuINT); //----------------------------------------------------- //==================================================== // Synchronous interface //----------------------------------------------------- struct { MPUData Sample1; MPUData Sample2; } MPU; //----------------------------------------------------- while (TRUE) { Alarm = TMRSetAlarm(500); //------------------------------------ if ( (RC = MPUReadSample(1, &MPU.Sample1)) ) BLIDeadStop("SOS", 3); //------------------------ if ( (RC = MPUReadSample(2, &MPU.Sample2)) ) BLIDeadStop("SOS", 3); //------------------------------------ BLISignalFlip(); //------------------------- SDLPostIfReady((byte*)&MPU, sizeof(MPU)); //------------------------- TMRWaitAlarm(Alarm); } //******************************************************************* return 0; }
int main(void) { //******************************************************************* Init(); TMRInit(2); // Initialize Timer interface with Priority=2 BLIInit(); // Initialize Signal interface I2CInit(5, 0); // Initialize I2C1 module with IPL=5 and Fscl=400 KHz //-------------------------- TMRDelay(1000); // Wait for 1 sec so that the shake from turning on // power switch dissipates... //-------------------------- if (MPUInit(3, 1)) // Initialize motion Sensor - 1 kHz/4 (250 Hz) BLIDeadStop("EA", 2); //-------------------------- #ifdef __MAG_Use__ if (HMCInit(6, 1, 0)) // Initialize magnetic Sensor // ODR = 6 (max, 75 Hz), // Gain = 2 (1.3 Gs) // DLPF = 0 (no averaging) BLIDeadStop("EM", 2); #endif //-------------------------- UARTInitTX(6, 48); // Initialize UART1 for TX on IPL=6 at 115200 bps // This initialization routine accepts BaudRate in multiples // of 2400 bps; Thus: // BaudRate = 1 => 2400 bps // BaudRate = 2 => 4800 bps // ... // BaudRate = 48 => 115200 bps //------------------------------------------------------------ // High speed //------------------------------------------------------------ // BaudRate = 100 => 250,000 bps // BaudRate = 200 => 500,000 bps // BaudRate = 250 => 625,000 bps // BaudRate = 350 => 833,333 bps // BaudRate = 500 => 1,250,000 bps // BaudRate = 1000 => 2,500,000 bps //******************************************************************* uint RC = 0; //-------------------------- MPUSample AGSample; #ifdef __MAG_Use__ HMCSample MSample; #endif //-------------------------- if (MPUAsyncStart()) BLIDeadStop("A", 1); //-------------------------- #ifdef __MAG_Use__ if (HMCAsyncStart()) BLIDeadStop("M", 1); #endif //-------------------------- struct { ulong TS; // Timestamp of the cycle //----------------------------------------------- ulong MPUCount; // Sequential number of MPU sample #ifdef __MAG_Use__ ulong MAGCount; // Sequential number of MAG sample #endif //----------------------------------------------- // Accelerometer (in units of G) //----------------------------------------------- Vector A; //----------------------------------------------- // Gyroscopes (in Rad/sec) //----------------------------------------------- Vector G; #ifdef __MAG_Use__ //----------------------------------------------- // Magnetometer (in mGs) //----------------------------------------------- Vector M; #endif } UData; //******************************************************************* BLISignalON(); while(1) { TMRDelay(100); //------------------------ #ifdef __MAG_Use__ RC = HMCAsyncReadWhenReady(&MSample); if (RC) BLIDeadStop("M", 1); #endif //------------------------ RC = MPUAsyncReadWhenReady(&AGSample); if (RC) BLIDeadStop("A", 1); //--------------------------------------------- UData.MPUCount = AGSample.Count; #ifdef __MAG_Use__ UData.MAGCount = MSample.Count; #endif //------------------------ VectorCopy(&AGSample.A, &UData.A); VectorCopy(&AGSample.G, &UData.G); #ifdef __MAG_Use__ VectorCopy(&MSample.M, &UData.M); #endif //------------------------ UData.TS = AGSample.TS; //--------------------------------------------- UARTPostWhenReady((uchar*)&UData, sizeof(UData)); //--------------------------------------------- BLISignalFlip(); } return 1; }
//************************************************************* uint MPUAsyncAdjustAccZBase(float Incl) { if (0 == _MPU_Async) return MPU_NACT; // Asynchronous read not active... //-------------------------------------------------- TMRDelay(2000); // Wait 2,000 msec (2 second) to // accumulate in the buffer enough // samples for for solid averaging //---------------------------------------------- uint Ready_Cnt; _MPURawData RawData; //---------------------------------------------- int current_cpu_ipl; //---------------------------------------------- //============================================== // Enter MPU/I2C CRITICAL SECTION //---------------------------------------------- SET_AND_SAVE_CPU_IPL(current_cpu_ipl, _MPU_IL); /* disable interrupts */ //----------------------------------------------- RawData.Temp = _MPU_Sample.Temp; //--------------------------------- RawData.AX = _MPU_Sample.AX; RawData.AY = _MPU_Sample.AY; RawData.AZ = _MPU_Sample.AZ; //--------------------------------- RawData.GX = _MPU_Sample.GX; RawData.GY = _MPU_Sample.GY; RawData.GZ = _MPU_Sample.GZ; //----------------------------------------------- Ready_Cnt = _MPU_Ready; //----------------------------------------------- _MPU_Ready = 0; // Sample consumed... //---------------------------------------------- // Leave MPU/I2C CRITICAL SECTION //============================================== RESTORE_CPU_IPL(current_cpu_ipl); //============================================== // Adjust Sample Weight to account for multiple samples //---------------------------------------------- float Weight; if (Ready_Cnt > 1) Weight = 1.0/(float)Ready_Cnt; else Weight = 1.0; //---------------------------------------------- // Process collected sample //---------------------------------------------- // Temperature (C) (will be used in subsequent // temperature compensation calculation) //----------------------------------------------- float Temp = (Weight * RawData.Temp - _MPU_Temp_OffsetTo0) * _MPU_Temp_Sensitivity; //----------------------------------------------- // Acceleration (G) //----------------------------------------------- float TDev = Temp - _MPU_Accl_BaseTemp; //----------------------------------------------- // AccZBase = (Weight * RawData.AZ - _MPU_Accl_ZOffset - _MPU_Accl_ZSlope*TDev) * _MPU_Accl_Sensitivity; // NOTE: Incl is the cosine of the angle between Earth gravity // ( -1G given given Z-axis orientation to be "down"), thus // Expected AccZ => -Incl; AccZ should appear on the right side // with the "-" sign and as two "-" becomes "+" we can use Incl // directly in the formula below _MPU_Accl_ZOffset = Incl/_MPU_Accl_Sensitivity + Weight * RawData.AZ - _MPU_Accl_ZSlope*TDev; //----------------------------------------------- return MPU_OK; // The return code was OK }
int main(void) { // <editor-fold defaultstate="collapsed" desc="Initialization of HW components/modules"> //******************************************************************* // Initialization of HW components/modules //=================================================================== Init(); TMRInit(2); // Initialize Timer interface with Priority=2 BLIInit(); // Initialize Signal interface //-------------------------- BLIAsyncMorse("S", 1); // dit-dit-dit MCMInitT(2.5, 2500); // Initialize Motor Control for PPM with setting // Throttle to HIGH for delay interval to let ESC // capture Throttle range BLIAsyncMorse("O", 1); // dah-dah-dah //-------------------------- ADCInit(3); // Initialize ADC to control battery //-------------------------- RCInit(4); // Initialize Receiver interface with Priority=4 //-------------------------- I2CInit(5, 1); // Initialize I2C1 module with IPL=5 and Fscl=400 KHz //-------------------------- UARTInitTX(6, 350); // Initialize UART1 for TX on IPL=6 at // BaudRate = 48 => 115,200 bps - ZigBEE //-------------------------------------- // BaudRate = 100 => 250,000 bps // BaudRate = 200 => 500,000 bps // BaudRate = 250 => 625,000 bps // BaudRate = 350 => 833,333 bps - SD Logger // BaudRate = 500 => 1,250,000 bps // BaudRate = 1000 => 2,500,000 bps //******************************************************************* // <editor-fold defaultstate="collapsed" desc="Initializing IMU"> //================================================================== #ifdef __MAG_Use__ //-------------------------------------------------------------- // Initialize Magnetometer //-------------------------------------------------------------- if (HMCInit(6, 1, 0)) // Initialize magnetic Sensor // ODR = 6 (max, 75 Hz), // Gain = 2 (1.3 Gs) // DLPF = 0 (no averaging) BLIDeadStop("EM", 2); #endif //******************************************************************* BLIAsyncMorse( "I", 1); // dit - dit //================================================================== #ifdef __MXB_Use__ //-------------------------------------------------------------- // Initialize MaxBotix range finder //-------------------------------------------------------------- if ( 0 == MXBInit(3, &TM.MXB) ) BLIDeadStop("ES", 2); #endif //================================================================== // Initialize motion sensor - rotation rate baseline established at // this time - QUAD should be motionless!!! //------------------------------------------------------------------ if ( MPUInit(0, 3 ) ) // Initialize motion Sensor // 1 kHz/(0+1) = 1000 Hz (1ms) // DLPF=3 => Bandwidth 44 Hz (delay: 4.9 msec) BLIDeadStop("EA", 2); //------------------------------------------------------------------ BLIAsyncStop(); //================================================================== // </editor-fold> //================================================================== BLISignalON(); TMRDelay(2000); // Wait for extra 2 sec - to let ESC arm... // (finish the song :) ) BLISignalOFF(); //=================================================================== // </editor-fold> //******************************************************************* // Quadrocopter control variables //------------------------------------------------------------------- ulong StartTS = 0; ulong StepTS; RCData RCNative; // Control input from Receiver RCData RC; // Smoothed control input used in all // control calculations DCMData IMU; // Orientation data from DCM MCMData MC; // Motor control Variables float BatNomV = ADCGetBatteryNomVoltage(); ulong Alarm = 0; #ifdef __CB_To_Model_Front__ // RC native input rotated to adjust CB orientation to model front RCData RC_CB_To_Model_Front; // Control board front does not coincide with the model front Matrix CB_To_Model; // Build rotation matrix to adjust for orientation discrepancy MatrixYawRotation(__CB_To_Model_Front__, &CB_To_Model); #else // If CB orientation coincides with the fron of the model // we will use Native RC input as base for RC input to // Quadrocopter control module #define RC_CB_To_Model_Front RCNative #endif Re_Start: //================================================================== // Wait for the Receiver ARMing: Control should go Down and then Up //------------------------------------------------------------------ BLIAsyncMorse( "O", 1); // doh - doh - doh RCSymArm(); //================================================================== BLIAsyncMorse("T", 1); // doh MPUAsyncStop(); if (MPUCalibrate() != MPU_OK) // Gyro Calibration filed BLIDeadStop("EA", 2); BLIAsyncStop(); //================================================================== // Start IMU and wait until orientation estimate stabilizes //------------------------------------------------------------------ BLIAsyncMorse( "E", 1); // dit IMUInit(); //------------------------------------------------------------------ QCMReset(); // Initialize (reset) QCM variables //------------------------------------------------------------------ BLIAsyncStop(); //================================================================== //****************************************************************** // Control variables used to smooth RC receiver data //------------------------------------------------------------------ // Reset Smothed RC data //------------------------------------------------------------------- RC.Roll = 0.0; RC.Pitch = 0.0; RC.Yaw = 0.0; //------------------------ RC.Throttle = 0.0; //------------------------ RC.Control = 1; //------------------------------------------------------------------- //******************************************************************* // Quadrocopter Control Loop //------------------------------------------------------------------- BLISignalON(); while (1) { // Sets the "frquency" of Control Loop Alarm = TMRSetAlarm(10); // Set Alarm 10 msec in the future //============================================================ // Read commands from receiver - non-blocking call! // (we will get out of this call even if connection to the // receiver is lost!) //============================================================ // <editor-fold defaultstate="collapsed" desc="Process Receiver feed"> if ( RCSymRead(&RCNative) ) { //------------------------------------------------------------ // Normalize Roll and Pitch control input from RC Receiver to // +/- 0.35 rad (~20 degrees) and Yaw control input to // +/- 3.00 rad (~172 degrees) //------------------------------------------------------------ RCNative.Roll = 0.35 * RCNative.Roll; RCNative.Pitch = 0.35 * RCNative.Pitch; RCNative.Yaw = 3.00 * RCNative.Yaw; #ifdef __CB_To_Model_Front__ { // Control board front does not coincide with the model front Vector RCInput; Vector RCRotated; VectorSet(RCNative.Roll, RCNative.Pitch, RCNative.Yaw, &RCInput); MatrixTimesVector(&CB_To_Model, &RCInput, &RCRotated); RC_CB_To_Model_Front.Roll = RCRotated.X; RC_CB_To_Model_Front.Pitch = RCRotated.Y; RC_CB_To_Model_Front.Yaw = RCRotated.Z; } #endif } //============================================================ // Smooth RC data //------------------------------------------------------------ // Roll, Pitch, and Yaw are smoothed with the IIR(8) //------------------------------------------------------------ RC.Roll = (RC.Roll * 7.0 + RC_CB_To_Model_Front.Roll ) * 0.125; // 1/8 = 0.125 RC.Pitch = (RC.Pitch * 7.0 + RC_CB_To_Model_Front.Pitch) * 0.125; RC.Yaw = (RC.Yaw * 7.0 + RC_CB_To_Model_Front.Yaw ) * 0.125; //------------------------------------------------------------ // Throttle is smoothed with the IIR(4) and adjusted to // account for actual battery voltage. This is done to // improve "hovering" when throttle stick is not moving. //------------------------------------------------------------ // Adjust Native (from RC) throttle to a value corresponding float BatAdjTh = RCNative.Throttle * BatNomV / ADCGetBatteryVoltage(); //----------------------------------------- RC.Throttle = (RC.Throttle * 3 + BatAdjTh) * 0.25; // 1/4 = 0.25 //----------------------------------------- RC.Control = RCNative.Control; // </editor-fold> //============================================================ //============================================================ // Implement Motor Cut-Off if RC Control is LOW //============================================================ // <editor-fold defaultstate="collapsed" desc="Process RC Control"> if ( 0 == RC.Control ) { // Yes, Control is reliably low! //-------------------------------------------- // Override motor control //-------------------------------------------- MC.F = MC.B = MC.L = MC.R = 0; MCMSet(&MC); //-------------------------------------------- // Reset Timing series StartTS = 0; //-------------------------------------------- // Flight terminated, post EOF to Data Logger //-------------------------------------------- TMRDelay(10); // Wait for pipe to clear UARTPostIfReady( NULL, 0); // ... and again - to be sure! TMRDelay(10); // Wait for pipe to clear UARTPostIfReady( NULL, 0); //---------------------------------- goto Re_Start; } // </editor-fold> //============================================================ //============================================================ // Obtain and process battery charge status //============================================================ // <editor-fold defaultstate="collapsed" desc="Process Battery level"> float BatteryCharge = QCMBatteryMgmt(); if (BatteryCharge < 0.35) // BC < 35% { float MaxLevel = 2.0 * BatteryCharge; if (RC.Throttle > MaxLevel) RC.Throttle = MaxLevel; } // </editor-fold> //============================================================ //============================================================ // Read current orientation and sensor data from the IMU // (non-blocking call) //============================================================ if (IMU_OK == IMUGetUpdate(&IMU)) { // Calculate motor control based // upon IMU data QCMPerformStep(&RC, &IMU, &MC); } else { // IMU Failed to provide update - // set Motor Control to native throttle MC.F = MC.B = MC.L = MC.R = RC.Throttle; } //============================================================ //***************************************** // Implement Motor Cut-Off if model is // dangerously tilted (> 60 degrees) while // RC Throttle is low - to protect props //---------------------------------------- if (IMU.Incl <= 0.5 && RC.Throttle <= 0.40) { // Override motor control MC.F = MC.B = MC.L = MC.R = 0; } //---------------------------------------- //***************************************** // Update motor control //***************************************** MCMSet(&MC); //----------------------------------------- //=========================================================== // Load and post telemetry data (non-blocking call) //----------------------------------------------------------- // <editor-fold defaultstate="collapsed" desc="Populating Telemetry"> StepTS = TMRGetTS(); //----------------------------------------- if ( 0 == StartTS ) // Time stamp of cycle start! StartTS = StepTS; //----------------------------------------- TM.TS = StepTS - StartTS; //---------------------- TM.Roll = IMU.Roll; TM.Pitch = IMU.Pitch; TM.Yaw = IMU.Yaw; TM.Inclination = IMU.Incl; TM.Azimuth = IMU.Azimuth; //---------------------- #ifdef _TMReport_GYRO_ #ifdef _TMReport_GYRO_Rotated //------------------------------------------------------ // We rotate Gyro vector using "partial" DCM built using // only Roll and Pitch angles as the actual Yaw does not // affect Angualr Velocity by axis //------------------------------------------------------ Matrix NoYawDCM; // Generate partial rotation matrix MatrixEulerRotation(IMU.Roll, IMU.Pitch, 0.0, &NoYawDCM); // Rotate Gyro vector MatrixTimesVector(&NoYawDCM, &IMU.GyroRate, &TM.Gyro); #else // Just report native Gyro data VectorCopy(&IMU.GyroRate, &TM.Gyro); #endif #endif //---------------------- #ifdef _TMReport_ACC_ // Just report native Gyro data VectorCopy(&IMU.Gravity, &TM.Acc); #endif //---------------------- TM.RollDer = QSD.RollDer; TM.PitchDer = QSD.PitchDer; TM.YawDer = QSD.YawDer; //---------------------- TM.RC_Throttle = RC.Throttle; TM.RC_Roll = RC.Roll; TM.RC_Pitch = RC.Pitch; TM.RC_Yaw = RC.Yaw; //---------------------- #ifdef _TMReport_NativeRC_ TM.RCN_Throttle = RCNative.Throttle; TM.RCN_Roll = RCNative.Roll; TM.RCN_Pitch = RCNative.Pitch; TM.RCN_Yaw = RCNative.Yaw; #endif //---------------------- #ifdef __MXB_Use__ MXBRead(&TM.MXB); #endif //---------------------- #ifdef _TMReport_PID_ #ifdef _TMReport_PID_Details TM.DRProp = QSD.DeltaRollProp; TM.DRDiff = QSD.DeltaRollDiff; TM.DRInt = QSD.DeltaRollInt; #endif TM.DRTot = QSD.DeltaRoll; //------------- #ifdef _TMReport_PID_Details TM.DPProp = QSD.DeltaPitchProp; TM.DPDiff = QSD.DeltaPitchDiff; TM.DPInt = QSD.DeltaPitchInt; #endif TM.DPTot = QSD.DeltaPitch; //------------- #ifdef _TMReport_PID_Details TM.DYProp = QSD.DeltaYawProp; TM.DYDiff = QSD.DeltaYawDiff; TM.DYInt = QSD.DeltaYawInt; #endif TM.DYTot = QSD.DeltaYaw; #endif //---------------------- TM.Throttle = QSD.Throttle; // Real Throttle //---------------------- TM.Voltage = QSD.Voltage; // </editor-fold> //=========================================================== UARTPostIfReady( (unsigned char *) &TM, sizeof(TM) ); //=========================================================== // Insert controlled "delay" to slow down the Control Loop TMRWaitAlarm(Alarm); } //******************************************************************* return 1; }
int main(void) { //******************************************************************* // Initialization of HW components/modules //=================================================================== Init(); TMRInit(2); // Initialize Timer interface with Priority=2 BLIInit(); // Initialize Signal interface //-------------------------- BLIAsyncMorse("S", 1); // dot-dot-dot UARTInitTX(6, 48); // Initialize UART1 for TX on IPL=6 at // BaudRate = 48 => 115,200 bps - ZigBEE //-------------------------------------- // BaudRate = 100 => 250,000 bps // BaudRate = 200 => 500,000 bps // BaudRate = 250 => 625,000 bps // BaudRate = 350 => 833,333 bps - SD Logger // BaudRate = 500 => 1,250,000 bps // BaudRate = 1000 => 2,500,000 bps //=================================================================== //******************************************************************* // Quadrocopter control variables //------------------------------------------------------------------- ulong StartTS = 0; ulong StepTS; //================================================================== // Initialize sensors... //------------------------------------------------------------------ BLIAsyncMorse( "I", 1); //============================================================ // Let MaxBotix initialize HW //============================================================ TMRDelay(1000); //================================================================== // Initialize MaxBotix range finder //-------------------------------------------------------------- if (0 == MXBInit(4, &TM.MXBDLPF)) BLIDeadStop("ES", 2); //-------------------------------------------------------------- BLIAsyncStop(); //******************************************************************* // Control Loop //------------------------------------------------------------------- BLISignalON(); while (1) { //============================================================ // Simulate control loop duration //============================================================ TMRDelay(10); //----------------------------------------- //=========================================================== // Load and post telemetry data (non-blocking call) //----------------------------------------------------------- StepTS = TMRGetTS(); //----------------------------------------- if (0 == StartTS) // Time stamp of cycle start! StartTS = StepTS; //----------------------------------------- TM.TS = StepTS - StartTS; //---------------------- TM.BaseOffset = _MXBOffset; MXBReadDLPF(8, &TM.MXBDLPF); memcpy(&TM.MXBNative, &_NewMXBReading, sizeof(TM.MXBNative)); //=========================================================== UARTPostIfReady( (unsigned char *) &TM, sizeof(TM) ); //=========================================================== } //******************************************************************* return 1; }
int main(void) { //******************************************************************* Init(); TMRInit(2); // Initialize Timer interface with Priority=2 BLIInit(); // Initialize Signal interface //-------------------------- BLIAsyncMorse("S", 1); // dot-dot-dot MCMInitF(50, 2500); // Initialize Motor Control at 50 Hz with setting // Throttle to HIGH for delay interval to let ESC // capture Throttle range BLIAsyncStop(); //-------------------------- RCInit(4); // Initialize Receiver interface with Priority=4 //-------------------------- UARTInitTX(6, 48); // Initialize UART1 for TX on IPL=6 at 115200 bps // This initialization routine accepts BaudRate in multiples // of 2400 bps; Thus: // BaudRate = 1 => 2400 bps // BaudRate = 2 => 4800 bps // ... // BaudRate = 48 => 115200 bps //------------------------------------------------------------ // High speed //------------------------------------------------------------ // BaudRate = 100 => 250,000 bps // BaudRate = 200 => 500,000 bps // BaudRate = 250 => 625,000 bps // BaudRate = 350 => 833,333 bps // BaudRate = 500 => 1,250,000 bps // BaudRate = 1000 => 2,500,000 bps //******************************************************************* BLISignalON(); TMRDelay(2000); // Wait for extra 2 sec - to let ESC arm... // (finish the song :) ) BLISignalOFF(); //================================================================== MCMData MC; RCData RC; //------------------------------------------------- BLIAsyncMorse("R", 1); // dot-doh-dot RCArm(); BLIAsyncStop(); //------------------------------------------------- BLISignalON(); while(1) { RCReadWhenReady(&RC); //--------------------------------------------- if (0 == RC.Control) MC.F = MC.B = MC.L = MC.R = 0.0; else { MC.F = RC.Throttle; MC.B = RC.Throttle; //-------------- MC.L = RC.Throttle; MC.R = RC.Throttle; //-------------- } //--------------------------------------------- MCMSet(&MC); //--------------------------------------------- UARTPostWhenReady((uchar*)&RC, sizeof(RC)); //--------------------------------------------- BLISignalFlip(); } return 1; }
int main(void) { //******************************************************************* Init(); TMRInit(2); // Initialize Timer interface with Priority=2 BLIInit(); // Initialize Signal interface //******************************************************************* // Switch 1 controls the Serial Data Logger (SDL) communication speed //------------------------------------------------------------------- if (_SW1) // Switch 1 is ON - Configuring SDL for PIC-to-PIC // high-speed communication at 1 MBaud SDLInit(3, BAUD_1M); else // Switch 1 is OFF - Configuring SDL for ZigBEE // wireless communication at 115.2 KBaud SDLInit(3, BAUD_115200); //******************************************************************* I2CInit(5, 2); // First param: IL = 5 (interrupt request priority // Second param: I2C speed // 0 - lowest (123 kHz at Fcy = 64MHz) // 1 - 200 kHz - MPU-6050 stable // 2 - 400 kHz // 3 - 1 MHz //------------------------------------------------------------------- uint RC = 0; ulong Alarm = 0; //================================================================== BLIAsyncStart(50,50); TMRDelay(5000); BLIAsyncStop(); //================================================================== if (_SW2) // Switch 2 is ON - Configuring MPU fo Alt. sensitivity RC = MPUInit(0, 1, MPU_GYRO_1000ds, MPU_ACC_4g); // Initialize motion Sensor // 1 kHz/(0+1) = 1000 Hz (1ms) // DLPF = 3 => Bandwidth 44 Hz (delay: 4.9 msec) else // Switch 2 is OFF - Configuring MPU fo normal sensitivity RC = MPUInit(0, 1, MPU_GYRO_2000ds, MPU_ACC_2g); // Initialize motion Sensor // 1 kHz/(0+1) = 1000 Hz (1ms) // DLPF = 1 => Bandwidth 184 Hz (delay: 2.0 msec) if (RC) BLIDeadStop("EG", 2); //******************************************************************* struct { MPUData Sample1; MPUData Sample2; } MPU; //===================================================== // Initialize Asynchronous mode //----------------------------------------------------- if ( (RC = MPUAsyncStart(2)) ) BLIDeadStop("S2", 2); //------------------------------ if ( (RC = MPUAsyncStart(1)) ) BLIDeadStop("S1", 2); //===================================================== // Calibrate Gyros //----------------------------------------------------- // BLIAsyncStart(100,100); // //------------------------------ // if ( (RC = MPUCalibrate(1)) ) // BLIDeadStop("C1", 2); // //------------------------------ // if ( (RC = MPUCalibrate(2)) ) // BLIDeadStop("C2", 2); // //------------------------------ // BLIAsyncStop(); //===================================================== // Main Loop //----------------------------------------------------- BLISignalOFF(); while (TRUE) { Alarm = TMRSetAlarm(1000); //------------------------------------ if ( (RC = MPUAsyncReadWhenReady(1, &MPU.Sample1)) ) BLIDeadStop("SOS", 3); //-------------------------- if ( (RC = MPUAsyncReadWhenReady(2, &MPU.Sample2)) ) BLIDeadStop("SOS", 3); //------------------------ BLISignalFlip(); //------------------------- SDLPostIfReady((byte*)&MPU, sizeof(MPU)); //------------------------- TMRWaitAlarm(Alarm); } //******************************************************************* return 0; }
int main(void) { //******************************************************************* Init(); // Initialize microprocessor TMRInit(2); // Initialize Timer interface with Priority=2 BLIInit(); // Initialize Signal interface //******************************************************************* // Switch 1 controls the Serial Data Logger (SDL) communication speed //------------------------------------------------------------------- if (_SW1) // Switch 1 is ON - Configuring SDL for PIC-to-PIC // high-speed communication at 1 MBaud SDLInit(3, BAUD_1M); else // Switch 1 is OFF - Configuring SDL for ZigBEE // wireless communication at 115.2 KBaud SDLInit(3, BAUD_115200); //******************************************************************* // Initialize I2C Library //------------------------------------------------------------------- I2CInit(5, 2); // First param: IL = 5 (interrupt request priority // Second param: I2C speed // 0 - lowest (123 kHz at Fcy = 64MHz) // 1 - 200 kHz // 2 - 400 kHz // 3 - 1 MHz //******************************************************************* uint RC = 0; ulong Alarm = 0; //================================================================== // Initialize MPUs //------------------------------------------------------------------ RC = MPUInit(0, 3, MPU_GYRO_2000ds, MPU_ACC_2g); // Initialize motion Sensor // 1 kHz/(0+1) = 1000 Hz (1msec) // DLPF = 3 => Bandwidth 44 Hz (delay: 4.9 msec) if (RC) BLIDeadStop("EG", 2); //================================================================== // Initialize MPL3115 Altimeter //------------------------------------------------------------------ // OSR = 0 => No averaging ( 2^0= 1), update rate about 166.6 Hz // OSR = 1 => Average 2^1= 2 samples, update rate about 111.1 Hz // OSR = 2 => Average 2^2= 4 samples, update rate about 67.8 Hz // OSR = 3 => Average 2^3= 8 samples, update rate about 37.7 Hz // OSR = 4 => Average 2^4= 16 samples, update rate about 20.1 Hz // OSR = 5 => Average 2^5= 32 samples, update rate about 10.4 Hz // OSR = 6 => Average 2^6= 64 samples, update rate about 5.3 Hz // OSR = 7 => Average 2^7= 128 samples, update rate about 2.7 Hz //------------------------------------------------------------------ byte OSR = 3; //------------------------------------------------------------------ if ( MPL_OK != MPLInit (OSR) ) BLIDeadStop ("EB", 2); //================================================================== // Initialize Asynchronous mode //----------------------------------------------------- if ( (RC = MPUAsyncStart(1)) ) BLIDeadStop("S1", 2); //------------------------------ if ( (RC = MPUAsyncStart(2)) ) BLIDeadStop("S2", 2); //----------------------------------------------------- if ( (RC = MPLAsyncStart()) ) BLIDeadStop("S3", 2); //================================================================== // Provide a few second delay prior to calibrating Gyros to make // sure that the board is stable after the "turn-on" shake //------------------------------------------------------------------ BLIAsyncStart(50,50); TMRDelay(5000); BLIAsyncStop(); //================================================================== // Calibrate Gyros //------------------------------------------------------------------ BLIAsyncStart(100,100); //------------------------------ if ( (RC = MPUCalibrateGyro(1)) ) BLIDeadStop("C1", 2); //------------------------------ if ( (RC = MPUCalibrateGyro(2)) ) BLIDeadStop("C2", 2); //------------------------------ BLIAsyncStop(); //================================================================== //================================================================== struct { MPUData MPUSample1; MPUData MPUSample2; MPLData MPLSample; } SensorData; //================================================================== // Main Loop //------------------------------------------------------------------ BLISignalOFF(); while (TRUE) { Alarm = TMRSetAlarm(1000); //----------------------------------------------------- if ( (RC = MPUAsyncReadWhenReady(1, &SensorData.MPUSample1)) ) BLIDeadStop("SOS", 3); //-------------------------- if ( (RC = MPUAsyncReadWhenReady(2, &SensorData.MPUSample2)) ) BLIDeadStop("SOS", 3); if (MPL_OK != MPLAsyncReadWhenReady(&SensorData.MPLSample)) BLIDeadStop("SOS", 3); //----------------------------------------------------- BLISignalFlip(); //------------------------- SDLPostIfReady((byte*)&SensorData, sizeof(SensorData)); //------------------------- TMRWaitAlarm(Alarm); } //******************************************************************* return 0; }
int main(void) { //******************************************************************* Init(); TMRInit(2); // Initialize Timer interface with Priority=2 BLIInit(); // Initialize Signal interface //================================================================== // Provide a 5 second delay prior to initialization of I2C interface // to avoid false-start during programming by PIC Kit 3 //------------------------------------------------------------------ BLIAsyncStart(50,200); TMRDelay(5000); BLIAsyncStop(); //******************************************************************* // Switch 1 controls the Serial Data Logger (SDL) communication speed //------------------------------------------------------------------- if (_SW1) // Switch 1 is ON - Configuring SDL for PIC-to-PIC // high-speed communication at 1 MBaud SDLInit(3, BAUD_1M); else // Switch 1 is OFF - Configuring SDL for ZigBEE // wireless communication at 115.2 KBaud SDLInit(3, BAUD_115200); //******************************************************************* // Initialize I2C Library //------------------------------------------------------------------- I2CInit(5, 2); // First param: IL = 5 (interrupt request priority // Second param: I2C speed // 0 - lowest (123 kHz at Fcy = 64MHz) // 1 - 200 kHz // 2 - 400 kHz // 3 - 1 MHz //------------------------------------------------------------------- uint RC = 0; ulong Alarm = 0; //================================================================== // Initialize MPUs //------------------------------------------------------------------ RC = MPUInit(0, 3, MPU_GYRO_2000ds, MPU_ACC_2g); // Initialize motion Sensor // 1 kHz/(0+1) = 1000 Hz (1msec) // DLPF = 3 => Bandwidth 44 Hz (delay: 4.9 msec) if (RC) BLIDeadStop("EG", 2); //===================================================== // Initialize Asynchronous mode //----------------------------------------------------- if ( (RC = MPUAsyncStart(2)) ) BLIDeadStop("S2", 2); //------------------------------ if ( (RC = MPUAsyncStart(1)) ) BLIDeadStop("S1", 2); //================================================================== // Provide a few second delay prior to calibrating Gyros to make // sure that the board is stable after the "turn-on" shake //------------------------------------------------------------------ BLIAsyncStart(50,50); TMRDelay(1000); BLIAsyncStop(); //******************************************************************* // Calibrate Gyros //----------------------------------------------------- // MPUSetOptions(Rotate, TempComp, CrossAxis) MPUSetOptions(TRUE, TRUE, TRUE); //----------------------------------------------------- BLIAsyncStart(100,100); //------------------------------ if ( (RC = MPUCalibrateGyro(1)) ) BLIDeadStop("C1", 2); //------------------------------ if ( (RC = MPUCalibrateGyro(2)) ) BLIDeadStop("C2", 2); //------------------------------ BLIAsyncStop(); //===================================================== struct { MPUData Sample1; MPUData Sample2; } MPU; //===================================================== // Main Loop //----------------------------------------------------- BLISignalOFF(); while (TRUE) { Alarm = TMRSetAlarm(20); //------------------------------------ if ( (RC = MPUAsyncReadWhenReady(1, &MPU.Sample1)) ) BLIDeadStop("SM1", 3); //-------------------------- if ( (RC = MPUAsyncReadWhenReady(2, &MPU.Sample2)) ) BLIDeadStop("SM2", 3); //------------------------ BLISignalFlip(); //------------------------- SDLPostIfReady((byte*)&MPU, sizeof(MPU)); //------------------------- TMRWaitAlarm(Alarm); } //******************************************************************* return 0; }
//============================================================================== // Common initialization routine //============================================================================== void _MCM_InitLocal(MCMConf frameConfig, float Period, uint UpTime) { if (_MC_Init) return; // Already initialized //-------------------------------------------------------------------------- _MC_Init = 1; _MC_Config = frameConfig; //-------------------------------------------------------------------------- // MCM Initialization utilizes TMRDelay(...) function, so if Timer is not // initialized, it is a good time to initialize it now... //-------------------------------------------------------------------------- TMRInitDefault(); //========================================================================== // The Output Compare module by default uses Timer2; we will stay with // default.... //-------------------------------------------------------------------------- // Stop/Disable Timer2; reset all flags //-------------------------------------------------------------------------- T2CON = 0; // Enable Timer2 if disabled through PMD _T2MD = 0; // Timer2 module enabled //========================================================================== // Timer pre-scaler and rollover counter are calculated assuming // Fcy = 64 MHz based upon specified refresh Period. //========================================================================== T2CONbits.TCKPS = 0b10; // Set Timer2 pre-scaler to 1:64 (tick = 1 usec) //----------------------- _MC_Base_Value = 1000; // 1 msec at current pre-scaler //-------------------------------------------------------------------------- // Re-set Timer2 counter TMR2 = 0; // Set Timer2 rollover threshold (MC frequency counter - number of 1 usec // ticks comprising Period set in msec) PR2 = floorf(Period * 1000) - 1; //-------------------------------------------------------------------------- // Configure Timer2 interrupts //-------------------------------------------------------------------------- _T2IF = 0 ; // Clear TMR2 interrupt flag _T2IE = 0 ; // Disable TMR2 CPU interrupt //========================================================================== //========================================================================== // Output-Compare module is being used in this application to generate PPM // signal to control 4 motors through respective ESCs. // 4 OC (Output Compare) channels are being used for Motor Control. //-------------------------------------------------------------------------- // Disable OC modules for configuration and reset all control bits //-------------------------------------------------------------------------- //<editor-fold defaultstate="collapsed" desc="OCxCON1 bits definitions"> // bit 15-14 Unimplemented: Read as ?0? // bit 13 OCSIDL: // Stop Output Compare x in Idle Mode Control bit // 1 = Output Compare x Halts in CPU Idle mode // 0 = Output Compare x continues to operate in CPU Idle mode // bit 12-10 OCTSEL<2:0>: // Output Compare x Clock Select bits // 111 = Peripheral clock (FP) // 110 = Reserved // 101 = Reserved // 100 = Clock source of T1CLK is the clock source of OCx // (only the synchronous clock is supported) // 011 = Clock source of T5CLK is the clock source of OCx // 010 = Clock source of T4CLK is the clock source of OCx // 001 = Clock source of T3CLK is the clock source of OCx // 000 = Clock source of T2CLK is the clock source of OCx // bit 9 ENFLTC: // Fault C Input Enable bit // 1 = Output Compare Fault C input (OCFC) is enabled // 0 = Output Compare Fault C input (OCFC) is disabled // bit 8 ENFLTB: // Fault B Input Enable bit // 1 = Output Compare Fault B input (OCFB) is enabled // 0 = Output Compare Fault B input (OCFB) is disabled // bit 7 ENFLTA: // Fault A Input Enable bit // 1 = Output Compare Fault A input (OCFA) is enabled // 0 = Output Compare Fault A input (OCFA) is disabled // bit 6 OCFLTC: // PWM Fault C Condition Status bit // 1 = PWM Fault C condition on OCFC pin has occurred // 0 = No PWM Fault C condition on OCFC pin has occurred // bit 5 OCFLTB: // PWM Fault B Condition Status bit // 1 = PWM Fault B condition on OCFB pin has occurred // 0 = No PWM Fault B condition on OCFB pin has occurred // bit 4 OCFLTA: // PWM Fault A Condition Status bit // 1 = PWM Fault A condition on OCFA pin has occurred // 0 = No PWM Fault A condition on OCFA pin has occurred // bit 3 TRIGMODE: // Trigger Status Mode Select bit // 1 = TRIGSTAT (OCxCON2<6>) is cleared when OCxRS = OCxTMR // or in software // 0 = TRIGSTAT is cleared only by software // bit 2-0 OCM<2:0>: // Output Compare Mode Select bits // 111 = Center-Aligned PWM mode: Output set high when // OCxTMR = OCxR and set low when OCxTMR = OCxRS // 110 = Edge-Aligned PWM mode: Output set high when OCxTMR = 0 // and set low when OCxTMR = OCxR // 101 = Double Compare Continuous Pulse mode: Initializes OCx pin // low, toggles OCx state continuously on alternate matches // of OCxR and OCxRS // 100 = Double Compare Single-Shot mode: Initializes OCx pin low, // toggles OCx state on matches of OCxR and OCxRS once // 011 = Single Compare mode: Compares events with OCxR, // continuously toggles OCx pin // 010 = Single Compare Single-Shot mode: Initializes OCx pin high, // compares event with OCxR, forces OCx pin low // 001 = Single Compare Single-Shot mode: Initializes OCx pin low, // compares event with OCxR, forces OCx pin high // 000 = Output compare channel is disabled// //</editor-fold> //-------------------------------------------------------------------------- // Setting OCxCON1 to 0 disables the module, sets clock source to Timer2, // and disables all fault control. //-------------------------------------------------------------------------- OC1CON1 = OC2CON1 = OC3CON1 = OC4CON1 = 0; //-------------------------------------------------------------------------- //<editor-fold defaultstate="collapsed" desc="OCxCON2 bits definitiona"> // bit 15 FLTMD: // Fault Mode Select bit // 1 = Fault mode is maintained until the Fault source is removed; // the corresponding OCFLTx bit is cleared in software and a // new PWM period starts // 0 = Fault mode is maintained until the Fault source is removed // and a new PWM period starts // bit 14 FLTOUT: // Fault Out bit // 1 = PWM output is driven high on a Fault // 0 = PWM output is driven low on a Fault // bit 13 FLTTRIEN: // Fault Output State Select bit // 1 = OCx pin is tri-stated on Fault condition // 0 = OCx pin I/O state defined by FLTOUT bit on Fault condition // bit 12 OCINV: // OCMP Invert bit // 1 = OCx output is inverted // 0 = OCx output is not inverted // bit 11-9 // Unimplemented: Read as ?0? // bit 8 OC32: // Cascade Two OCx Modules Enable bit (32-bit operation) // 1 = Cascade module operation is enabled // 0 = Cascade module operation is disabled // bit 7 OCTRIG: // OCx Trigger/Sync Select bit // 1 = Triggers OCx from source designated by SYNCSELx bits // 0 = Synchronizes OCx with source designated by SYNCSELx bits //bit 6 TRIGSTAT: // Timer Trigger Status bit // 1 = Timer source has been triggered and is running // 0 = Timer source has not been triggered and is being held clear // bit 5 OCTRIS: // OCx Output Pin Direction Select bit // 1 = OCx is tri-stated // 0 = Output compare module drives the OCx pin // bit 4-0 SYNCSEL<4:0>: // Trigger/Synchronization Source Selection bits // 11111 = No Sync or Trigger source for OCx // 11110 = INT2 pin synchronizes or triggers OCx // 11101 = INT1 pin synchronizes or triggers OCx // 11100 = Reserved // 11011 = ADC1 module synchronizes or triggers OCx // 11010 = CMP3 module synchronizes or triggers OCx // 11001 = CMP2 module synchronizes or triggers OCx // 11000 = CMP1 module synchronizes or triggers OCx // 10111 = IC8 module synchronizes or triggers OCx // 10110 = IC7 module synchronizes or triggers OCx // 10101 = IC6 module synchronizes or triggers OCx // 10100 = IC5 module synchronizes or triggers OCx // 10011 = IC4 module synchronizes or triggers OCx // 10010 = IC3 module synchronizes or triggers OCx // 10001 = IC2 module synchronizes or triggers OCx // 10000 = IC1 module synchronizes or triggers OCx // 01111 = Timer5 synchronizes or triggers OCx // 01110 = Timer4 synchronizes or triggers OCx // 01101 = Timer3 synchronizes or triggers OCx // 01100 = Timer2 synchronizes or triggers OCx (default) // 01011 = Timer1 synchronizes or triggers OCx // 01010 = No Sync or Trigger source for OCx // 01001 = OC9 module synchronizes or triggers OCx // 01000 = OC8 module synchronizes or triggers OCx // 00111 = OC7 module synchronizes or triggers OCx // 00110 = OC6 module synchronizes or triggers OCx // 00101 = OC5 module synchronizes or triggers OCx // 00100 = OC4 module synchronizes or triggers OCx // 00011 = OC3 module synchronizes or triggers OCx // 00010 = OC2 module synchronizes or triggers OCx // 00001 = OC1 module synchronizes or triggers OCx // 00000 = No Sync or Trigger source for OCx // </editor-fold> //-------------------------------------------------------------------------- // Setting OCxCON2 to 0 disables fault mode, sets OCx pins as non-inverted // and driven by OCx module (vs. tri-state), and disables OCx trigger //-------------------------------------------------------------------------- OC1CON2 = OC2CON2 = OC3CON2 = OC4CON2 = 0; //-------------------------------------------------------------------------- // Enable OC1-OC4 in PMD //-------------------------------------------------------------------------- _OC1MD = 0; _OC2MD = 0; _OC3MD = 0; _OC4MD = 0; //-------------------------------------------------------------------------- // Enabling OCx in PMD sets control registers to their default values, // which includes setting SYNCSEL (OCxCON2) to 0b01100; this default // setting establishes Timer2 as synchronization source for OC1-OC4 //-------------------------------------------------------------------------- // Now that OCx modules are enabled in PMD, we may assign output pins //-------------------------------------------------------------------------- _MCM_InitPinMap(); //-------------------------------------------------------------------------- //-------------------------------------------------------------------------- // In this application we do not use Output Compare interrupts so we need // to reset any outstanding requests and block OC interrupts. //-------------------------------------------------------------------------- _OC1IE = _OC2IE = _OC3IE = _OC4IE = 0; _OC1IF = _OC2IF = _OC3IF = _OC4IF = 0; _OC1IP = _OC2IP = _OC3IP = _OC4IP = 0; //========================================================================== //========================================================================== // Configure OC modules: //-------------------------------------------------------------------------- // Timer2 is already selected as clock source for OC1-OC4 by OCxCON1=0 //-------------------------------------------------------------------------- // Put all OC modules into the Edge-Aligned PWM mode //-------------------------------------------------------------------------- OC1CON1bits.OCM = 0b110; // Edge-Aligned PWM mode on OC1 OC2CON1bits.OCM = 0b110; // Edge-Aligned PWM mode on OC2 OC3CON1bits.OCM = 0b110; // Edge-Aligned PWM mode on OC3 OC4CON1bits.OCM = 0b110; // Edge-Aligned PWM mode on OC4 //========================================================================== //========================================================================== // Set initial duty cycle (OCxR) registers for PPM mode. //-------------------------------------------------------------------------- if (UpTime > 0) { //---------------------------------------------------------------------- // Special case: start with 100% motor control output so that ESC can // capture "high" throttle //---------------------------------------------------------------------- uint MaxValue = 2 * _MC_Base_Value; //----------------------------------- OC1R = OC2R = OC3R = OC4R = MaxValue; } else { //---------------------------------------------------------------------- // Normal start: Set Motor Control to 0% //---------------------------------------------------------------------- OC1R = OC2R = OC3R = OC4R = _MC_Base_Value; //---------------------------------------------------------------------- } //========================================================================== // Start OC1-OC4 by enabling Timer2, which provides both clock source and // synchronization source for selected Output Compare modules //-------------------------------------------------------------------------- T2CONbits.TON = 0b1; //========================================================================== if (UpTime > 0) { //---------------------------------------------------------------------- // Keep output HIGH for defined UpTime //---------------------------------------------------------------------- TMRDelay(UpTime); //---------------------------------------------------------------------- // Reset Motor Control to 0% //---------------------------------------------------------------------- OC1R = OC2R = OC3R = OC4R = _MC_Base_Value; //---------------------------------------------------------------------- // Wait until ESC captures Low Throttle Value //---------------------------------------------------------------------- TMRDelay(UpTime); } }
int main(void) { //******************************************************************* Init(); TMRInit(2); // Initialize Timer interface with Priority=2 BLIInit(); // Initialize Signal interface //------------------------------------------------------------------- BLIAsyncStart(100, 100); TMRDelay(5000); // To avoid false-start at Reset BLIAsyncStop(); BLISignalOFF(); //******************************************************************* // Switch 1 controls the Serial Data Logger (SDL) communication speed //------------------------------------------------------------------- if (_SW1) // Switch 1 is ON - Configuring SDL for PIC-to-PIC // high-speed communication at 1 MBaud SDLInit(3, BAUD_1M); else // Switch 1 is OFF - Configuring SDL for ZigBEE // wireless communication at 115.2 KBaud SDLInit(3, BAUD_115200); //******************************************************************* // HMC5983 Magnetometer initialization //------------------------------------------------------------------- byte IL = 5; // Interrupt level //------------------------------------------------------ // <editor-fold defaultstate="collapsed" desc="Output Data Rate (ODR"> //------------------------------------------------------ // Typical Data Output Rate (Hz) for various ODR values //------------------------------------------------------ // ODR = 0: 0.75 // ODR = 2: 1.5 // ODR = 2: 3 // ODR = 3: 7.5 // ODR = 4: 15 (Default) // ODR = 5: 30 // ODR = 6: 75 // ODR = 7: 220 (fastest) // </editor-fold> byte ODR = 7; // Rate (fastest): 220 Hz // <editor-fold defaultstate="collapsed" desc="Low-pass filtering (DLPF)"> //------------------------------------------------------ // Low-pass filtering achieved through sample averaging. // Averaging does not affect effective ODR, so I assume // that wit averaging enabled each successive reported // sample is an average of the new measurement with "n" // previous measurements - something like a FIR filter. //------------------------------------------------------ // DLPF = 0 => 1-average // DLPF = 1 => 2-average // DLPF = 2 => 4-average // DLPF = 3 => 8-average //------------------------------------------------------ // </editor-fold> byte DLPF = 0; // <editor-fold defaultstate="collapsed" desc="Sensor Field Range (Gain)"> //------------------------------------------------------ // Recommended sensor field range (Ga) for various Gains //------------------------------------------------------ // Gain = 0: 0.9 // Gain = 1: 1.3 // Gain = 2: 1.9 // Gain = 3: 2.5 // Gain = 4: 4.0 // Gain = 5: 4.7 // Gain = 6: 5.6 // Gain = 7: 8.1 //------------------------------------------------------ // The magnitude of Earth magnetic field varies over the // surface of the Earth in the range 0.3 to 0.6 Gauss. //------------------------------------------------------ // </editor-fold> byte Gain = 1; // +/- 1.3 Ga //------------------------------------------------------ HMC_Init(IL, ODR, Gain, DLPF); //******************************************************************* HMC_RC RC; //---------------------- ulong i = 0; //----------------------------------------- byte RegA; byte RegB; //------------------------------- RC = HMC_ReadA(&RegA); i++; //------------------------------- RC = HMC_ReadB(&RegB); i++; //------------------------------- //---------------------- struct { HMCData Sample; ulong Count; } SDLData; SDLData.Count = 0; //---------------------- while (1) { //------------------------------- if ( HMC_OK != (RC = HMC_ReadSample(&SDLData.Sample)) ) BLIDeadStop("SOS", 3); SDLData.Count++; //------------------------------- BLISignalFlip(); //------------------------------- SDLPostIfReady((byte*)&SDLData, sizeof(SDLData)); //------------------------------- } //******************************************************************* return 0; }
int main(void) { // <editor-fold defaultstate="collapsed" desc="Initialization of HW components/modules"> //******************************************************************* // Initialization of HW components/modules //=================================================================== Init(); TMRInit(2); // Initialize Timer interface with Priority=2 BLIInit(); // Initialize Signal interface //=================================================================== BLIAsyncStart(50, 50); // Fast blinking - initialization //=================================================================== MCMInitT(2.5, 2500); // Initialize Motor Control for PPM with setting // Throttle to HIGH for delay interval to let ESC // capture Throttle range //-------------------------- ADCInit(3); // Initialize ADC to control battery //-------------------------- RCInit(4); // Initialize Receiver interface with Priority=4 //-------------------------- I2CInit(5, 1); // Initialize I2C1 module with IPL=5 and Fscl=400 KHz //-------------------------- UARTInitTX(6, 350); // Initialize UART1 for TX on IPL=6 at // BaudRate = 48 => 115,200 bps - ZigBEE //-------------------------------------- // BaudRate = 100 => 250,000 bps // BaudRate = 200 => 500,000 bps // BaudRate = 250 => 625,000 bps // BaudRate = 350 => 833,333 bps - SD Logger, FTDI // BaudRate = 500 => 1,250,000 bps // BaudRate = 1000 => 2,500,000 bps //******************************************************************* // Initialize Magnetometer //-------------------------------------------------------------- if (HMCInit(6, 1, 0)) // Initialize magnetic Sensor // ODR = 6 (max, 75 Hz), // Gain = 1 (1.3 Gs) // DLPF = 0 (no averaging) BLIDeadStop("EM", 2); //================================================================== // Initialize motion sensor - No calibration at this time!!! //------------------------------------------------------------------ if ( MPUInit(0, 3) ) // Initialize motion Sensor // 1 kHz/(0+1) = 1000 Hz (1ms) // DLPF=3 => Bandwidth 44 Hz (delay: 4.9 msec) BLIDeadStop("EA", 2); //------------------------------------------------------------------ //================================================================== AltimeterInit(4); // Initialize Altimeter with IPL=4 (if needed) // NOTE: All sensors areinitialized, so no more synchronous I2C // operations are needed. Thus we may start Altimeter to give // it more time to "warm-up" AltimeterReset(); // NOTE: will be reset again later //================================================================== BLIAsyncStop(); // Initialization complete //=================================================================== // </editor-fold> //******************************************************************* // Quadrocopter control variables //------------------------------------------------------------------- // <editor-fold defaultstate="collapsed" desc="Timing control variables"> ulong StartTS = 0; // Flight start time ulong StepTS; // Control Loop step start time ulong Alarm = 0; // "alarm" variable used to set // the duration of the Control Loop // </editor-fold> // <editor-fold defaultstate="collapsed" desc="Variable required to process RC Feed "> //------------------------------------------------------------------- RCData RCFeed; // Control input from Receiver adjusted // by control rates. It may also be // corrected (if necessary) to account // for "+" or "X" configuration as // specified by MODE_CB_to_MF flag //--------------------------------------- RCData RCSmthd; // Smoothed control input used in all // subsequent control calculations //--------------------------------------- RCData RCFinal; // Smoothed RC input adjusted for the // Yaw angle to provide "Course Lock" (if // required). Final RC input provided to // Quarocopter Control Module //------------------------------------------------------------------- // We need battery Nominal voltage to adjust RC Throttle as battery // charge depletes. float BatNomV = ADCGetBatteryNomVoltage (); //------------------------------------------------------------------- Matrix Mtr_CB_To_MF; // Rotation matrix used to adjust Control // Board orientation to Model front //--------------------------------------- Matrix Mtr_CrsLck; // Rotation matrix used to adjust RC input // to account for current Yaw to implement // "Course Lock" //--------------------------------------- if (MODE_CB_to_MF) // Initialize Rotation Matrix for pre-set angle MatrixYawRotation(__CB_To_MF_Angle__, &Mtr_CB_To_MF); else // Reset Rotation matrix to Identity MatrixSetIdentity(&Mtr_CB_To_MF); //------------------------------------------------------------------- // </editor-fold> DCMData IMU; // Orientation data from DCM MCMData MC; // Motor control Variables //------------------------------------------------------------------- // <editor-fold defaultstate="collapsed" desc="ARMing RC and initializing IMU"> //================================================================== // Wait for the Receiver ARMing: Throttle should go up and then down //------------------------------------------------------------------ BLIAsyncStart(200, 200); // Really slow RCArm(); BLIAsyncStop(); //================================================================== // </editor-fold> Re_Start: //================================================================== // <editor-fold defaultstate="collapsed" desc="Intialize Re-Start"> //------------------------------------------------------------------ StartTS = 0; // Reset timing of control Loop //------------------------------------------------------------------ BLIAsyncMorse( "I", 1); // dit - dit RCReadWhenReady (&RCFeed); // Get reading from receiver //------------------------------------------------------------------ // Safety check: CONTROL should be UP and THROTTLE - LOW //------------------------------------------------------------------ while ( 0 == RCFeed.Control || RCFeed.Throttle > 0.1 ) { RCReadWhenReady (&RCFeed); // Get next reading from receiver } //------------------------------------------------------------------ BLIAsyncStop (); //================================================================== //================================================================== // Normalize last read RC values as they will represent the first // input into the control algorithm, so should be adjusted accordingly //------------------------------------------------------------------ NormalizeRCFeed(&RCFeed); //------------------------------------------------------------------ // Reset Smothed RC data (initialize filter) //------------------------------------------------------------------ RCDataReset(&RCSmthd); //================================================================== // Initialize sensors... Performed after ARMing to ensure that model // is stable //-------------------------------------------------------------- BLIAsyncStart(100, 50); //------------------------------------------------------------------ // Start IMU and wait until orientation estimate stabilizes //------------------------------------------------------------------ IMUReset(); //-------------------------------------------------------------- BLIAsyncStart(50, 100); //------------------------------------------------------------------ // Reset Altimeter //------------------------------------------------------------------ AltimeterReset(); //------------------------------------------------------------------ QCMReset (); // Initialize (reset) QCM variables //------------------------------------------------------------------ // Force update of DCMData as inside the control Loop this // call is non-blocking! //------------------------------------------------------------------ if (IMU_OK != IMUGetUpdateWhenReady (&IMU)) // Failure... BLIDeadStop ("A", 1); //-------------------------------------------------------------- BLIAsyncStop(); //-------------------------------------------------------------- // </editor-fold> //================================================================== //******************************************************************* // Quadrocopter Control Loop //------------------------------------------------------------------- BLISignalON(); while (1) { //============================================================ // Timing of control Loop //============================================================ // <editor-fold defaultstate="collapsed" desc="Control Loop timing management"> // Sets the "frquency" of Control Loop Alarm = TMRSetAlarm (10); // Set Alarm 10 msec in the future //------------------------------------------------------------ StepTS = TMRGetTS (); // Capture TS of this step if ( 0 == StartTS ) // First iteration of Control? StartTS = StepTS; // Yes, capture timestamp // </editor-fold> //============================================================ //============================================================ // Read commands from receiver - non-blocking call! // (we will get out of this call even if connection to the // receiver is lost!) // At the end processed Receiver data is stored in RCSmthd //============================================================ // <editor-fold defaultstate="collapsed" desc="Process Receiver feed"> if ( RCRead(&RCFeed) ) { //------------------------------------------------------------ // Every NEW sample need to be "normalized" //------------------------------------------------------------ NormalizeRCFeed(&RCFeed); //------------------------------------------------------------ // Adjust orientation from "+" to "X" if needed //------------------------------------------------------------ if (MODE_CB_to_MF) { // Control board front does not coincide with the model front Vector RCInput; Vector RCRotated; VectorSet(RCFeed.Roll, RCFeed.Pitch, RCFeed.Yaw, &RCInput); MatrixTimesVector(&Mtr_CB_To_MF, &RCInput, &RCRotated); RCFeed.Roll = RCRotated.X; RCFeed.Pitch = RCRotated.Y; RCFeed.Yaw = RCRotated.Z; // NOTE: Yaw is not affected! } } //============================================================ // Smooth RC data //------------------------------------------------------------ // Roll, Pitch, and Yaw are smoothed with the IIR(4) //------------------------------------------------------------ RCSmthd.Roll = (RCSmthd.Roll * 3.0 + RCFeed.Roll ) * 0.25; // 1/4 = 0.25 RCSmthd.Pitch = (RCSmthd.Pitch* 3.0 + RCFeed.Pitch) * 0.25; if (MODE_Course_Lock) { // In this mode Yaw is integrated over the Control Loop // duration (0.01 sec) and normalized to +/-Pi range RCSmthd.Yaw = QCMRangeToPi(RCSmthd.Yaw + RCFeed.Yaw * 0.01); } else { // Without Course-Lock Yaw is treated as "direct input" and // just smothed similar to other control variables. RCSmthd.Yaw = (RCSmthd.Yaw * 3.0 + RCFeed.Yaw ) * 0.25; } //------------------------------------------------------------ // Throttle is smoothed with the IIR(4) and adjusted to // account for actual battery voltage. This is done to // improve "hovering" when throttle stick is not moving. //------------------------------------------------------------ // Adjust Native (from RC) throttle to a value corresponding float BatV = ADCGetBatteryVoltage(); float BatAdjTh = RCFeed.Throttle; if (BatV > 2.0) // Sanity check :) BatAdjTh = BatAdjTh * BatNomV / BatV; //----------------------------------------- RCSmthd.Throttle = (RCSmthd.Throttle*3.0 + BatAdjTh) * 0.25; // 1/4 = 0.25 //----------------------------------------- RCSmthd.Control = RCFeed.Control; // </editor-fold> //============================================================ //============================================================ // Implement Motor Cut-Off if RC Control is LOW //============================================================ // <editor-fold defaultstate="collapsed" desc="Process RC Control"> if ( 0 == RCSmthd.Control ) { // Yes, Control is reliably low! //-------------------------------------------- // Override motor control //-------------------------------------------- MC.F = MC.B = MC.L = MC.R = 0; MCMSet(&MC); //-------------------------------------------- // Flight terminated, post EOF to Data Logger //-------------------------------------------- TMRDelay(10); // Wait for pipe to clear UARTPostIfReady( NULL, 0); // ... and again - to be sure! TMRDelay(10); // Wait for pipe to clear UARTPostIfReady( NULL, 0); //---------------------------------- goto Re_Start; } // </editor-fold> //============================================================ //============================================================ // Obtain and process battery charge status //============================================================ // <editor-fold defaultstate="collapsed" desc="Process Battery level"> float BatteryCharge = QCMBatteryMgmt(); if (BatteryCharge < 0.35) // BC < 35% { float MaxLevel = 2.0 * BatteryCharge; if (RCSmthd.Throttle > MaxLevel) RCSmthd.Throttle = MaxLevel; } // </editor-fold> //============================================================ //============================================================ // Read current orientation from the IMU with sensors' raw // measurements (non-blocking call) and, if necessary, perform // adjustment to RCSmthd to obtain RCFinal //============================================================ // <editor-fold defaultstate="collapsed" desc="Process IMU data and generate Motor Control"> // First, unconditionally promote RCSmthd to RCFinal RCDataCopy(&RCSmthd, &RCFinal); //------------------------------------------------------------ if (IMU_OK == IMUGetUpdate(&IMU)) { if (MODE_Course_Lock) { // We need to rotate RC input to adjust for current Yaw // to implement "Course Lock" - RC Roll and Pitch commands // are interpreted as being applied to the original (pre- // takeoff) orientation of the model //------------------------------------------------------- Vector RCInput; Vector RCRotated; // Build rotation matrix to adjust for orientation discrepancy MatrixYawRotation(-IMU.Yaw, &Mtr_CrsLck); // Load RC input into RCInput vector VectorSet(RCSmthd.Roll, RCSmthd.Pitch, RCSmthd.Yaw, &RCInput); // Rotate RCInput vector MatrixTimesVector(&Mtr_CrsLck, &RCInput, &RCRotated); // Save rotated values RCFinal.Roll = RCRotated.X; RCFinal.Pitch = RCRotated.Y; RCFinal.Yaw = RCRotated.Z; } // Calculate motor control based // upon IMU data QCMPerformStep(&RCFinal, &IMU, &MC); } else { // IMU Failed to provide update - // set Motor Control to RC throttle MC.F = MC.B = MC.L = MC.R = RCFinal.Throttle; } // </editor-fold> //============================================================ //************************************************************ // Implement Motor Cut-Off to protect props if model is // dangerously tilted (> 60 degrees) while RC Throttle is low //------------------------------------------------------------ if (IMU.Incl <= 0.5 && RCFinal.Throttle <= 0.40) { // Override motor control MC.F = MC.B = MC.L = MC.R = 0.0; } //------------------------------------------------------------ //************************************************************ // Update motor control //************************************************************ MCMSet(&MC); //------------------------------------------------------------ //=========================================================== // Load and post telemetry data (non-blocking call) //----------------------------------------------------------- // <editor-fold defaultstate="collapsed" desc="Populating Telemetry"> //----------------------------------------- TM.TS = StepTS - StartTS; //---------------------- TM.Roll = IMU.Roll; TM.Pitch = IMU.Pitch; TM.Yaw = IMU.Yaw; TM.Inclination = IMU.Incl; TM.Azimuth = IMU.Azimuth; //---------------------- #ifdef _TMReport_GYRO_ #ifdef _TMReport_GYRO_Rotated //------------------------------------------------------ // We rotate Gyro vector using "partial" DCM built using // only Roll and Pitch angles as the actual Yaw does not // affect Angualr Velocity by axis //------------------------------------------------------ Vector Earth; // One of the Earth axis in Body frame Vector Cross; // Temporary vector to hold the cross // product //------------------------------------------------------ // Calculate Roll and Pitch rates //------------------------------------------------------ // Retrieve Earth Z-axis and calculate cross-product of // Z-axis with Gyro vector VectorCrossProduct(DCMEarthZ(&Earth), &IMU.GyroRate, &Cross); TM.Gyro.X = Cross.X; TM.Gyro.Y = Cross.Y; //------------------------------------------------------ // Calculate Yaw rate //------------------------------------------------------ // Retrieve Earth X-axis and calculate cross-product of // X-axis with Gyro vector VectorCrossProduct(DCMEarthX(&Earth), &IMU.GyroRate, &Cross); TM.Gyro.Z = Cross.Z; #else // Just report native Gyro data VectorCopy(&IMU.GyroRate, &TM.Gyro); #endif #endif //---------------------- #ifdef _TMReport_ACC_ // Just report native Gyro data VectorCopy(&IMU.Gravity, &TM.Acc); #endif //---------------------- TM.RollDer = QSD.RollDer; TM.PitchDer = QSD.PitchDer; TM.YawDer = QSD.YawDer; //---------------------- TM.RC_Throttle = RCFinal.Throttle; TM.RC_Roll = RCFinal.Roll; TM.RC_Pitch = RCFinal.Pitch; TM.RC_Yaw = RCFinal.Yaw; //---------------------- #ifdef _TMReport_RCSmthd_ TM.RCS_Throttle = RCSmthd.Throttle; TM.RCS_Roll = RCSmthd.Roll; TM.RCS_Pitch = RCSmthd.Pitch; TM.RCS_Yaw = RCSmthd.Yaw; #endif //---------------------- #ifdef _TMReport_RCFeed_ TM.RCN_Throttle = RCFeed.Throttle; TM.RCN_Roll = RCFeed.Roll; TM.RCN_Pitch = RCFeed.Pitch; TM.RCN_Yaw = RCFeed.Yaw; #endif //---------------------- #ifdef _TMReport_Altimetry_ if ( 0 == AltimeterGetAltData( IMU.TS, &IMU.Gravity, &TM.AltResult) ) goto EndOfCycle; // Skip reporting on this step #endif //---------------------- #ifdef _TMReport_PID_ #ifdef _TMReport_PID_Details TM.DRProp = QSD.DeltaRollProp; TM.DRDiff = QSD.DeltaRollDiff; TM.DRInt = QSD.DeltaRollInt; #endif TM.DRTot = QSD.DeltaRoll; //------------- #ifdef _TMReport_PID_Details TM.DPProp = QSD.DeltaPitchProp; TM.DPDiff = QSD.DeltaPitchDiff; TM.DPInt = QSD.DeltaPitchInt; #endif TM.DPTot = QSD.DeltaPitch; //------------- #ifdef _TMReport_PID_Details TM.DYProp = QSD.DeltaYawProp; TM.DYDiff = QSD.DeltaYawDiff; TM.DYInt = QSD.DeltaYawInt; #endif TM.DYTot = QSD.DeltaYaw; #endif //---------------------- TM.Throttle = QSD.Throttle; // Real Throttle //---------------------- TM.Voltage = ADCGetBatteryVoltage(); // </editor-fold> //=========================================================== UARTPostIfReady( (unsigned char *) &TM, sizeof(TM) ); //=========================================================== // Insert controlled "delay" to slow down the Control Loop // to pre-set frequency EndOfCycle: TMRWaitAlarm(Alarm); //=========================================================== } //******************************************************************* return 1; }