From 78a6b9f7a8b8e52a1f5ec00a37edc98121460674 Mon Sep 17 00:00:00 2001 From: Kamil Ritz Date: Wed, 15 Apr 2020 21:01:51 +0200 Subject: [PATCH] SensorSimulator: Fix GPS horizontal position step --- test/sensor_simulator/gps.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/test/sensor_simulator/gps.cpp b/test/sensor_simulator/gps.cpp index ca05d30247..84ee3c0331 100644 --- a/test/sensor_simulator/gps.cpp +++ b/test/sensor_simulator/gps.cpp @@ -68,13 +68,17 @@ void Gps::stepHorizontalPositionByMeters(Vector2f hpos_change) { float hposN_curr; float hposE_curr; - map_projection_global_project((float)_gps_data.lat, (float)_gps_data.lon, &hposN_curr, &hposE_curr); + map_projection_reference_s origin; + uint64_t time; + float alt; + _ekf->get_ekf_origin(&time, &origin, &alt); + map_projection_project(&origin, _gps_data.lat * 1e-7, _gps_data.lon * 1e-7, &hposN_curr, &hposE_curr); Vector2f hpos_new = Vector2f{hposN_curr, hposE_curr} + hpos_change; double lat_new; double lon_new; - map_projection_global_reproject(hpos_new(0), hpos_new(1), &lat_new, &lon_new); - _gps_data.lon = (uint32_t)lon_new; - _gps_data.lat = (uint32_t)lat_new; + map_projection_reproject(&origin, hpos_new(0), hpos_new(1), &lat_new, &lon_new); + _gps_data.lon = static_cast(lon_new * 1e7); + _gps_data.lat = static_cast(lat_new * 1e7); }