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; } }