From 0a665a526c9249b69c95c967106b30d74fd06383 Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 27 May 2024 09:28:40 +0200 Subject: [PATCH] ekf2: add mag type init In this mode, the mag is used to inilialize the heading. During flight, the heading is predicted using gyros and corrected by GNSS measurements if available. --- .../EKF/aid_sources/magnetometer/mag_control.cpp | 4 +++- src/modules/ekf2/EKF/common.h | 3 ++- src/modules/ekf2/EKF2.cpp | 1 + src/modules/ekf2/module.yaml | 15 +++++++-------- 4 files changed, 13 insertions(+), 10 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp index 458ca5c1db..2e288adfbc 100644 --- a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp @@ -130,7 +130,9 @@ void Ekf::controlMagFusion() _innov_check_fail_status.flags.reject_mag_z = (aid_src.test_ratio[2] > 1.f); // determine if we should use mag fusion - bool continuing_conditions_passing = (_params.mag_fusion_type != MagFuseType::NONE) + bool continuing_conditions_passing = ((_params.mag_fusion_type == MagFuseType::INIT) + || (_params.mag_fusion_type == MagFuseType::AUTO) + || (_params.mag_fusion_type == MagFuseType::HEADING)) && _control_status.flags.tilt_align && (_control_status.flags.yaw_align || (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align)) && mag_sample.mag.longerThan(0.f) diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 8624501cb2..845c2d4edb 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -101,7 +101,8 @@ enum MagFuseType : uint8_t { // Integer definitions for mag_fusion_type AUTO = 0, ///< The selection of either heading or 3D magnetometer fusion will be automatic HEADING = 1, ///< Simple yaw angle fusion will always be used. This is less accurate, but less affected by earth field distortions. It should not be used for pitch angles outside the range from -60 to +60 deg - NONE = 5 ///< Do not use magnetometer under any circumstance.. + NONE = 5, ///< Do not use magnetometer under any circumstance. + INIT = 6 ///< Use the mag for heading initialization only. }; #endif // CONFIG_EKF2_MAGNETOMETER diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 7cfefd9d2c..2d15b7262c 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -769,6 +769,7 @@ void EKF2::VerifyParams() if ((_param_ekf2_mag_type.get() != MagFuseType::AUTO) && (_param_ekf2_mag_type.get() != MagFuseType::HEADING) && (_param_ekf2_mag_type.get() != MagFuseType::NONE) + && (_param_ekf2_mag_type.get() != MagFuseType::INIT) ) { mavlink_log_critical(&_mavlink_log_pub, "EKF2_MAG_TYPE invalid, resetting to default"); diff --git a/src/modules/ekf2/module.yaml b/src/modules/ekf2/module.yaml index 9f2f835196..77a6ab547d 100644 --- a/src/modules/ekf2/module.yaml +++ b/src/modules/ekf2/module.yaml @@ -401,19 +401,18 @@ parameters: heading or 3-component vector. The fusion of magnetometer data as a three component vector enables vehicle body fixed hard iron errors to be learned, but requires a stable earth field. If set to 'Automatic' magnetic heading - fusion is used when on-ground and 3-axis magnetic field fusion in-flight - with fallback to magnetic heading fusion if there is insufficient motion - to make yaw or magnetic field states observable. If set to 'Magnetic heading' - magnetic heading fusion is used at all times. If set to 'None' the magnetometer - will not be used under any circumstance. If no external source of yaw is - available, it is possible to use post-takeoff horizontal movement combined - with GPS velocity measurements to align the yaw angle with the timer required - (depending on the amount of movement and GPS data quality). + fusion is used when on-ground and 3-axis magnetic field fusion in-flight. + If set to 'Magnetic heading' magnetic heading fusion is used at all times. + If set to 'None' the magnetometer will not be used under any circumstance. + If no external source of yaw is available, it is possible to use post-takeoff + horizontal movement combined with GNSS velocity measurements to align the yaw angle. + If set to 'Init' the magnetometer is only used to initalize the heading. type: enum values: 0: Automatic 1: Magnetic heading 5: None + 6: Init default: 0 reboot_required: true EKF2_MAG_ACCLIM: