From e68ba81d6dd6bbe136f561760c94a256b7748216 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 15 Nov 2022 11:14:50 +0100 Subject: [PATCH] MissionFeasibilityChecker: for MC only find if landing is present, but don't do any validation Signed-off-by: Silvan Fuhrer --- .../navigator/mission_feasibility_checker.cpp | 36 +++++++++++-------- .../navigator/mission_feasibility_checker.h | 6 ++-- 2 files changed, 24 insertions(+), 18 deletions(-) diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 72c870966c..f7561222d1 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -76,7 +76,7 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission, events::send(events::ID("navigator_mis_no_pos_lock"), events::Log::Info, "Not yet ready for mission, no position lock"); } else { - failed = failed || !checkDistanceToFirstWaypoint(mission, max_distance_to_1st_waypoint); + failed |= !checkDistanceToFirstWaypoint(mission, max_distance_to_1st_waypoint); } const float home_alt = _navigator->get_home_position()->alt; @@ -85,23 +85,27 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission, _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); + // run generic (for all vehicle types) checks + failed |= !checkMissionItemValidity(mission); + failed |= !checkDistancesBetweenWaypoints(mission, max_distance_between_waypoints); + failed |= !checkGeofence(mission, home_alt, home_valid); + failed |= !checkHomePositionAltitude(mission, home_alt, home_alt_valid); + failed |= !checkTakeoff(mission, home_alt); + // run type-specifc landing checks, which also include seting _has_landing that is used in checkTakeoffLandAvailable() if (_navigator->get_vstatus()->is_vtol) { - failed |= (!checkTakeoff(mission, home_alt) || !checkVTOLLanding(mission) || !checkTakeoffLandAvailable()); - - } else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - failed |= (!checkTakeoff(mission, home_alt) || !checkRotaryWingLanding(mission) || !checkTakeoffLandAvailable()); + failed |= !checkVTOLLanding(mission); } else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { - failed |= (!checkTakeoff(mission, home_alt) || !checkFixedWingLanding(mission) || !checkTakeoffLandAvailable()); + failed |= !checkFixedWingLanding(mission); + + } else { + // if neither VTOL nor FW, only check if mission has landing but don't check that one for validity + _has_landing = hasMissionLanding(mission); } + failed |= !checkTakeoffLandAvailable(); + return !failed; } @@ -413,11 +417,13 @@ MissionFeasibilityChecker::checkTakeoff(const mission_s &mission, float home_alt } bool -MissionFeasibilityChecker::checkRotaryWingLanding(const mission_s &mission) +MissionFeasibilityChecker::hasMissionLanding(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. + bool mission_landing_found = false; + for (size_t i = 0; i < mission.count; i++) { struct mission_item_s missionitem; const ssize_t len = sizeof(missionitem); @@ -428,11 +434,11 @@ MissionFeasibilityChecker::checkRotaryWingLanding(const mission_s &mission) } if (missionitem.nav_cmd == NAV_CMD_LAND) { - _has_landing = true; + mission_landing_found = true; } } - return true; + return mission_landing_found; } bool diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index ec99536640..948f6f1553 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -125,12 +125,12 @@ private: bool checkTakeoffLandAvailable(); /** - * @brief Rotary-wing vehicle: Check if mission contains landing (safe in _has_landing), and if that landing is valid + * @brief Check if mission contains landing * * @param mission Mission struct - * @return True if the planned landing is valid, or no landing planned, False otherwise + * @return True if mission contains a landing, False if otherwise */ - bool checkRotaryWingLanding(const mission_s &mission); + bool hasMissionLanding(const mission_s &mission); /** * @brief Fixed-wing vehicle: Check if mission contains landing (safe in _has_landing), and if that landing is valid