diff --git a/src/modules/fake_ev/fake_ev.cpp b/src/modules/fake_ev/fake_ev.cpp index 4328aa5c67..c85fad4bab 100644 --- a/src/modules/fake_ev/fake_ev.cpp +++ b/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); diff --git a/src/modules/fake_ev/fake_ev.hpp b/src/modules/fake_ev/fake_ev.hpp index 36a68447b7..977d76d053 100644 --- a/src/modules/fake_ev/fake_ev.hpp +++ b/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) _param_fev_en, + (ParamBool) _param_fev_stale, + (ParamFloat) _param_fev_h_drift_rate + ) }; diff --git a/src/modules/fake_ev/fake_ev_params.c b/src/modules/fake_ev/fake_ev_params.c index a51a795a06..8a715460d4 100644 --- a/src/modules/fake_ev/fake_ev_params.c +++ b/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);