From ec09f080083fc297228857397eef0651085887d9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 22 Sep 2014 17:20:50 +0200 Subject: mf checker: fix landing check, ensure feedback from all checks is sent --- .../navigator/mission_feasibility_checker.cpp | 20 +++++++++++++++----- 1 file changed, 15 insertions(+), 5 deletions(-) (limited to 'src/modules') diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 606521f20..7ae7f60cb 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -84,7 +84,12 @@ bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_ bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) { - return (checkGeofence(dm_current, nMissionItems, geofence) && checkHomePositionAltitude(dm_current, nMissionItems, home_alt)); + /* Perform checks and issue feedback to the user for all checks */ + bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence); + bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt); + + /* Mission is only marked as feasible if all checks return true */ + return (resGeofence && resHomeAltitude); } bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) @@ -93,7 +98,13 @@ bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_curre 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) && checkHomePositionAltitude(dm_current, nMissionItems, home_alt)); + /* Perform checks and issue feedback to the user for all checks */ + bool resLanding = checkFixedWingLanding(dm_current, nMissionItems); + bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence); + bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt); + + /* Mission is only marked as feasible if all checks return true */ + return (resLanding && resGeofence && resHomeAltitude); } bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) @@ -216,9 +227,8 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size } } - -// float slope_alt = wp_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_distance)/_flare_constant) - _H1_virt; - return false; + /* No landing waypoints or no waypoints */ + return true; } void MissionFeasibilityChecker::updateNavigationCapabilities() -- cgit v1.2.3