diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 880df1d4fe..1b72e12098 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -334,6 +334,9 @@ struct parameters { float req_hdrift{0.3f}; ///< maximum acceptable horizontal drift speed (m/s) float req_vdrift{0.5f}; ///< maximum acceptable vertical drift speed (m/s) + float gps_kill_t{0}; + + # if defined(CONFIG_EKF2_GNSS_YAW) // GNSS heading fusion float gps_heading_noise{0.1f}; ///< measurement noise standard deviation used for GNSS heading fusion (rad) diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index b6de89f544..8cc33310fc 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -84,6 +84,9 @@ void Ekf::reset() _control_status.flags.in_air = true; _control_status_prev.flags.in_air = true; + _time_last_on_ground_us = _time_delayed_us; + _time_last_in_air = _time_delayed_us; + _ang_rate_delayed_raw.zero(); _fault_status.value = 0; diff --git a/src/modules/ekf2/EKF/gps_control.cpp b/src/modules/ekf2/EKF/gps_control.cpp index 03cf0a326f..a4f64a6d70 100644 --- a/src/modules/ekf2/EKF/gps_control.cpp +++ b/src/modules/ekf2/EKF/gps_control.cpp @@ -85,6 +85,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) } updateGnssVel(gnss_sample, _aid_src_gnss_vel); + controlGnssYawEstimator(_aid_src_gnss_vel); } else if (_control_status.flags.gps) { if (!isNewestSampleRecent(_time_last_gps_buffer_push, _params.reset_timeout_max)) { @@ -94,14 +95,33 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) } } + +#if 1 + // GPS HACK + if (_control_status.flags.in_air + && _params.gps_kill_t > 0.f + && _time_last_on_ground_us > 0 + && _time_delayed_us > _time_last_on_ground_us + (1'000'000 * _params.gps_kill_t) + //&& false + ) { + _gps_data_ready = false; + // _control_status.flags.wind_dead_reckoning + _control_status.flags.gps = false; + return; + } else { + if (_time_last_on_ground_us > 0) { + //ECL_INFO("time: %llu, in_air:%d, gnd: %llu, ?:%d", _time_delayed_us, _control_status.flags.in_air, _time_last_on_ground_us, _time_delayed_us > _time_last_on_ground_us + (1'000'000 * 100)); + } + } +#endif + + if (_gps_data_ready) { #if defined(CONFIG_EKF2_GNSS_YAW) const gnssSample &gnss_sample = _gps_sample_delayed; controlGpsYawFusion(gnss_sample); #endif // CONFIG_EKF2_GNSS_YAW - controlGnssYawEstimator(_aid_src_gnss_vel); - const bool gnss_vel_enabled = (_params.gnss_ctrl & static_cast(GnssCtrl::VEL)); const bool gnss_pos_enabled = (_params.gnss_ctrl & static_cast(GnssCtrl::HPOS)); diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index dbfaa1ef64..f0c2edb017 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -93,6 +93,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config): _param_ekf2_req_pdop(_params->req_pdop), _param_ekf2_req_hdrift(_params->req_hdrift), _param_ekf2_req_vdrift(_params->req_vdrift), + _param_ekf2_gps_kill_t(_params->gps_kill_t), _param_ekf2_gsf_tas_default(_params->EKFGSF_tas_default), #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_BAROMETER) @@ -262,6 +263,14 @@ bool EKF2::multi_init(int imu, int mag) #endif // CONFIG_EKF2_BAROMETER +#if defined(CONFIG_EKF2_DRAG_FUSION) + + if (_param_ekf2_drag_ctrl.get()) { + _estimator_aid_src_drag_pub.advertise(); + } + +#endif // CONFIG_EKF2_DRAG_FUSION + #if defined(CONFIG_EKF2_EXTERNAL_VISION) // EV advertise diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 8e509c38b7..8be0318342 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -543,6 +543,10 @@ private: (ParamExtFloat) _param_ekf2_req_pdop, (ParamExtFloat) _param_ekf2_req_hdrift, (ParamExtFloat) _param_ekf2_req_vdrift, + + (ParamExtFloat) _param_ekf2_gps_kill_t, + + (ParamFloat) _param_ekf2_req_gps_h, // Used by EKF-GSF experimental yaw estimator diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index 396ba96515..efee306238 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -265,6 +265,17 @@ PARAM_DEFINE_FLOAT(EKF2_REQ_HDRIFT, 0.1f); */ PARAM_DEFINE_FLOAT(EKF2_REQ_VDRIFT, 0.2f); +/** + * GPS kill time (disabled if 0) + * + * @group EKF2 + * @min 0.0 + * @max 9999 + * @decimal 1 + * @unit s + */ +PARAM_DEFINE_FLOAT(EKF2_GPS_KILL_T, 0.0f); + /** * Rate gyro noise for covariance prediction. * diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index da01877c06..9001577971 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -160,7 +160,7 @@ void LoggedTopics::add_default_topics() #endif // always add the first instance -#if 0 +#if 1 add_topic_multi("estimator_baro_bias", 0, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_gnss_hgt_bias", 0, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_rng_hgt_bias", 0, MAX_ESTIMATOR_INSTANCES);