From 79d2247b44aaeb0f2211a1c66c994e0fa7ca4b9f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 5 Apr 2014 18:11:51 +0400 Subject: [PATCH] position_estimator_inav, mc_pos_control: precise position reprojection on home position changes --- .../mc_pos_control/mc_pos_control_main.cpp | 24 +++++++---- .../position_estimator_inav_main.c | 42 +++++++++++-------- 2 files changed, 41 insertions(+), 25 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 21070e1e9b..dc0aa172a4 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -473,18 +473,26 @@ void MulticopterPositionControl::update_ref() { if (_local_pos.ref_timestamp != _ref_timestamp) { + double lat_sp, lon_sp; + float alt_sp; + if (_ref_timestamp != 0) { - /* reproject local position setpoint to new reference */ - float dx, dy; - map_projection_project(&_ref_pos, _local_pos.ref_lat, _local_pos.ref_lon, &dx, &dy); - _pos_sp(0) -= dx; - _pos_sp(1) -= dy; + /* calculate current position setpoint in global frame */ + map_projection_reproject(&_ref_pos, _pos_sp(0), _pos_sp(1), &lat_sp, &lon_sp); + alt_sp = _ref_alt - _pos_sp(2); + } + + /* update local projection reference */ + map_projection_init(&_ref_pos, _local_pos.ref_lat, _local_pos.ref_lon); + _ref_alt = _local_pos.ref_alt; + + if (_ref_timestamp != 0) { + /* reproject position setpoint to new reference */ + map_projection_project(&_ref_pos, lat_sp, lon_sp, &_pos_sp.data[0], &_pos_sp.data[1]); + _pos_sp(2) = -(alt_sp - _ref_alt); } _ref_timestamp = _local_pos.ref_timestamp; - - map_projection_init(&_ref_pos, _local_pos.ref_lat, _local_pos.ref_lon); - _ref_alt = _local_pos.ref_alt; } } 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 5a713d309b..40f7069ca0 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; - 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; - } - /* update baro offset */ - baro_offset -= home.alt - local_pos.ref_alt; + double est_lat, est_lon; + float est_alt; + + if (ref_inited) { + /* 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 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) {