fake EV: add param to enable, drift and stale data

src/modules/fake_ev/fake_ev.cpp

@@ -107,10 +107,22 @@ vehicle_odometry_s FakeEV::gpsToOdom(const sensor_gps_s &gps)
 	/* odom.timestamp_sample = gps.timestamp_sample; */ // gps timestamp_sample isn't set in SITL
 	odom.timestamp_sample = gps.timestamp;
 
+	const hrt_abstime now = odom.timestamp_sample;
+	const float dt = math::constrain(((now - _last_run) * 1e-6f), 0.000125f, 0.5f);
+	_last_run = now;
+
 	if (gps.fix_type >= 3) {
 		if (PX4_ISFINITE(_alt_ref)) {
-			const matrix::Vector2f hpos = _pos_ref.project(gps.lat / 1.0e7, gps.lon / 1.0e7);
-			const float vpos = (float)gps.alt * 1e-3f - _alt_ref;
+			matrix::Vector2f hpos = _hpos_prev;
+
+			if (!_param_fev_stale.get()) {
+				hpos = _pos_ref.project(gps.lat / 1.0e7, gps.lon / 1.0e7);
+				_hpos_prev = hpos;
+			}
+
+			_h_drift += _param_fev_h_drift_rate.get() * dt;
+			hpos += matrix::Vector2f(_h_drift, _h_drift);
+			const float vpos = -((float)gps.alt * 1e-3f - _alt_ref);
 			matrix::Vector3f(hpos(0), hpos(1), vpos).copyTo(odom.position);
 
 			const float hvar = std::pow(gps.eph, 2.f);

src/modules/fake_ev/fake_ev.hpp

@@ -108,5 +108,18 @@ private:
 	MapProjection _pos_ref{}; // Contains WGS-84 position latitude and longitude
 	float _alt_ref{NAN};
 
+	hrt_abstime _last_run{0};
+	float _h_drift{0.f};
+	float _h_drift_rate{0.05f};
+
+	bool _is_stale{false};
+	matrix::Vector2f _hpos_prev;
+
 	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")};
+
+	DEFINE_PARAMETERS(
+		(ParamBool<px4::params::FEV_EN>) _param_fev_en,
+		(ParamBool<px4::params::FEV_STALE>) _param_fev_stale,
+		(ParamFloat<px4::params::FEV_H_DRIFT_RATE>) _param_fev_h_drift_rate
+	)
 };

src/modules/fake_ev/fake_ev_params.c

@@ -34,3 +34,37 @@
 /**
  * @file fake_ev_params.c
  */
+
+
+/**
+ * Enable fake EV
+ *
+ * Generates fake vision output based on GNSS data
+ *
+ * @boolean
+ * @group Fake EV
+ */
+PARAM_DEFINE_INT32(FEV_EN, 1);
+
+/**
+ * Stale sensor
+ *
+ * Enable to freeze the current output
+ *
+ * @boolean
+ * @group Fake EV
+ */
+PARAM_DEFINE_INT32(FEV_STALE, 0);
+
+/**
+ * Drift rate
+ *
+ * Set the horizontal drift rate
+ *
+ * @min 0
+ * @max 5
+ * @decimal 2
+ * @unit m/s
+ * @group Fake EV
+ */
+PARAM_DEFINE_FLOAT(FEV_H_DRIFT_RATE, 0);