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);