[Backport 1.16] Navigator: Fix mission RTL for fixed-wing by setting previous waypoint correctly(#25861)

This aligns setActiveMissionItems() in rtl_direct_mission_land.cpp and in rtl_mission_fast.cpp with what was already in mission.cpp. It probably was on oversight when the RTL restructure happened. The FW landing requires the previous waypoint to be correctly set, that's why it was only noticeable there.

* Fix position setpoint update logic in Mission RTL

Currently, when proceeding to the landing point the previous setpoint is not updated, which results in an unexpected and off course landing pattern in fixed wing. (see #25436)

* Change to work more like `mission.cpp`

* Fix rtl_direct_misssion_land formatting for style guide

* rtl_mission_fast: fix FW landing by setting previous wp in landing

Signed-off-by: Silvan <silvan@auterion.com>

---------

Signed-off-by: Silvan <silvan@auterion.com>
Co-authored-by: Silvan <silvan@auterion.com>

src/modules/navigator/rtl_direct_mission_land.cpp

@@ -122,6 +122,7 @@ void RtlDirectMissionLand::setActiveMissionItems()
 {
 	WorkItemType new_work_item_type{WorkItemType::WORK_ITEM_TYPE_DEFAULT};
 	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+	const position_setpoint_s current_setpoint_copy = pos_sp_triplet->current;
 
 	// Climb to altitude
 	if (_needs_climbing && _work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) {
@@ -203,8 +204,6 @@ void RtlDirectMissionLand::setActiveMissionItems()
 
 			_mission_item.autocontinue = true;
 			_mission_item.time_inside = 0.0f;
-
-			pos_sp_triplet->previous = pos_sp_triplet->current;
 		}
 
 		if (num_found_items > 0) {
@@ -213,6 +212,12 @@ void RtlDirectMissionLand::setActiveMissionItems()
 
 		mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 
+		// Only set the previous position item if the current one really changed
+		if ((_work_item_type != WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND) &&
+		    !position_setpoint_equal(&pos_sp_triplet->current, &current_setpoint_copy)) {
+			pos_sp_triplet->previous = current_setpoint_copy;
+		}
+
 		// prevent lateral guidance from loitering at a waypoint as part of a mission landing if the altitude
 		// is not achieved.
 		const bool fw_on_mission_landing = _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING

src/modules/navigator/rtl_mission_fast.cpp

@@ -91,6 +91,7 @@ void RtlMissionFast::setActiveMissionItems()
 {
 	WorkItemType new_work_item_type{WorkItemType::WORK_ITEM_TYPE_DEFAULT};
 	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+	const position_setpoint_s current_setpoint_copy = pos_sp_triplet->current;
 
 	/* Skip VTOL/FW Takeoff item if in air, fixed-wing and didn't start the takeoff already*/
 	if ((_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) &&
@@ -158,17 +159,20 @@ void RtlMissionFast::setActiveMissionItems()
 			_mission_item.autocontinue = true;
 			_mission_item.time_inside = 0.0f;
 
-			pos_sp_triplet->previous = pos_sp_triplet->current;
 		}
 
-
-
 		if (num_found_items > 0) {
 			mission_item_to_position_setpoint(next_mission_items[0u], &pos_sp_triplet->next);
 		}
 
 		mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 
+		// Only set the previous position item if the current one really changed
+		if ((_work_item_type != WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND) &&
+		    !position_setpoint_equal(&pos_sp_triplet->current, &current_setpoint_copy)) {
+			pos_sp_triplet->previous = current_setpoint_copy;
+		}
+
 		if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && isLanding() &&
 		    _mission_item.nav_cmd == NAV_CMD_WAYPOINT) {
 			pos_sp_triplet->current.alt_acceptance_radius = FLT_MAX;