From 54e5dd788b9b526ae903645da21e1564efef9d7d Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 17 Oct 2023 14:08:24 -0400 Subject: [PATCH 1/2] ekf2: fully disable yaw estimator EKFGSF_yaw with CONFIG_EKF2_GNSS --- src/modules/ekf2/EKF/CMakeLists.txt | 2 +- src/modules/ekf2/EKF/airspeed_fusion.cpp | 9 +++- src/modules/ekf2/EKF/ekf.h | 44 +++++++++------- src/modules/ekf2/EKF/ekf_helper.cpp | 50 ------------------ src/modules/ekf2/EKF/gps_control.cpp | 66 +++++++++++++++++++----- src/modules/ekf2/EKF/mag_control.cpp | 7 ++- src/modules/ekf2/EKF2.hpp | 2 + 7 files changed, 94 insertions(+), 86 deletions(-) diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index 23ebe86cbdf8..e5712402ba45 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -38,7 +38,6 @@ list(APPEND EKF_SRCS covariance.cpp ekf.cpp ekf_helper.cpp - EKFGSF_yaw.cpp estimator_interface.cpp fake_height_control.cpp fake_pos_control.cpp @@ -86,6 +85,7 @@ if(CONFIG_EKF2_GNSS) gnss_height_control.cpp gps_checks.cpp gps_control.cpp + EKFGSF_yaw.cpp ) endif() diff --git a/src/modules/ekf2/EKF/airspeed_fusion.cpp b/src/modules/ekf2/EKF/airspeed_fusion.cpp index f0bf82223100..fde836232b34 100644 --- a/src/modules/ekf2/EKF/airspeed_fusion.cpp +++ b/src/modules/ekf2/EKF/airspeed_fusion.cpp @@ -62,6 +62,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed) _control_status.flags.wind = false; } +#if defined(CONFIG_EKF2_GNSS) // clear yaw estimator airspeed (updated later with true airspeed if airspeed fusion is active) if (_control_status.flags.fixed_wing) { if (_control_status.flags.in_air && !_control_status.flags.vehicle_at_rest) { @@ -73,6 +74,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed) _yawEstimator.setTrueAirspeed(0.f); } } +#endif // CONFIG_EKF2_GNSS if (_params.arsp_thr <= 0.f) { stopAirspeedFusion(); @@ -99,7 +101,9 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed) fuseAirspeed(airspeed_sample, _aid_src_airspeed); } +#if defined(CONFIG_EKF2_GNSS) _yawEstimator.setTrueAirspeed(airspeed_sample.true_airspeed); +#endif // CONFIG_EKF2_GNSS const bool is_fusion_failing = isTimedOut(_aid_src_airspeed.time_last_fuse, (uint64_t)10e6); @@ -219,8 +223,11 @@ void Ekf::stopAirspeedFusion() if (_control_status.flags.fuse_aspd) { ECL_INFO("stopping airspeed fusion"); resetEstimatorAidStatus(_aid_src_airspeed); - _yawEstimator.setTrueAirspeed(NAN); _control_status.flags.fuse_aspd = false; + +#if defined(CONFIG_EKF2_GNSS) + _yawEstimator.setTrueAirspeed(NAN); +#endif // CONFIG_EKF2_GNSS } } diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 1691b9a7aa50..265650333f9a 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -45,7 +45,10 @@ #include "estimator_interface.h" -#include "EKFGSF_yaw.h" +#if defined(CONFIG_EKF2_GNSS) +# include "EKFGSF_yaw.h" +#endif // CONFIG_EKF2_GNSS + #include "bias_estimator.hpp" #include "height_bias_estimator.hpp" #include "position_bias_estimator.hpp" @@ -488,9 +491,6 @@ class Ekf final : public EstimatorInterface Vector3f calcRotVecVariances() const; float getYawVar() const; - // Returns true if the output of the yaw emergency estimator can be used for a reset - bool isYawEmergencyEstimateAvailable() const; - uint8_t getHeightSensorRef() const { return _height_sensor_ref; } #if defined(CONFIG_EKF2_EXTERNAL_VISION) @@ -533,11 +533,6 @@ class Ekf final : public EstimatorInterface bool gps_checks_passed() const { return _gps_checks_passed; }; - // get solution data from the EKF-GSF emergency yaw estimator - // returns false when data is not available - bool getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF], - float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]); - const BiasEstimator::status &getGpsHgtBiasEstimatorStatus() const { return _gps_hgt_b_est.getStatus(); } const auto &aid_src_gnss_hgt() const { return _aid_src_gnss_hgt; } @@ -547,6 +542,15 @@ class Ekf final : public EstimatorInterface # if defined(CONFIG_EKF2_GNSS_YAW) const auto &aid_src_gnss_yaw() const { return _aid_src_gnss_yaw; } # endif // CONFIG_EKF2_GNSS_YAW + + // Returns true if the output of the yaw emergency estimator can be used for a reset + bool isYawEmergencyEstimateAvailable() const; + + // get solution data from the EKF-GSF emergency yaw estimator + // returns false when data is not available + bool getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF], + float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]); + #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_MAGNETOMETER) @@ -1086,7 +1090,6 @@ class Ekf final : public EstimatorInterface void stopGpsFusion(); bool shouldResetGpsFusion() const; - bool isYawFailure() const; // return true id the GPS quality is good enough to set an origin and start aiding bool gps_is_good(const gpsMessage &gps); @@ -1094,11 +1097,6 @@ class Ekf final : public EstimatorInterface void controlGnssHeightFusion(const gpsSample &gps_sample); void stopGpsHgtFusion(); - // Resets the main Nav EKf yaw to the estimator from the EKF-GSF yaw estimator - // Resets the horizontal velocity and position to the default navigation sensor - // Returns true if the reset was successful - bool resetYawToEKFGSF(); - void resetGpsDriftCheckFilters(); # if defined(CONFIG_EKF2_GNSS_YAW) @@ -1115,6 +1113,17 @@ class Ekf final : public EstimatorInterface void updateGpsYaw(const gpsSample &gps_sample); # endif // CONFIG_EKF2_GNSS_YAW + + // Declarations used to control use of the EKF-GSF yaw estimator + bool isYawFailure() const; + + // Resets the main Nav EKf yaw to the estimator from the EKF-GSF yaw estimator + // Returns true if the reset was successful + bool resetYawToEKFGSF(); + + // yaw estimator instance + EKFGSF_yaw _yawEstimator{}; + #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_MAGNETOMETER) @@ -1231,11 +1240,6 @@ class Ekf final : public EstimatorInterface // yaw_variance : yaw error variance (rad^2) void resetQuatStateYaw(float yaw, float yaw_variance); - // Declarations used to control use of the EKF-GSF yaw estimator - - // yaw estimator instance - EKFGSF_yaw _yawEstimator{}; - uint8_t _height_sensor_ref{HeightSensor::UNKNOWN}; uint8_t _position_sensor_ref{static_cast(PositionSensor::GNSS)}; diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 76360fb018bf..e2eb52f3d38d 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -1028,56 +1028,6 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance) _time_last_heading_fuse = _time_delayed_us; } -#if defined(CONFIG_EKF2_GNSS) -bool Ekf::resetYawToEKFGSF() -{ - if (!isYawEmergencyEstimateAvailable()) { - return false; - } - - // don't allow reset if there's just been a yaw reset - const bool yaw_alignment_changed = (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align); - const bool quat_reset = (_state_reset_status.reset_count.quat != _state_reset_count_prev.quat); - - if (yaw_alignment_changed || quat_reset) { - return false; - } - - ECL_INFO("yaw estimator reset heading %.3f -> %.3f rad", - (double)getEulerYaw(_R_to_earth), (double)_yawEstimator.getYaw()); - - resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar()); - - _control_status.flags.yaw_align = true; - _information_events.flags.yaw_aligned_to_imu_gps = true; - - return true; -} -#endif // CONFIG_EKF2_GNSS - -bool Ekf::isYawEmergencyEstimateAvailable() const -{ -#if defined(CONFIG_EKF2_GNSS) - // don't allow reet using the EKF-GSF estimate until the filter has started fusing velocity - // data and the yaw estimate has converged - if (!_yawEstimator.isActive()) { - return false; - } - - return _yawEstimator.getYawVar() < sq(_params.EKFGSF_yaw_err_max); -#else - return false; -#endif -} - -#if defined(CONFIG_EKF2_GNSS) -bool Ekf::getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF], - float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) -{ - return _yawEstimator.getLogData(yaw_composite, yaw_variance, yaw, innov_VN, innov_VE, weight); -} -#endif // CONFIG_EKF2_GNSS - #if defined(CONFIG_EKF2_WIND) void Ekf::resetWind() { diff --git a/src/modules/ekf2/EKF/gps_control.cpp b/src/modules/ekf2/EKF/gps_control.cpp index 2f25153ccce1..a5671b111e70 100644 --- a/src/modules/ekf2/EKF/gps_control.cpp +++ b/src/modules/ekf2/EKF/gps_control.cpp @@ -302,18 +302,6 @@ bool Ekf::shouldResetGpsFusion() const return (is_reset_required || is_inflight_nav_failure); } -bool Ekf::isYawFailure() const -{ - if (!isYawEmergencyEstimateAvailable()) { - return false; - } - - const float euler_yaw = getEulerYaw(_R_to_earth); - const float yaw_error = wrap_pi(euler_yaw - _yawEstimator.getYaw()); - - return fabsf(yaw_error) > math::radians(25.f); -} - #if defined(CONFIG_EKF2_GNSS_YAW) void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passing, bool gps_checks_failing) { @@ -436,3 +424,57 @@ void Ekf::stopGpsFusion() stopGpsYawFusion(); #endif // CONFIG_EKF2_GNSS_YAW } + +bool Ekf::isYawEmergencyEstimateAvailable() const +{ + // don't allow reet using the EKF-GSF estimate until the filter has started fusing velocity + // data and the yaw estimate has converged + if (!_yawEstimator.isActive()) { + return false; + } + + return _yawEstimator.getYawVar() < sq(_params.EKFGSF_yaw_err_max); +} + +bool Ekf::isYawFailure() const +{ + if (!isYawEmergencyEstimateAvailable()) { + return false; + } + + const float euler_yaw = getEulerYaw(_R_to_earth); + const float yaw_error = wrap_pi(euler_yaw - _yawEstimator.getYaw()); + + return fabsf(yaw_error) > math::radians(25.f); +} + +bool Ekf::resetYawToEKFGSF() +{ + if (!isYawEmergencyEstimateAvailable()) { + return false; + } + + // don't allow reset if there's just been a yaw reset + const bool yaw_alignment_changed = (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align); + const bool quat_reset = (_state_reset_status.reset_count.quat != _state_reset_count_prev.quat); + + if (yaw_alignment_changed || quat_reset) { + return false; + } + + ECL_INFO("yaw estimator reset heading %.3f -> %.3f rad", + (double)getEulerYaw(_R_to_earth), (double)_yawEstimator.getYaw()); + + resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar()); + + _control_status.flags.yaw_align = true; + _information_events.flags.yaw_aligned_to_imu_gps = true; + + return true; +} + +bool Ekf::getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF], + float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) +{ + return _yawEstimator.getLogData(yaw_composite, yaw_variance, yaw, innov_VN, innov_VE, weight); +} diff --git a/src/modules/ekf2/EKF/mag_control.cpp b/src/modules/ekf2/EKF/mag_control.cpp index 0dacfad3e101..524e61379c73 100644 --- a/src/modules/ekf2/EKF/mag_control.cpp +++ b/src/modules/ekf2/EKF/mag_control.cpp @@ -155,7 +155,9 @@ void Ekf::resetMagStates(const Vector3f &mag, bool reset_heading) * Vector3f(_mag_strength_gps, 0, 0); // mag_B: reset +#if defined(CONFIG_EKF2_GNSS) if (isYawEmergencyEstimateAvailable()) { + const Dcmf R_to_earth = updateYawInRotMat(_yawEstimator.getYaw(), _R_to_earth); const Dcmf R_to_body = R_to_earth.transpose(); @@ -163,8 +165,9 @@ void Ekf::resetMagStates(const Vector3f &mag, bool reset_heading) _state.mag_B = mag - (R_to_body * mag_earth_pred); ECL_INFO("resetMagStates using yaw estimator"); - - } else if (!reset_heading && _control_status.flags.yaw_align) { + } else +#endif // CONFIG_EKF2_GNSS + if (!reset_heading && _control_status.flags.yaw_align) { // mag_B: reset using WMM const Dcmf R_to_body = quatToInverseRotMat(_state.quat_nominal); _state.mag_B = mag - (R_to_body * mag_earth_pred); diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 2465b774cc46..1d78dfd3101e 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -511,7 +511,9 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem uORB::PublicationMulti _estimator_aid_src_gnss_hgt_pub{ORB_ID(estimator_aid_src_gnss_hgt)}; uORB::PublicationMulti _estimator_aid_src_gnss_pos_pub{ORB_ID(estimator_aid_src_gnss_pos)}; uORB::PublicationMulti _estimator_aid_src_gnss_vel_pub{ORB_ID(estimator_aid_src_gnss_vel)}; + uORB::PublicationMulti _yaw_est_pub{ORB_ID(yaw_estimator_status)}; + # if defined(CONFIG_EKF2_GNSS_YAW) hrt_abstime _status_gnss_yaw_pub_last {0}; uORB::PublicationMulti _estimator_aid_src_gnss_yaw_pub {ORB_ID(estimator_aid_src_gnss_yaw)}; From f59592a87a9b92e529ba936b7b5f3a52d15176e8 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 18 Oct 2023 10:56:05 -0400 Subject: [PATCH 2/2] make clang-tidy happy --- src/modules/ekf2/EKF/mag_control.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf2/EKF/mag_control.cpp b/src/modules/ekf2/EKF/mag_control.cpp index 524e61379c73..cd714e91ecc8 100644 --- a/src/modules/ekf2/EKF/mag_control.cpp +++ b/src/modules/ekf2/EKF/mag_control.cpp @@ -165,9 +165,11 @@ void Ekf::resetMagStates(const Vector3f &mag, bool reset_heading) _state.mag_B = mag - (R_to_body * mag_earth_pred); ECL_INFO("resetMagStates using yaw estimator"); - } else -#endif // CONFIG_EKF2_GNSS + + } else if (!reset_heading && _control_status.flags.yaw_align) { +#else if (!reset_heading && _control_status.flags.yaw_align) { +#endif // mag_B: reset using WMM const Dcmf R_to_body = quatToInverseRotMat(_state.quat_nominal); _state.mag_B = mag - (R_to_body * mag_earth_pred);