Skip to content

Commit 33cfbd2

Browse files
committed
ArduCopter: ensure hover Z-bias message prints when EKF preloads correction
Since the EKF now loads hover Z-bias from INS params at init, the correction is already set when vehicle code tries to apply it. Use a one-shot flag to ensure the informational GCS message is still printed for non-zero corrections, regardless of whether the EKF already has the value.
1 parent e57b8f6 commit 33cfbd2

2 files changed

Lines changed: 9 additions & 6 deletions

File tree

ArduCopter/Attitude.cpp

Lines changed: 8 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -168,6 +168,8 @@ void Copter::init_hover_bias_correction(void)
168168
// called from one_hz_loop while disarmed until values match
169169
void Copter::set_hover_z_bias_correction(void)
170170
{
171+
static bool message_sent;
172+
171173
if ((g2.accel_zbias_learn & ACC_ZBIAS_LEARN_USE) == 0) {
172174
return;
173175
}
@@ -176,16 +178,17 @@ void Copter::set_hover_z_bias_correction(void)
176178
const float saved_bias = _hover_bias_learning[imu];
177179
const float current_correction = ahrs.get_hover_z_bias_correction(imu);
178180

179-
// Skip if already set correctly or nothing to set
180-
if (is_equal(saved_bias, current_correction)) {
181-
continue;
181+
// Set the correction if not already matching
182+
if (!is_equal(saved_bias, current_correction)) {
183+
ahrs.set_hover_z_bias_correction(imu, saved_bias);
182184
}
183185

184-
// Try to set the correction
185-
if (ahrs.set_hover_z_bias_correction(imu, saved_bias)) {
186+
// Report non-zero corrections once (EKF may have loaded from params)
187+
if (!message_sent && !is_zero(saved_bias)) {
186188
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Hover Z-bias IMU%u: %.3f m/s^2", imu, saved_bias);
187189
}
188190
}
191+
message_sent = true;
189192
}
190193

191194
// update_hover_bias_learning - learns Z-axis accelerometer bias during hover

ArduCopter/Parameters.cpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1129,7 +1129,7 @@ const AP_Param::GroupInfo ParametersG2::var_info2[] = {
11291129
// @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).
11301130
// @Bitmask: 0:Learn and Save,1:Use Saved Values,2:Disable Ground Learning
11311131
// @User: Advanced
1132-
AP_GROUPINFO("ACC_ZBIAS_LEARN", 17, ParametersG2, accel_zbias_learn, 3),
1132+
AP_GROUPINFO("ACC_ZBIAS_LEARN", 17, ParametersG2, accel_zbias_learn, 0),
11331133

11341134
// @Param: TKOFF_GNDEFF_TMO
11351135
// @DisplayName: Ground Effect Timeout

0 commit comments

Comments
 (0)