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

Signed-off-by: RomanBapst <bapstroman@gmail.com>

msg/PositionSetpoint.msg

@@ -35,3 +35,4 @@ float32 acceptance_radius   # navigation acceptance_radius if we're doing waypoi
 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)
 float32 cruising_throttle	# the generally desired cruising throttle (not a hard constraint), only has an effect for rover
+bool ignore_alt_acceptance  # can be set to avoid lateral guidance to loiter at waypoint in order to achieve altitude acceptance

src/modules/fw_pos_control/FixedwingPositionControl.cpp

@@ -1023,11 +1023,17 @@ FixedwingPositionControl::handle_setpoint_type(const position_setpoint_s &pos_sp
 					   _current_latitude, _current_longitude, _current_altitude,
 					   &dist_xy, &dist_z);
 
+		float acc_rad_z = _param_nav_fw_alt_rad.get();
+
+		if (pos_sp_curr.ignore_alt_acceptance) {
+			acc_rad_z = INFINITY;
+		}
+
 		// 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

@@ -227,6 +227,12 @@ void Mission::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 (isLanding() && _mission_item.nav_cmd == NAV_CMD_WAYPOINT) {
+			pos_sp_triplet->current.ignore_alt_acceptance = true;
+		}
+
 		// 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_block.cpp

@@ -359,6 +359,12 @@ MissionBlock::is_mission_item_reached_or_completed()
 
 			}
 
+			// prevent lateral guidance from loitering at a waypoint as part of a mission landing if the altitude
+			// is not achieved.
+			if (_mission_item.nav_cmd == NAV_CMD_WAYPOINT && _navigator->on_mission_landing()) {
+				alt_acc_rad_m = INFINITY;
+			}
+
 			bool passed_curr_wp = false;
 
 			if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {

src/modules/navigator/navigator.h

@@ -176,6 +176,8 @@ public:
 
 	float get_loiter_radius() { return _param_nav_loiter_rad.get(); }
 
+	bool on_mission_landing() { return _mission.isLanding(); }
+
 	/**
 	 * Returns the default acceptance radius defined by the parameter
 	 */

src/modules/navigator/rtl_direct_mission_land.cpp

@@ -213,6 +213,12 @@ 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 (_mission_item.nav_cmd == NAV_CMD_WAYPOINT) {
+			pos_sp_triplet->current.ignore_alt_acceptance = true;
+		}
 	}
 
 	issue_command(_mission_item);