From 518222bae464647a34d261720b45a1443d044feb Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 19 May 2021 16:05:17 +0200 Subject: [PATCH] navigator: fix home altitude feasibility check This includes the following: - Remove warning/throw_error flag as it is always !home_alt_valid. - Remove impossible code path when home_alt_valid = false and throw_error = false. - Add home_alt_valid check in second check before using home_alt. - Only use warning for the second check to allow waypoints below home. - Don't return early when only warning. --- .../navigator/mission_feasibility_checker.cpp | 27 +++++-------------- .../navigator/mission_feasibility_checker.h | 2 +- 2 files changed, 7 insertions(+), 22 deletions(-) diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 4b5710c528..053ea9894d 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -67,7 +67,6 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission, } bool failed = false; - bool warned = false; // first check if we have a valid position const bool home_valid = _navigator->home_position_valid(); @@ -75,7 +74,6 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission, if (!home_alt_valid) { failed = true; - warned = true; mavlink_log_info(_navigator->get_mavlink_log_pub(), "Not yet ready for mission, no position lock."); } else { @@ -88,7 +86,7 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission, 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, warned); + failed = failed || !checkHomePositionAltitude(mission, home_alt, home_alt_valid); if (_navigator->get_vstatus()->is_vtol) { failed = failed || !checkVTOL(mission, home_alt, false); @@ -174,8 +172,7 @@ MissionFeasibilityChecker::checkGeofence(const mission_s &mission, float home_al } bool -MissionFeasibilityChecker::checkHomePositionAltitude(const mission_s &mission, float home_alt, bool home_alt_valid, - bool throw_error) +MissionFeasibilityChecker::checkHomePositionAltitude(const mission_s &mission, float home_alt, bool home_alt_valid) { /* Check if all waypoints are above the home altitude */ for (size_t i = 0; i < mission.count; i++) { @@ -193,31 +190,19 @@ MissionFeasibilityChecker::checkHomePositionAltitude(const mission_s &mission, f _navigator->get_mission_result()->warning = true; - if (throw_error) { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: No home pos, WP %zu uses rel alt", i + 1); - return false; + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: No home pos, WP %zu uses rel alt", i + 1); + return false; - } else { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Warning: No home pos, WP %zu uses rel alt", i + 1); - return true; - } } /* calculate the global waypoint altitude */ float wp_alt = (missionitem.altitude_is_relative) ? missionitem.altitude + home_alt : missionitem.altitude; - if ((home_alt > wp_alt) && MissionBlock::item_contains_position(missionitem)) { + if (home_alt_valid && home_alt > wp_alt && MissionBlock::item_contains_position(missionitem)) { _navigator->get_mission_result()->warning = true; - if (throw_error) { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Waypoint %zu below home", i + 1); - return false; - - } else { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Warning: Waypoint %zu below home", i + 1); - return true; - } + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Warning: Waypoint %zu below home", i + 1); } } diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index 97c6e165f7..6091f31347 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -56,7 +56,7 @@ 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 throw_error); + bool checkHomePositionAltitude(const mission_s &mission, float home_alt, bool home_alt_valid); bool checkMissionItemValidity(const mission_s &mission);