AP_NavEKF3: load hover Z-bias correction from INS params at init

Load _accelBiasHoverZ_correction from INS_ACC_VRFB_Z parameters during
InitialiseFilter() instead of initializing to zero. This ensures the
Replay tool (which has no vehicle code to call setHoverZBiasCorrection)
gets the correct correction values from the log's parameter set.

Without this, if a previous flight saved a non-zero bias to INS params,
the recording would apply the correction but Replay would not, causing
deterministic mismatch in all EKF outputs.

Author: andyp1per

libraries/AP_DAL/examples/AP_DAL_Standalone/main.cpp

@@ -7,6 +7,7 @@
 #include <AP_NavEKF2/AP_NavEKF2.h>
 #include <AP_NavEKF3/AP_NavEKF3.h>
 #include <AP_Logger/AP_Logger.h>
+#include <AP_InertialSensor/AP_InertialSensor.h>
 
 void AP_Param::setup_object_defaults(void const*, AP_Param::GroupInfo const*) {}
 
@@ -21,6 +22,11 @@ void *nologger = nullptr;
 AP_Logger &AP::logger() {
     return *((AP_Logger*)nologger);  // this is not usually a good idea...
 }
+
+void *noins = nullptr;
+AP_InertialSensor &AP::ins() {
+    return *((AP_InertialSensor*)noins);  // link stub only
+}
 void AP_Logger::WriteBlock(void const*, unsigned short) {}
 
 class AP_HAL_DAL_Standalone : public AP_HAL::HAL {

libraries/AP_NavEKF3/AP_NavEKF3.cpp

@@ -5,6 +5,7 @@
 #include <AP_HAL/AP_HAL.h>
 
 #include <GCS_MAVLink/GCS.h>
+#include <AP_InertialSensor/AP_InertialSensor.h>
 #include <AP_Logger/AP_Logger.h>
 #include <AP_Vehicle/AP_Vehicle_Type.h>
 #include <AP_BoardConfig/AP_BoardConfig.h>
@@ -816,10 +817,18 @@ bool NavEKF3::InitialiseFilter(void)
         // breaking the feedback loop between learning and correction. The INS parameter
         // may be updated by learning during flight, but only the frozen value is used
         // for correction.
-        // Initialize all IMU corrections to zero
-        // Vehicle code will set actual corrections via setHoverZBiasCorrection()
-        for (uint8_t i = 0; i < INS_MAX_INSTANCES; i++) {
-            _accelBiasHoverZ_correction[i] = 0.0f;
+        // Load from INS parameters so Replay (which has no vehicle code) gets the
+        // correct correction value. Vehicle code may override via
+        // setHoverZBiasCorrection() based on its learning mode setting.
+        {
+            const auto &real_ins = AP::ins();
+            for (uint8_t i = 0; i < INS_MAX_INSTANCES; i++) {
+                constexpr float MAX_HOVER_BIAS_CORRECTION = 0.3f;
+                _accelBiasHoverZ_correction[i] = constrain_float(
+                    real_ins.get_accel_vrf_bias_z(i),
+                    -MAX_HOVER_BIAS_CORRECTION,
+                    MAX_HOVER_BIAS_CORRECTION);
+            }
         }
 
         // check if there is enough memory to create the EKF cores