mc_pos_control: reproject local position setpoint on local reference updates

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -473,8 +473,15 @@ void
 MulticopterPositionControl::update_ref()
 {
 	if (_local_pos.ref_timestamp != _ref_timestamp) {
+		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;
+		}
+
 		_ref_timestamp = _local_pos.ref_timestamp;
-		// TODO mode position setpoint in assisted modes
 
 		map_projection_init(&_ref_pos, _local_pos.ref_lat, _local_pos.ref_lon);
 		_ref_alt = _local_pos.ref_alt;