Navigator: Set altitude acceptance radius to infinity when moving to …

…land point after transition (#24115) * Navigator: set alt acceptance radius to infinity for land waypoint after backtransition -> avoid vehicle with depleted battery from not reaching the alt setpoint and getting stuck Signed-off-by: RomanBapst <bapstroman@gmail.com>
PX4 · Dec 20, 2024 · b765769 · b765769
1 parent 6196135
commit b765769
Show file tree

Hide file tree

Showing 5 changed files with 31 additions and 6 deletions.
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
@@ -211,8 +211,14 @@ void Mission::setActiveMissionItems()
 
 		// prevent fixed wing lateral guidance from loitering at a waypoint as part of a mission landing if the altitude
 		// is not achieved.
-		if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && isLanding() &&
-		    _mission_item.nav_cmd == NAV_CMD_WAYPOINT) {
+		const bool fw_on_mission_landing = _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING
+						   && isLanding() &&
+						   _mission_item.nav_cmd == NAV_CMD_WAYPOINT;
+		const bool mc_landing_after_transition = _vehicle_status_sub.get().vehicle_type ==
+				vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && _vehicle_status_sub.get().is_vtol &&
+				new_work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND;
+
+		if (fw_on_mission_landing || mc_landing_after_transition) {
 			pos_sp_triplet->current.alt_acceptance_radius = FLT_MAX;
 		}
 

diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
@@ -1145,8 +1145,13 @@ float Navigator::get_altitude_acceptance_radius()
 
 		const position_controller_status_s &pos_ctrl_status = _position_controller_status_sub.get();
 
-		if ((pos_ctrl_status.timestamp > _pos_sp_triplet.timestamp)
-		    && pos_ctrl_status.altitude_acceptance > alt_acceptance_radius) {
+		const position_setpoint_s &curr_sp = get_position_setpoint_triplet()->current;
+
+		if (PX4_ISFINITE(curr_sp.alt_acceptance_radius) && curr_sp.alt_acceptance_radius > FLT_EPSILON) {
+			alt_acceptance_radius = curr_sp.alt_acceptance_radius;
+
+		} else if ((pos_ctrl_status.timestamp > _pos_sp_triplet.timestamp)
+			   && pos_ctrl_status.altitude_acceptance > alt_acceptance_radius) {
 			alt_acceptance_radius = pos_ctrl_status.altitude_acceptance;
 		}
 

diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp
@@ -355,6 +355,7 @@ void RtlDirect::set_rtl_item()
 			pos_yaw_sp.alt = loiter_altitude;
 			pos_yaw_sp.yaw = !_param_wv_en.get() ? _destination.yaw : NAN; // set final yaw if weather vane is disabled
 
+			altitude_acceptance_radius = FLT_MAX;
 			setMoveToPositionMissionItem(_mission_item, pos_yaw_sp);
 			_navigator->reset_position_setpoint(pos_sp_triplet->previous);
 

diff --git a/src/modules/navigator/rtl_direct_mission_land.cpp b/src/modules/navigator/rtl_direct_mission_land.cpp
@@ -215,8 +215,14 @@ void RtlDirectMissionLand::setActiveMissionItems()
 
 		// prevent lateral guidance from loitering at a waypoint as part of a mission landing if the altitude
 		// is not achieved.
-		if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && MissionBase::isLanding()
-		    && _mission_item.nav_cmd == NAV_CMD_WAYPOINT) {
+		const bool fw_on_mission_landing = _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING
+						   && isLanding() &&
+						   _mission_item.nav_cmd == NAV_CMD_WAYPOINT;
+		const bool mc_landing_after_transition = _vehicle_status_sub.get().vehicle_type ==
+				vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && _vehicle_status_sub.get().is_vtol &&
+				new_work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND;
+
+		if (fw_on_mission_landing || mc_landing_after_transition) {
 			pos_sp_triplet->current.alt_acceptance_radius = FLT_MAX;
 		}
 	}

diff --git a/src/modules/navigator/rtl_mission_fast_reverse.cpp b/src/modules/navigator/rtl_mission_fast_reverse.cpp
@@ -155,6 +155,13 @@ void RtlMissionFastReverse::setActiveMissionItems()
 		}
 
 		mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
+		const bool mc_landing_after_transition = _vehicle_status_sub.get().vehicle_type ==
+				vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && _vehicle_status_sub.get().is_vtol &&
+				new_work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND;
+
+		if (mc_landing_after_transition) {
+			pos_sp_triplet->current.alt_acceptance_radius = FLT_MAX;
+		}
 	}
 
 	issue_command(_mission_item);