//================================================================================== uint DCMUpdate( ulong GyroTS, // Gyro timestamp Vector* Gyro, // Gyro measurement //-------------------------------------- ulong AccTS, // Acc timestamp Vector* Acc, // Acc measurement //-------------------------------------- ulong MagTS, // Mag timestamp Vector* Mag, // Mag measurement //-------------------------------------- DCMData* IMUResult) { //========================================================================== if (_DCMInit != 1) return DCM_NOTINIT; //========================================================================== // Calculate integration time interval for respective sensors //-------------------------------------------------------------------------- float GyroDT = TMRGetTSRate() * (GyroTS - _DCM_GyroTS); float AccDT = TMRGetTSRate() * (AccTS - _DCM_AccTS); float MagDT = TMRGetTSRate() * (MagTS - _DCM_MagTS); //-------------------------------------------------------------------------- // Update last sample timestamps for respective sensors //-------------------------------------------------------------------------- _DCM_GyroTS = GyroTS; _DCM_AccTS = AccTS; _DCM_MagTS = MagTS; //========================================================================== //========================================================================== // Accelerometer-based and Magnetometer-based rotation correction vectors //-------------------------------------------------------------------------- Vector DriftComp; // Current step total drift compensation VectorSet(0.0, 0.0, 0.0, &DriftComp); //-------------------------------------------------------------------------- if (TRUE == _DCM_UseAcc) { // <editor-fold defaultstate="collapsed" desc="Calculate Acc-based correction"> //-------------------------------------------------------------------------- // Local variables used in calculations of the Accelerometer-based correction //-------------------------------------------------------------------------- Vector EarthZ; // Earth vertical in the Body frame Vector AccNorm; // Normalized Accelerometer measurement // in the Body frame Vector AccError; // Rotational "error" between the Earth // vertical (in Body frame) and // acceleration vector. Vector AccPterm; // Proportional term Vector AccIterm; // Current step component of Integral term //-------------------------------------------------------------------------- // Calculating Accelerometer-based correction //-------------------------------------------------------------------------- // Extract Z-axis in the Earth frame as seen from the Body frame // from the DCM DCMEarthZ(&EarthZ); //--------------------------------------------------------- // We have to "normalize" gravity vector so that calculated // error proportional to rotation error and independent of // the level of current acceleration //--------------------------------------------------------- VectorNormalize(Acc, &AccNorm); //--------------------------------------------------------- // Cross-product of the axis Z in the Earth frame (as seen // from the Body frame) of the DCM with the normalized // Accelerometer measurement (true Z-axis of Earth frame // in the Body frame ignoring linear accelerations) is the // ERROR accumulated in DCM and defines how much the Rotation // Matrix need to be rotated so that these vectors match. //--------------------------------------------------------- VectorCrossProduct(&EarthZ, &AccNorm, &AccError); //--------------------------------------------------------- // Accelerometer correction P-Term //--------------------------------------------------------- VectorScale(&AccError, _DCMAccKp, &AccPterm); //--------------------------------------------------------- // Accelerometer correction I-Term //--------------------------------------------------------- VectorScale(&AccError, (_DCMAccKi * AccDT), &AccIterm); VectorAdd(&_DCMAccIterm, &AccIterm, &_DCMAccIterm); //--------------------------------------------------------- // Adjust drift compensation vector with Accelerometer- // based correction factors //--------------------------------------------------------- VectorAdd(&DriftComp, &AccPterm, &DriftComp); VectorAdd(&DriftComp, &_DCMAccIterm, &DriftComp); //-------------------------------------------------------------------------- // </editor-fold> } //-------------------------------------------------------------------------- if (TRUE == _DCM_UseMag) { // <editor-fold defaultstate="collapsed" desc="Calculate Mag-based correction"> //-------------------------------------------------------------------------- // Local variables used in calculations of the Magnetometer-based correction //-------------------------------------------------------------------------- Vector EarthMag; // Magnetic vector measurement // transformed to Earth frame using // current rotation matrix Vector MagNorm; // Normalized Magnetometer measurement // in the Earth frame Vector MagError; // Rotational "error" between rotated // magnetic vector (in Body frame) and // measured magnetic vector. Vector MagPterm; // Proportional term Vector MagIterm; // Current step component of Integral term //-------------------------------------------------------------------------- // Calculating Magnetometer-based correction //-------------------------------------------------------------------------- // Transform magnetic vector measurement to Earth frame DCMToEarth(Mag, &EarthMag); //--------------------------------------------------------- // We have to "normalize" magnetic vector so that calculated // error proportional to rotation error and independent of // the strength of magnetic field //--------------------------------------------------------- VectorNormalize(&EarthMag, &MagNorm); //------------------------------------------------------------------ // If magnetic vector is used ONLY for Yaw drift compensation we // should nullify Z-axis component (Z-axis component of the reference // vector _DCM_BaseMAG was nullified in DCMReset(...) ) //------------------------------------------------------------------ if (TRUE == _DCM_UseMagYawOnly) MagNorm.Z = 0.0; //--------------------------------------------------------- // Cross-product of the original magnetic vector transformed // to Body frame using the DCM with the normalized // magnetometer measurement in the Body frame is the ERROR // accumulated in DCM and defines how much the Rotation // Matrix need to be rotated so that these vectors match. //--------------------------------------------------------- VectorCrossProduct(&MagNorm, &_DCM_BaseMAG, &MagError); //--------------------------------------------------------- // Magnetometer correction P-Term //--------------------------------------------------------- VectorScale(&MagError, _DCMMagKp, &MagPterm); //--------------------------------------------------------- // Magnetometer correction I-Term //--------------------------------------------------------- VectorScale(&MagError, (_DCMMagKi * MagDT), &MagIterm); VectorAdd(&_DCMMagIterm, &MagIterm, &_DCMMagIterm); //--------------------------------------------------------- // Adjust drift compensation vector with Magnetometer- // based correction factors //--------------------------------------------------------- VectorAdd(&DriftComp, &MagPterm, &DriftComp); VectorAdd(&DriftComp, &_DCMMagIterm, &DriftComp); //-------------------------------------------------------------------------- // </editor-fold> } //========================================================================== //========================================================================== // Local variables used in "rotating" Rotational Matrix //-------------------------------------------------------------------------- Vector RotationDelta; Matrix SmallRotation; Matrix Rotated; //-------------------------------------------------------------------------- // Calculating total adjustment and "rotate" Rotational Matrix //-------------------------------------------------------------------------- // Calculate rotation delta in body frame in radians through "integration" // of the gyro rotation rates VectorScale( Gyro, GyroDT, &RotationDelta); //-------------------------------------------------------------------------- VectorAdd(&RotationDelta, &DriftComp, &RotationDelta); // Construct "infinitesimal" rotation matrix MatrixSetSmallRotation(&RotationDelta, &SmallRotation); // Rotate DCM by "small rotation" MatrixMult(&_DCMRM, &SmallRotation, &Rotated); // Normalize and store current DCM Rotation Matrix _DCMNormalize(&Rotated, &_DCMRM); //========================================================================== //-------------------------------------------------------------------------- // Load Current orientation parameters into DCMData //-------------------------------------------------------------------------- IMUResult->Roll = _DCMRoll(&_DCMRM); IMUResult->Pitch = _DCMPitch(&_DCMRM); IMUResult->Yaw = _DCMYaw(&_DCMRM); //------------------------------------------------------------ IMUResult->Incl = _DCMIncl(&_DCMRM); //-------------------------------------------------------------------------- IMUResult->Azimuth = DCMRangeTo2Pi(_DCM_BaseAzimuth + IMUResult->Yaw); //-------------------------------------------------------------------------- IMUResult->TS = TMRGetTS(); //-------------------------------------------------------------------------- return DCM_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 //=================================================================== 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; }