mission feasibility / fw pos ctrl: add limited landing checks back, allow glide slopes below max

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -1618,6 +1618,7 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
 
 	initializeAutoLanding(now, pos_sp_prev, pos_sp_curr, local_position, local_land_point);
 
+	// touchdown may get nudged by manual inputs
 	local_land_point = calculateTouchdownPosition(control_interval, local_land_point);
 	const Vector2f landing_approach_vector = calculateLandingApproachVector();
 
@@ -1627,9 +1628,9 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
 	// calculate the altitude setpoint based on the landing glide slope
 	const float along_track_dist_to_touchdown = -landing_approach_vector.unit_or_zero().dot(
 				local_position - local_land_point);
-	const float glide_slope_rel_alt = math::constrain(along_track_dist_to_touchdown * tanf(math::radians(
-			_param_fw_lnd_ang.get())),
-					  0.0f, _landing_approach_entrance_rel_alt);
+	const float glide_slope = _landing_approach_entrance_rel_alt / _landing_approach_entrance_offset_vector.norm();
+	const float glide_slope_rel_alt = math::constrain(along_track_dist_to_touchdown * glide_slope, 0.0f,
+					  _landing_approach_entrance_rel_alt);
 
 	const float terrain_alt = getLandingTerrainAltitudeEstimate(now, pos_sp_curr.alt);
 	float altitude_setpoint;
@@ -2514,13 +2515,21 @@ FixedwingPositionControl::initializeAutoLanding(const hrt_abstime &now, const po
 
 		_landing_approach_entrance_rel_alt = height_above_land_point;
 
-		const float landing_approach_distance = _landing_approach_entrance_rel_alt / tanf(math::radians(
-				_param_fw_lnd_ang.get()));
+		const Vector2f landing_approach_vector = local_land_point - local_approach_entrance;
+		float landing_approach_distance = landing_approach_vector.norm();
 
-		const Vector2f vector_aircraft_to_land_point = local_land_point - local_approach_entrance;
+		const float max_glide_slope = tanf(math::radians(_param_fw_lnd_ang.get()));
+		const float glide_slope = _landing_approach_entrance_rel_alt / landing_approach_distance;
 
-		if (vector_aircraft_to_land_point.norm_squared() > FLT_EPSILON) {
-			_landing_approach_entrance_offset_vector = -vector_aircraft_to_land_point.unit_or_zero() * landing_approach_distance;
+		if (glide_slope > max_glide_slope) {
+			// rescale the landing distance - this will have the same effect as dropping down the approach
+			// entrance altitude on the vehicle's behavior. if we reach here.. it means the navigator checks
+			// didn't work, or something is using the control_auto_landing() method inappropriately
+			landing_approach_distance = _landing_approach_entrance_rel_alt / max_glide_slope;
+		}
+
+		if (landing_approach_vector.norm_squared() > FLT_EPSILON) {
+			_landing_approach_entrance_offset_vector = -landing_approach_vector.unit_or_zero() * landing_approach_distance;
 
 		} else {
 			// land in direction of airframe

src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c

@@ -368,7 +368,10 @@ PARAM_DEFINE_FLOAT(FW_THR_IDLE, 0.15f);
 PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 10.0f);
 
 /**
- * Landing slope angle
+ * Maximum landing slope angle
+ *
+ * Typically the desired landing slope angle when landing configuration (flaps, airspeed) is enabled.
+ * Set this value within the vehicle's performance limits.
  *
  * @unit deg
  * @min 1.0

src/modules/navigator/mission_feasibility_checker.cpp

@@ -496,12 +496,91 @@ MissionFeasibilityChecker::checkFixedWingLanding(const mission_s &mission, bool
 				}
 
 				if (MissionBlock::item_contains_position(missionitem_previous)) {
-					// exact handling of the previous waypoint is currently done in the fixed-wing
-					// position control module and primarily supports approach entrances from
-					// orbit-to-alt mission items. Behavior with other entrance waypoint types, e.g.
-					// plain position setpoints is not guaranteed. it is the user's responsibility to
-					// appropriately plan the entrance point if not using orbit-to-alt until extended
-					// functionality is implemented.
+
+					const float land_alt_amsl = missionitem.altitude_is_relative ? missionitem.altitude +
+								    _navigator->get_home_position()->alt : missionitem.altitude;
+					const float entrance_alt_amsl = missionitem_previous.altitude_is_relative ? missionitem_previous.altitude +
+									_navigator->get_home_position()->alt : missionitem_previous.altitude;
+					const float relative_approach_altitude = entrance_alt_amsl - land_alt_amsl;
+
+					if (relative_approach_altitude < FLT_EPSILON) {
+						mavlink_log_critical(_navigator->get_mavlink_log_pub(),
+								     "Mission rejected: the approach waypoint must be above the landing point.\t");
+						events::send(events::ID("navigator_mis_approach_wp_below_land"), {events::Log::Error, events::LogInternal::Info},
+							     "Mission rejected: the approach waypoint must be above the landing point");
+						return false;
+					}
+
+					float landing_approach_distance;
+
+					if (missionitem_previous.nav_cmd == NAV_CMD_LOITER_TO_ALT) {
+						// assume this is a fixed-wing landing pattern with orbit to alt followed
+						// by tangent exit to landing approach and touchdown at landing waypoint
+
+						const float distance_orbit_center_to_land = get_distance_to_next_waypoint(missionitem_previous.lat,
+								missionitem_previous.lon, missionitem.lat, missionitem.lon);
+						const float orbit_radius = fabsf(missionitem_previous.loiter_radius);
+
+						if (distance_orbit_center_to_land <= orbit_radius) {
+							mavlink_log_critical(_navigator->get_mavlink_log_pub(),
+									     "Mission rejected: the landing point must be outside the orbit radius.\t");
+							events::send(events::ID("navigator_mis_land_wp_inside_orbit_radius"), {events::Log::Error, events::LogInternal::Info},
+								     "Mission rejected: the landing point must be outside the orbit radius");
+							return false;
+						}
+
+						landing_approach_distance = sqrtf(distance_orbit_center_to_land * distance_orbit_center_to_land - orbit_radius *
+										  orbit_radius);
+
+					} else if (missionitem_previous.nav_cmd == NAV_CMD_WAYPOINT) {
+						// approaching directly from waypoint position
+
+						const float waypoint_distance = get_distance_to_next_waypoint(missionitem_previous.lat, missionitem_previous.lon,
+										missionitem.lat, missionitem.lon);
+						landing_approach_distance = waypoint_distance;
+
+					} else {
+						mavlink_log_critical(_navigator->get_mavlink_log_pub(),
+								     "Mission rejected: unsupported landing approach entrance waypoint type. Only ORBIT_TO_ALT or WAYPOINT allowed.\t");
+						events::send(events::ID("navigator_mis_unsupported_landing_approach_wp"), {events::Log::Error, events::LogInternal::Info},
+							     "Mission rejected: unsupported landing approach entrance waypoint type. Only ORBIT_TO_ALT or WAYPOINT allowed");
+						return false;
+					}
+
+					const float glide_slope = relative_approach_altitude / landing_approach_distance;
+
+					// respect user setting as max glide slope, but account for floating point
+					// rounding on next check with small (arbitrary) 0.1 deg buffer, as the
+					// landing angle parameter is what is typically used for steepest glide
+					// in landing config
+					const float max_glide_slope = tanf(math::radians(_param_fw_lnd_ang.get() + 0.1f));
+
+					if (glide_slope > max_glide_slope) {
+
+						const uint8_t land_angle_left_of_decimal = (uint8_t)_param_fw_lnd_ang.get();
+						const uint8_t land_angle_first_after_decimal = (uint8_t)((_param_fw_lnd_ang.get() - floorf(
+									_param_fw_lnd_ang.get())) * 10.0f);
+
+						mavlink_log_critical(_navigator->get_mavlink_log_pub(),
+								     "Mission rejected: the landing glide slope is steeper than the vehicle setting of %d.%d degrees.\t",
+								     (int)land_angle_left_of_decimal, (int)land_angle_first_after_decimal);
+						events::send<uint8_t, uint8_t>(events::ID("navigator_mis_glide_slope_too_steep"), {events::Log::Error, events::LogInternal::Info},
+									       "Mission rejected: the landing glide slope is steeper than the vehicle setting of {1}.{2} degrees",
+									       land_angle_left_of_decimal, land_angle_first_after_decimal);
+
+						const uint32_t acceptable_entrance_alt = (uint32_t)(max_glide_slope * landing_approach_distance);
+						const uint32_t acceptable_landing_dist = (uint32_t)ceilf(relative_approach_altitude / max_glide_slope);
+
+						mavlink_log_critical(_navigator->get_mavlink_log_pub(),
+								     "Reduce the glide slope, lower the entrance altitude %d meters, or increase the landing approach distance %d meters.\t",
+								     (int)acceptable_entrance_alt, (int)acceptable_landing_dist);
+						events::send<uint32_t, uint32_t>(events::ID("navigator_mis_correct_glide_slope"), {events::Log::Error, events::LogInternal::Info},
+										 "Reduce the glide slope, lower the entrance altitude {1} meters, or increase the landing approach distance {2} meters",
+										 acceptable_entrance_alt, acceptable_landing_dist);
+
+						return false;
+					}
+
 					landing_valid = true;
 
 				} else {

src/modules/navigator/mission_feasibility_checker.h

@@ -44,11 +44,12 @@
 
 #include <dataman/dataman.h>
 #include <uORB/topics/mission.h>
+#include <px4_platform_common/module_params.h>
 
 class Geofence;
 class Navigator;
 
-class MissionFeasibilityChecker
+class MissionFeasibilityChecker: public ModuleParams
 {
 private:
 	Navigator *_navigator{nullptr};
@@ -77,7 +78,7 @@ private:
 	bool checkVTOLLanding(const mission_s &mission, bool land_start_req);
 
 public:
-	MissionFeasibilityChecker(Navigator *navigator) : _navigator(navigator) {}
+	MissionFeasibilityChecker(Navigator *navigator) : ModuleParams(nullptr), _navigator(navigator) {}
 	~MissionFeasibilityChecker() = default;
 
 	MissionFeasibilityChecker(const MissionFeasibilityChecker &) = delete;
@@ -90,4 +91,7 @@ public:
 				  float max_distance_to_1st_waypoint, float max_distance_between_waypoints,
 				  bool land_start_req);
 
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::FW_LND_ANG>) _param_fw_lnd_ang
+	)
 };