fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-24 16:10:35 +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 521fefc155
commit 4234e9a390
3 changed files with 23 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf_helper.cpp
+23 -1
View File
@@ -1210,13 +1210,31 @@ void Ekf::stopMag3DFusion()
// save covariance data for re-use if currently doing 3-axis fusion
if (_control_status.flags.mag_3D) {
saveMagCovData();
_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()
@@ -1231,7 +1249,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;
src/modules/ekf2/EKF/gps_yaw_fusion.cpp
-3
View File
@@ -146,9 +146,6 @@ void Ekf::fuseGpsYaw()
// innovation test ratio
_yaw_test_ratio = sq(_heading_innov) / (sq(innov_gate) * _heading_innov_var);
// we are no longer using 3-axis fusion so set the reported test levels to zero
_mag_test_ratio.setZero();
if (_yaw_test_ratio > 1.0f) {
_innov_check_fail_status.flags.reject_yaw = true;
return;
src/modules/ekf2/EKF/mag_fusion.cpp
-6
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;
@@ -665,9 +662,6 @@ bool Ekf::updateQuaternion(const float innovation, const float variance, const f
// innovation test ratio
_yaw_test_ratio = sq(innovation) / (sq(gate_sigma) * _heading_innov_var);
// we are no longer using 3-axis fusion so set the reported test levels to zero
_mag_test_ratio.setZero();
// set the magnetometer unhealthy if the test fails
if (_yaw_test_ratio > 1.0f) {
_innov_check_fail_status.flags.reject_yaw = true;