diff options
Diffstat (limited to 'src/modules/position_estimator_inav')
-rw-r--r-- | src/modules/position_estimator_inav/position_estimator_inav_main.c | 40 |
1 files changed, 24 insertions, 16 deletions
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 5a713d309..40f7069ca 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -543,25 +543,34 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (home.timestamp != home_timestamp) { home_timestamp = home.timestamp; + + double est_lat, est_lon; + float est_alt; + if (ref_inited) { - /* reproject position estimate to new reference */ - float dx, dy; - map_projection_project(&ref, home.lat, home.lon, &dx, &dy); - x_est[0] -= dx; - y_est[0] -= dy; - z_est[0] += home.alt - local_pos.ref_alt; + /* calculate current estimated position in global frame */ + est_alt = local_pos.ref_alt - local_pos.z; + map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon); } - /* update baro offset */ - baro_offset -= home.alt - local_pos.ref_alt; - /* update reference */ map_projection_init(&ref, home.lat, home.lon); + /* update baro offset */ + baro_offset += home.alt - local_pos.ref_alt; + local_pos.ref_lat = home.lat; local_pos.ref_lon = home.lon; local_pos.ref_alt = home.alt; local_pos.ref_timestamp = home.timestamp; + + if (ref_inited) { + /* reproject position estimate with new reference */ + map_projection_project(&ref, est_lat, est_lon, &x_est[0], &y_est[0]); + z_est[0] = -(est_alt - local_pos.ref_alt); + } + + ref_inited = true; } } @@ -589,6 +598,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } if (gps_valid) { + double lat = gps.lat * 1e-7; + double lon = gps.lon * 1e-7; + float alt = gps.alt * 1e-3; + /* initialize reference position if needed */ if (!ref_inited) { if (ref_init_start == 0) { @@ -596,11 +609,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } else if (t > ref_init_start + ref_init_delay) { ref_inited = true; - /* reference GPS position */ - double lat = gps.lat * 1e-7; - double lon = gps.lon * 1e-7; - float alt = gps.alt * 1e-3; - /* update baro offset */ baro_offset -= z_est[0]; @@ -628,7 +636,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (ref_inited) { /* project GPS lat lon to plane */ float gps_proj[2]; - map_projection_project(&ref, gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1])); + map_projection_project(&ref, lat, lon, &(gps_proj[0]), &(gps_proj[1])); /* reset position estimate when GPS becomes good */ if (reset_est) { @@ -643,7 +651,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* calculate correction for position */ corr_gps[0][0] = gps_proj[0] - x_est[0]; corr_gps[1][0] = gps_proj[1] - y_est[0]; - corr_gps[2][0] = local_pos.ref_alt - gps.alt * 1e-3 - z_est[0]; + corr_gps[2][0] = local_pos.ref_alt - alt - z_est[0]; /* calculate correction for velocity */ if (gps.vel_ned_valid) { |