//============================================================= // Synchronous READ SAMPLE API (visible externally) //------------------------------------------------------------- uint MPLReadSample(MPLData* pSample) { if (!_MPL_Init) return MPL_NOTINIT; // Not initialized... //----------------------- if (_MPL_Async) return MPL_ABSY; // Asynchronous operation in progress... //********************************************************* uint RC = 0; long Alt; //----------------------------------------- // Read MPL measurement! //----------------------------------------- if ( (RC = _MPLReadRawData(&Alt)) ) return RC; // Error... //----------------------------------------------- // Capture measurement Timestamp //----------------------------------------------- _MPL_DataTS = TMRGetTS(); //----------------------------------------------- // Report Timestamp and Altitude //----------------------------------------------- pSample->TS = _MPL_DataTS; pSample->Alt = ((float)Alt )*0.0625 - _MPL_BaseAlt; //----------------------------------------------- return MPL_OK; }
//============================================================= // Synchronous READ SAMPLE API (visible externally) //------------------------------------------------------------- uint MPUReadSample(uint MPUx, MPUData* pSample) { if (!_MPU_Init) return MPU_NOTINIT; // Not initialized... //--------------------------------------------------------- MPU_CB* pCB = MPUpCB(MPUx); if (NULL == pCB) return MPU_NOTA; // Should never happened! //------------------------------------------------------------------ if (pCB->_MPU_Async) return MPU_ABSY; // Asynchronous operation in progress... //********************************************************* uint RC = 0; _MPURawData RawData; //----------------------------------------- // Read MPU measurement! //----------------------------------------- if ( (RC = _MPUReadRawData(pCB, &RawData)) ) return RC; // Error... //----------------------------------------------- // Sample Timestamp //----------------------------------------------- pSample->TS = TMRGetTS(); //----------------------------------------------- // Temperature (C) //----------------------------------------------- pSample->Temp = (RawData.Temp - pCB->_MPU_Temp_OffsetTo0) * pCB->_MPU_Temp_Sensitivity; //----------------------------------------------- // Acceleration (G) //----------------------------------------------- VectorSet ( RawData.AX * pCB->_MPU_Accl_Sensitivity, RawData.AY * pCB->_MPU_Accl_Sensitivity, RawData.AZ * pCB->_MPU_Accl_Sensitivity, //--------- &pSample->A ); //----------------------------------------------- // Gyroscopes (Rad/sec) //----------------------------------------------- VectorSet ( RawData.GX * pCB->_MPU_Gyro_Sensitivity, RawData.GY * pCB->_MPU_Gyro_Sensitivity, RawData.GZ * pCB->_MPU_Gyro_Sensitivity, &pSample->G ); //----------------------------------------------- // Applying calibration and tuning parameters //----------------------------------------------- _MPU_ApplyCalibration(pCB, pSample); //----------------------------------------------- return MPU_OK; }
//************************************************************ // SR04-related Interrupt Routines //************************************************************ void __attribute__((__interrupt__, __no_auto_psv__)) ICInterrupt(void) { //------------- ICIF = 0; // Reset ICx interrupt request //========================================================= // Obtain timestamp for this interrupt //--------------------------------------------------------- ulong TS = TMRGetTS(); //--------------------------------------------------------- // Process interrupt based upon the captured edge //--------------------------------------------------------- if (ICPORT) { // Rising edge - start of the measurement cycle - // just save the timestamp... _SR04Start = TS; //===================================================== } else // Falling edge - if conditions are right, calculate // and save duration of the sound travel (in ticks) { if (0 != _SR04Start) // The rising edge was captured { ulong SampleInt = TS - _SR04Start; // Is this a valid measurement - duration < 25 msec // (asuming primary interval timer is "ticking" at 0.2 usec) if (SampleInt > 125000) // Invalid: SampleInt > 25 msec SampleInt = 125000; // Force to valid range //------------------------------------------------- if (0 == _SR04Ready || _SR04Ready > 16383) // Either no sample or too many samples { _SR04Sum = SampleInt; _SR04Ready = 1; } else { _SR04Sum += SampleInt; _SR04Ready++; } //------------------------------------------------- // Reset to wait for rising edge _SR04Start = 0; // Capture TS of the most recent measurement // (required for Speed calculation) _SR04RecentTS = TS; } //----------------------------------------------------- } return; }
//============================================================= // Synchronous READ SAMPLE API (visible externally) //------------------------------------------------------------- uint MPUReadSample(MPUData* pSample) { if (!_MPU_Init) return MPU_NOTINIT; // Not initialized... //----------------------- if (_MPU_Async) return MPU_ABSY; // Asynchronous operation in progress... //********************************************************* uint RC = 0; _MPURawData RawData; float TDev; //----------------------------------------- // Read MPU measurement! //----------------------------------------- if ( (RC = _MPUReadRawData(&RawData)) ) return RC; // Error... //----------------------------------------------- // Timestamp and Count //----------------------------------------------- pSample->TS = TMRGetTS(); pSample->Count = ++_MPU_Count; //----------------------------------------------- // Temperature (C) (will be used in subsequent // temperature compensation calculation) //----------------------------------------------- pSample->Temp = (RawData.Temp - _MPU_Temp_OffsetTo0) * _MPU_Temp_Sensitivity; //----------------------------------------------- // Acceleration (G) //----------------------------------------------- TDev = pSample->Temp - _MPU_Accl_BaseTemp; //----------------------------------------------- VectorSet ( ((float)RawData.AX - (_MPU_Accl_XOffset + _MPU_Accl_XSlope*TDev)) * _MPU_Accl_Sensitivity, ((float)RawData.AY - (_MPU_Accl_YOffset + _MPU_Accl_YSlope*TDev)) * _MPU_Accl_Sensitivity, ((float)RawData.AZ - (_MPU_Accl_ZOffset + _MPU_Accl_ZSlope*TDev)) * _MPU_Accl_Sensitivity, &pSample->A ); //----------------------------------------------- // Gyroscopes (Rad/sec) //----------------------------------------------- TDev = pSample->Temp - _MPU_Gyro_BaseTemp; //----------------------------------------------- VectorSet ( ((float)RawData.GX - (_MPU_Gyro_XOffset + _MPU_Gyro_XSlope*TDev)) * _MPU_Gyro_Sensitivity, ((float)RawData.GY - (_MPU_Gyro_YOffset + _MPU_Gyro_YSlope*TDev)) * _MPU_Gyro_Sensitivity, ((float)RawData.GZ - (_MPU_Gyro_ZOffset + _MPU_Gyro_ZSlope*TDev)) * _MPU_Gyro_Sensitivity, &pSample->G ); //----------------------------------------------- return MPU_OK; }
//************************************************************* uint MPLAsyncReadIfReady(MPLData* pSample) { if (0 == _MPL_Async) return MPL_NACT; // Asynchronous read not active... //-------------------------------------------------- if (0 == _MPL_Ready) { // Check for potentially "missing" interrupt if (TMRGetTS() - _MPL_DataTS > _MPL_MaxInt) // Looks like the Interrupt went missing... MPL_IF = 1; // Trigger interrupt manually - // emulate rising edge //------------------------- return MPL_NRDY; } //-------------------------------------------------- return _MPLAsyncRead(pSample); }
//************************************************************* 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); }
//************************************************************* uint MPLAsyncStart() { if (!_MPL_Init) return MPL_NOTINIT; // Not initialized... //--------------------------------------------------------- if (_MPL_Async) return MPL_OK; // Already started... //========================================================= _MPL_Ready = 0; // Discard async sample, if any //--------------------------------------------------------- // Set flag indicating that Async mode enabled //--------------------------------------------------------- _MPL_Async = 1; //--------------------------------------------------------- MPL_IF = 0; // Clear the interrupt flag MPL_IE = 1; // Enable MPL interrupt //--------------------------------------------------------- // Let's set the "last read timestamp" to the current time // so that Async read routines may trigger interrupt if // maximum time for sample acquisition has expired. //--------------------------------------------------------- _MPL_DataTS = TMRGetTS(); //--------------------------------------------------------- // Due to the idiosyncrasy of the MPL3115, if the interrupt // line is asserted it stays asserted until the data is // read, thus "robbing" us from the rising edge trigger... //--------------------------------------------------------- if ( MPL_INT_PORT ) // Data Ready line ASSERTed { MPL_IF = 1; // Trigger interrupt manually - // emulate rising edge } //========================================================= return MPL_OK; }
int main(void) { //******************************************************************* Init(); TMRInit(2); // Initialize Timer interface with Priority=2 //-------------------------- BLIInit(); // Initialize Signal interface 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 cable // BaudRate = 500 => 1,250,000 bps // BaudRate = 1000 => 2,500,000 bps //******************************************************************* if ( MPUInit(0, 3) ) // Initialize motion Sensor // 1 kHz/(0+1) = 1000 Hz (1ms) // DLPF=3 => Bandwidth 44 Hz (delay: 4.9 msec) BLIDeadStop("EG", 2); //-------------------------- 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); //-------------------------- if ( MPLInit(5) ) // Average over 32 samples providing // update rate about 10 Hz BLIDeadStop("EA", 2); //******************************************************************* uint RC = 0; ulong Alarm = 0; ulong TS = 0; //------------------------------- struct { ulong TS; MPUSample IMUData; HMCSample MagData; MPLSample AltData; } UData; //******************************************************************* BLIAsyncStart(100, 50); RC = MPLSetGround(); if (RC) BLIDeadStop("SOS", 3); // Failure... BLIAsyncStop(); //==================================================== BLISignalOFF(); //==================================================== // Testing MPU, HMC, and MPL together in a real-life // scenario //==================================================== if(MPUAsyncStart()) BLIDeadStop("SG", 2); //------------------------ if(HMCAsyncStart()) BLIDeadStop("SM", 2); //------------------------ if(MPLAsyncStart()) BLIDeadStop("SA", 2); RC = MPLAsyncReadWhenReady(&UData.AltData); if (RC) BLIDeadStop("SAS", 3); // Failure... //==================================================== while (TRUE) { Alarm = TMRSetAlarm(500); //------------------------------------ RC = MPUAsyncReadIfReady(&UData.IMUData); if (MPU_OK != RC && MPU_NRDY != RC) BLIDeadStop("G", 1); //------------------------ RC = HMCAsyncReadIfReady(&UData.MagData); if (HMC_OK != RC && HMC_NRDY != RC) BLIDeadStop("M", 1); //------------------------ RC = MPLAsyncReadIfReady(&UData.AltData); if (MPL_OK != RC && MPL_NRDY != RC) BLIDeadStop("A", 3); // Failure... //------------------------- UData.TS = TMRGetTS(); //------------------------- if (0 == TS) TS = UData.TS; UData.TS -= TS; BLISignalFlip(); //------------------------- UARTPostIfReady((byte*)&UData, sizeof(UData)); //------------------------- TMRWaitAlarm(Alarm); } //==================================================== 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 //******************************************************************* //------------------------------------------------------------------- #define MaxRec 60 //------------------------------------------------------------------- struct { DWORD TS; WORD Index; char filler[506]; } Data; //------------------------------------------------------------------- uint i; for (i = 0; i < sizeof(Data.filler); i++) { Data.filler[i] = 0; } //------------------------------------------------------------------- DWORD TimeStart; float TimePoints[MaxRec]; // WORD TimeIndex[MaxRec]; //------------------------------------------------------------------- char pFN[15]; WORD RC; uint ReadCnt; FSFILE File; //******************************************************************* BLISignalON(); //------------------------------ // Create (Open) Log file for Writing and Reading //------------------------------ RC = LogNewFile(&File); while ( RC != LE_GOOD ); //------------------------------ // Retrieve and save for future new log file name //------------------------------ RC = FS_GetName(pFN, 15, &File); while ( RC != LE_GOOD ); //------------------------------ // Write sample data to file //------------------------------ for (i=0; i < MaxRec; i++) { Data.Index = i; Data.TS = TMRGetTS(); RC = FS_WriteSector(&File, &Data); while (CE_GOOD != RC); } //------------------------------ // Check position in the file //------------------------------ i = FS_GetPosition (&File); while (i != 512*MaxRec); //------------------------------ // Close the file - save changes //------------------------------ RC = FS_Flush(&File); while ( RC != CE_GOOD ); //------------------------------ //------------------------------ // Open file for Reading //------------------------------ RC = FS_CreateFile(pFN, FS_READ_ONLY, &File); while ( RC != CE_GOOD ); //------------------------------ // Reed records //------------------------------ for (i=0; i < MaxRec; i++) { RC = FS_Read (&File, &Data, sizeof(Data), &ReadCnt); while (CE_GOOD != RC); //-------------------------- // TimeIndex[i] = Data.Index; //-------------------------- if (0 == i) TimeStart = Data.TS; //-------------------------- TimePoints[i] = (Data.TS - TimeStart) * TMRGetTSRate() * 1000.0; // in msec //-------------------------- TimeStart = Data.TS; } //------------------------------ // Close the file - save changes //------------------------------ RC = FS_Flush(&File); while ( RC != CE_GOOD ); //******************************************************************* BLISignalOFF(); //------------------------------ i = 1 + i; //------------------------------ while(1); return 0; }
//================================================================ void _MPLCallBack() { //=============================================================== // NOTE: This I2C interrupt call-back routine is geared specifi- // cally to supporting asynchronous data read operation for // MPL3115 Altitude/Pressure sensor //=============================================================== // General status checks - valid under all conditions //=============================================================== if ( I2C_ACKSTAT // 1 = NACK received from slave || I2C_BCL // 1 = Master Bus Collision || I2C_IWCOL // 1 = Write Collision || I2C_I2COV // 1 = READ Overflow condition ) { //----------------------------------------------------------- // Terminate current ASYNC session //----------------------------------------------------------- I2CAsyncStop(); // Un-subscribe from I2C and stop ASYNC return; } //=============================================================== // Process conditions based upon the state of the State Machine //=============================================================== switch (_MPL_State) { //============================================================= // Set MPL Register Address to Data (0xC0) //============================================================= case 0: // Interrupt after initial SEN // Sending device address with WRITE cond. I2C_TRM_Reg = _MPL_Addr & 0xFE; _MPL_State = 1; // Move state break; //----------------------------------------------------------- case 1: // Interrupt after device address // Slave send ACK (confirmed in General Checks); // We proceed with register address I2C_TRM_Reg = 0x01; // MPL3115 Data register address. _MPL_State = 2; // Move state break; //============================================================= // Transition to the Reading Data Mode //============================================================= case 2: // Interrupt after register address // Slave send ACK (confirmed in General Checks); // Initiate Repeated Start condition I2C_RSEN = 1; _MPL_State = 3; // Move state break; //----------------------------------------------------------- case 3: // Interrupt after Repeated Start // Sending device address with READ cond. I2C_TRM_Reg = _MPL_Addr | 0x01; _MPL_State = 4; // Move state break; //----------------------------------------------------------- case 4: // Interrupt after Device Address with READ // Slave send ACK; we switch to READ I2C_I2COV = 0; // Clear receive OVERFLOW bit, if any I2C_RCEN = 1; // Allow READ. _MPL_BufPos = 0; // Reset buffer position //---------------------------------------- _MPL_State = 5; // Move state break; //============================================================= // Read and process data sample //============================================================= case 5: // Interrupt after READing Data byte // Slave completed sending byte... // Retrieve and store data byte _MPL_Buffer[_MPL_BufPos] = I2C_RCV_Reg; _MPL_BufPos++; // Move buffer address pointer //--------------------------------------- if (_MPL_BufPos < 5) // More bytes to read { // Generate ACK for the byte read I2C_ACKDT = 0; I2C_ACKEN = 1; //-------------- _MPL_State = 6; // Move state } else // All 5 bytes are read { // Generate NACK for the last byte read I2C_ACKDT = 1; I2C_ACKEN = 1; //-------------- _MPL_State = 7; // Move state } break; //----------------------------------------- case 6: // Interrupt after ACK I2C_I2COV = 0; // Clear receive OVERFLOW bit, if any I2C_RCEN = 1; // Allow READ. //---------------------------------------- _MPL_State = 5; // Move state BACK to read next byte break; //============================================================= // After we consumed the Sample, we need to innitiate the new // OST cycle by reading the CtrlR1 and then writing it back // with OST bit set. //============================================================= case 7: // Interrupt after NACK - switch direction to // WRITE starting with "Repeated Start" // Initiate Repeated Start condition I2C_RSEN = 1; _MPL_State = 8; // Move state break; //----------------------------------------------------------- case 8: // Interrupt after RSEN // Sending device address with WRITE cond. I2C_TRM_Reg = _MPL_Addr & 0xFE; _MPL_State = 9; // Move state break; //----------------------------------------------------------- case 9: // Interrupt after device address // Slave send ACK (confirmed in General Checks); // We proceed with register address I2C_TRM_Reg = CtrlR1Addr; // MPL3115 CtrlR1 address. _MPL_State = 10; // Move state break; //============================================================= // Transition to the Reading Data Mode //============================================================= case 10: // Interrupt after sending register address // Slave send ACK (confirmed in General Checks); // Initiate Repeated Start condition I2C_RSEN = 1; _MPL_State = 11; // Move state break; //----------------------------------------------------------- case 11: // Interrupt after Repeated Start // Sending device address with READ cond. I2C_TRM_Reg = _MPL_Addr | 0x01; _MPL_State = 12; // Move state break; //----------------------------------------------------------- case 12: // Interrupt after Device Address with READ // Slave send ACK; we switch to READ I2C_I2COV = 0; // Clear receive OVERFLOW bit, if any I2C_RCEN = 1; // Allow READ. _MPL_BufPos = 0; // Reset buffer position //---------------------------------------- _MPL_State = 13; // Move state break; //============================================================= // Read CtrlR1 //============================================================= case 13: // Interrupt after READing Data byte // Slave completed sending byte... _MPL_CtrlR1 = I2C_RCV_Reg; // Retrieve and store data byte //--------------------------------------- // Generate NACK for the last byte read I2C_ACKDT = 1; I2C_ACKEN = 1; //--------------------------------------- _MPL_State = 14; // Move state break; //============================================================= // Write CtrlR1 with OST bit set //============================================================= case 14: // Interrupt after ACK - switch direction to // WRITE starting with "Repeated Start" // Initiate Repeated Start condition I2C_RSEN = 1; _MPL_State = 15; // Move state break; //----------------------------------------------------------- case 15: // Interrupt after RSEN // Sending device address with WRITE cond. I2C_TRM_Reg = _MPL_Addr & 0xFE; _MPL_State = 16; // Move state break; //----------------------------------------------------------- case 16: // Interrupt after device address // Slave send ACK (confirmed in General Checks); // We proceed with register address I2C_TRM_Reg = CtrlR1Addr; // MPL3115 CtrlR1 address. _MPL_State = 17; // Move state break; //----------------------------------------------------------- case 17: // Interrupt after CtrlR1 address // Slave send ACK (confirmed in General Checks); // We proceed with new value for CtrlR1 I2C_TRM_Reg = _MPL_CtrlR1 | CtrlR1SetOST; _MPL_State = 18; // Move state break; //----------------------------------------------------------- case 18: // Interrupt after CtrlR1 write // Slave send ACK (confirmed in General Checks); //----------------------------------------------------------- // Terminate current ASYNC session //----------------------------------------------------------- I2CAsyncStop(); // Un-subscribe from I2C and stop ASYNC //----------------------------------------------------------- if (_MPL_PortLvl > 0) { //=============================================== // Construct 20-bit ALT sample as integer //----------------------------------------------- long Alt = *((sbyte*)&_MPL_Buffer[0]); Alt = Alt << 8; Alt = Alt | _MPL_Buffer[1]; Alt = Alt << 8; Alt = Alt | _MPL_Buffer[2]; Alt = Alt >> 4; //----------------------------------------------- if (0 == _MPL_Ready || _MPL_Ready > 512) // First sample in sequence or we // need to start a new "sequence" // to preclude overflow of _MPL_Data //--------------------------------- _MPL_Data = Alt; else // Subsequent sample in sequence: accumulate data // for subsequent averaging in retrieval routine. //--------------------------------- _MPL_Data += Alt; //----------------------------------------------- _MPL_Ready++; // _MPL_Ready > 0 -> Sample is ready! } _MPL_DataTS = TMRGetTS(); //----------------------------------------------------------- break; //============================================================= // Oops! something is terribly wrong with the State Machine... //============================================================= default: //----------------------------------------------------------- // Terminate current ASYNC session //----------------------------------------------------------- I2CAsyncStop(); // Un-subscribe from I2C and stop ASYNC }
//================================================================================== 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; }
//************************************************************* uint _MPUAsyncRead(MPUData* pSample) { //---------------------------------------------- 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 */ //----------------------------------------------- pSample->Count = _MPU_Count; //--------------------------------- 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); //============================================== // Set sample timestamp //---------------------------------------------- pSample->TS = TMRGetTS(); //============================================== float TDev; //---------------------------------------------- // 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) //----------------------------------------------- pSample->Temp = (Weight * RawData.Temp - _MPU_Temp_OffsetTo0) * _MPU_Temp_Sensitivity; //----------------------------------------------- // Acceleration (G) //----------------------------------------------- TDev = pSample->Temp - _MPU_Accl_BaseTemp; //----------------------------------------------- VectorSet ( (Weight * RawData.AX - (_MPU_Accl_XOffset + _MPU_Accl_XSlope*TDev)) * _MPU_Accl_Sensitivity, (Weight * RawData.AY - (_MPU_Accl_YOffset + _MPU_Accl_YSlope*TDev)) * _MPU_Accl_Sensitivity, (Weight * RawData.AZ - (_MPU_Accl_ZOffset + _MPU_Accl_ZSlope*TDev)) * _MPU_Accl_Sensitivity, &pSample->A ); //----------------------------------------------- // Gyroscopes (Rad/sec) //----------------------------------------------- TDev = pSample->Temp - _MPU_Gyro_BaseTemp; //----------------------------------------------- VectorSet ( (Weight * RawData.GX - (_MPU_Gyro_XOffset + _MPU_Gyro_XSlope*TDev)) * _MPU_Gyro_Sensitivity, (Weight * RawData.GY - (_MPU_Gyro_YOffset + _MPU_Gyro_YSlope*TDev)) * _MPU_Gyro_Sensitivity, (Weight * RawData.GZ - (_MPU_Gyro_ZOffset + _MPU_Gyro_ZSlope*TDev)) * _MPU_Gyro_Sensitivity, &pSample->G ); //----------------------------------------------- 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; }
//============================================================================== // RC-RX UART Receive Interrupt routine //------------------------------------------------------------------------------ void __attribute__((__interrupt__,__no_auto_psv__)) RCRXInterrupt(void) { uint Code; while (URXDA) { byte Byte = URXREG; // Read received byte if( Byte == _RCRX_Start ) Code = 1; // 1 - Start code else if (Byte == _RCRX_Stop ) Code = 2; // 2 - Stop code else Code = 0; // 0 - Data byte //--------------------------- // Advance state //--------------------------- _RCRX_RX_CurrentState = _RCRX_RX_StateControl[_RCRX_RX_CurrentState][Code]; //-------------------------------- // Perform Action on received byte //-------------------------------- switch (_RCRX_RX_CurrentState) { //------------------------------------------------------------- case 0: // 0: Scan to Start - No action... break; //------------------------------------------------------------- //------------------------------------------------------------- case 1: // 1: First Start byte during scan, wait for second... break; //------------------------------------------------------------- //------------------------------------------------------------- case 2: // 2: Second Start byte during scan, ready for frame data... break; //------------------------------------------------------------- //------------------------------------------------------------- case 3: // 3: Data byte received _RCRX_SaveByte(Byte); // Store data byte break; //------------------------------------------------------------- //------------------------------------------------------------- case 4: // 4: Potential "STOP"? break; //------------------------------------------------------------- //------------------------------------------------------------- case 5: // 5: Data after miscellaneous "STOP" _RCRX_SaveByte(_RCRX_Stop); // Store skipped STOP _RCRX_SaveByte(Byte); // Store data byte //--------------------------------------------------------- break; //------------------------------------------------------------- //------------------------------------------------------------- case 6: // 6: Frame terminated if ( _RCRX_RX_FrameIdx == _RCRX_FrameSize ) { // Valid frame... submit frame //---------------------------------------------------------- bytecopy( (byte*)_RCRX_RX_NewFrame, (byte*)_RCRX_RX_FrameBuf, _RCRX_FrameSize); //-------------------------------------- _RCRX_IsNewFrame = TRUE; _RCRX_IsConnected = TRUE; _RCRX_LastReadTS = TMRGetTS(); //---------------------------------------------------------- } //-------------------------------------------------------------- // Reset receiver //-------------------------------------------------------------- _RCRX_RX_CurrentState = 0; // Reset automaton _RCRX_RX_FrameIdx = 0; // Reset index in Receive buffer //-------------------------------------------------------------- break; //------------------------------------------------------------- } } //------------------------------------------------- URXIF = 0; // Clear UART RX interrupt flag //------------------------------------------------- return ; }
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; }
int main(void) { //******************************************************************* Init(); TMRInit(2); // Initialize Timer interface with Priority=2 BLIInit(); // Initialize Signal interface ADCInit(3); // Initialize ADC to control battery I2CInit(5, 0); // Initialize I2C1 module with IPL=5 and Fscl=400 KHz //-------------------------- BLISignalON(); //-------------------------- if (MPUInit(3, 1)) // Initialize motion Sensor - 1 kHz/4 (250 Hz) DeadStop("A", 1); //-------------------------- BLISignalOFF(); //-------------------------- 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; //-------------------------- _MPURawData RawData; //-------------------------- struct { ulong TS; // Timestamp of the cycle //----------------------------------------------- // Temperature //----------------------------------------------- float Temp; //----------------------------------------------- // Accelerometer //----------------------------------------------- float AX; float AY; float AZ; //----------------------------------------------- // Gyroscopes //----------------------------------------------- float GX; float GY; float GZ; //----------------------------------------------- } UData; //******************************************************************* long AX, AY, AZ, GX, GY, GZ, Temp; //******************************************************************* while(1) { //------------------------ if (ADCGetBatteryStatus() < 30) DeadStop("SOS", 3); //------------------------ AX= AY= AZ= GX= GY= GZ= Temp = 0; //------------------------ int i; for (i=0; i<128; i++) { RC = _MPUReadRawData(&RawData); if (RC) DeadStop("A", 1); //----------------------------- AX += RawData.AX; AY += RawData.AY; AZ += RawData.AZ; //----------------- GX += RawData.GX; GY += RawData.GY; GZ += RawData.GZ; //----------------- Temp+= RawData.Temp; //----------------------------- } //--------------------------------------------- UData.TS = TMRGetTS(); //------------------------ UData.Temp = ((float)Temp/128.0 - _MPU_Temp_OffsetTo0) * _MPU_Temp_Sensitivity - 25.0; //------------------------ UData.AX = (float)AX/128.0; UData.AY = (float)AY/128.0; UData.AZ = (float)AZ/128.0; //------------------------ UData.GX = (float)GX/128.0; UData.GY = (float)GY/128.0; UData.GZ = (float)GZ/128.0; //--------------------------------------------- UARTPostWhenReady((uchar*)&UData, sizeof(UData)); //--------------------------------------------- } return 1; }
//============================================================================== void DCMReset(BOOL UseAcc, BOOL UseMag, BOOL MagYawOnly, Vector* Acc, Vector* Mag) { // First, some "housekeeping" ... TMRInitDefault(); // Most probably timer is already initialized // so this call is just a NOP //------------------------------------------------------------------ // Set gyro drift compensation options //------------------------------------------------------------------ _DCM_UseAcc = UseAcc; _DCM_UseMag = UseMag; _DCM_UseMagYawOnly = MagYawOnly; //------------------------------------------------------------------ // Hopefully the Acceleration vector A we received was averaged over // some time (maybe, 200-300 msec) and is rather stable as we will // derive from it initial orientation of the quad... //------------------------------------------------------------------ // Please NOTE: As we have Axis Z pointing down and gravity // acceleration vector pointing up (-1.0), we have to change sign // on arguments of the Atan2 function to get the correct quadrant float Roll = atan2f(-Acc->Y, -Acc->Z); // float Pitch = atan2f(Acc->X, sqrtf(Acc->Y*Acc->Y + Acc->Z*Acc->Z)); float Yaw = 0.0; // Using calculated values for Roll, Pitch, and Yaw we may build // initial Rotation Matrix MatrixEulerRotation(Roll, Pitch, Yaw, &_DCMRM); //------------------------------------------------------------------ //------------------------------------------------------------------ // Now that we established rotation matrix, we may convert magnetic // vector measured in Body frame to Earth frame, normalize (as we do // not care about the strength of magnetic field, just its direction) // and store for future use... //------------------------------------------------------------------ DCMToEarth(Mag, &_DCM_BaseMAG); VectorNormalize(&_DCM_BaseMAG, &_DCM_BaseMAG); //----------------------------------------------- // ...and then calculate Base Azimuth //----------------------------------------------- if (0 != _DCM_BaseMAG.X || 0 != _DCM_BaseMAG.Y ) _DCM_BaseAzimuth = atan2f(-_DCM_BaseMAG.Y, _DCM_BaseMAG.X); else _DCM_BaseAzimuth = 0.0; //------------------------------------------------------------------ // If magnetic vector is used ONLY for Yaw drift compensation we // should nullify Z-axis component //------------------------------------------------------------------ if (TRUE == _DCM_UseMag && TRUE == _DCM_UseMagYawOnly) _DCM_BaseMAG.Z = 0.0; //================================================================== // Accelerometer-based and Magnetometer-based integral correction // terms are cumulative by nature, so we need to reset their values //================================================================== VectorSet(0.0, 0.0, 0.0, &_DCMAccIterm); VectorSet(0.0, 0.0, 0.0, &_DCMMagIterm); //================================================================== _DCMInit = 1; // DCM initialization is successful // Timestamps of DCM initialization - later used as the last timestamp // of respective sensor measurements _DCM_GyroTS = _DCM_AccTS = _DCM_MagTS = TMRGetTS(); }
//************************************************************* uint _MPUAsyncRead(MPU_CB* pCB, MPUData* pSample) { //---------------------------------------------- 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 = pCB->_MPU_Sample.Temp; //--------------------------------- RawData.AX = pCB->_MPU_Sample.AX; RawData.AY = pCB->_MPU_Sample.AY; RawData.AZ = pCB->_MPU_Sample.AZ; //--------------------------------- RawData.GX = pCB->_MPU_Sample.GX; RawData.GY = pCB->_MPU_Sample.GY; RawData.GZ = pCB->_MPU_Sample.GZ; //----------------------------------------------- Ready_Cnt = pCB->_MPU_Ready; //----------------------------------------------- pCB->_MPU_Ready = 0; // Sample consumed... //---------------------------------------------- // Leave MPU/I2C CRITICAL SECTION //============================================== RESTORE_CPU_IPL(current_cpu_ipl); //============================================== // Set sample timestamp //---------------------------------------------- pSample->TS = TMRGetTS(); //============================================== // 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) //----------------------------------------------- pSample->Temp = (Weight * RawData.Temp - pCB->_MPU_Temp_OffsetTo0) * pCB->_MPU_Temp_Sensitivity; //----------------------------------------------- // Acceleration (G) //----------------------------------------------- VectorSet ( Weight * RawData.AX * pCB->_MPU_Accl_Sensitivity, Weight * RawData.AY * pCB->_MPU_Accl_Sensitivity, Weight * RawData.AZ * pCB->_MPU_Accl_Sensitivity, //--------- &pSample->A ); //----------------------------------------------- // Gyroscopes (Rad/sec) //----------------------------------------------- VectorSet ( Weight * RawData.GX * pCB->_MPU_Gyro_Sensitivity, Weight * RawData.GY * pCB->_MPU_Gyro_Sensitivity, Weight * RawData.GZ * pCB->_MPU_Gyro_Sensitivity, //--------- &pSample->G ); //----------------------------------------------- // Applying calibration and tuning parameters //----------------------------------------------- _MPU_ApplyCalibration(pCB, pSample); //----------------------------------------------- return MPU_OK; // The return code was OK }