mc pos ctrl: revert to local map projection

1 files changed, 6 insertions, 3 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 bf7b6a19c..6b797f222 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -468,15 +468,17 @@ MulticopterPositionControl::update_ref()
 
 		if (_ref_timestamp != 0) {
 			/* calculate current position setpoint in global frame */
-			map_projection_reproject(_pos_sp(0), _pos_sp(1), &lat_sp, &lon_sp);
+			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(lat_sp, lon_sp, &_pos_sp.data[0], &_pos_sp.data[1]);
+			map_projection_project(&_ref_pos, lat_sp, lon_sp, &_pos_sp.data[0], &_pos_sp.data[1]);
 			_pos_sp(2) = -(alt_sp - _ref_alt);
 		}
 
@@ -675,7 +677,8 @@ MulticopterPositionControl::task_main()
 					_reset_alt_sp = true;
 
 					/* project setpoint to local frame */
-					map_projection_project(_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon,
+					map_projection_project(&_ref_pos,
+							       _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon,
 							       &_pos_sp.data[0], &_pos_sp.data[1]);
 					_pos_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt);