Navigator: Support straight line mission landings (#23576)

* introduced altitude acceptance radius in position setpoint for fixed
wing guidance
- allows navigator to explicitly set the altitude acceptance radius
- needed for staright line landing support

* added ignore_alt_acceptance to position setpoint message to allow guidance
logic to ignore altitude error on waypoint
- can be useful to prevent loitering at a waypoint within a mission landing sequence

* only set altitude acceptance radius to infinity for a waypoint inside a mission landing
for fixed wing vehicles

* navigator: return altitude acceptance radius from triplet if it's valid

* FixedWingPositionControl: check if alt acceptance radius provided in position setpoint
is larger 0

---------

Signed-off-by: RomanBapst <bapstroman@gmail.com>
Co-authored-by: Alvaro Fernandez <alvaro@auterion.com>

msg/PositionSetpoint.msg

@@ -30,7 +30,8 @@ bool loiter_direction_counter_clockwise # loiter direction is clockwise by defau
 float32 loiter_orientation 	# Orientation of the major axis with respect to true north in rad [-pi,pi)
 uint8 	loiter_pattern		# loitern pattern to follow
 
-float32 acceptance_radius   # navigation acceptance_radius if we're doing waypoint navigation
+float32 acceptance_radius   # horizontal acceptance_radius (meters)
+float32 alt_acceptance_radius # vertical acceptance radius, only used for fixed wing guidance, NAN = let guidance choose (meters)
 
 float32 cruising_speed		# the generally desired cruising speed (not a hard constraint)
 bool gliding_enabled		# commands the vehicle to glide if the capability is available (fixed wing only)

src/modules/fw_pos_control/FixedwingPositionControl.cpp

@@ -1029,11 +1029,15 @@ FixedwingPositionControl::handle_setpoint_type(const position_setpoint_s &pos_sp
 					   _current_latitude, _current_longitude, _current_altitude,
 					   &dist_xy, &dist_z);
 
+		const float acc_rad_z = (PX4_ISFINITE(pos_sp_curr.alt_acceptance_radius)
+					 && pos_sp_curr.alt_acceptance_radius > FLT_EPSILON) ? pos_sp_curr.alt_acceptance_radius :
+					_param_nav_fw_alt_rad.get();
+
 		// Achieve position setpoint altitude via loiter when laterally close to WP.
 		// Detect if system has switchted into a Loiter before (check _position_sp_type), and in that
 		// case remove the dist_xy check (not switch out of Loiter until altitude is reached).
 		if ((!_vehicle_status.in_transition_mode) && (dist >= 0.f)
-		    && (dist_z > _param_nav_fw_alt_rad.get())
+		    && (dist_z > acc_rad_z)
 		    && (dist_xy < acc_rad || _position_sp_type == position_setpoint_s::SETPOINT_TYPE_LOITER)) {
 
 			// SETPOINT_TYPE_POSITION -> SETPOINT_TYPE_LOITER

src/modules/navigator/mission.cpp

@@ -91,41 +91,6 @@ Mission::on_activation()
 	MissionBase::on_activation();
 }
 
-bool
-Mission::isLanding()
-{
-	if (get_land_start_available()) {
-		static constexpr size_t max_num_next_items{1u};
-		int32_t next_mission_items_index[max_num_next_items];
-		size_t num_found_items;
-
-		getNextPositionItems(_mission.land_start_index + 1, next_mission_items_index, num_found_items, max_num_next_items);
-
-		// vehicle is currently landing if
-		//  mission valid, still flying, and in the landing portion of mission (past land start marker)
-		bool on_landing_stage = (num_found_items > 0U) &&  _mission.current_seq > next_mission_items_index[0U];
-
-		// special case: if the land start index is at a LOITER_TO_ALT WP, then we're in the landing sequence already when the
-		// distance to the WP is below the loiter radius + acceptance.
-		if ((num_found_items > 0U) && _mission.current_seq == next_mission_items_index[0U]
-		    && _mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT) {
-			const float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon,
-						_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
-
-			// consider mission_item.loiter_radius invalid if NAN or 0, use default value in this case.
-			const float mission_item_loiter_radius_abs = (PX4_ISFINITE(_mission_item.loiter_radius)
-					&& fabsf(_mission_item.loiter_radius) > FLT_EPSILON) ? fabsf(_mission_item.loiter_radius) :
-					_navigator->get_loiter_radius();
-
-			on_landing_stage = d_current <= (_navigator->get_acceptance_radius() + mission_item_loiter_radius_abs);
-		}
-
-		return _navigator->get_mission_result()->valid && on_landing_stage;
-
-	} else {
-		return false;
-	}
-}
 
 bool
 Mission::set_current_mission_index(uint16_t index)
@@ -244,6 +209,13 @@ void Mission::setActiveMissionItems()
 			mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 		}
 
+		// 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) {
+			pos_sp_triplet->current.alt_acceptance_radius = FLT_MAX;
+		}
+
 		// Allow a rotary wing vehicle to decelerate before reaching a wp with a hold time or a timeout
 		// This is done by setting the position triplet's next position's valid flag to false,
 		// which makes the FlightTask disregard the next position

src/modules/navigator/mission.h

@@ -67,7 +67,6 @@ public:
 	uint16_t get_land_start_index() const { return _mission.land_start_index; }
 	bool get_land_start_available() const { return hasMissionLandStart(); }
 
-	bool isLanding();
 
 private:
 

src/modules/navigator/mission_base.cpp

@@ -367,6 +367,42 @@ MissionBase::on_active()
 	updateAltToAvoidTerrainCollisionAndRepublishTriplet(_mission_item);
 }
 
+bool
+MissionBase::isLanding()
+{
+	if (hasMissionLandStart() && (_mission.current_seq > _mission.land_start_index)) {
+		static constexpr size_t max_num_next_items{1u};
+		int32_t next_mission_items_index[max_num_next_items];
+		size_t num_found_items;
+
+		getNextPositionItems(_mission.land_start_index + 1, next_mission_items_index, num_found_items, max_num_next_items);
+
+		// vehicle is currently landing if
+		//  mission valid, still flying, and in the landing portion of mission (past land start marker)
+		bool on_landing_stage = (num_found_items > 0U) &&  _mission.current_seq > next_mission_items_index[0U];
+
+		// special case: if the land start index is at a LOITER_TO_ALT WP, then we're in the landing sequence already when the
+		// distance to the WP is below the loiter radius + acceptance.
+		if ((num_found_items > 0U) && _mission.current_seq == next_mission_items_index[0U]
+		    && _mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT) {
+			const float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon,
+						_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
+
+			// consider mission_item.loiter_radius invalid if NAN or 0, use default value in this case.
+			const float mission_item_loiter_radius_abs = (PX4_ISFINITE(_mission_item.loiter_radius)
+					&& fabsf(_mission_item.loiter_radius) > FLT_EPSILON) ? fabsf(_mission_item.loiter_radius) :
+					_navigator->get_loiter_radius();
+
+			on_landing_stage = d_current <= (_navigator->get_acceptance_radius() + mission_item_loiter_radius_abs);
+		}
+
+		return _navigator->get_mission_result()->valid && on_landing_stage;
+
+	} else {
+		return false;
+	}
+}
+
 void MissionBase::update_mission()
 {
 	if (_mission.count == 0u || !_is_current_planned_mission_item_valid || !isMissionValid()) {

src/modules/navigator/mission_base.h

@@ -73,6 +73,8 @@ public:
 	virtual void on_activation() override;
 	virtual void on_active() override;
 
+	bool isLanding();
+
 protected:
 
 	/**
@@ -321,6 +323,7 @@ protected:
 	 */
 	void setMissionIndex(int32_t index);
 
+
 	bool _is_current_planned_mission_item_valid{false};	/**< Flag indicating if the currently loaded mission item is valid*/
 	bool _mission_has_been_activated{false};		/**< Flag indicating if the mission has been activated*/
 	bool _mission_checked{false};				/**< Flag indicating if the mission has been checked by the mission validator*/

src/modules/navigator/mission_block.cpp

@@ -673,6 +673,10 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi
 		sp->acceptance_radius = _navigator->get_default_acceptance_radius();
 	}
 
+	// by default, FW guidance logic will take alt acceptance from NAV_FW_ALT_RAD, in some special cases
+	// we override it after this
+	sp->alt_acceptance_radius = NAN;
+
 	sp->cruising_speed = _navigator->get_cruising_speed();
 	sp->cruising_throttle = _navigator->get_cruising_throttle();
 

src/modules/navigator/navigator_main.cpp

@@ -1116,9 +1116,14 @@ float Navigator::get_default_acceptance_radius()
 float Navigator::get_altitude_acceptance_radius()
 {
 	if (get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
+
+		const position_setpoint_s &curr_sp = get_position_setpoint_triplet()->current;
 		const position_setpoint_s &next_sp = get_position_setpoint_triplet()->next;
 
-		if (!force_vtol() && next_sp.type == position_setpoint_s::SETPOINT_TYPE_LAND && next_sp.valid) {
+		if ((PX4_ISFINITE(curr_sp.alt_acceptance_radius) && curr_sp.alt_acceptance_radius > FLT_EPSILON)) {
+			return curr_sp.alt_acceptance_radius;
+
+		} else if (!force_vtol() && next_sp.type == position_setpoint_s::SETPOINT_TYPE_LAND && next_sp.valid) {
 			// Use separate (tighter) altitude acceptance for clean altitude starting point before FW landing
 			return _param_nav_fw_altl_rad.get();
 

src/modules/navigator/rtl_direct_mission_land.cpp

@@ -212,6 +212,13 @@ void RtlDirectMissionLand::setActiveMissionItems()
 		}
 
 		mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
+
+		// 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) {
+			pos_sp_triplet->current.alt_acceptance_radius = FLT_MAX;
+		}
 	}
 
 	issue_command(_mission_item);

src/modules/navigator/rtl_mission_fast.cpp

@@ -168,6 +168,11 @@ void RtlMissionFast::setActiveMissionItems()
 		}
 
 		mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
+
+		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;
+		}
 	}
 
 	issue_command(_mission_item);