From 805647df85902d20ff6292cf2c1b783a0e9a6136 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 19 Oct 2018 16:06:55 +1100 Subject: [PATCH] AP_NavEKF2: enable use of in-flight compass learning --- .../AP_NavEKF2/AP_NavEKF2_Measurements.cpp | 13 ++++ libraries/AP_NavEKF2/AP_NavEKF2_core.cpp | 59 ++++++++++++------- libraries/AP_NavEKF2/AP_NavEKF2_core.h | 3 + 3 files changed, 54 insertions(+), 21 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index 31d49e8193..d30942760a 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -200,6 +200,19 @@ void NavEKF2_core::readMagData() return; } + if (_ahrs->get_compass()->learn_offsets_enabled()) { + // while learning offsets keep all mag states reset + InitialiseVariablesMag(); + wasLearningCompass_ms = imuSampleTime_ms; + } else if (wasLearningCompass_ms != 0 && imuSampleTime_ms - wasLearningCompass_ms > 1000) { + wasLearningCompass_ms = 0; + // force a new yaw alignment 1s after learning completes. The + // delay is to ensure any buffered mag samples are discarded + yawAlignComplete = false; + InitialiseVariablesMag(); + } + + // do not accept new compass data faster than 14Hz (nominal rate is 10Hz) to prevent high processor loading // because magnetometer fusion is an expensive step and we could overflow the FIFO buffer if (use_compass() && _ahrs->get_compass()->last_update_usec() - lastMagUpdate_us > 70000) { diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index 5f417b0e2d..1fd2016384 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp @@ -113,10 +113,8 @@ void NavEKF2_core::InitialiseVariables() // initialise time stamps imuSampleTime_ms = frontend->imuSampleTime_us / 1000; - lastHealthyMagTime_ms = imuSampleTime_ms; prevTasStep_ms = imuSampleTime_ms; prevBetaStep_ms = imuSampleTime_ms; - lastMagUpdate_us = 0; lastBaroReceived_ms = imuSampleTime_ms; lastVelPassTime_ms = 0; lastPosPassTime_ms = 0; @@ -136,7 +134,6 @@ void NavEKF2_core::InitialiseVariables() lastGpsVelFail_ms = 0; lastGpsAidBadTime_ms = 0; timeTasReceived_ms = 0; - magYawResetTimer_ms = imuSampleTime_ms; lastPreAlignGpsCheckTime_ms = imuSampleTime_ms; lastPosReset_ms = 0; lastVelReset_ms = 0; @@ -147,13 +144,8 @@ void NavEKF2_core::InitialiseVariables() // initialise other variables gpsNoiseScaler = 1.0f; hgtTimeout = true; - magTimeout = false; - allMagSensorsFailed = false; tasTimeout = true; - badMagYaw = false; badIMUdata = false; - finalInflightYawInit = false; - finalInflightMagInit = false; dtIMUavg = 0.0025f; dtEkfAvg = EKF_TARGET_DT; dt = 0; @@ -188,7 +180,6 @@ void NavEKF2_core::InitialiseVariables() prevInFlight = false; manoeuvring = false; inhibitWindStates = true; - inhibitMagStates = true; gndOffsetValid = false; validOrigin = false; takeoffExpectedSet_ms = 0; @@ -203,7 +194,6 @@ void NavEKF2_core::InitialiseVariables() lastYawReset_ms = 0; tiltErrFilt = 1.0f; tiltAlignComplete = false; - yawAlignComplete = false; stateIndexLim = 23; baroStoreIndex = 0; rangeStoreIndex = 0; @@ -244,24 +234,15 @@ void NavEKF2_core::InitialiseVariables() velResetNE.zero(); posResetD = 0.0f; hgtInnovFiltState = 0.0f; - if (_ahrs->get_compass()) { - magSelectIndex = _ahrs->get_compass()->get_primary(); - } + imuDataDownSampledNew.delAng.zero(); imuDataDownSampledNew.delVel.zero(); imuDataDownSampledNew.delAngDT = 0.0f; imuDataDownSampledNew.delVelDT = 0.0f; runUpdates = false; framesSincePredict = 0; - lastMagOffsetsValid = false; - magStateResetRequest = false; - magStateInitComplete = false; - magYawResetRequest = false; gpsYawResetRequest = false; - posDownAtLastMagReset = stateStruct.position.z; - yawInnovAtLastMagReset = 0.0f; quatAtLastMagReset = stateStruct.quat; - magFieldLearned = false; delAngBiasLearned = false; memset(&filterStatus, 0, sizeof(filterStatus)); gpsInhibit = false; @@ -328,13 +309,49 @@ void NavEKF2_core::InitialiseVariables() // zero data buffers storedIMU.reset(); storedGPS.reset(); - storedMag.reset(); storedBaro.reset(); storedTAS.reset(); storedRange.reset(); storedOutput.reset(); storedRangeBeacon.reset(); storedExtNav.reset(); + + // now init mag variables + yawAlignComplete = false; + + InitialiseVariablesMag(); +} + + +/* + separate out the mag reset so it can be used when compass learning completes + */ +void NavEKF2_core::InitialiseVariablesMag() +{ + lastHealthyMagTime_ms = imuSampleTime_ms; + lastMagUpdate_us = 0; + magYawResetTimer_ms = imuSampleTime_ms; + magTimeout = false; + allMagSensorsFailed = false; + badMagYaw = false; + finalInflightYawInit = false; + finalInflightMagInit = false; + + inhibitMagStates = true; + + if (_ahrs->get_compass()) { + magSelectIndex = _ahrs->get_compass()->get_primary(); + } + lastMagOffsetsValid = false; + magStateResetRequest = false; + magStateInitComplete = false; + magYawResetRequest = false; + + posDownAtLastMagReset = stateStruct.position.z; + yawInnovAtLastMagReset = 0.0f; + magFieldLearned = false; + + storedMag.reset(); } // Initialise the states from accelerometer and magnetometer data (if present) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index ffaa119ef1..af208fddbd 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -624,6 +624,8 @@ private: // zero stored variables void InitialiseVariables(); + void InitialiseVariablesMag(); + // reset the horizontal position states uing the last GPS measurement void ResetPosition(void); @@ -918,6 +920,7 @@ private: Vector3f delAngCorrected; // corrected IMU delta angle vector at the EKF time horizon (rad) Vector3f delVelCorrected; // corrected IMU delta velocity vector at the EKF time horizon (m/s) bool magFieldLearned; // true when the magnetic field has been learned + uint32_t wasLearningCompass_ms; // time when we were last waiting for compass learn to complete Vector3f earthMagFieldVar; // NED earth mag field variances for last learned field (mGauss^2) Vector3f bodyMagFieldVar; // XYZ body mag field variances for last learned field (mGauss^2) bool delAngBiasLearned; // true when the gyro bias has been learned