From c01567c047f295e3ffea7d01a7e239ac4f79a498 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 17 Jul 2014 21:02:17 +0200 Subject: [PATCH 1/2] Check if waypoint altitude is relative. Fixes #1197 --- src/modules/navigator/mission_feasibility_checker.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index dd7f4c8013..d45488d5a3 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -135,12 +135,15 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, } } - if (home_alt > missionitem.altitude) { + /* calculate the global waypoint altitude */ + float wp_alt = (missionitem.altitude_is_relative) ? missionitem.altitude + home_alt ? missionitem.altitude; + + if (home_alt > wp_alt) { if (throw_error) { - mavlink_log_info(_mavlink_fd, "Waypoint %d below home", i); + mavlink_log_critical(_mavlink_fd, "Warning: Waypoint %d below home", i); return false; } else { - mavlink_log_info(_mavlink_fd, "#audio: warning waypoint %d below home", i); + mavlink_log_critical(_mavlink_fd, "Rejecting Mission: Waypoint %d below home", i); return true; } } From 101e7b1383b83c9b06ef147dbef1ff375c11b84f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 17 Jul 2014 21:06:42 +0200 Subject: [PATCH 2/2] navigator: Feedback strings / text and logic was not consistent in previous state, fixed. --- src/modules/navigator/mission_feasibility_checker.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index d45488d5a3..606521f200 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -136,14 +136,14 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, } /* calculate the global waypoint altitude */ - float wp_alt = (missionitem.altitude_is_relative) ? missionitem.altitude + home_alt ? missionitem.altitude; + float wp_alt = (missionitem.altitude_is_relative) ? missionitem.altitude + home_alt : missionitem.altitude; if (home_alt > wp_alt) { if (throw_error) { - mavlink_log_critical(_mavlink_fd, "Warning: Waypoint %d below home", i); + mavlink_log_critical(_mavlink_fd, "Rejecting Mission: Waypoint %d below home", i); return false; } else { - mavlink_log_critical(_mavlink_fd, "Rejecting Mission: Waypoint %d below home", i); + mavlink_log_critical(_mavlink_fd, "Warning: Waypoint %d below home", i); return true; } }