//=============================================================
// 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
}
