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

@@ -1,4 +1,5 @@
 #include "AP_NavEKF3_core.h"
+#include <AP_DAL/AP_DAL.h>
 
 // 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;
 }
 

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

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