diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 543d53eb881d..074d4112f20f 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -819,82 +819,6 @@ void EKF2::Run() void EKF2::VerifyParams() { -#if defined(CONFIG_EKF2_GNSS) - - 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()); - } - -#endif // CONFIG_EKF2_GNSS - -#if defined(CONFIG_EKF2_BAROMETER) - - 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()); - } - -#endif // CONFIG_EKF2_BAROMETER - -#if defined(CONFIG_EKF2_RANGE_FINDER) - - 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()); - } - -#endif // CONFIG_EKF2_RANGE_FINDER - -#if defined(CONFIG_EKF2_GNSS) - - 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()); - } - -#endif // CONFIG_EKF2_GNSS - -#if defined(CONFIG_EKF2_EXTERNAL_VISION) - - if ((_param_ekf2_hgt_ref.get() == HeightSensor::EV) - && !(_param_ekf2_ev_ctrl.get() & static_cast(EvCtrl::VPOS))) { - _param_ekf2_ev_ctrl.set(_param_ekf2_ev_ctrl.get() | static_cast(EvCtrl::VPOS)); - _param_ekf2_ev_ctrl.commit(); - mavlink_log_critical(&_mavlink_log_pub, "EV vertical position enabled by EKF2_HGT_REF\n"); - /* EVENT - * @description EKF2_EV_CTRL is set to {1:.0}. - */ - events::send(events::ID("ekf2_hgt_ref_ev"), events::Log::Warning, - "EV vertical position enabled by EKF2_HGT_REF", _param_ekf2_ev_ctrl.get()); - } - -#endif // CONFIG_EKF2_EXTERNAL_VISION - #if defined(CONFIG_EKF2_MAGNETOMETER) // EKF2_MAG_TYPE obsolete options