From de58fb4637873af7e949bb4d034cef226efa18a4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 19 Oct 2018 18:26:01 +1100 Subject: [PATCH] AP_NavEKF3: support in-flight compass learning --- .../AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 12 +++++ libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 54 +++++++++++-------- libraries/AP_NavEKF3/AP_NavEKF3_core.h | 4 ++ 3 files changed, 47 insertions(+), 23 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 8504d2eb6b..e98583efdb 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -247,6 +247,18 @@ void NavEKF3_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(); + } + // limit compass update rate 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) > 1000 * frontend->sensorIntervalMin_ms)) { frontend->logging.log_compass = true; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 1be2ecf208..919cf7fe7b 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -159,10 +159,8 @@ void NavEKF3_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; @@ -182,7 +180,6 @@ void NavEKF3_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; @@ -193,13 +190,9 @@ void NavEKF3_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 = ins.get_loop_delta_t(); dtEkfAvg = EKF_TARGET_DT; dt = 0; @@ -225,15 +218,12 @@ void NavEKF3_core::InitialiseVariables() velTimeout = true; memset(&faultStatus, 0, sizeof(faultStatus)); hgtRate = 0.0f; - mag_state.q0 = 1; - mag_state.DCM.identity(); onGround = true; prevOnGround = true; inFlight = false; prevInFlight = false; manoeuvring = false; inhibitWindStates = true; - inhibitMagStates = true; inhibitDelVelBiasStates = true; inhibitDelAngBiasStates = true; gndOffsetValid = false; @@ -253,7 +243,6 @@ void NavEKF3_core::InitialiseVariables() stateIndexLim = 23; baroStoreIndex = 0; rangeStoreIndex = 0; - magStoreIndex = 0; last_gps_idx = 0; tasStoreIndex = 0; ofStoreIndex = 0; @@ -291,24 +280,13 @@ void NavEKF3_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; @@ -380,7 +358,6 @@ void NavEKF3_core::InitialiseVariables() // zero data buffers storedIMU.reset(); storedGPS.reset(); - storedMag.reset(); storedBaro.reset(); storedTAS.reset(); storedRange.reset(); @@ -388,6 +365,37 @@ void NavEKF3_core::InitialiseVariables() storedRangeBeacon.reset(); storedBodyOdm.reset(); storedWheelOdm.reset(); + + InitialiseVariablesMag(); +} + + +// Use a function call rather than a constructor to initialise variables because it enables the filter to be re-started in flight if necessary. +void NavEKF3_core::InitialiseVariablesMag() +{ + lastHealthyMagTime_ms = imuSampleTime_ms; + lastMagUpdate_us = 0; + magYawResetTimer_ms = imuSampleTime_ms; + magTimeout = false; + allMagSensorsFailed = false; + badMagYaw = false; + finalInflightMagInit = false; + mag_state.q0 = 1; + mag_state.DCM.identity(); + inhibitMagStates = true; + magStoreIndex = 0; + 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; + quatAtLastMagReset = stateStruct.quat; + magFieldLearned = false; + storedMag.reset(); } // Initialise the states from accelerometer and magnetometer data (if present) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 79c43d5759..56c04d745f 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -658,6 +658,9 @@ private: // zero stored variables void InitialiseVariables(); + // zero stored variables related to mag + void InitialiseVariablesMag(); + // reset the horizontal position states uing the last GPS measurement void ResetPosition(void); @@ -953,6 +956,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