fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 01:24:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

SensorSimulator: Fix GPS horizontal position step

Browse Source
This commit is contained in:
Kamil Ritz 2020-04-15 21:01:51 +02:00 committed by Mathieu Bresciani
parent 050298f6c1
commit 78a6b9f7a8
1 changed files with 8 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
test/sensor_simulator/gps.cpp
View File

@ -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<int32_t>(lon_new * 1e7);
_gps_data.lat = static_cast<int32_t>(lat_new * 1e7);
}