From a2a29ba0ddd64d5b06201708dfee341c2e8ee52e Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 12 Aug 2022 16:17:18 +0200 Subject: [PATCH] ekf2: verify parameter configuration for height sources --- src/modules/ekf2/EKF/common.h | 4 +-- src/modules/ekf2/EKF2.cpp | 63 +++++++++++++++++++++++++++++++++++ src/modules/ekf2/EKF2.hpp | 5 +++ 3 files changed, 70 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index eae10b8778..d3f5da12b0 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -128,14 +128,14 @@ enum RngCtrl : uint8_t { enum SensorFusionMask : uint16_t { // Bit locations for fusion_mode - /* USE_GPS = (1<<0), ///< set to true to use GPS data */ + DEPRECATED_USE_GPS = (1<<0), ///< set to true to use GPS data (DEPRECATED, use gnss_ctrl) USE_OPT_FLOW = (1<<1), ///< set to true to use optical flow data INHIBIT_ACC_BIAS = (1<<2), ///< set to true to inhibit estimation of accelerometer delta velocity bias USE_EXT_VIS_POS = (1<<3), ///< set to true to use external vision position data USE_EXT_VIS_YAW = (1<<4), ///< set to true to use external vision quaternion data for yaw USE_DRAG = (1<<5), ///< set to true to use the multi-rotor drag model to estimate wind ROTATE_EXT_VIS = (1<<6), ///< set to true to if the EV observations are in a non NED reference frame and need to be rotated before being used - /* USE_GPS_YAW = (1<<7), ///< set to true to use GPS yaw data if available */ + DEPRECATED_USE_GPS_YAW = (1<<7),///< set to true to use GPS yaw data if available (DEPRECATED, use gnss_ctrl) USE_EXT_VIS_VEL = (1<<8), ///< set to true to use external vision velocity data }; diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index b0be3c8a9d..0efae65908 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -31,6 +31,7 @@ * ****************************************************************************/ +#include #include "EKF2.hpp" using namespace time_literals; @@ -284,6 +285,8 @@ void EKF2::Run() // update parameters from storage updateParams(); + VerifyParams(); + _ekf.set_min_required_gps_health_time(_param_ekf2_req_gps_h.get() * 1_s); // The airspeed scale factor correcton is only available via parameter as used by the airspeed module @@ -643,6 +646,66 @@ void EKF2::Run() ScheduleDelayed(100_ms); } +void EKF2::VerifyParams() +{ + if ((_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_USE_GPS) + || (_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_USE_GPS_YAW)) { + _param_ekf2_aid_mask.set(_param_ekf2_aid_mask.get() & ~(SensorFusionMask::DEPRECATED_USE_GPS | + SensorFusionMask::DEPRECATED_USE_GPS_YAW)); + _param_ekf2_aid_mask.commit(); + mavlink_log_critical(&_mavlink_log_pub, "Use EKF2_GPS_CTRL instead\n"); + /* EVENT + * @description EKF2_AID_MASK is set to {1:.0}. + */ + events::send(events::ID("ekf2_aid_mask_gps"), events::Log::Warning, + "Use EKF2_GPS_CTRL instead", _param_ekf2_aid_mask.get()); + } + + if ((_param_ekf2_gps_ctrl.get() & GnssCtrl::VPOS) && !(_param_ekf2_gps_ctrl.get() & GnssCtrl::HPOS)) { + _param_ekf2_gps_ctrl.set(_param_ekf2_gps_ctrl.get() & ~GnssCtrl::VPOS); + _param_ekf2_gps_ctrl.commit(); + mavlink_log_critical(&_mavlink_log_pub, "GPS lon/lat is required for altitude fusion\n"); + /* EVENT + * @description EKF2_GPS_CTRL is set to {1:.0}. + */ + events::send(events::ID("ekf2_gps_ctrl_alt"), events::Log::Warning, + "GPS lon/lat is required for altitude fusion", _param_ekf2_gps_ctrl.get()); + } + + if ((_param_ekf2_hgt_ref.get() == HeightSensor::BARO) && (_param_ekf2_baro_ctrl.get() == 0)) { + _param_ekf2_baro_ctrl.set(1); + _param_ekf2_baro_ctrl.commit(); + mavlink_log_critical(&_mavlink_log_pub, "Baro enabled by EKF2_HGT_REF\n"); + /* EVENT + * @description EKF2_BARO_CTRL is set to {1:.0}. + */ + events::send(events::ID("ekf2_hgt_ref_baro"), events::Log::Warning, + "Baro enabled by EKF2_HGT_REF", _param_ekf2_baro_ctrl.get()); + } + + if ((_param_ekf2_hgt_ref.get() == HeightSensor::RANGE) && (_param_ekf2_rng_ctrl.get() == RngCtrl::DISABLED)) { + _param_ekf2_rng_ctrl.set(1); + _param_ekf2_rng_ctrl.commit(); + mavlink_log_critical(&_mavlink_log_pub, "Range enabled by EKF2_HGT_REF\n"); + /* EVENT + * @description EKF2_RNG_CTRL is set to {1:.0}. + */ + events::send(events::ID("ekf2_hgt_ref_rng"), events::Log::Warning, + "Range enabled by EKF2_HGT_REF", _param_ekf2_rng_ctrl.get()); + } + + if ((_param_ekf2_hgt_ref.get() == HeightSensor::GNSS) && !(_param_ekf2_gps_ctrl.get() & GnssCtrl::VPOS)) { + _param_ekf2_gps_ctrl.set(_param_ekf2_gps_ctrl.get() | (GnssCtrl::VPOS | GnssCtrl::HPOS | GnssCtrl::VEL)); + _param_ekf2_gps_ctrl.commit(); + mavlink_log_critical(&_mavlink_log_pub, "GPS enabled by EKF2_HGT_REF\n"); + /* EVENT + * @description EKF2_GPS_CTRL is set to {1:.0}. + */ + events::send(events::ID("ekf2_hgt_ref_gps"), events::Log::Warning, + "GPS enabled by EKF2_HGT_REF", _param_ekf2_gps_ctrl.get()); + } +} + void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp) { // airspeed diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 283b7df7ef..c80831820c 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -52,6 +52,7 @@ #include #include #include +#include #include #include #include @@ -134,6 +135,8 @@ private: void Run() override; + void VerifyParams(); + void PublishAidSourceStatus(const hrt_abstime ×tamp); void PublishAttitude(const hrt_abstime ×tamp); void PublishBaroBias(const hrt_abstime ×tamp); @@ -281,6 +284,8 @@ private: float _airspeed_scale_factor{1.0f}; ///< scale factor correction applied to airspeed measurements hrt_abstime _airspeed_validated_timestamp_last{0}; + orb_advert_t _mavlink_log_pub{nullptr}; + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Subscription _airdata_sub{ORB_ID(vehicle_air_data)};