position_estimator_inav, mc_pos_control: precise position reprojection on home position changes

2 files changed, 39 insertions, 23 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 21070e1e9..dc0aa172a 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);
 		}
 
-		_ref_timestamp = _local_pos.ref_timestamp;
-
+		/* 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;
 	}
 }
 
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) {