From 95ae5a657d900d80ed07772ff711ed5909d6140b Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 29 Apr 2024 11:37:16 -0400 Subject: [PATCH] ekf2: merge mag_3d_control + mag_control --- src/modules/ekf2/CMakeLists.txt | 1 - src/modules/ekf2/EKF/CMakeLists.txt | 1 - .../magnetometer/mag_3d_control.cpp | 225 ------------------ .../aid_sources/magnetometer/mag_control.cpp | 205 +++++++++++++++- src/modules/ekf2/EKF/ekf.h | 1 - 5 files changed, 196 insertions(+), 237 deletions(-) delete mode 100644 src/modules/ekf2/EKF/aid_sources/magnetometer/mag_3d_control.cpp diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 7e9fcb7abf..f21ce26d11 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -187,7 +187,6 @@ endif() if(CONFIG_EKF2_MAGNETOMETER) list(APPEND EKF_SRCS - EKF/aid_sources/magnetometer/mag_3d_control.cpp EKF/aid_sources/magnetometer/mag_control.cpp EKF/aid_sources/magnetometer/mag_fusion.cpp ) diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index 03c9a48166..fa57df04c7 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -109,7 +109,6 @@ endif() if(CONFIG_EKF2_MAGNETOMETER) list(APPEND EKF_SRCS - aid_sources/magnetometer/mag_3d_control.cpp aid_sources/magnetometer/mag_control.cpp aid_sources/magnetometer/mag_fusion.cpp ) diff --git a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_3d_control.cpp b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_3d_control.cpp deleted file mode 100644 index 41192f2cbf..0000000000 --- a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_3d_control.cpp +++ /dev/null @@ -1,225 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2023 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file mag_3d_control.cpp - * Control functions for ekf mag 3D fusion - */ - -#include "ekf.h" - -void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_starting_conditions_passing, - estimator_aid_source3d_s &aid_src) -{ - static constexpr const char *AID_SRC_NAME = "mag"; - - resetEstimatorAidStatus(aid_src); - - const bool wmm_updated = (_wmm_gps_time_last_set >= aid_src.time_last_fuse); // WMM update can occur on the last epoch, just after mag fusion - - // determine if we should use mag fusion - bool continuing_conditions_passing = (_params.mag_fusion_type != MagFuseType::NONE) - && _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) - && mag_sample.mag.isAllFinite(); - - const bool starting_conditions_passing = common_starting_conditions_passing - && continuing_conditions_passing; - - // For the first few seconds after in-flight alignment we allow the magnetic field state estimates to stabilise - // before they are used to constrain heading drift - _control_status.flags.mag_3D = (_params.mag_fusion_type == MagFuseType::AUTO) - && _control_status.flags.mag - && _control_status.flags.mag_aligned_in_flight - && (_control_status.flags.mag_heading_consistent || !_control_status.flags.gps) - && !_control_status.flags.mag_fault - && !_control_status.flags.ev_yaw - && !_control_status.flags.gps_yaw; - - const bool mag_consistent_or_no_gnss = _control_status.flags.mag_heading_consistent || !_control_status.flags.gps; - - _control_status.flags.mag_hdg = ((_params.mag_fusion_type == MagFuseType::HEADING) - || (_params.mag_fusion_type == MagFuseType::AUTO && !_control_status.flags.mag_3D)) - && _control_status.flags.tilt_align - && ((_control_status.flags.yaw_align && mag_consistent_or_no_gnss) - || (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align)) - && !_control_status.flags.mag_fault - && !_control_status.flags.mag_field_disturbed - && !_control_status.flags.ev_yaw - && !_control_status.flags.gps_yaw; - - // TODO: allow clearing mag_fault if mag_3d is good? - - if (_control_status.flags.mag_3D && !_control_status_prev.flags.mag_3D) { - ECL_INFO("starting mag 3D fusion"); - - } else if (!_control_status.flags.mag_3D && _control_status_prev.flags.mag_3D) { - ECL_INFO("stopping mag 3D fusion"); - } - - // if we are using 3-axis magnetometer fusion, but without external NE aiding, - // then the declination must be fused as an observation to prevent long term heading drift - // fusing declination when gps aiding is available is optional. - const bool not_using_ne_aiding = !_control_status.flags.gps && !_control_status.flags.aux_gpos; - _control_status.flags.mag_dec = (_control_status.flags.mag && (not_using_ne_aiding || !_control_status.flags.mag_aligned_in_flight)); - - if (_control_status.flags.mag) { - aid_src.timestamp_sample = mag_sample.time_us; - - if (continuing_conditions_passing && _control_status.flags.yaw_align) { - - if (mag_sample.reset || checkHaglYawResetReq() || wmm_updated) { - ECL_INFO("reset to %s", AID_SRC_NAME); - resetMagStates(_mag_lpf.getState(), _control_status.flags.mag_hdg || _control_status.flags.mag_3D); - aid_src.time_last_fuse = _time_delayed_us; - - } else { - if (!_mag_decl_cov_reset) { - // After any magnetic field covariance reset event the earth field state - // covariances need to be corrected to incorporate knowledge of the declination - // before fusing magnetometer data to prevent rapid rotation of the earth field - // states for the first few observations. - fuseDeclination(0.02f); - _mag_decl_cov_reset = true; - fuseMag(mag_sample.mag, aid_src, false); - - } else { - // The normal sequence is to fuse the magnetometer data first before fusing - // declination angle at a higher uncertainty to allow some learning of - // declination angle over time. - const bool update_all_states = _control_status.flags.mag_3D || _control_status.flags.mag_hdg; - const bool update_tilt = _control_status.flags.mag_3D; - fuseMag(mag_sample.mag, aid_src, update_all_states, update_tilt); - - if (_control_status.flags.mag_dec) { - fuseDeclination(0.5f); - } - } - } - - const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.reset_timeout_max); - - if (is_fusion_failing) { - if (_nb_mag_3d_reset_available > 0) { - // Data seems good, attempt a reset (mag states only unless mag_3D currently active) - ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME); - resetMagStates(_mag_lpf.getState(), _control_status.flags.mag_hdg || _control_status.flags.mag_3D); - aid_src.time_last_fuse = _time_delayed_us; - - if (_control_status.flags.in_air) { - _nb_mag_3d_reset_available--; - } - - } else if (starting_conditions_passing) { - // Data seems good, but previous reset did not fix the issue - // something else must be wrong, declare the sensor faulty and stop the fusion - //_control_status.flags.mag_fault = true; - ECL_WARN("stopping %s fusion, starting conditions failing", AID_SRC_NAME); - stopMagFusion(); - - } else { - // A reset did not fix the issue but all the starting checks are not passing - // This could be a temporary issue, stop the fusion without declaring the sensor faulty - ECL_WARN("stopping %s, fusion failing", AID_SRC_NAME); - stopMagFusion(); - } - } - - } else { - // Stop fusion but do not declare it faulty - ECL_DEBUG("stopping %s fusion, continuing conditions no longer passing", AID_SRC_NAME); - stopMagFusion(); - } - - } else { - if (starting_conditions_passing) { - - _control_status.flags.mag = true; - - // activate fusion, reset mag states and initialize variance if first init or in flight reset - if (!_control_status.flags.yaw_align - || wmm_updated - || !_mag_decl_cov_reset - || !_state.mag_I.longerThan(0.f) - || (getStateVariance().min() < kMagVarianceMin) - || (getStateVariance().min() < kMagVarianceMin) - ) { - ECL_INFO("starting %s fusion, resetting states", AID_SRC_NAME); - - bool reset_heading = !_control_status.flags.yaw_align; - - resetMagStates(_mag_lpf.getState(), reset_heading); - - if (reset_heading) { - _control_status.flags.yaw_align = true; - } - - } else { - ECL_INFO("starting %s fusion", AID_SRC_NAME); - fuseMag(mag_sample.mag, aid_src, false); - } - - aid_src.time_last_fuse = _time_delayed_us; - - _nb_mag_3d_reset_available = 2; - } - } -} - -void Ekf::stopMagFusion() -{ - if (_control_status.flags.mag) { - ECL_INFO("stopping mag fusion"); - - _control_status.flags.mag = false; - _control_status.flags.mag_dec = false; - - if (_control_status.flags.mag_3D) { - ECL_INFO("stopping mag 3D fusion"); - _control_status.flags.mag_3D = false; - } - - if (_control_status.flags.mag_hdg) { - ECL_INFO("stopping mag heading fusion"); - _control_status.flags.mag_hdg = false; - _fault_status.flags.bad_hdg = false; - } - - _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; - } -} 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 5497d427f4..936d141eb0 100644 --- a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp @@ -41,6 +41,9 @@ void Ekf::controlMagFusion() { + static constexpr const char *AID_SRC_NAME = "mag"; + estimator_aid_source3d_s &aid_src = _aid_src_mag; + // reset the flight alignment flag so that the mag fields will be // re-initialised next time we achieve flight altitude if (!_control_status_prev.flags.in_air && _control_status.flags.in_air) { @@ -65,8 +68,10 @@ void Ekf::controlMagFusion() if (_mag_buffer && _mag_buffer->pop_first_older_than(_time_delayed_us, &mag_sample)) { + resetEstimatorAidStatus(aid_src); + if (mag_sample.reset || (_mag_counter == 0)) { - // sensor or calibration has changed, reset low pass filter (reset handled by controlMag3DFusion/controlMagHeadingFusion) + // sensor or calibration has changed, reset low pass filter _control_status.flags.mag_fault = false; _state.mag_B.zero(); @@ -80,13 +85,6 @@ void Ekf::controlMagFusion() _mag_counter++; } - const bool starting_conditions_passing = (_params.mag_fusion_type != MagFuseType::NONE) - && checkMagField(mag_sample.mag) - && (_mag_counter > 5) // wait until we have more than a few samples through the filter - && (_control_status.flags.yaw_align == _control_status_prev.flags.yaw_align) // no yaw alignment change this frame - && (_state_reset_status.reset_count.quat == _state_reset_count_prev.quat) // don't allow starting on same frame as yaw reset - && isNewestSampleRecent(_time_last_mag_buffer_push, MAG_MAX_INTERVAL); - // if enabled, use knowledge of theoretical magnetic field vector to calculate a synthetic magnetomter Z component value. // this is useful if there is a lot of interference on the sensor measurement. if (_params.synthesize_mag_z && (_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL) @@ -103,8 +101,166 @@ void Ekf::controlMagFusion() _control_status.flags.synthetic_mag_z = false; } + // determine if we should use mag fusion + bool continuing_conditions_passing = (_params.mag_fusion_type != MagFuseType::NONE) + && _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) + && mag_sample.mag.isAllFinite(); + + const bool starting_conditions_passing = continuing_conditions_passing + && checkMagField(mag_sample.mag) + && (_mag_counter > 5) // wait until we have more than a few samples through the filter + && (_control_status.flags.yaw_align == _control_status_prev.flags.yaw_align) // no yaw alignment change this frame + && (_state_reset_status.reset_count.quat == _state_reset_count_prev.quat) // don't allow starting on same frame as yaw reset + && isNewestSampleRecent(_time_last_mag_buffer_push, MAG_MAX_INTERVAL); + + checkMagHeadingConsistency(mag_sample); - controlMag3DFusion(mag_sample, starting_conditions_passing, _aid_src_mag); + + // WMM update can occur on the last epoch, just after mag fusion + const bool wmm_updated = (_wmm_gps_time_last_set >= aid_src.time_last_fuse); + + + // For the first few seconds after in-flight alignment we allow the magnetic field state estimates to stabilise + // before they are used to constrain heading drift + _control_status.flags.mag_3D = (_params.mag_fusion_type == MagFuseType::AUTO) + && _control_status.flags.mag + && _control_status.flags.mag_aligned_in_flight + && (_control_status.flags.mag_heading_consistent || !_control_status.flags.gps) + && !_control_status.flags.mag_fault + && !_control_status.flags.ev_yaw + && !_control_status.flags.gps_yaw; + + const bool mag_consistent_or_no_gnss = _control_status.flags.mag_heading_consistent || !_control_status.flags.gps; + + _control_status.flags.mag_hdg = ((_params.mag_fusion_type == MagFuseType::HEADING) + || (_params.mag_fusion_type == MagFuseType::AUTO && !_control_status.flags.mag_3D)) + && _control_status.flags.tilt_align + && ((_control_status.flags.yaw_align && mag_consistent_or_no_gnss) + || (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align)) + && !_control_status.flags.mag_fault + && !_control_status.flags.mag_field_disturbed + && !_control_status.flags.ev_yaw + && !_control_status.flags.gps_yaw; + + // TODO: allow clearing mag_fault if mag_3d is good? + + if (_control_status.flags.mag_3D && !_control_status_prev.flags.mag_3D) { + ECL_INFO("starting mag 3D fusion"); + + } else if (!_control_status.flags.mag_3D && _control_status_prev.flags.mag_3D) { + ECL_INFO("stopping mag 3D fusion"); + } + + // if we are using 3-axis magnetometer fusion, but without external NE aiding, + // then the declination must be fused as an observation to prevent long term heading drift + // fusing declination when gps aiding is available is optional. + const bool not_using_ne_aiding = !_control_status.flags.gps && !_control_status.flags.aux_gpos; + _control_status.flags.mag_dec = _control_status.flags.mag + && (not_using_ne_aiding || !_control_status.flags.mag_aligned_in_flight); + + if (_control_status.flags.mag) { + aid_src.timestamp_sample = mag_sample.time_us; + + if (continuing_conditions_passing && _control_status.flags.yaw_align) { + + if (mag_sample.reset || checkHaglYawResetReq() || wmm_updated) { + ECL_INFO("reset to %s", AID_SRC_NAME); + resetMagStates(_mag_lpf.getState(), _control_status.flags.mag_hdg || _control_status.flags.mag_3D); + aid_src.time_last_fuse = _time_delayed_us; + + } else { + if (!_mag_decl_cov_reset) { + // After any magnetic field covariance reset event the earth field state + // covariances need to be corrected to incorporate knowledge of the declination + // before fusing magnetometer data to prevent rapid rotation of the earth field + // states for the first few observations. + fuseDeclination(0.02f); + _mag_decl_cov_reset = true; + fuseMag(mag_sample.mag, aid_src, false); + + } else { + // The normal sequence is to fuse the magnetometer data first before fusing + // declination angle at a higher uncertainty to allow some learning of + // declination angle over time. + const bool update_all_states = _control_status.flags.mag_3D || _control_status.flags.mag_hdg; + const bool update_tilt = _control_status.flags.mag_3D; + fuseMag(mag_sample.mag, aid_src, update_all_states, update_tilt); + + if (_control_status.flags.mag_dec) { + fuseDeclination(0.5f); + } + } + } + + const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.reset_timeout_max); + + if (is_fusion_failing) { + if (_nb_mag_3d_reset_available > 0) { + // Data seems good, attempt a reset (mag states only unless mag_3D currently active) + ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME); + resetMagStates(_mag_lpf.getState(), _control_status.flags.mag_hdg || _control_status.flags.mag_3D); + aid_src.time_last_fuse = _time_delayed_us; + + if (_control_status.flags.in_air) { + _nb_mag_3d_reset_available--; + } + + } else if (starting_conditions_passing) { + // Data seems good, but previous reset did not fix the issue + // something else must be wrong, declare the sensor faulty and stop the fusion + //_control_status.flags.mag_fault = true; + ECL_WARN("stopping %s fusion, starting conditions failing", AID_SRC_NAME); + stopMagFusion(); + + } else { + // A reset did not fix the issue but all the starting checks are not passing + // This could be a temporary issue, stop the fusion without declaring the sensor faulty + ECL_WARN("stopping %s, fusion failing", AID_SRC_NAME); + stopMagFusion(); + } + } + + } else { + // Stop fusion but do not declare it faulty + ECL_DEBUG("stopping %s fusion, continuing conditions no longer passing", AID_SRC_NAME); + stopMagFusion(); + } + + } else { + if (starting_conditions_passing) { + + _control_status.flags.mag = true; + + // activate fusion, reset mag states and initialize variance if first init or in flight reset + if (!_control_status.flags.yaw_align + || wmm_updated + || !_mag_decl_cov_reset + || !_state.mag_I.longerThan(0.f) + || (getStateVariance().min() < kMagVarianceMin) + || (getStateVariance().min() < kMagVarianceMin) + ) { + ECL_INFO("starting %s fusion, resetting states", AID_SRC_NAME); + + bool reset_heading = !_control_status.flags.yaw_align; + + resetMagStates(_mag_lpf.getState(), reset_heading); + + if (reset_heading) { + _control_status.flags.yaw_align = true; + } + + } else { + ECL_INFO("starting %s fusion", AID_SRC_NAME); + fuseMag(mag_sample.mag, aid_src, false); + } + + aid_src.time_last_fuse = _time_delayed_us; + + _nb_mag_3d_reset_available = 2; + } + } } else if (!isNewestSampleRecent(_time_last_mag_buffer_push, 2 * MAG_MAX_INTERVAL)) { // No data anymore. Stop until it comes back. @@ -112,9 +268,37 @@ void Ekf::controlMagFusion() } } +void Ekf::stopMagFusion() +{ + if (_control_status.flags.mag) { + ECL_INFO("stopping mag fusion"); + + _control_status.flags.mag = false; + _control_status.flags.mag_dec = false; + + if (_control_status.flags.mag_3D) { + ECL_INFO("stopping mag 3D fusion"); + _control_status.flags.mag_3D = false; + } + + if (_control_status.flags.mag_hdg) { + ECL_INFO("stopping mag heading fusion"); + _control_status.flags.mag_hdg = false; + _fault_status.flags.bad_hdg = false; + } + + _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; + } +} + bool Ekf::checkHaglYawResetReq() const { #if defined(CONFIG_EKF2_TERRAIN) + // We need to reset the yaw angle after climbing away from the ground to enable // recovery from ground level magnetic interference. if (_control_status.flags.in_air && _control_status.flags.yaw_align && !_control_status.flags.mag_aligned_in_flight) { @@ -124,6 +308,7 @@ bool Ekf::checkHaglYawResetReq() const const bool above_mag_anomalies = (getTerrainVPos() - _state.pos(2)) > mag_anomalies_max_hagl; return above_mag_anomalies; } + #endif // CONFIG_EKF2_TERRAIN return false; @@ -147,6 +332,7 @@ void Ekf::resetMagStates(const Vector3f &mag, bool reset_heading) // mag_B: reset #if defined(CONFIG_EKF2_GNSS) + if (isYawEmergencyEstimateAvailable()) { const Dcmf R_to_earth = updateYawInRotMat(_yawEstimator.getYaw(), _R_to_earth); @@ -159,6 +345,7 @@ void Ekf::resetMagStates(const Vector3f &mag, bool reset_heading) } else if (!reset_heading && _control_status.flags.yaw_align) { #else + if (!reset_heading && _control_status.flags.yaw_align) { #endif // mag_B: reset using WMM diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 77ddfc7214..91b4682d82 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -1025,7 +1025,6 @@ private: #if defined(CONFIG_EKF2_MAGNETOMETER) // control fusion of magnetometer observations void controlMagFusion(); - void controlMag3DFusion(const magSample &mag_sample, const bool common_starting_conditions_passing, estimator_aid_source3d_s &aid_src); bool checkHaglYawResetReq() const;