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

ekf2-mag: add inclination check

Browse Source
This commit is contained in:
bresch
2023-07-24 10:16:37 -04:00
committed by Daniel Agar
parent e4a16bfc80
commit 357bf024f6
9 changed files with 122 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/mag_control.cpp
+22
View File
@@ -471,6 +471,28 @@ bool Ekf::checkMagField(const Vector3f &mag_sample)
}
}
const Vector3f mag_earth = _R_to_earth * mag_sample;
const float mag_inclination = asin(mag_earth(2) / fmaxf(mag_earth.norm(), 1e-4f));
if (_params.mag_check & static_cast<int32_t>(MagCheckMask::INCLINATION)) {
if (PX4_ISFINITE(_mag_inclination_gps)) {
const float inc_tol_rad = radians(_params.mag_check_inclination_tolerance_deg);
const float inc_error_rad = wrap_pi(mag_inclination - _mag_inclination_gps);
if (fabsf(inc_error_rad) > inc_tol_rad) {
_control_status.flags.mag_field_disturbed = true;
is_check_failing = true;
}
} else if (_params.mag_check & static_cast<int32_t>(MagCheckMask::FORCE_WMM)) {
is_check_failing = true;
} else {
// No check possible when the global position is unknown
// TODO: add parameter to remember the inclination between boots
}
}
if (is_check_failing || (_time_last_mag_check_failing == 0)) {
_time_last_mag_check_failing = _time_delayed_us;
}