From 1f302f2aae73dc2fffc19cd94ba52ba45ac0114b Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 2 Apr 2026 17:20:22 +0100 Subject: [PATCH] AP_NavEKF3: query low drift from DAL at runtime Replace compile-time #if guards with runtime queries to dal.ins().is_low_noise() for gyro bias limit and initial uncertainty. Each EKF core now uses its own IMU's low noise flag, enabling per-sensor behaviour and correct EKF replay. --- libraries/AP_NavEKF3/AP_NavEKF3_GyroBias.cpp | 19 +++++++++++++++---- libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 3 ++- libraries/AP_NavEKF3/AP_NavEKF3_core.h | 8 ++++---- 3 files changed, 21 insertions(+), 9 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_GyroBias.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_GyroBias.cpp index c5397e0169b..039493f9b54 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_GyroBias.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_GyroBias.cpp @@ -1,4 +1,5 @@ #include "AP_NavEKF3_core.h" +#include // reset the body axis gyro bias states to zero and re-initialise the corresponding covariances // Assume that the calibration is performed to an accuracy of 0.5 deg/sec which will require averaging under static conditions @@ -19,10 +20,20 @@ void NavEKF3_core::resetGyroBias(void) */ ftype NavEKF3_core::InitialGyroBiasUncertainty(void) const { -#if AP_INERTIALSENSOR_LOW_NOISE - return 1.0f; -#else + if (dal.ins().is_low_drift(imu_index)) { + return 1.0f; + } return 2.5f; -#endif +} + +/* + get the gyro bias limit for this core's IMU + */ +ftype NavEKF3_core::getGyroBiasLimit(void) const +{ + if (dal.ins().is_low_drift(imu_index)) { + return GYRO_BIAS_LIMIT_LOW_DRIFT; + } + return GYRO_BIAS_LIMIT; } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 700ff58ebad..6e6c78f1cc9 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -2087,7 +2087,8 @@ void NavEKF3_core::ConstrainStates() // height limit covers home alt on everest through to home alt at SL and balloon drop stateStruct.position.z = constrain_ftype(stateStruct.position.z,-4.0e4f,1.0e4f); // gyro bias limit (this needs to be set based on manufacturers specs) - for (uint8_t i=10; i<=12; i++) statesArray[i] = constrain_ftype(statesArray[i],-GYRO_BIAS_LIMIT*dtEkfAvg,GYRO_BIAS_LIMIT*dtEkfAvg); + const ftype gyro_bias_limit = getGyroBiasLimit(); + for (uint8_t i=10; i<=12; i++) statesArray[i] = constrain_ftype(statesArray[i],-gyro_bias_limit*dtEkfAvg,gyro_bias_limit*dtEkfAvg); // the accelerometer bias limit is controlled by a user adjustable parameter for (uint8_t i=13; i<=15; i++) statesArray[i] = constrain_ftype(statesArray[i],-frontend->_accBiasLim*dtEkfAvg,frontend->_accBiasLim*dtEkfAvg); // earth magnetic field limit diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index ac09f2a243a..b25a854fd34 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -49,11 +49,8 @@ #define earthRate 0.000072921f // earth rotation rate (rad/sec) // maximum allowed gyro bias (rad/sec) -#if AP_INERTIALSENSOR_LOW_NOISE -#define GYRO_BIAS_LIMIT radians(2.0f) -#else #define GYRO_BIAS_LIMIT 0.5f -#endif +#define GYRO_BIAS_LIMIT_LOW_DRIFT radians(2.0f) // initial accel bias uncertainty as a fraction of the state limit #define ACCEL_BIAS_LIM_SCALER 0.2f @@ -1619,6 +1616,9 @@ private: // vehicle specific initial gyro bias uncertainty ftype InitialGyroBiasUncertainty(void) const; + // get the gyro bias limit for this core's IMU + ftype getGyroBiasLimit(void) const; + /* learn magnetometer biases from GPS yaw. Return true if the resulting mag vector is close enough to the one predicted by GPS