fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: request mag yaw reset after calibration or sensor change

Browse Source
This commit is contained in:
Daniel Agar 2022-03-08 11:13:13 -05:00
parent c7cec4252c
commit b81a5b3efa
3 changed files with 11 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/modules/ekf2/EKF/ekf.h
View File

@ -193,7 +193,8 @@ public:
void resetAccelBias();
// Reset all magnetometer bias states and covariances to initial alignment values.
void resetMagBias();
// Requests full mag yaw reset (if using mag)
void resetMagBiasAndYaw();
Vector3f getVelocityVariance() const { return P.slice<3, 3>(4, 4).diag(); };

8
src/modules/ekf2/EKF/ekf_helper.cpp
View File

@ -907,7 +907,7 @@ void Ekf::resetAccelBias()
_prev_dvel_bias_var = P.slice<3, 3>(13, 13).diag();
}
void Ekf::resetMagBias()
void Ekf::resetMagBiasAndYaw()
{
// Zero the magnetometer bias states
_state.mag_B.zero();
@ -918,6 +918,12 @@ void Ekf::resetMagBias()
// reset any saved covariance data for re-use when auto-switching between heading and 3-axis fusion
_saved_mag_bf_variance.zero();
if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
_mag_yaw_reset_req = true;
}
_control_status.flags.mag_fault = false;
}
// get EKF innovation consistency check status information comprising of:

4
src/modules/ekf2/EKF2.cpp
View File

@ -1818,14 +1818,14 @@ void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps)
reset = true;
} else if (magnetometer.calibration_count > _mag_calibration_count) {
} else if (magnetometer.calibration_count != _mag_calibration_count) {
// existing calibration has changed, reset saved mag bias
PX4_DEBUG("%d - mag %" PRIu32 " calibration updated, resetting bias", _instance, _device_id_mag);
reset = true;
}
if (reset) {
_ekf.resetMagBias();
_ekf.resetMagBiasAndYaw();
_device_id_mag = magnetometer.device_id;
_mag_calibration_count = magnetometer.calibration_count;