ekf2: properly reset IMU biases on calibration change (non-multi-EKF)

- this was working in the multi-EKF case using vehicle_imu, but missing
in sensor_combined
This commit is contained in:
Daniel Agar
2022-04-10 11:07:01 -04:00
parent 8f891332f1
commit bb2ea574aa
3 changed files with 29 additions and 1 deletions
+22
View File
@@ -446,6 +446,28 @@ void EKF2::Run()
}
imu_dt = sensor_combined.gyro_integral_dt;
if (sensor_combined.accel_calibration_count != _accel_calibration_count) {
PX4_DEBUG("%d - resetting accelerometer bias", _instance);
_ekf.resetAccelBias();
_accel_calibration_count = sensor_combined.accel_calibration_count;
// reset bias learning
_accel_cal = {};
}
if (sensor_combined.gyro_calibration_count != _gyro_calibration_count) {
PX4_DEBUG("%d - resetting rate gyro bias", _instance);
_ekf.resetGyroBias();
_gyro_calibration_count = sensor_combined.gyro_calibration_count;
// reset bias learning
_gyro_cal = {};
}
}
if (_sensor_selection_sub.updated() || (_device_id_accel == 0 || _device_id_gyro == 0)) {