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: clear test ratios, flags, etc when stopping mag fusion

Browse Source
This commit is contained in:
Daniel Agar 2022-03-22 10:04:50 -04:00
parent 65a587e56a
commit 05133aed27
2 changed files with 22 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1255,16 +1255,30 @@ void Ekf::stopMag3DFusion()
if (_control_status.flags.mag_3D) {
saveMagCovData();
// we are no longer using 3-axis fusion so set the reported test levels to zero
_mag_test_ratio.setZero();
_control_status.flags.mag_3D = false;
_control_status.flags.mag_dec = false;
_mag_innov.zero();
_mag_innov_var.zero();
_mag_test_ratio.zero();
_fault_status.flags.bad_mag_x = false;
_fault_status.flags.bad_mag_y = false;
_fault_status.flags.bad_mag_z = false;
_fault_status.flags.bad_mag_decl = false;
}
}
void Ekf::stopMagHdgFusion()
{
_control_status.flags.mag_hdg = false;
if (_control_status.flags.mag_hdg) {
_control_status.flags.mag_hdg = false;
_fault_status.flags.bad_hdg = false;
_yaw_test_ratio = 0.f;
}
}
void Ekf::startMagHdgFusion()
@ -1279,7 +1293,11 @@ void Ekf::startMagHdgFusion()
void Ekf::startMag3DFusion()
{
if (!_control_status.flags.mag_3D) {
stopMagHdgFusion();
_yaw_test_ratio = 0.0f;
zeroMagCov();
loadMagCovData();
_control_status.flags.mag_3D = true;

3
src/modules/ekf2/EKF/mag_fusion.cpp
View File

@ -195,9 +195,6 @@ bool Ekf::fuseMag(const Vector3f &mag, bool update_all_states)
}
}
// we are no longer using heading fusion so set the reported test level to zero
_yaw_test_ratio = 0.0f;
// if any axis fails, abort the mag fusion
if (!all_innovation_checks_passed) {
return false;