//=============================================================
// Synchronous READ of the raw sensor data
//-------------------------------------------------------------
uint _MPUReadRawData(MPU_CB* pCB, _MPURawData* pData)
{
int RC = 0;
byte Status = 0;
ulong Alarm;
//-----------------------------------
if ( (RC = _MPURead(pCB, MPU6050_INT_STATUS, &Status, 1)) )
return RC; // Error...
//-----------------------------------
Alarm = TMRSetAlarm(50); // Expecting to get sample within 50 msec
//-----------------------------------
while ( (Status & 0x01) != 0x01 )
{
TMRDelayTicks(1); // Short delay...
if ( (RC = _MPURead(pCB, MPU6050_INT_STATUS, &Status, 1)) )
return RC; // Error...
if (TMRCheckAlarm(Alarm))
return MPU_TMOUT;
}
//-----------------------------------------
// MPU has a measurement!
//-----------------------------------------
byte Data[14];
// Read measurement
if ( (RC = _MPURead(pCB, MPU6050_DATA_START, Data, 14) ) )
return RC; // Error...
//===============================================
// Process sensor data
_MPU_ConvertReadBuffer(Data, pData);
//===============================================
return MPU_OK;
}
//************************************************************
// Blocks for "Delay" milliseconds
//************************************************************
void TMRDelay(ulong Delay)
{
//----------------------------------------------------
// Implements "delay" by combining setting Alarm and
// waiting for the Alarm in one function
//----------------------------------------------------
TMRWaitAlarm(TMRSetAlarm(Delay));
//---------------------------------------------------
return;
}
int main(void)
{
//*******************************************************************
Init();
TMRInit(2); // Initialize Timer interface with Priority=2
BLIInit(); // Initialize Signal interface
//*******************************************************************
// Switch 1 controls the Serial Data Logger (SDL) communication speed
//-------------------------------------------------------------------
if (_SW1)
// Switch 1 is ON - Configuring SDL for PIC-to-PIC
// high-speed communication at 1 MBaud
SDLInit(3, BAUD_1M);
else
// Switch 1 is OFF - Configuring SDL for ZigBEE
// wireless communication at 115.2 KBaud
SDLInit(3, BAUD_115200);
//*******************************************************************
I2CInit(5, 1); // First param: IL = 5 (interrupt request priority
// Second param: I2C speed
// 0 - lowest (123 kHz at Fcy = 64MHz)
// 1 - 200 kHz
// 2 - 400 kHz
// 3 - 1 MHz
//-------------------------------------------------------------------
uint RC = 0;
ulong Alarm = 0;
//==================================================================
BLIAsyncStart(100,100);
TMRDelay(2000);
BLIAsyncStop();
//==================================================================
BLIAsyncStart(50,50);
if (_SW2)
// Switch 2 is ON - Configuring MPU fo Alt. sensitivity
RC = MPUInit(0, 3, MPU_GYRO_1000ds, MPU_ACC_4g);
// Initialize motion Sensor
// 1 kHz/(0+1) = 1000 Hz (1ms)
// DLPF=3 => Bandwidth 44 Hz (delay: 4.9 msec)
else
// Switch 2 is OFF - Configuring MPU fo normal sensitivity
RC = MPUInit(0, 3, MPU_GYRO_2000ds, MPU_ACC_2g);
// Initialize motion Sensor
// 1 kHz/(0+1) = 1000 Hz (1ms)
if (RC) BLIDeadStop("EG", 2);
BLIAsyncStop();
//*******************************************************************
BLISignalOFF();
//====================================================
// byte mpuID;
// byte mpuDLPF;
// byte mpuINT;
// byte mpuPWRM1;
// //---------------------------
// RC = MPUReadID(2, &mpuID);
// RC = MPUGetPWRM1(2, &mpuPWRM1);
// RC = MPUGetDLPF(2, &mpuDLPF);
// RC = MPUGetINT(2, &mpuINT);
//-----------------------------------------------------
//====================================================
// Synchronous interface
//-----------------------------------------------------
struct
{
MPUData Sample1;
MPUData Sample2;
} MPU;
//-----------------------------------------------------
while (TRUE)
{
Alarm = TMRSetAlarm(500);
//------------------------------------
if ( (RC = MPUReadSample(1, &MPU.Sample1)) )
BLIDeadStop("SOS", 3);
//------------------------
if ( (RC = MPUReadSample(2, &MPU.Sample2)) )
BLIDeadStop("SOS", 3);
//------------------------------------
BLISignalFlip();
//-------------------------
SDLPostIfReady((byte*)&MPU, sizeof(MPU));
//-------------------------
TMRWaitAlarm(Alarm);
}
//*******************************************************************
return 0;
}
//=============================================================
uint _MPUCalibrateAsync()
{
if (!_MPU_Init)
return MPU_NOTINIT; // Not initialized...
//-----------------------
if (0 == _MPU_Async)
return MPU_NACT; // Asynchronous read not active...
//=========================================================
// Local Variables
//---------------------------------------------------------
ulong Alarm = TMRSetAlarm(2000); // Set Alarm time 2 sec
// into the future.
//---------------------------------------------------------
uint Ready_Cnt;
_MPURawData RawData;
//----------------------------------------------
int current_cpu_ipl;
//=========================================================
//=========================================================
// Reset Gyro offsets...
//---------------------------------------------------------
_MPU_Gyro_XOffset = 0;
_MPU_Gyro_YOffset = 0;
_MPU_Gyro_ZOffset = 0;
//---------------------------------------------------------
// To collect averages we would like to "watch" MPU for at
// least 2 seconds; number of samples will be variable
// depending on the value of RateDiv
//---------------------------------------------------------
TMRWaitAlarm(Alarm);
//---------------------------------------------------------
//=========================================================
// Enter MPU/I2C CRITICAL SECTION
//---------------------------------------------------------
SET_AND_SAVE_CPU_IPL(current_cpu_ipl, _MPU_IL); /* disable interrupts */
//---------------------------------------------------------
RawData.Temp = _MPU_Sample.Temp;
//---------------------------------
RawData.AX = _MPU_Sample.AX;
RawData.AY = _MPU_Sample.AY;
RawData.AZ = _MPU_Sample.AZ;
//---------------------------------
RawData.GX = _MPU_Sample.GX;
RawData.GY = _MPU_Sample.GY;
RawData.GZ = _MPU_Sample.GZ;
//---------------------------------------------------------
Ready_Cnt = _MPU_Ready;
//---------------------------------------------------------
_MPU_Ready = 0; // Sample consumed...
//---------------------------------------------------------
// Leave MPU/I2C CRITICAL SECTION
//=========================================================
RESTORE_CPU_IPL(current_cpu_ipl);
//=========================================================
//=========================================================
// Adjust Sample Weight to account for multiple samples
//---------------------------------------------------------
float Weight;
if (Ready_Cnt > 1)
Weight = 1.0/(float)Ready_Cnt;
else
Weight = 1.0;
//=========================================================
//=========================================================
// Process collected sample
//---------------------------------------------------------
// Gyro offset is calculated in LSB units
//---------------------------------------
_MPU_Gyro_XOffset = (float)RawData.GX * Weight;
_MPU_Gyro_YOffset = (float)RawData.GY * Weight;
_MPU_Gyro_ZOffset = (float)RawData.GZ * Weight;
//---------------------------------------
// Base temperature converted to degrees C
//---------------------------------------
_MPU_Gyro_BaseTemp = ((float)RawData.Temp * Weight - _MPU_Temp_OffsetTo0)
* _MPU_Temp_Sensitivity;
//*********************************************************
return MPU_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;
}
//=============================================================
uint _MPUCalibrateSync()
{
if (!_MPU_Init)
return MPU_NOTINIT; // Not initialized...
//-----------------------
if (_MPU_Async)
return MPU_ABSY; // Asynchronous operation in progress...
//=========================================================
// Local Variables
//---------------------------------------------------------
ulong Alarm = TMRSetAlarm(2000); // Set Alarm time 2 sec
// into the future.
//---------------------------------------------------------
_MPURawData RawData;
//------------------------
long GX = 0;
long GY = 0;
long GZ = 0;
long T = 0;
//------------------------
long SampleCount = 0;
//------------------------
uint RC = MPU_OK; // Pre-set to Success
//=========================================================
//=========================================================
// Reset Gyro offsets...
//---------------------------------------------------------
_MPU_Gyro_XOffset = 0;
_MPU_Gyro_YOffset = 0;
_MPU_Gyro_ZOffset = 0;
//---------------------------------------------------------
// To collect averages we would like to "watch" MPU for at
// least 2 seconds; number of samples will be variable
// depending on the value of RateDiv
//---------------------------------------------------------
do
{
if ( (RC = _MPUReadRawData(&RawData)) )
return RC; // Error...
//------------------
GX += RawData.GX;
GY += RawData.GY;
GZ += RawData.GZ;
T += RawData.Temp;
//------------------
SampleCount++;
}
while ( FALSE == TMRTestAlarm(Alarm) );
//---------------------------------------
// Gyro offset is calculated in LSB units
//---------------------------------------
_MPU_Gyro_XOffset = (float)GX/(float)SampleCount;
_MPU_Gyro_YOffset = (float)GY/(float)SampleCount;
_MPU_Gyro_ZOffset = (float)GZ/(float)SampleCount;
//---------------------------------------
// Base temperature converted to degrees C
//---------------------------------------
_MPU_Gyro_BaseTemp = ((float)T/(float)SampleCount - _MPU_Temp_OffsetTo0)
* _MPU_Temp_Sensitivity;
//*********************************************************
return MPU_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
//*******************************************************************
// Switch 1 controls the Serial Data Logger (SDL) communication speed
//-------------------------------------------------------------------
if (_SW1)
// Switch 1 is ON - Configuring SDL for PIC-to-PIC
// high-speed communication at 1 MBaud
SDLInit(3, BAUD_1M);
else
// Switch 1 is OFF - Configuring SDL for ZigBEE
// wireless communication at 115.2 KBaud
SDLInit(3, BAUD_115200);
//*******************************************************************
I2CInit(5, 2); // First param: IL = 5 (interrupt request priority
// Second param: I2C speed
// 0 - lowest (123 kHz at Fcy = 64MHz)
// 1 - 200 kHz - MPU-6050 stable
// 2 - 400 kHz
// 3 - 1 MHz
//-------------------------------------------------------------------
uint RC = 0;
ulong Alarm = 0;
//==================================================================
BLIAsyncStart(50,50);
TMRDelay(5000);
BLIAsyncStop();
//==================================================================
if (_SW2)
// Switch 2 is ON - Configuring MPU fo Alt. sensitivity
RC = MPUInit(0, 1, MPU_GYRO_1000ds, MPU_ACC_4g);
// Initialize motion Sensor
// 1 kHz/(0+1) = 1000 Hz (1ms)
// DLPF = 3 => Bandwidth 44 Hz (delay: 4.9 msec)
else
// Switch 2 is OFF - Configuring MPU fo normal sensitivity
RC = MPUInit(0, 1, MPU_GYRO_2000ds, MPU_ACC_2g);
// Initialize motion Sensor
// 1 kHz/(0+1) = 1000 Hz (1ms)
// DLPF = 1 => Bandwidth 184 Hz (delay: 2.0 msec)
if (RC) BLIDeadStop("EG", 2);
//*******************************************************************
struct
{
MPUData Sample1;
MPUData Sample2;
} MPU;
//=====================================================
// Initialize Asynchronous mode
//-----------------------------------------------------
if ( (RC = MPUAsyncStart(2)) )
BLIDeadStop("S2", 2);
//------------------------------
if ( (RC = MPUAsyncStart(1)) )
BLIDeadStop("S1", 2);
//=====================================================
// Calibrate Gyros
//-----------------------------------------------------
// BLIAsyncStart(100,100);
// //------------------------------
// if ( (RC = MPUCalibrate(1)) )
// BLIDeadStop("C1", 2);
// //------------------------------
// if ( (RC = MPUCalibrate(2)) )
// BLIDeadStop("C2", 2);
// //------------------------------
// BLIAsyncStop();
//=====================================================
// Main Loop
//-----------------------------------------------------
BLISignalOFF();
while (TRUE)
{
Alarm = TMRSetAlarm(1000);
//------------------------------------
if ( (RC = MPUAsyncReadWhenReady(1, &MPU.Sample1)) )
BLIDeadStop("SOS", 3);
//--------------------------
if ( (RC = MPUAsyncReadWhenReady(2, &MPU.Sample2)) )
BLIDeadStop("SOS", 3);
//------------------------
BLISignalFlip();
//-------------------------
SDLPostIfReady((byte*)&MPU, sizeof(MPU));
//-------------------------
TMRWaitAlarm(Alarm);
}
//*******************************************************************
return 0;
}
int main(void)
{
//*******************************************************************
Init(); // Initialize microprocessor
TMRInit(2); // Initialize Timer interface with Priority=2
BLIInit(); // Initialize Signal interface
//*******************************************************************
// Switch 1 controls the Serial Data Logger (SDL) communication speed
//-------------------------------------------------------------------
if (_SW1)
// Switch 1 is ON - Configuring SDL for PIC-to-PIC
// high-speed communication at 1 MBaud
SDLInit(3, BAUD_1M);
else
// Switch 1 is OFF - Configuring SDL for ZigBEE
// wireless communication at 115.2 KBaud
SDLInit(3, BAUD_115200);
//*******************************************************************
// Initialize I2C Library
//-------------------------------------------------------------------
I2CInit(5, 2); // First param: IL = 5 (interrupt request priority
// Second param: I2C speed
// 0 - lowest (123 kHz at Fcy = 64MHz)
// 1 - 200 kHz
// 2 - 400 kHz
// 3 - 1 MHz
//*******************************************************************
uint RC = 0;
ulong Alarm = 0;
//==================================================================
// Initialize MPUs
//------------------------------------------------------------------
RC = MPUInit(0, 3, MPU_GYRO_2000ds, MPU_ACC_2g);
// Initialize motion Sensor
// 1 kHz/(0+1) = 1000 Hz (1msec)
// DLPF = 3 => Bandwidth 44 Hz (delay: 4.9 msec)
if (RC) BLIDeadStop("EG", 2);
//==================================================================
// Initialize MPL3115 Altimeter
//------------------------------------------------------------------
// OSR = 0 => No averaging ( 2^0= 1), update rate about 166.6 Hz
// OSR = 1 => Average 2^1= 2 samples, update rate about 111.1 Hz
// OSR = 2 => Average 2^2= 4 samples, update rate about 67.8 Hz
// OSR = 3 => Average 2^3= 8 samples, update rate about 37.7 Hz
// OSR = 4 => Average 2^4= 16 samples, update rate about 20.1 Hz
// OSR = 5 => Average 2^5= 32 samples, update rate about 10.4 Hz
// OSR = 6 => Average 2^6= 64 samples, update rate about 5.3 Hz
// OSR = 7 => Average 2^7= 128 samples, update rate about 2.7 Hz
//------------------------------------------------------------------
byte OSR = 3;
//------------------------------------------------------------------
if ( MPL_OK != MPLInit (OSR) )
BLIDeadStop ("EB", 2);
//==================================================================
// Initialize Asynchronous mode
//-----------------------------------------------------
if ( (RC = MPUAsyncStart(1)) )
BLIDeadStop("S1", 2);
//------------------------------
if ( (RC = MPUAsyncStart(2)) )
BLIDeadStop("S2", 2);
//-----------------------------------------------------
if ( (RC = MPLAsyncStart()) )
BLIDeadStop("S3", 2);
//==================================================================
// Provide a few second delay prior to calibrating Gyros to make
// sure that the board is stable after the "turn-on" shake
//------------------------------------------------------------------
BLIAsyncStart(50,50);
TMRDelay(5000);
BLIAsyncStop();
//==================================================================
// Calibrate Gyros
//------------------------------------------------------------------
BLIAsyncStart(100,100);
//------------------------------
if ( (RC = MPUCalibrateGyro(1)) )
BLIDeadStop("C1", 2);
//------------------------------
if ( (RC = MPUCalibrateGyro(2)) )
BLIDeadStop("C2", 2);
//------------------------------
BLIAsyncStop();
//==================================================================
//==================================================================
struct
{
MPUData MPUSample1;
MPUData MPUSample2;
MPLData MPLSample;
} SensorData;
//==================================================================
// Main Loop
//------------------------------------------------------------------
BLISignalOFF();
while (TRUE)
{
Alarm = TMRSetAlarm(1000);
//-----------------------------------------------------
if ( (RC = MPUAsyncReadWhenReady(1, &SensorData.MPUSample1)) )
BLIDeadStop("SOS", 3);
//--------------------------
if ( (RC = MPUAsyncReadWhenReady(2, &SensorData.MPUSample2)) )
BLIDeadStop("SOS", 3);
if (MPL_OK != MPLAsyncReadWhenReady(&SensorData.MPLSample))
BLIDeadStop("SOS", 3);
//-----------------------------------------------------
BLISignalFlip();
//-------------------------
SDLPostIfReady((byte*)&SensorData, sizeof(SensorData));
//-------------------------
TMRWaitAlarm(Alarm);
}
//*******************************************************************
return 0;
}
int main(void)
{
//*******************************************************************
Init();
TMRInit(2); // Initialize Timer interface with Priority=2
BLIInit(); // Initialize Signal interface
//==================================================================
// Provide a 5 second delay prior to initialization of I2C interface
// to avoid false-start during programming by PIC Kit 3
//------------------------------------------------------------------
BLIAsyncStart(50,200);
TMRDelay(5000);
BLIAsyncStop();
//*******************************************************************
// Switch 1 controls the Serial Data Logger (SDL) communication speed
//-------------------------------------------------------------------
if (_SW1)
// Switch 1 is ON - Configuring SDL for PIC-to-PIC
// high-speed communication at 1 MBaud
SDLInit(3, BAUD_1M);
else
// Switch 1 is OFF - Configuring SDL for ZigBEE
// wireless communication at 115.2 KBaud
SDLInit(3, BAUD_115200);
//*******************************************************************
// Initialize I2C Library
//-------------------------------------------------------------------
I2CInit(5, 2); // First param: IL = 5 (interrupt request priority
// Second param: I2C speed
// 0 - lowest (123 kHz at Fcy = 64MHz)
// 1 - 200 kHz
// 2 - 400 kHz
// 3 - 1 MHz
//-------------------------------------------------------------------
uint RC = 0;
ulong Alarm = 0;
//==================================================================
// Initialize MPUs
//------------------------------------------------------------------
RC = MPUInit(0, 3, MPU_GYRO_2000ds, MPU_ACC_2g);
// Initialize motion Sensor
// 1 kHz/(0+1) = 1000 Hz (1msec)
// DLPF = 3 => Bandwidth 44 Hz (delay: 4.9 msec)
if (RC) BLIDeadStop("EG", 2);
//=====================================================
// Initialize Asynchronous mode
//-----------------------------------------------------
if ( (RC = MPUAsyncStart(2)) )
BLIDeadStop("S2", 2);
//------------------------------
if ( (RC = MPUAsyncStart(1)) )
BLIDeadStop("S1", 2);
//==================================================================
// Provide a few second delay prior to calibrating Gyros to make
// sure that the board is stable after the "turn-on" shake
//------------------------------------------------------------------
BLIAsyncStart(50,50);
TMRDelay(1000);
BLIAsyncStop();
//*******************************************************************
// Calibrate Gyros
//-----------------------------------------------------
// MPUSetOptions(Rotate, TempComp, CrossAxis)
MPUSetOptions(TRUE, TRUE, TRUE);
//-----------------------------------------------------
BLIAsyncStart(100,100);
//------------------------------
if ( (RC = MPUCalibrateGyro(1)) )
BLIDeadStop("C1", 2);
//------------------------------
if ( (RC = MPUCalibrateGyro(2)) )
BLIDeadStop("C2", 2);
//------------------------------
BLIAsyncStop();
//=====================================================
struct
{
MPUData Sample1;
MPUData Sample2;
} MPU;
//=====================================================
// Main Loop
//-----------------------------------------------------
BLISignalOFF();
while (TRUE)
{
Alarm = TMRSetAlarm(20);
//------------------------------------
if ( (RC = MPUAsyncReadWhenReady(1, &MPU.Sample1)) )
BLIDeadStop("SM1", 3);
//--------------------------
if ( (RC = MPUAsyncReadWhenReady(2, &MPU.Sample2)) )
BLIDeadStop("SM2", 3);
//------------------------
BLISignalFlip();
//-------------------------
SDLPostIfReady((byte*)&MPU, sizeof(MPU));
//-------------------------
TMRWaitAlarm(Alarm);
}
//*******************************************************************
return 0;
}
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;
}
//************************************************************
uint IMUReset()
{
_IMUReady = 0;
//*******************************************************
BLIAsyncMorse("W", 1);
//*******************************************************
MPUData MPUReading;
DCMData IMUResult;
//-------------------------------------------------------
HMCSample HMCReading;
//*******************************************************
// Start HMC module so that it may accumulate multiple
// samples for averaging while DCM stabilizes
//-------------------------------------------------------
if (HMCAsyncStart())
BLIDeadStop("M", 1);
// Clear accumulated sensor readings
if (HMCAsyncReadWhenReady(&HMCReading))
BLIDeadStop("M", 1);
//*******************************************************
// Reset DCM algorithm
//-------------------------------------------------------
DCMReset();
//*******************************************************
uint IsReady = 0;
ulong StCount = 0;
ulong Alarm = 0;
//-------------------------------------------------------
// Start MPU6050 and calibrate Gyro offset
//-------------------------------------------------------
if (MPUAsyncStart())
// Async start failed...
BLIDeadStop("A", 1);
if (MPUCalibrate () != MPU_OK)
// Gyro Calibration failed
BLIDeadStop ("CA", 2);
//*******************************************************
// Read first sample orientation vector
//-------------------------------------------------------
if (MPUAsyncReadWhenReady(&MPUReading))
BLIDeadStop("A", 1);
//-------------------------------------------------------
// Update DCM until it is synchronized with current
// orientation vector
//-------------------------------------------------------
// Set Alarm for 10 msec so we read an average of
// about 10 MPU6050 samples for each DCM step
Alarm = TMRSetAlarm(10);
//-------------------------------------------------------
while (0 == IsReady)
{
TMRWaitAlarm(Alarm);
//--------------------------------------
// Read average of MPU6050 samples
//--------------------------------------
if (MPUAsyncReadWhenReady(&MPUReading))
BLIDeadStop("A", 1);
//--------------------------------------
Alarm = TMRSetAlarm(10); // Set Alarm for next measurement
//--------------------------------------
IsReady = DCMPerformStep( MPUReading.TS,
&MPUReading.G,
&MPUReading.A,
&IMUResult);
//----------------------------
StCount++;
}
//*******************************************************
// Attitude calculation stabilized; now we may adjust
// Acc Zx base...
//*******************************************************
if (MPU_OK != MPUAsyncAdjustAccZBase(IMUResult.Incl))
BLIDeadStop("A", 1);
//*******************************************************
// Now that we are done with the attitude calculation we
// may finalize calculation of true Azimuth from the
// magnetometer.
//*******************************************************
if (HMC_OK == HMCAsyncReadWhenReady(&HMCReading))
DCMSetAzimuth(&HMCReading.M);
//----------------------------------------------
// Stop magnetometer as it is not used in flight
//----------------------------------------------
HMCAsyncStop();
//*******************************************************
_IMUReady = 1;
//*******************************************************
BLIAsyncStop();
//*******************************************************
return StCount;
}
