Navigator: Do not change target position if user is only changing altitude

This is important for multicopters for drift-free altitude changes and important for fixed wing to keep the current loiter center.

src/modules/navigator/navigator_main.cpp

@@ -383,6 +383,7 @@ Navigator::task_main()
 			if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_REPOSITION) {
 
 				struct position_setpoint_triplet_s *rep = get_reposition_triplet();
+				struct position_setpoint_triplet_s *curr = get_position_setpoint_triplet();
 
 				// store current position as previous position and goal as next
 				rep->previous.yaw = get_global_position()->yaw;
@@ -406,6 +407,12 @@ Navigator::task_main()
 					rep->current.lat = (cmd.param5 < 1000) ? cmd.param5 : cmd.param5 / (double)1e7;
 					rep->current.lon = (cmd.param6 < 1000) ? cmd.param6 : cmd.param6 / (double)1e7;
 
+				} else if (curr->current.valid
+					   && PX4_ISFINITE(curr->current.lat)
+					   && PX4_ISFINITE(curr->current.lon)) {
+					rep->current.lat = curr->current.lat;
+					rep->current.lon = curr->current.lon;
+
 				} else {
 					rep->current.lat = get_global_position()->lat;
 					rep->current.lon = get_global_position()->lon;