From 5d6a72e383f36557a3381ec31b952ccc0ab2eae3 Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 3 Jun 2020 11:14:47 +0200 Subject: [PATCH] sensor_simulator: add possibility to set GPS position rate one can set the position rate = velocity to obtain consistent gps data --- test/sensor_simulator/gps.cpp | 11 ++++++++++- test/sensor_simulator/gps.h | 2 ++ 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/test/sensor_simulator/gps.cpp b/test/sensor_simulator/gps.cpp index d98a10a72f..1d4fc1d1d2 100644 --- a/test/sensor_simulator/gps.cpp +++ b/test/sensor_simulator/gps.cpp @@ -15,7 +15,17 @@ Gps::~Gps() void Gps::send(uint64_t time) { + const float dt = static_cast(time - _gps_data.time_usec) * 1e-6f; + _gps_data.time_usec = time; + + if (fabsf(_gps_pos_rate(0)) > FLT_EPSILON || fabsf(_gps_pos_rate(1)) > FLT_EPSILON) { + stepHorizontalPositionByMeters(Vector2f(_gps_pos_rate) * dt); + } + if (fabsf(_gps_pos_rate(2)) > FLT_EPSILON) { + stepHeightByMeters(-_gps_pos_rate(2) * dt); + } + _ekf->setGpsData(_gps_data); } @@ -85,7 +95,6 @@ void Gps::stepHorizontalPositionByMeters(Vector2f hpos_change) _gps_data.lat = static_cast(lat_new * 1e7); } - gps_message Gps::getDefaultGpsData() { gps_message gps_data{}; diff --git a/test/sensor_simulator/gps.h b/test/sensor_simulator/gps.h index 944132638d..46016a0e22 100644 --- a/test/sensor_simulator/gps.h +++ b/test/sensor_simulator/gps.h @@ -53,6 +53,7 @@ public: void setData(const gps_message& gps); void stepHeightByMeters(float hgt_change); void stepHorizontalPositionByMeters(Vector2f hpos_change); + void setPositionRateNED(const Vector3f& rate) { _gps_pos_rate = rate; } void setAltitude(int32_t alt); void setLatitude(int32_t lat); void setLongitude(int32_t lon); @@ -66,6 +67,7 @@ public: private: gps_message _gps_data; + Vector3f _gps_pos_rate{}; void send(uint64_t time) override;