ekf2: add EKF2_GPS_KILL_T parameter (for replay) and restore logged topics

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)

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;

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<int32_t>(GnssCtrl::VEL));
 		const bool gnss_pos_enabled = (_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::HPOS));
 

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

src/modules/ekf2/EKF2.hpp

@@ -543,6 +543,10 @@ private:
 		(ParamExtFloat<px4::params::EKF2_REQ_PDOP>)   _param_ekf2_req_pdop,
 		(ParamExtFloat<px4::params::EKF2_REQ_HDRIFT>) _param_ekf2_req_hdrift,
 		(ParamExtFloat<px4::params::EKF2_REQ_VDRIFT>) _param_ekf2_req_vdrift,
+
+		(ParamExtFloat<px4::params::EKF2_GPS_KILL_T>) _param_ekf2_gps_kill_t,
+
+
 		(ParamFloat<px4::params::EKF2_REQ_GPS_H>)     _param_ekf2_req_gps_h,
 
 		// Used by EKF-GSF experimental yaw estimator

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.
  *

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);