fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-10 09:41:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
bresch
2023-06-02 10:16:00 +02:00
parent 704bd22bc1
commit c1e1026734
3 changed files with 61 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fake_ev/fake_ev.cpp
+14 -2
View File
@@ -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);