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

ekf2: update mag calibration requirements

Browse Source
This commit is contained in:
Daniel Agar 2023-03-13 10:08:48 -04:00
parent fd33e60f78
commit ff39b360f7
No known key found for this signature in database
GPG Key ID: FD3CBA98017A69DE
1 changed files with 9 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -2283,10 +2283,16 @@ void EKF2::UpdateGyroCalibration(const hrt_abstime &timestamp)
void EKF2::UpdateMagCalibration(const hrt_abstime &timestamp)
{
const bool bias_valid = (_ekf.control_status_flags().mag_hdg || _ekf.control_status_flags().mag_3D)
const bool bias_valid = (_param_ekf2_mag_type.get() == static_cast<int32_t>(MagFuseType::AUTO)
|| _param_ekf2_mag_type.get() == static_cast<int32_t>(MagFuseType::MAG_3D))
&& _ekf.control_status_flags().tilt_align
&& _ekf.control_status_flags().yaw_align
&& _ekf.control_status_flags().mag_aligned_in_flight
&& (_ekf.fault_status().value == 0)
&& !_ekf.control_status_flags().mag_fault
&& !_ekf.control_status_flags().mag_field_disturbed;
&& !_ekf.control_status_flags().mag_field_disturbed
&& !_ekf.warning_event_flags().stopping_mag_use
&& !_ekf.warning_event_flags().emergency_yaw_reset_mag_stopped;
const bool learning_valid = bias_valid && _ekf.control_status_flags().mag_3D;
@ -2294,7 +2300,7 @@ void EKF2::UpdateMagCalibration(const hrt_abstime &timestamp)
bias_valid, learning_valid);
// update stored declination value
if (!_mag_decl_saved) {
if (!_mag_decl_saved && !_ekf.control_status_flags().in_air) {
float declination_deg;
if (_ekf.get_mag_decl_deg(&declination_deg)) {