forked from Archive/PX4-Autopilot
fake EV: add param to enable, drift and stale data
This commit is contained in:
parent
ab026f463c
commit
9803b09a16
|
@ -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);
|
||||
|
|
|
@ -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
|
||||
)
|
||||
};
|
||||
|
|
|
@ -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);
|
||||
|
|
Loading…
Reference in New Issue