From d1aca4032ddf6b6b457e6816874c63c53ddba58d Mon Sep 17 00:00:00 2001 From: Thomas Stastny Date: Fri, 1 Jul 2022 16:00:28 +0200 Subject: [PATCH] mission feasibility / fw pos ctrl: add limited landing checks back, allow glide slopes below max --- .../FixedwingPositionControl.cpp | 25 +++-- .../fw_pos_control_l1_params.c | 5 +- .../navigator/mission_feasibility_checker.cpp | 91 +++++++++++++++++-- .../navigator/mission_feasibility_checker.h | 8 +- 4 files changed, 112 insertions(+), 17 deletions(-) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 4db7565170..3bb9f9a8f6 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/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 diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index f0eee7fed8..a6a22fdf14 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/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 diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index f23d118bda..03f495ac39 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/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(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(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 { diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index 6091f31347..f3c90eb770 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -44,11 +44,12 @@ #include #include +#include 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) _param_fw_lnd_ang + ) };