mission: skip a vtol takoff mission item if already in air (#23319)

* mission: skip a vtol takoff mission item if already in air and a fixed wing * MissionBase: also skip FW takeoff when already in-air Signed-off-by: Silvan Fuhrer <[email protected]> * mission: use setNextMissionItem to skip vtol takeoff when already in air * mission: Only skip the VTOL takeoff in air for mission and rtl mission If flying RTL mission reverse it must still include the takeoff point. --------- Signed-off-by: Silvan Fuhrer <[email protected]> Co-authored-by: Silvan Fuhrer <[email protected]>
PX4 · Jun 25, 2024 · 09f066a · 09f066a
1 parent 6fd0e98
commit 09f066a
Show file tree

Hide file tree

Showing 2 changed files with 34 additions and 0 deletions.
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
@@ -216,6 +216,23 @@ void Mission::setActiveMissionItems()
 	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) &&
+	    (_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) &&
+	    (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) &&
+	    !_land_detected_sub.get().landed) {
+		if (setNextMissionItem()) {
+			if (!loadCurrentMissionItem()) {
+				setEndOfMissionItems();
+				return;
+			}
+
+		} else {
+			setEndOfMissionItems();
+			return;
+		}
+	}
+
 	if (item_contains_position(_mission_item)) {
 
 		handleTakeoff(new_work_item_type, next_mission_items, num_found_items);

diff --git a/src/modules/navigator/rtl_mission_fast.cpp b/src/modules/navigator/rtl_mission_fast.cpp
@@ -92,6 +92,23 @@ 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();
 
+	/* 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) &&
+	    (_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) &&
+	    (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) &&
+	    !_land_detected_sub.get().landed) {
+		if (setNextMissionItem()) {
+			if (!loadCurrentMissionItem()) {
+				setEndOfMissionItems();
+				return;
+			}
+
+		} else {
+			setEndOfMissionItems();
+			return;
+		}
+	}
+
 	// Transition to fixed wing if necessary.
 	if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING &&
 	    _vehicle_status_sub.get().is_vtol &&