From 4cd7dfa162fe750916f3552e4f01f29784d42052 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Thu, 10 Nov 2022 17:11:20 +0100 Subject: [PATCH] MissionFeasibility: add combined takeoff/landing requirement check Replace the existing check for the availability of a takeoff mission item with a combined check for takeoff and landing item (or landing pattern). New param MIS_TKO_LAND_REQ can be set to require only a takeoff, only a landing, both takeoff and landing, and both or none. The latter is meant to be set if is e.g. deemed unsafe to start a flight through a Takeoff WP without though defining a Landing - as then in case of a RTL the vehicle doesn't follow a pre-defined path but instead only can do default RTL that especially for FW and VTOL isn't always safe. Signed-off-by: Silvan Fuhrer --- ROMFS/px4fmu_common/init.d/rc.fw_defaults | 1 + ROMFS/px4fmu_common/init.d/rc.vtol_defaults | 1 + src/modules/navigator/mission.cpp | 3 +- .../navigator/mission_feasibility_checker.cpp | 210 ++++++++++-------- .../navigator/mission_feasibility_checker.h | 25 +-- src/modules/navigator/mission_params.c | 13 +- src/modules/navigator/navigator.h | 6 +- 7 files changed, 144 insertions(+), 115 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults index df26a114ca..8cf2cff58c 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults @@ -45,6 +45,7 @@ param set-default NAV_ACC_RAD 10 param set-default MIS_DIST_WPS 5000 param set-default MIS_LTRMIN_ALT 25 param set-default MIS_TAKEOFF_ALT 25 +param set-default MIS_TKO_LAND_REQ 2 # # FW takeoff acceleration can easily exceed ublox GPS 2G default. diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults index 2745a250f9..41217999d7 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults @@ -19,6 +19,7 @@ param set-default EKF2_FUSE_BETA 1 param set-default HTE_VXY_THR 2.0 param set-default MIS_DIST_WPS 5000 +param set-default MIS_TKO_LAND_REQ 2 param set-default MPC_ACC_HOR_MAX 2 param set-default MPC_VEL_MANUAL 5 diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 04f02358f7..e29198c00a 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -1743,8 +1743,7 @@ Mission::check_mission_valid(bool force) _navigator->get_mission_result()->valid = _missionFeasibilityChecker.checkMissionFeasible(_mission, _param_mis_dist_1wp.get(), - _param_mis_dist_wps.get(), - _navigator->mission_landing_required()); + _param_mis_dist_wps.get()); _navigator->get_mission_result()->seq_total = _mission.count; _navigator->increment_mission_instance_count(); diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 27f98e44f1..72c870966c 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -54,8 +54,7 @@ bool MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission, - float max_distance_to_1st_waypoint, float max_distance_between_waypoints, - bool land_start_req) + float max_distance_to_1st_waypoint, float max_distance_between_waypoints) { // Reset warning flag _navigator->get_mission_result()->warning = false; @@ -82,57 +81,30 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission, const float home_alt = _navigator->get_home_position()->alt; + // reset for next check + _has_takeoff = false; + _has_landing = false; + // check if all mission item commands are supported failed = failed || !checkMissionItemValidity(mission); failed = failed || !checkDistancesBetweenWaypoints(mission, max_distance_between_waypoints); failed = failed || !checkGeofence(mission, home_alt, home_valid); failed = failed || !checkHomePositionAltitude(mission, home_alt, home_alt_valid); + failed = failed || !checkTakeoff(mission, home_alt); if (_navigator->get_vstatus()->is_vtol) { - failed = failed || !checkVTOL(mission, home_alt, false); + failed |= (!checkTakeoff(mission, home_alt) || !checkVTOLLanding(mission) || !checkTakeoffLandAvailable()); } else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - failed = failed || !checkRotarywing(mission, home_alt); + failed |= (!checkTakeoff(mission, home_alt) || !checkRotaryWingLanding(mission) || !checkTakeoffLandAvailable()); - } else { - failed = failed || !checkFixedwing(mission, home_alt, land_start_req); + } else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + failed |= (!checkTakeoff(mission, home_alt) || !checkFixedWingLanding(mission) || !checkTakeoffLandAvailable()); } return !failed; } -bool -MissionFeasibilityChecker::checkRotarywing(const mission_s &mission, float home_alt) -{ - /* - * Perform check and issue feedback to the user - * Mission is only marked as feasible if takeoff check passes - */ - return checkTakeoff(mission, home_alt); -} - -bool -MissionFeasibilityChecker::checkFixedwing(const mission_s &mission, float home_alt, bool land_start_req) -{ - /* Perform checks and issue feedback to the user for all checks */ - bool resTakeoff = checkTakeoff(mission, home_alt); - bool resLanding = checkFixedWingLanding(mission, land_start_req); - - /* Mission is only marked as feasible if all checks return true */ - return (resTakeoff && resLanding); -} - -bool -MissionFeasibilityChecker::checkVTOL(const mission_s &mission, float home_alt, bool land_start_req) -{ - /* Perform checks and issue feedback to the user for all checks */ - bool resTakeoff = checkTakeoff(mission, home_alt); - bool resLanding = checkVTOLLanding(mission, land_start_req); - - /* Mission is only marked as feasible if all checks return true */ - return (resTakeoff && resLanding); -} - bool MissionFeasibilityChecker::checkGeofence(const mission_s &mission, float home_alt, bool home_valid) { @@ -327,7 +299,6 @@ MissionFeasibilityChecker::checkMissionItemValidity(const mission_s &mission) bool MissionFeasibilityChecker::checkTakeoff(const mission_s &mission, float home_alt) { - bool has_takeoff = false; bool takeoff_first = false; int takeoff_index = -1; @@ -341,7 +312,7 @@ MissionFeasibilityChecker::checkTakeoff(const mission_s &mission, float home_alt } // look for a takeoff waypoint - if (missionitem.nav_cmd == NAV_CMD_TAKEOFF) { + if (missionitem.nav_cmd == NAV_CMD_TAKEOFF || missionitem.nav_cmd == NAV_CMD_VTOL_TAKEOFF) { // make sure that the altitude of the waypoint is at least one meter larger than the acceptance radius // this makes sure that the takeoff waypoint is not reached before we are at least one meter in the air @@ -367,7 +338,7 @@ MissionFeasibilityChecker::checkTakeoff(const mission_s &mission, float home_alt } // tell that mission has a takeoff waypoint - has_takeoff = true; + _has_takeoff = true; // tell that a takeoff waypoint is the first "waypoint" // mission item @@ -427,25 +398,14 @@ MissionFeasibilityChecker::checkTakeoff(const mission_s &mission, float home_alt } } - if (_navigator->get_takeoff_required() && _navigator->get_land_detected()->landed) { - // check for a takeoff waypoint, after the above conditions have been met - // MIS_TAKEOFF_REQ param has to be set and the vehicle has to be landed - one can load a mission - // while the vehicle is flying and it does not require a takeoff waypoint - if (!has_takeoff) { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: takeoff waypoint required.\t"); - events::send(events::ID("navigator_mis_takeoff_missing"), {events::Log::Error, events::LogInternal::Info}, - "Mission rejected: takeoff waypoint required"); - return false; - - } else if (!takeoff_first) { - // check if the takeoff waypoint is the first waypoint item on the mission - // i.e, an item with position/attitude change modification - // if it is not, the mission should be rejected - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: takeoff not first waypoint item\t"); - events::send(events::ID("navigator_mis_takeoff_not_first"), {events::Log::Error, events::LogInternal::Info}, - "Mission rejected: takeoff is not the first waypoint item"); - return false; - } + if (_has_takeoff && !takeoff_first) { + // check if the takeoff waypoint is the first waypoint item on the mission + // i.e, an item with position/attitude change modification + // if it is not, the mission should be rejected + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: takeoff not first waypoint item\t"); + events::send(events::ID("navigator_mis_takeoff_not_first"), {events::Log::Error, events::LogInternal::Info}, + "Mission rejected: takeoff is not the first waypoint item"); + return false; } // all checks have passed @@ -453,14 +413,36 @@ MissionFeasibilityChecker::checkTakeoff(const mission_s &mission, float home_alt } bool -MissionFeasibilityChecker::checkFixedWingLanding(const mission_s &mission, bool land_start_req) +MissionFeasibilityChecker::checkRotaryWingLanding(const mission_s &mission) +{ + // Go through all mission items and search for a landing waypoint. + // For MC we currently do not run any checks on the validity of the planned landing. + + for (size_t i = 0; i < mission.count; i++) { + struct mission_item_s missionitem; + const ssize_t len = sizeof(missionitem); + + if (dm_read((dm_item_t)mission.dataman_id, i, &missionitem, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + return false; + } + + if (missionitem.nav_cmd == NAV_CMD_LAND) { + _has_landing = true; + } + } + + return true; +} + +bool +MissionFeasibilityChecker::checkFixedWingLanding(const mission_s &mission) { /* Go through all mission items and search for a landing waypoint * if landing waypoint is found: the previous waypoint is checked to be at a feasible distance and altitude given the landing slope */ bool landing_valid = false; - bool land_start_found = false; size_t do_land_start_index = 0; size_t landing_approach_index = 0; @@ -475,14 +457,14 @@ MissionFeasibilityChecker::checkFixedWingLanding(const mission_s &mission, bool // if DO_LAND_START found then require valid landing AFTER if (missionitem.nav_cmd == NAV_CMD_DO_LAND_START) { - if (land_start_found) { + if (_has_landing) { mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: more than one land start.\t"); events::send(events::ID("navigator_mis_multiple_land"), {events::Log::Error, events::LogInternal::Info}, "Mission rejected: more than one land start commands"); return false; } else { - land_start_found = true; + _has_landing = true; do_land_start_index = i; } } @@ -615,7 +597,7 @@ MissionFeasibilityChecker::checkFixedWingLanding(const mission_s &mission, bool } } else if (missionitem.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { - if (land_start_found && do_land_start_index < i) { + if (_has_landing && do_land_start_index < i) { mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: land start item before RTL item not possible.\t"); events::send(events::ID("navigator_mis_land_before_rtl"), {events::Log::Error, events::LogInternal::Info}, @@ -625,14 +607,7 @@ MissionFeasibilityChecker::checkFixedWingLanding(const mission_s &mission, bool } } - if (land_start_req && !land_start_found) { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: landing pattern required.\t"); - events::send(events::ID("navigator_mis_land_missing"), {events::Log::Error, events::LogInternal::Info}, - "Mission rejected: landing pattern required"); - return false; - } - - if (land_start_found && (!landing_valid || (do_land_start_index > landing_approach_index))) { + if (_has_landing && (!landing_valid || (do_land_start_index > landing_approach_index))) { mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: invalid land start.\t"); events::send(events::ID("navigator_mis_invalid_land"), {events::Log::Error, events::LogInternal::Info}, "Mission rejected: invalid land start"); @@ -644,12 +619,10 @@ MissionFeasibilityChecker::checkFixedWingLanding(const mission_s &mission, bool } bool -MissionFeasibilityChecker::checkVTOLLanding(const mission_s &mission, bool land_start_req) +MissionFeasibilityChecker::checkVTOLLanding(const mission_s &mission) { - /* Go through all mission items and search for a landing waypoint - * if landing waypoint is found: the previous waypoint is checked to be at a feasible distance and altitude given the landing slope */ + // Go through all mission items and search for a landing waypoint, then run some checks on it - bool land_start_found = false; size_t do_land_start_index = 0; size_t landing_approach_index = 0; @@ -664,14 +637,14 @@ MissionFeasibilityChecker::checkVTOLLanding(const mission_s &mission, bool land_ // if DO_LAND_START found then require valid landing AFTER if (missionitem.nav_cmd == NAV_CMD_DO_LAND_START) { - if (land_start_found) { + if (_has_landing) { mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: more than one land start.\t"); events::send(events::ID("navigator_mis_multi_land"), {events::Log::Error, events::LogInternal::Info}, "Mission rejected: more than one land start commands"); return false; } else { - land_start_found = true; + _has_landing = true; do_land_start_index = i; } } @@ -696,7 +669,7 @@ MissionFeasibilityChecker::checkVTOLLanding(const mission_s &mission, bool land_ } } else if (missionitem.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { - if (land_start_found && do_land_start_index < i) { + if (_has_landing && do_land_start_index < i) { mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: land start item before RTL item not possible.\t"); events::send(events::ID("navigator_mis_land_before_rtl2"), {events::Log::Error, events::LogInternal::Info}, @@ -706,14 +679,7 @@ MissionFeasibilityChecker::checkVTOLLanding(const mission_s &mission, bool land_ } } - if (land_start_req && !land_start_found) { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: landing pattern required.\t"); - events::send(events::ID("navigator_mis_land_missing2"), {events::Log::Error, events::LogInternal::Info}, - "Mission rejected: landing pattern required"); - return false; - } - - if (land_start_found && (do_land_start_index > landing_approach_index)) { + if (_has_landing && (do_land_start_index > landing_approach_index)) { mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: invalid land start.\t"); events::send(events::ID("navigator_mis_invalid_land2"), {events::Log::Error, events::LogInternal::Info}, "Mission rejected: invalid land start"); @@ -724,6 +690,74 @@ MissionFeasibilityChecker::checkVTOLLanding(const mission_s &mission, bool land_ return true; } +bool +MissionFeasibilityChecker::checkTakeoffLandAvailable() +{ + bool result = true; + + switch (_navigator->get_takeoff_land_required()) { + case 0: + result = true; + break; + + case 1: + result = _has_takeoff; + + if (!result) { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Takeoff waypoint required.\t"); + events::send(events::ID("navigator_mis_takeoff_missing"), {events::Log::Error, events::LogInternal::Info}, + "Mission rejected: Takeoff waypoint required"); + return false; + } + + break; + + case 2: + result = _has_landing; + + if (!result) { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Landing waypoint/pattern required.\t"); + events::send(events::ID("navigator_mis_land_missing"), {events::Log::Error, events::LogInternal::Info}, + "Mission rejected: Landing waypoint/pattern required"); + } + + break; + + case 3: + result = _has_takeoff && _has_landing; + + if (!result) { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Takeoff or Landing item missing.\t"); + events::send(events::ID("navigator_mis_takeoff_or_land_missing"), {events::Log::Error, events::LogInternal::Info}, + "Mission rejected: Takeoff or Landing item missing"); + } + + break; + + case 4: + result = _has_takeoff == _has_landing; + + if (!result && (_has_takeoff)) { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Add Landing item or remove Takeoff.\t"); + events::send(events::ID("navigator_mis_add_land_or_rm_to"), {events::Log::Error, events::LogInternal::Info}, + "Mission rejected: Add Landing item or remove Takeoff"); + + } else if (!result && (_has_landing)) { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Add Takeoff item or remove Landing.\t"); + events::send(events::ID("navigator_mis_add_to_or_rm_land"), {events::Log::Error, events::LogInternal::Info}, + "Mission rejected: Add Takeoff item or remove Landing"); + } + + break; + + default: + result = true; + break; + } + + return result; +} + bool MissionFeasibilityChecker::checkDistanceToFirstWaypoint(const mission_s &mission, float max_distance) { diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index da05399291..0cb7f699ea 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -56,26 +56,18 @@ private: /* Checks for all airframes */ bool checkGeofence(const mission_s &mission, float home_alt, bool home_valid); - bool checkHomePositionAltitude(const mission_s &mission, float home_alt, bool home_alt_valid); - bool checkMissionItemValidity(const mission_s &mission); - bool checkDistanceToFirstWaypoint(const mission_s &mission, float max_distance); bool checkDistancesBetweenWaypoints(const mission_s &mission, float max_distance); - bool checkTakeoff(const mission_s &mission, float home_alt); + bool checkTakeoffLandAvailable(); + bool checkRotaryWingLanding(const mission_s &mission); + bool checkFixedWingLanding(const mission_s &mission); + bool checkVTOLLanding(const mission_s &mission); - /* Checks specific to fixedwing airframes */ - bool checkFixedwing(const mission_s &mission, float home_alt, bool land_start_req); - bool checkFixedWingLanding(const mission_s &mission, bool land_start_req); - - /* Checks specific to rotarywing airframes */ - bool checkRotarywing(const mission_s &mission, float home_alt); - - /* Checks specific to VTOL airframes */ - bool checkVTOL(const mission_s &mission, float home_alt, bool land_start_req); - bool checkVTOLLanding(const mission_s &mission, bool land_start_req); + bool _has_takeoff{false}; + bool _has_landing{false}; public: MissionFeasibilityChecker(Navigator *navigator) : ModuleParams(nullptr), _navigator(navigator) {} @@ -88,6 +80,5 @@ public: * Returns true if mission is feasible and false otherwise */ bool checkMissionFeasible(const mission_s &mission, - float max_distance_to_1st_waypoint, float max_distance_between_waypoints, - bool land_start_req); + float max_distance_to_1st_waypoint, float max_distance_between_waypoints); }; diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index 29133fcfd1..584e5a9ac5 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -58,14 +58,19 @@ PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 2.5f); /** - * Take-off waypoint required + * Mission takeoff/landing required * - * If set, the mission feasibility checker will check for a takeoff waypoint on the mission. + * Specifies if a mission has to contain a takeoff and/or mission landing. + * Validity of configured takeoffs/landings is checked independently of the setting here. * - * @boolean + * @value 0 No requirements + * @value 1 Require a takeoff + * @value 2 Require a landing + * @value 3 Require a takeoff and a landing + * @value 4 Require a takeoff and a landing, or neither of both * @group Mission */ -PARAM_DEFINE_INT32(MIS_TAKEOFF_REQ, 0); +PARAM_DEFINE_INT32(MIS_TKO_LAND_REQ, 0); /** * Minimum Loiter altitude diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 004f8e1f4a..f191fdeda5 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -297,8 +297,6 @@ public: float get_mission_landing_loiter_radius() { return _mission.get_landing_loiter_rad(); } // RTL - bool mission_landing_required() { return _rtl.get_rtl_type() == RTL::RTL_TYPE_MISSION_LANDING; } - bool in_rtl_state() const { return _vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; } bool abort_landing(); @@ -308,7 +306,7 @@ public: // Param access float get_loiter_min_alt() const { return _param_mis_ltrmin_alt.get(); } float get_takeoff_min_alt() const { return _param_mis_takeoff_alt.get(); } - bool get_takeoff_required() const { return _param_mis_takeoff_req.get(); } + int get_takeoff_land_required() const { return _para_mis_takeoff_land_req.get(); } float get_yaw_timeout() const { return _param_mis_yaw_tmt.get(); } float get_yaw_threshold() const { return math::radians(_param_mis_yaw_err.get()); } float get_lndmc_alt_max() const { return _param_lndmc_alt_max.get(); } @@ -451,7 +449,7 @@ private: // non-navigator parameters: Mission (MIS_*) (ParamFloat) _param_mis_ltrmin_alt, (ParamFloat) _param_mis_takeoff_alt, - (ParamBool) _param_mis_takeoff_req, + (ParamInt) _para_mis_takeoff_land_req, (ParamFloat) _param_mis_yaw_tmt, (ParamFloat) _param_mis_yaw_err, (ParamFloat) _param_mis_payload_delivery_timeout,