fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 16:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: integrate mag heading into mag 3D

Browse Source
This commit is contained in:
bresch
2024-04-05 22:26:03 -04:00
committed by Daniel Agar
parent 0c9e4013d2
commit 51883fe5d4
14 changed files with 66 additions and 265 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf_helper.cpp
+2 -11
View File
@@ -381,11 +381,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
mag = 0.f;
#if defined(CONFIG_EKF2_MAGNETOMETER)
if (_control_status.flags.mag_hdg) {
mag = math::max(mag, sqrtf(_aid_src_mag_heading.test_ratio));
}
if (_control_status.flags.mag_3D) {
if (_control_status.flags.mag_hdg ||_control_status.flags.mag_3D) {
mag = math::max(mag, sqrtf(Vector3f(_aid_src_mag.test_ratio).max()));
}
#endif // CONFIG_EKF2_MAGNETOMETER
@@ -523,12 +519,7 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const
bool mag_innov_good = true;
#if defined(CONFIG_EKF2_MAGNETOMETER)
if (_control_status.flags.mag_hdg) {
if (_aid_src_mag_heading.test_ratio < 1.f) {
mag_innov_good = false;
}
} else if (_control_status.flags.mag_3D) {
if (_control_status.flags.mag_hdg ||_control_status.flags.mag_3D) {
if (Vector3f(_aid_src_mag.test_ratio).max() < 1.f) {
mag_innov_good = false;
}