double HMC5883L::getHeading()
{
float magData[3];
readMagData(magData);
magData[0] -= 6.8f; // calibration
/* Calculate the heading while Z axis of the module is pointing up */
double heading = atan2(magData[1], magData[0]);
// After calculating heading declination angle should be added to heading which is the error of the magnetic field in specific location.
// declinationAngle can be found here http://www.magnetic-declination.com/
// For Ankara (my location) declinationAngle is ~5.5 degrees (0.096 radians)
float declinationAngle = 0.096;
heading += declinationAngle;
// Correct for when signs are reversed.
if(heading < 0)
heading += 2*PI;
// Check for wrap due to addition of declination.
if(heading > 2*PI)
heading -= 2*PI;
/* Convert radian to degrees */
heading = heading * 180 / PI;
return heading;
}
// run a 9-state EKF used to calculate orientation
void SmallEKF::RunEKF(float delta_time, const Vector3f &delta_angles, const Vector3f &delta_velocity, const Vector3f &joint_angles)
{
imuSampleTime_ms = hal.scheduler->millis();
dtIMU = delta_time;
// initialise variables and constants
if (!FiltInit) {
StartTime_ms = imuSampleTime_ms;
newDataMag = false;
YawAligned = false;
state.quat[0] = 1.0f;
const float Sigma_velNED = 0.5f; // 1 sigma uncertainty in horizontal velocity components
const float Sigma_dAngBias = 0.01745f*dtIMU; // 1 Sigma uncertainty in delta angle bias
const float Sigma_angErr = 1.0f; // 1 Sigma uncertainty in angular misalignment (rad)
for (uint8_t i=0; i <= 2; i++) Cov[i][i] = sq(Sigma_angErr);
for (uint8_t i=3; i <= 5; i++) Cov[i][i] = sq(Sigma_velNED);
for (uint8_t i=6; i <= 8; i++) Cov[i][i] = sq(Sigma_dAngBias);
FiltInit = true;
hal.console->printf("SmallEKF Alignment Started\n");
}
// We are using the IMU data from the flight vehicle and setting joint angles to zero for the time being
gSense.gPsi = joint_angles.z; // yaw
gSense.gPhi = joint_angles.x; // roll
gSense.gTheta = joint_angles.y; // pitch
cosPhi = cosf(gSense.gPhi);
cosTheta = cosf(gSense.gTheta);
sinPhi = sinf(gSense.gPhi);
sinTheta = sinf(gSense.gTheta);
sinPsi = sinf(gSense.gPsi);
cosPsi = cosf(gSense.gPsi);
gSense.delAng = delta_angles;
gSense.delVel = delta_velocity;
// predict states
predictStates();
// predict the covariance
predictCovariance();
// fuse SmallEKF velocity data
fuseVelocity(YawAligned);
// Align the heading once there has been enough time for the filter to settle and the tilt corrections have dropped below a threshold
if ((((imuSampleTime_ms - StartTime_ms) > 5000 && TiltCorrection < 1e-4f) || ((imuSampleTime_ms - StartTime_ms) > 30000)) && !YawAligned) {
//calculate the initial heading using magnetometer, estimated tilt and declination
alignHeading();
YawAligned = true;
hal.console->printf("SmallEKF Alignment Completed\n");
}
// Fuse magnetometer data if we have new measurements and an aligned heading
readMagData();
if (newDataMag && YawAligned) {
fuseCompass();
newDataMag = false;
}
}
/*********************************************************************
* @fn SensorTag_ProcessEvent
*
* @brief Simple BLE Peripheral Application Task event processor. This function
* is called to process all events for the task. Events
* include timers, messages and any other user defined events.
*
* @param task_id - The OSAL assigned task ID.
* @param events - events to process. This is a bit map and can
* contain more than one event.
*
* @return events not processed
*/
uint16 SensorTag_ProcessEvent( uint8 task_id, uint16 events )
{
VOID task_id; // OSAL required parameter that isn't used in this function
if ( events & SYS_EVENT_MSG )
{
uint8 *pMsg;
if ( (pMsg = osal_msg_receive( sensorTag_TaskID )) != NULL )
{
sensorTag_ProcessOSALMsg( (osal_event_hdr_t *)pMsg );
// Release the OSAL message
VOID osal_msg_deallocate( pMsg );
}
// return unprocessed events
return (events ^ SYS_EVENT_MSG);
}
// Handle system reset (long press on side key)
if ( events & ST_SYS_RESET_EVT )
{
if (sysResetRequest)
{
HAL_SYSTEM_RESET();
}
return ( events ^ ST_SYS_RESET_EVT );
}
if ( events & ST_START_DEVICE_EVT )
{
// Start the Device
VOID GAPRole_StartDevice( &sensorTag_PeripheralCBs );
// Start Bond Manager
VOID GAPBondMgr_Register( &sensorTag_BondMgrCBs );
return ( events ^ ST_START_DEVICE_EVT );
}
//////////////////////////
// IR TEMPERATURE //
//////////////////////////
if ( events & ST_IRTEMPERATURE_READ_EVT )
{
if ( irTempEnabled )
{
if (HalIRTempStatus() == TMP006_DATA_READY)
{
readIrTempData();
osal_start_timerEx( sensorTag_TaskID, ST_IRTEMPERATURE_READ_EVT, sensorTmpPeriod-TEMP_MEAS_DELAY );
}
else if (HalIRTempStatus() == TMP006_OFF)
{
HalIRTempTurnOn();
osal_start_timerEx( sensorTag_TaskID, ST_IRTEMPERATURE_READ_EVT, TEMP_MEAS_DELAY );
}
}
else
{
//Turn off Temperatur sensor
VOID HalIRTempTurnOff();
VOID resetCharacteristicValue(IRTEMPERATURE_SERV_UUID,SENSOR_DATA,0,IRTEMPERATURE_DATA_LEN);
VOID resetCharacteristicValue(IRTEMPERATURE_SERV_UUID,SENSOR_CONF,ST_CFG_SENSOR_DISABLE,sizeof ( uint8 ));
}
return (events ^ ST_IRTEMPERATURE_READ_EVT);
}
//////////////////////////
// Accelerometer //
//////////////////////////
if ( events & ST_ACCELEROMETER_SENSOR_EVT )
{
if(accConfig != ST_CFG_SENSOR_DISABLE)
{
readAccData();
osal_start_timerEx( sensorTag_TaskID, ST_ACCELEROMETER_SENSOR_EVT, sensorAccPeriod );
}
else
{
VOID resetCharacteristicValue( ACCELEROMETER_SERV_UUID, SENSOR_DATA, 0, ACCELEROMETER_DATA_LEN );
VOID resetCharacteristicValue( ACCELEROMETER_SERV_UUID, SENSOR_CONF, ST_CFG_SENSOR_DISABLE, sizeof ( uint8 ));
}
return (events ^ ST_ACCELEROMETER_SENSOR_EVT);
}
//////////////////////////
// Humidity //
//////////////////////////
if ( events & ST_HUMIDITY_SENSOR_EVT )
{
if (humiEnabled)
{
HalHumiExecMeasurementStep(humiState);
if (humiState == 2)
{
readHumData();
humiState = 0;
osal_start_timerEx( sensorTag_TaskID, ST_HUMIDITY_SENSOR_EVT, sensorHumPeriod );
}
else
{
humiState++;
osal_start_timerEx( sensorTag_TaskID, ST_HUMIDITY_SENSOR_EVT, HUM_FSM_PERIOD );
}
}
else
{
resetCharacteristicValue( HUMIDITY_SERV_UUID, SENSOR_DATA, 0, HUMIDITY_DATA_LEN);
resetCharacteristicValue( HUMIDITY_SERV_UUID, SENSOR_CONF, ST_CFG_SENSOR_DISABLE, sizeof ( uint8 ));
}
return (events ^ ST_HUMIDITY_SENSOR_EVT);
}
//////////////////////////
// Magnetometer //
//////////////////////////
if ( events & ST_MAGNETOMETER_SENSOR_EVT )
{
if(magEnabled)
{
if (HalMagStatus() == MAG3110_DATA_READY)
{
readMagData();
}
else if (HalMagStatus() == MAG3110_OFF)
{
HalMagTurnOn();
}
osal_start_timerEx( sensorTag_TaskID, ST_MAGNETOMETER_SENSOR_EVT, sensorMagPeriod );
}
else
{
HalMagTurnOff();
resetCharacteristicValue( MAGNETOMETER_SERV_UUID, SENSOR_DATA, 0, MAGNETOMETER_DATA_LEN);
resetCharacteristicValue( MAGNETOMETER_SERV_UUID, SENSOR_CONF, ST_CFG_SENSOR_DISABLE, sizeof ( uint8 ));
}
return (events ^ ST_MAGNETOMETER_SENSOR_EVT);
}
//////////////////////////
// Barometer //
//////////////////////////
if ( events & ST_BAROMETER_SENSOR_EVT )
{
if (barEnabled)
{
if (barBusy)
{
barBusy = FALSE;
readBarData();
osal_start_timerEx( sensorTag_TaskID, ST_BAROMETER_SENSOR_EVT, sensorBarPeriod );
}
else
{
barBusy = TRUE;
HalBarStartMeasurement();
osal_start_timerEx( sensorTag_TaskID, ST_BAROMETER_SENSOR_EVT, BAR_FSM_PERIOD );
}
}
else
{
resetCharacteristicValue( BAROMETER_SERV_UUID, SENSOR_DATA, 0, BAROMETER_DATA_LEN);
resetCharacteristicValue( BAROMETER_SERV_UUID, SENSOR_CONF, ST_CFG_SENSOR_DISABLE, sizeof ( uint8 ));
resetCharacteristicValue( BAROMETER_SERV_UUID, SENSOR_CALB, 0, BAROMETER_CALI_LEN);
}
return (events ^ ST_BAROMETER_SENSOR_EVT);
}
//////////////////////////
// Gyroscope //
//////////////////////////
if ( events & ST_GYROSCOPE_SENSOR_EVT )
{
uint8 status;
status = HalGyroStatus();
if(gyroEnabled)
{
if (status == HAL_GYRO_STOPPED)
{
HalGyroSelectAxes(sensorGyroAxes);
HalGyroTurnOn();
osal_start_timerEx( sensorTag_TaskID, ST_GYROSCOPE_SENSOR_EVT, GYRO_STARTUP_TIME);
}
else
{
if(sensorGyroUpdateAxes)
{
HalGyroSelectAxes(sensorGyroAxes);
sensorGyroUpdateAxes = FALSE;
}
if (status == HAL_GYRO_DATA_READY)
{
readGyroData();
osal_start_timerEx( sensorTag_TaskID, ST_GYROSCOPE_SENSOR_EVT, sensorGyrPeriod - GYRO_STARTUP_TIME);
}
else
{
// Gyro needs to be activated;
HalGyroWakeUp();
osal_start_timerEx( sensorTag_TaskID, ST_GYROSCOPE_SENSOR_EVT, GYRO_STARTUP_TIME);
}
}
}
else
{
HalGyroTurnOff();
resetCharacteristicValue( GYROSCOPE_SERV_UUID, SENSOR_DATA, 0, GYROSCOPE_DATA_LEN);
resetCharacteristicValue( GYROSCOPE_SERV_UUID, SENSOR_CONF, ST_CFG_SENSOR_DISABLE, sizeof( uint8 ));
}
return (events ^ ST_GYROSCOPE_SENSOR_EVT);
}
#if defined ( PLUS_BROADCASTER )
if ( events & ST_ADV_IN_CONNECTION_EVT )
{
uint8 turnOnAdv = TRUE;
// Turn on advertising while in a connection
GAPRole_SetParameter( GAPROLE_ADVERT_ENABLED, sizeof( uint8 ), &turnOnAdv );
return (events ^ ST_ADV_IN_CONNECTION_EVT);
}
#endif // PLUS_BROADCASTER
// Discard unknown events
return 0;
}
int main()
{
// open data files
pImuFile = fopen ("IMU.txt","r");
pMagFile = fopen ("MAG.txt","r");
pGpsFile = fopen ("GPS.txt","r");
pAhrsFile = fopen ("ATT.txt","r");
pAdsFile = fopen ("NTUN.txt","r");
pTimeFile = fopen ("timing.txt","r");
pStateOutFile = fopen ("StateDataOut.txt","w");
pEulOutFile = fopen ("EulDataOut.txt","w");
pCovOutFile = fopen ("CovDataOut.txt","w");
pRefPosVelOutFile = fopen ("RefVelPosDataOut.txt","w");
pVelPosFuseFile = fopen ("VelPosFuse.txt","w");
pMagFuseFile = fopen ("MagFuse.txt","w");
pTasFuseFile = fopen ("TasFuse.txt","w");
pGpsRawINFile = fopen ("GPSraw.txt","r");
pGpsRawOUTFile = fopen ("GPSrawOut.txt","w");
memset(gpsRaw, 0, sizeof(gpsRaw));
// read test data from files for first timestamp
readIMUData();
readGpsData();
readMagData();
readAirSpdData();
readAhrsData();
readTimingData();
while (!endOfData)
{
if ((IMUmsec >= msecStartTime) && (IMUmsec <= msecEndTime))
{
// Initialise states, covariance and other data
if ((IMUmsec > msecAlignTime) && !statesInitialised && (GPSstatus == 3))
{
InitialiseFilter();
}
// If valid IMU data and states initialised, predict states and covariances
if (statesInitialised)
{
// Run the strapdown INS equations every IMU update
UpdateStrapdownEquationsNED();
// debug code - could be tunred into a filter mnitoring/watchdog function
float tempQuat[4];
for (uint8_t j=0; j<=3; j++) tempQuat[j] = states[j];
quat2eul(eulerEst, tempQuat);
for (uint8_t j=0; j<=2; j++) eulerDif[j] = eulerEst[j] - ahrsEul[j];
if (eulerDif[2] > pi) eulerDif[2] -= 2*pi;
if (eulerDif[2] < -pi) eulerDif[2] += 2*pi;
// store the predicted states for subsequent use by measurement fusion
StoreStates();
// Check if on ground - status is used by covariance prediction
OnGroundCheck();
// sum delta angles and time used by covariance prediction
summedDelAng = summedDelAng + correctedDelAng;
summedDelVel = summedDelVel + correctedDelVel;
dt += dtIMU;
// perform a covariance prediction if the total delta angle has exceeded the limit
// or the time limit will be exceeded at the next IMU update
if ((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax))
{
CovariancePrediction();
summedDelAng = summedDelAng.zero();
summedDelVel = summedDelVel.zero();
dt = 0.0f;
}
}
// Fuse GPS Measurements
if (newDataGps && statesInitialised)
{
// Convert GPS measurements to Pos NE, hgt and Vel NED
calcvelNED(velNED, gpsCourse, gpsGndSpd, gpsVelD);
calcposNED(posNED, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef);
posNE[0] = posNED[0];
posNE[1] = posNED[1];
hgtMea = -posNED[2];
// set fusion flags
fuseVelData = true;
fusePosData = true;
fuseHgtData = true;
// recall states stored at time of measurement after adjusting for delays
RecallStates(statesAtVelTime, (IMUmsec - msecVelDelay));
RecallStates(statesAtPosTime, (IMUmsec - msecPosDelay));
RecallStates(statesAtHgtTime, (IMUmsec - msecHgtDelay));
// run the fusion step
FuseVelposNED();
}
else
{
fuseVelData = false;
fusePosData = false;
fuseHgtData = false;
}
// Fuse Magnetometer Measurements
if (newDataMag && statesInitialised)
{
fuseMagData = true;
RecallStates(statesAtMagMeasTime, (IMUmsec - msecMagDelay)); // Assume 50 msec avg delay for magnetometer data
}
else
{
fuseMagData = false;
}
if (statesInitialised) FuseMagnetometer();
// Fuse Airspeed Measurements
if (newAdsData && statesInitialised && VtasMeas > 8.0f)
{
fuseVtasData = true;
RecallStates(statesAtVtasMeasTime, (IMUmsec - msecTasDelay)); // assume 100 msec avg delay for airspeed data
FuseAirspeed();
}
else
{
fuseVtasData = false;
}
// debug output
//printf("Euler Angle Difference = %3.1f , %3.1f , %3.1f deg\n", rad2deg*eulerDif[0],rad2deg*eulerDif[1],rad2deg*eulerDif[2]);
WriteFilterOutput();
}
// read test data from files for next timestamp
readIMUData();
readGpsData();
readMagData();
readAirSpdData();
readAhrsData();
if (IMUmsec > msecEndTime)
{
CloseFiles();
endOfData = true;
}
}
}
void updateYPR(void) {
int16_t accelCount[3]; // Stores the 16-bit signed accelerometer sensor output
int16_t gyroCount[3]; // Stores the 16-bit signed gyro sensor output
int16_t magCount[3]; // Stores the 16-bit signed magnetometer sensor output
int16_t tempCount; // Stores the real internal chip temperature in degrees Celsius
float temperature;
// If intPin goes high, all data registers have new data
if (MPU9250_I2C_ByteRead(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt
readAccelData(accelCount); // Read the x/y/z adc values
// Now we'll calculate the accleration value into actual g's
ax = (float) accelCount[0] * aRes - accelBias[0]; // get actual g value, this depends on scale being set
ay = (float) accelCount[1] * aRes - accelBias[1];
az = (float) accelCount[2] * aRes - accelBias[2];
readGyroData(gyroCount); // Read the x/y/z adc values
// Calculate the gyro value into actual degrees per second
gx = (float) gyroCount[0] * gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set
gy = (float) gyroCount[1] * gRes - gyroBias[1];
gz = (float) gyroCount[2] * gRes - gyroBias[2];
readMagData(magCount); // Read the x/y/z adc values
// Calculate the magnetometer values in milliGauss
// Include factory calibration per data sheet and user environmental corrections
mx = (float) magCount[0] * mRes * magCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set
my = (float) magCount[1] * mRes * magCalibration[1] - magbias[1];
mz = (float) magCount[2] * mRes * magCalibration[2] - magbias[2];
Now = timer_get_us();
deltat = (float) ((Now - lastUpdate) / 1000000.0f); // set integration time by time elapsed since last filter update
lastUpdate = Now;
sum += deltat;
sumCount++;
//MadgwickQuaternionUpdate(ax, ay, az, gx*_PI/180.0f, gy*_PI/180.0f, gz*_PI/180.0f, my, mx, mz);
MahonyQuaternionUpdate(ax, ay, az, gx * _PI / 180.0f, gy * _PI / 180.0f, gz * _PI / 180.0f, my, mx, mz);
// Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
// In this coordinate system, the positive z-axis is down toward Earth.
// Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
// Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
// Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
// These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
// Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
// applied in the correct order which for this configuration is yaw, pitch, and then roll.
// For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
ypr[0] = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]),
q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
// ypr[0] = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]),
// q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
// ypr[0] = atan2(2.0f * (q[1] * q[2] - q[0] * q[3]),
// q[0] * q[0] + q[1] * q[1] - 1);
ypr[1] = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
ypr[2] = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]),
q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
ypr[1] *= 180.0f / _PI;
ypr[0] *= 180.0f / _PI;
ypr[0] -= -5.08f; // Declination at Hefei, Anhui is 5 degrees and 5 minutes (negative) on 2015-08-08
ypr[2] *= 180.0f / _PI;
}
// Serial print and/or display at 0.5 s rate independent of data rates
delt_t = systemTime - count;
if (delt_t > 1500) { // update LCD once per half-second independent of read rate
trace_printf("ax = %f", 1000 * ax);
trace_printf(" ay = %f", 1000 * ay);
trace_printf(" az = %f mg\n", 1000 * az);
trace_printf("gx = %f", gx);
trace_printf(" gy = %f", gy);
trace_printf(" gz = %f deg/s\n", gz);
trace_printf("gx = %f", mx);
trace_printf(" gy = %f", my);
trace_printf(" gz = %f mG\n", mz);
tempCount = readTempData(); // Read the adc values
temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
trace_printf("temperature = %f C\n", temperature);
trace_printf("q0 = %f\n", q[0]);
trace_printf("q1 = %f\n", q[1]);
trace_printf("q2 = %f\n", q[2]);
trace_printf("q3 = %f\n", q[3]);
trace_printf("Yaw, Pitch, Roll: %f %f %f\n", ypr[0], ypr[1], ypr[2]);
trace_printf("average rate = %f\n\n\n\r", (float) sumCount / sum);
count = systemTime;
if (count > 1 << 21) {
systemTime = 0; // start the timer over again if ~30 minutes has passed
count = 0;
deltat = 0;
lastUpdate = timer_get_us();
}
sum = 0;
sumCount = 0;
}
}
// run a 9-state EKF used to calculate orientation
void SoloGimbalEKF::RunEKF(float delta_time, const Vector3f &delta_angles, const Vector3f &delta_velocity, const Vector3f &joint_angles)
{
imuSampleTime_ms = AP_HAL::millis();
dtIMU = delta_time;
// initialise variables and constants
if (!FiltInit) {
// Note: the start time is initialised to 0 in the constructor
if (StartTime_ms == 0) {
StartTime_ms = imuSampleTime_ms;
}
// Set data to pre-initialsation defaults
FiltInit = false;
newDataMag = false;
YawAligned = false;
memset(&state, 0, sizeof(state));
state.quat[0] = 1.0f;
bool main_ekf_healthy = false;
nav_filter_status main_ekf_status;
if (_ahrs.get_filter_status(main_ekf_status)) {
if (main_ekf_status.flags.attitude) {
main_ekf_healthy = true;
}
}
// Wait for gimbal to stabilise to body fixed position for a few seconds before starting small EKF
// Also wait for navigation EKF to be healthy beasue we are using the velocity output data
// This prevents jerky gimbal motion from degrading the EKF initial state estimates
if (imuSampleTime_ms - StartTime_ms < 5000 || !main_ekf_healthy) {
return;
}
Quaternion ned_to_vehicle_quat;
ned_to_vehicle_quat.from_rotation_matrix(_ahrs.get_rotation_body_to_ned());
Quaternion vehicle_to_gimbal_quat;
vehicle_to_gimbal_quat.from_vector312(joint_angles.x,joint_angles.y,joint_angles.z);
// calculate initial orientation
state.quat = ned_to_vehicle_quat * vehicle_to_gimbal_quat;
const float Sigma_velNED = 0.5f; // 1 sigma uncertainty in horizontal velocity components
const float Sigma_dAngBias = 0.002f*dtIMU; // 1 Sigma uncertainty in delta angle bias (rad)
const float Sigma_angErr = 0.1f; // 1 Sigma uncertainty in angular misalignment (rad)
for (uint8_t i=0; i <= 2; i++) Cov[i][i] = sq(Sigma_angErr);
for (uint8_t i=3; i <= 5; i++) Cov[i][i] = sq(Sigma_velNED);
for (uint8_t i=6; i <= 8; i++) Cov[i][i] = sq(Sigma_dAngBias);
FiltInit = true;
hal.console->printf("\nSoloGimbalEKF Alignment Started\n");
// Don't run the filter in this timestep becasue we have already used the delta velocity data to set an initial orientation
return;
}
// We are using IMU data and joint angles from the gimbal
gSense.gPsi = joint_angles.z; // yaw
gSense.gPhi = joint_angles.x; // roll
gSense.gTheta = joint_angles.y; // pitch
cosPhi = cosf(gSense.gPhi);
cosTheta = cosf(gSense.gTheta);
sinPhi = sinf(gSense.gPhi);
sinTheta = sinf(gSense.gTheta);
sinPsi = sinf(gSense.gPsi);
cosPsi = cosf(gSense.gPsi);
gSense.delAng = delta_angles;
gSense.delVel = delta_velocity;
// predict states
predictStates();
// predict the covariance
predictCovariance();
// fuse SoloGimbalEKF velocity data
fuseVelocity();
// Align the heading once there has been enough time for the filter to settle and the tilt corrections have dropped below a threshold
// Force it to align if too much time has lapsed
if (((((imuSampleTime_ms - StartTime_ms) > 8000 && TiltCorrection < 1e-4f) || (imuSampleTime_ms - StartTime_ms) > 30000)) && !YawAligned) {
//calculate the initial heading using magnetometer, estimated tilt and declination
alignHeading();
YawAligned = true;
hal.console->printf("\nSoloGimbalEKF Alignment Completed\n");
}
// Fuse magnetometer data if we have new measurements and an aligned heading
readMagData();
if (newDataMag && YawAligned) {
fuseCompass();
newDataMag = false;
}
}
