AP_NavEKF3/ArduCopter: hover Z-bias learning for vibration rectification

ArduCopter/AP_Arming_Copter.cpp

@@ -757,6 +757,10 @@ bool AP_Arming_Copter::arm(const AP_Arming::Method method, const bool do_arming_
     // finally actually arm the motors
     copter.motors->armed(true);
 
+    // Clear accel bias learning inhibit that may have been set while disarmed
+    // (ACC_ZBIAS_LEARN bit 2). Mode-specific code (e.g., acro) may set it again.
+    ahrs.set_inhibit_accel_bias_learning(false);
+
 #if HAL_LOGGING_ENABLED
     // log flight mode in case it was changed while vehicle was disarmed
     AP::logger().Write_Mode((uint8_t)copter.flightmode->mode_number(), copter.control_mode_reason);
@@ -826,6 +830,9 @@ bool AP_Arming_Copter::disarm(const AP_Arming::Method method, bool do_disarm_che
         }
     }
 
+    // save hover accel bias learned by Copter if enabled
+    copter.save_hover_bias_learning();
+
     // we are not in the air
     copter.set_land_complete(true);
     copter.set_land_complete_maybe(true);

ArduCopter/Attitude.cpp

@@ -63,6 +63,8 @@ void Copter::update_throttle_hover()
 #if HAL_GYROFFT_ENABLED
         gyro_fft.update_freq_hover(0.01f, motors->get_throttle_out());
 #endif
+        // update learned hover accel bias (checks ground effect flags internally)
+        update_hover_bias_learning(0.01f);
     }
 }
 
@@ -135,3 +137,126 @@ float Copter::get_pilot_speed_dn_ms() const
         return fabsf(g2.pilot_speed_dn_ms);
     }
 }
+
+// Time constant for hover bias learning filter (seconds)
+#define HOVER_BIAS_TC 2.0f
+
+// ACC_ZBIAS_LEARN bitmask values
+#define ACC_ZBIAS_LEARN_SAVE   (1U << 0)  // Bit 0: Learn during hover and save on disarm
+#define ACC_ZBIAS_LEARN_USE    (1U << 1)  // Bit 1: Use saved values (apply correction)
+#define ACC_ZBIAS_LEARN_INHIBIT_DISARMED (1U << 2)  // Bit 2: Disable EKF bias learning when disarmed
+
+// init_hover_bias_correction - loads saved hover Z-bias from INS parameters
+// into _hover_bias_learning array. The frozen correction in EKF is set later
+// from one_hz_loop once EKF3 is active.
+// called once from startup_INS_ground() after ahrs.reset()
+void Copter::init_hover_bias_correction(void)
+{
+    if ((g2.accel_zbias_learn & ACC_ZBIAS_LEARN_USE) == 0) {
+        return;
+    }
+
+    // Initialize learning state from INS parameters
+    // The actual EKF correction will be set later once EKF3 is active
+    for (uint8_t imu = 0; imu < INS_MAX_INSTANCES; imu++) {
+        const float raw_bias = AP::ins().get_accel_vrf_bias_z(imu);
+        _hover_bias_learning[imu] = raw_bias;
+    }
+}
+
+// set_hover_z_bias_correction - sets the frozen hover Z-bias correction in EKF
+// called from one_hz_loop while disarmed until values match
+void Copter::set_hover_z_bias_correction(void)
+{
+    static bool message_sent;
+
+    if ((g2.accel_zbias_learn & ACC_ZBIAS_LEARN_USE) == 0) {
+        return;
+    }
+
+    for (uint8_t imu = 0; imu < INS_MAX_INSTANCES; imu++) {
+        const float saved_bias = _hover_bias_learning[imu];
+        const float current_correction = ahrs.get_hover_z_bias_correction(imu);
+
+        // Set the correction if not already matching
+        if (!is_equal(saved_bias, current_correction)) {
+            ahrs.set_hover_z_bias_correction(imu, saved_bias);
+        }
+
+        // Report non-zero corrections once (EKF may have loaded from params)
+        if (!message_sent && !is_zero(saved_bias)) {
+            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Hover Z-bias IMU%u: %.3f m/s^2", imu, saved_bias);
+        }
+    }
+    message_sent = true;
+}
+
+// update_hover_bias_learning - learns Z-axis accelerometer bias during hover
+// to compensate for vibration rectification effects
+// called at 100Hz from update_throttle_hover() when already in a stable hover
+// (hover conditions checked by caller: armed, level, low velocity, throttle > 0)
+void Copter::update_hover_bias_learning(float dt)
+{
+    if ((g2.accel_zbias_learn & ACC_ZBIAS_LEARN_SAVE) == 0) {
+        return;
+    }
+
+    // Don't learn during ground effect (motor thrust offset corrupts bias)
+    if (ahrs.get_takeoff_expected() || ahrs.get_touchdown_expected()) {
+        return;
+    }
+
+    const float alpha = dt / (dt + HOVER_BIAS_TC);
+
+    for (uint8_t imu = 0; imu < INS_MAX_INSTANCES; imu++) {
+        // Get current EKF bias for this IMU (returns false if no core uses it)
+        float currentBiasZ;
+        if (!ahrs.get_accel_bias_z_for_imu(imu, currentBiasZ)) {
+            continue;
+        }
+
+        // Get frozen correction (already applied at IMU level)
+        const float frozenCorrection = ahrs.get_hover_z_bias_correction(imu);
+
+        // Total bias = EKF residual + frozen correction
+        const float totalBias = currentBiasZ + frozenCorrection;
+
+        // Apply low-pass filter
+        _hover_bias_learning[imu] += alpha * (totalBias - _hover_bias_learning[imu]);
+
+        // Update INS parameter (RAM only, not saved yet)
+        AP::ins().set_accel_vrf_bias_z(imu, _hover_bias_learning[imu]);
+    }
+}
+
+// save_hover_bias_learning - saves learned hover bias to EEPROM
+// called on disarm
+void Copter::save_hover_bias_learning(void)
+{
+    if ((g2.accel_zbias_learn & ACC_ZBIAS_LEARN_SAVE) == 0) {
+        return;
+    }
+
+    for (uint8_t imu = 0; imu < INS_MAX_INSTANCES; imu++) {
+        // Only save if an EKF core is using this IMU
+        float bias_z;
+        if (ahrs.get_accel_bias_z_for_imu(imu, bias_z)) {
+            AP::ins().save_accel_vrf_bias_z(imu);
+        }
+    }
+}
+
+// update_accel_bias_inhibit - controls EKF accel bias learning based on armed state
+// When bit 2 is set in ACC_ZBIAS_LEARN, inhibit EKF bias learning while disarmed
+// to avoid learning incorrect bias from the zero velocity assumption on ground.
+// Only sets inhibit=true when disarmed; clearing is done in arm() to avoid
+// overriding mode-specific inhibit (e.g., acro during high-G maneuvers).
+// called from one_hz_loop
+void Copter::update_accel_bias_inhibit(void)
+{
+    const bool inhibit_disarmed = (g2.accel_zbias_learn & ACC_ZBIAS_LEARN_INHIBIT_DISARMED) != 0;
+    if (inhibit_disarmed && !motors->armed()) {
+        ahrs.set_inhibit_accel_bias_learning(true);
+    }
+    // Don't clear here - let arm() or mode-specific code handle clearing
+}

ArduCopter/Copter.cpp

@@ -784,6 +784,9 @@ void Copter::one_hz_loop()
 #endif
 
     if (!motors->armed()) {
+        // Set hover Z-bias corrections in EKF once it's active
+        set_hover_z_bias_correction();
+
         update_using_interlock();
 
         // check the user hasn't updated the frame class or type
@@ -795,6 +798,9 @@ void Copter::one_hz_loop()
 #endif
     }
 
+    // Update EKF accel bias learning inhibit based on armed state
+    update_accel_bias_inhibit();
+
     // update assigned functions and enable auxiliary servos
     AP::srv().enable_aux_servos();
 

ArduCopter/Copter.h

@@ -305,6 +305,10 @@ class Copter : public AP_Vehicle {
     // Inertial Navigation EKF - different viewpoint
     AP_AHRS_View *ahrs_view;
 
+    // Hover accel bias learning state (per-IMU)
+    // Tracks total Z-axis bias during hover for vibration rectification compensation
+    float _hover_bias_learning[INS_MAX_INSTANCES];
+
     // Arming/Disarming management class
     AP_Arming_Copter arming;
 
@@ -726,6 +730,11 @@ class Copter : public AP_Vehicle {
 
     // Attitude.cpp
     void update_throttle_hover();
+    void init_hover_bias_correction(void);
+    void set_hover_z_bias_correction(void);
+    void update_hover_bias_learning(float dt);
+    void save_hover_bias_learning(void);
+    void update_accel_bias_inhibit(void);
     float get_pilot_desired_climb_rate_ms();
     float get_non_takeoff_throttle();
     void set_accel_throttle_I_from_pilot_throttle();

ArduCopter/Parameters.cpp

@@ -1073,6 +1073,29 @@ const AP_Param::GroupInfo ParametersG2::var_info2[] = {
     AP_GROUPINFO("TKOFF_RPM_MAX", 7, ParametersG2, takeoff_rpm_max, 0),
 #endif
 
+    // @Param: TKOFF_GNDEFF_ALT
+    // @DisplayName: Takeoff ground effect altitude
+    // @Description: Altitude above which ground effect compensation is disabled. Ground effect compensation is re-enabled when the vehicle descends below this altitude. Set to zero to disable ground effect altitude re-activation.
+    // @Range: 0 5
+    // @Units: m
+    // @User: Advanced
+    AP_GROUPINFO("TKOFF_GNDEFF_ALT", 16, ParametersG2, tkoff_gndeff_alt, 0.5),
+
+    // @Param: ACC_ZBIAS_LEARN
+    // @DisplayName: Accel Z-axis Bias Learning
+    // @Description: Bitmask controlling accelerometer Z-axis bias learning during hover to compensate for vibration rectification. Only active when using EKF3 (AHRS_EKF_TYPE=3). Bit 0: Learn bias during hover and save to EEPROM on disarm. Bit 1: Use saved bias values (apply correction to EKF). Bit 2: Disable EKF bias learning while disarmed (don't use zero velocity assumption on ground).
+    // @Bitmask: 0:Learn and Save,1:Use Saved Values,2:Disable Ground Learning
+    // @User: Advanced
+    AP_GROUPINFO("ACC_ZBIAS_LEARN", 17, ParametersG2, accel_zbias_learn, 0),
+
+    // @Param: TKOFF_GNDEFF_TMO
+    // @DisplayName: Ground Effect Timeout
+    // @Description: Time after throttle up before ground effect compensation can be disabled. When set, ground effect will only be disabled after BOTH this timeout has elapsed AND altitude exceeds TKOFF_GNDEFF_ALT. This prevents premature ground effect disabling when baro noise causes false altitude readings. Set to zero to disable (uses altitude threshold only). Maximum timeout is always 5 seconds regardless of this setting.
+    // @Range: 0 5
+    // @Units: s
+    // @User: Advanced
+    AP_GROUPINFO("TKOFF_GNDEFF_TMO", 18, ParametersG2, tkoff_gndeff_tmo, 0),
+
     // @Param: FS_EKF_FILT
     // @DisplayName: EKF Failsafe filter cutoff
     // @Description: EKF Failsafe filter cutoff frequency. EKF variances are filtered using this value to avoid spurious failsafes from transient high variances. A higher value means the failsafe is more likely to trigger.

ArduCopter/Parameters.h

@@ -668,6 +668,15 @@ class ParametersG2 {
     AP_Int16 takeoff_rpm_max;
 #endif
 
+    // ground effect compensation altitude threshold
+    AP_Float tkoff_gndeff_alt;
+
+    // ground effect compensation timeout (requires both timeout AND altitude)
+    AP_Float tkoff_gndeff_tmo;
+
+    // hover Z-axis accel bias learning mode
+    AP_Int8 accel_zbias_learn;
+
     // EKF variance filter cutoff
     AP_Float fs_ekf_filt_hz;
 

ArduCopter/baro_ground_effect.cpp

@@ -43,10 +43,35 @@ void Copter::update_ground_effect_detector(void)
 
     // if we are in takeoff_expected and we meet the conditions for having taken off
     // end the takeoff_expected state
-    if (gndeffect_state.takeoff_expected && (tnow_ms - gndeffect_state.takeoff_time_ms > 5000 || (-pos_d_m - gndeffect_state.takeoff_alt_m) > 0.50)) {
+    // Use configured altitude threshold, or the original 0.50m default when not set
+    const float gndeff_alt_m = is_positive(g2.tkoff_gndeff_alt) ? g2.tkoff_gndeff_alt : 0.50f;
+    const float height_above_takeoff_m = -pos_d_m - gndeffect_state.takeoff_alt_m;
+    const uint32_t time_since_takeoff_ms = tnow_ms - gndeffect_state.takeoff_time_ms;
+    const bool above_gndeff_alt = height_above_takeoff_m > gndeff_alt_m;
+    const bool max_timeout = time_since_takeoff_ms > 5000;
+
+    // Determine if ground effect should be disabled
+    bool should_clear_gndeff;
+    if (is_positive(g2.tkoff_gndeff_tmo)) {
+        // With timeout parameter: require BOTH timeout AND altitude, but always clear after 5s max
+        const uint32_t tmo_ms = uint32_t(g2.tkoff_gndeff_tmo * 1000.0f);
+        const bool tmo_elapsed = time_since_takeoff_ms > tmo_ms;
+        should_clear_gndeff = max_timeout || (tmo_elapsed && above_gndeff_alt);
+    } else {
+        // Without timeout parameter: original behavior (altitude OR 5s timeout)
+        should_clear_gndeff = max_timeout || above_gndeff_alt;
+    }
+
+    if (gndeffect_state.takeoff_expected && should_clear_gndeff) {
         gndeffect_state.takeoff_expected = false;
     }
 
+    // re-enable ground effect compensation when descending back below threshold
+    if (!ap.land_complete && !gndeffect_state.takeoff_expected &&
+        is_positive(g2.tkoff_gndeff_alt) && height_above_takeoff_m < gndeff_alt_m) {
+        gndeffect_state.takeoff_expected = true;
+    }
+
     // landing logic
     Vector3f angle_target_rad = attitude_control->get_att_target_euler_rad();
     bool small_angle_request = cosf(angle_target_rad.x) * cosf(angle_target_rad.y) > cosf(radians(7.5f));
@@ -60,7 +85,11 @@ void Copter::update_ground_effect_detector(void)
     bool speed_low_d_ms = AP::ahrs().get_velocity_D(vel_ned_ms.z, vibration_check.high_vibes) && fabsf(vel_ned_ms.z) <= 0.6f;
     bool slow_descent = (slow_descent_demanded || (speed_low_d_ms && descent_demanded));
 
-    gndeffect_state.touchdown_expected = slow_horizontal && slow_descent;
+    // Only expect touchdown when near ground (below TKOFF_GNDEFF_ALT threshold)
+    // This allows EKF bias learning when hovering at altitude
+    const bool near_ground_for_touchdown = is_positive(g2.tkoff_gndeff_alt) ?
+                                           (height_above_takeoff_m < gndeff_alt_m) : true;
+    gndeffect_state.touchdown_expected = slow_horizontal && slow_descent && near_ground_for_touchdown;
 
     // Prepare the EKF for ground effect if either takeoff or touchdown is expected.
     ahrs.set_takeoff_expected(gndeffect_state.takeoff_expected);

ArduCopter/mode_acro.cpp

@@ -51,6 +51,10 @@ void ModeAcro::run()
         break;
     }
 
+    // inhibit accel bias learning during acro when flying, since high-G
+    // maneuvers can cause unwanted bias learning
+    ahrs.set_inhibit_accel_bias_learning(motors->get_spool_state() == AP_Motors::SpoolState::THROTTLE_UNLIMITED);
+
     // run attitude controller
     if (g2.acro_options.get() & uint8_t(AcroOptions::RATE_LOOP_ONLY)) {
         // scale I by the value of angle P to mimic betaflight tunes
@@ -83,6 +87,9 @@ void ModeAcro::exit()
         copter.air_mode = AirMode::AIRMODE_DISABLED;
     }
     disable_air_mode_reset = false;
+
+    // re-enable accel bias learning when exiting acro
+    ahrs.set_inhibit_accel_bias_learning(false);
 }
 
 void ModeAcro::air_mode_aux_changed()

ArduCopter/system.cpp

@@ -215,6 +215,9 @@ void Copter::startup_INS_ground()
 
     // reset ahrs including gyro bias
     ahrs.reset();
+
+    // Load saved hover Z-bias corrections into EKF (must be after ahrs.reset())
+    init_hover_bias_correction();
 }
 
 // position_ok - returns true if the horizontal absolute position is ok and home position is set