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

EKF: set global origin method

Browse Source 
EKF: add convergence after global origin reset test
This commit is contained in:
Daniel Agar
2021-01-12 18:33:00 -05:00
parent 81937370ac
commit 63f64b57c1
7 changed files with 147 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/sensor_simulator/gps.cpp
+2 -6
View File
@@ -86,15 +86,11 @@ void Gps::stepHorizontalPositionByMeters(Vector2f hpos_change)
{
float hposN_curr;
float 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);
map_projection_project(&_ekf->global_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_reproject(&origin, hpos_new(0), hpos_new(1), &lat_new, &lon_new);
map_projection_reproject(&_ekf->global_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);
}