From 88fe2d3052eccd542ffb5d3473d720b66b8679fd Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 24 Feb 2014 19:04:26 +0100 Subject: mission feasibility checker: add check for waypoint altitude relative to home position altitude --- .../navigator/mission_feasibility_checker.cpp | 43 ++++++++++++++++++---- 1 file changed, 35 insertions(+), 8 deletions(-) (limited to 'src/modules/navigator/mission_feasibility_checker.cpp') diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index eaafa217d..41670e247 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -62,7 +62,7 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capab } -bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) +bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) { /* Init if not done yet */ init(); @@ -75,24 +75,24 @@ bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_ if (isRotarywing) - return checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence); + return checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt); else - return checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence); + return checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt); } -bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) +bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) { - return checkGeofence(dm_current, nMissionItems, geofence); + return (checkGeofence(dm_current, nMissionItems, geofence) && checkHomePositionAltitude(dm_current, nMissionItems, home_alt)); } -bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) +bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) { /* Update fixed wing navigation capabilites */ updateNavigationCapabilities(); // warnx("_nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_horizontal_slope_displacement %.4f", _nav_caps.landing_slope_angle_rad, _nav_caps.landing_horizontal_slope_displacement); - return (checkFixedWingLanding(dm_current, nMissionItems) && checkGeofence(dm_current, nMissionItems, geofence)); + return (checkFixedWingLanding(dm_current, nMissionItems) && checkGeofence(dm_current, nMissionItems, geofence) && checkHomePositionAltitude(dm_current, nMissionItems, home_alt)); } bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) @@ -108,7 +108,7 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss return false; } - if (!geofence.inside(missionitem.lat, missionitem.lon, missionitem.altitude)) { //xxx: handle relative altitude + if (!geofence.inside(missionitem.lat, missionitem.lon, missionitem.altitude)) { mavlink_log_info(_mavlink_fd, "#audio: Geofence violation waypoint %d", i); return false; } @@ -118,6 +118,33 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss return true; } +bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool throw_error) +{ + /* Check if all all waypoints are above the home altitude, only return false if bool throw_error = true */ + for (size_t i = 0; i < nMissionItems; i++) { + static struct mission_item_s missionitem; + const ssize_t len = sizeof(struct mission_item_s); + + if (dm_read(dm_current, i, &missionitem, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + if (throw_error) { + return false; + } else { + return true; + } + } + + if (home_alt > missionitem.altitude) { + mavlink_log_info(_mavlink_fd, "#audio: warning waypoint %d below home", i); + if (throw_error) { + return false; + } else { + return true; + } + } + } +} + bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems) { /* Go through all mission items and search for a landing waypoint -- cgit v1.2.3 From 053ad5b6388ef653d1dfe255ea4f3eb00aeccaba Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 24 Feb 2014 22:19:21 +0100 Subject: mission feasibility checker: remove audio warning for waypoint below home altitude --- src/modules/navigator/mission_feasibility_checker.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) (limited to 'src/modules/navigator/mission_feasibility_checker.cpp') diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 41670e247..b41e9d355 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -135,10 +135,11 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, } if (home_alt > missionitem.altitude) { - mavlink_log_info(_mavlink_fd, "#audio: warning waypoint %d below home", i); if (throw_error) { + mavlink_log_info(_mavlink_fd, "Waypoint %d below home", i); return false; } else { + mavlink_log_info(_mavlink_fd, "#audio: warning waypoint %d below home", i); return true; } } -- cgit v1.2.3 From c6e2ad918b50f01ec9e26ccd7fdb88c7c0d2a60c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 25 Feb 2014 00:34:51 +0100 Subject: mission feasibility checker: add missing default return in checkHomePositionAltitude --- src/modules/navigator/mission_feasibility_checker.cpp | 2 ++ 1 file changed, 2 insertions(+) (limited to 'src/modules/navigator/mission_feasibility_checker.cpp') diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index b41e9d355..02e35f1a8 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -144,6 +144,8 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, } } } + + return true; } bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems) -- cgit v1.2.3