diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2014-09-28 13:41:21 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2014-09-28 13:41:21 +0400 |
commit | 1cc5f3778dd6c18f3a14c64959d28f978b0305ef (patch) | |
tree | f2b8138e6f85b5f760fa22bbe3aecbce61539277 /src/modules/navigator/mission_feasibility_checker.cpp | |
parent | cf4604f5c3614bc5cd1b82dfee693cbac36181ab (diff) | |
parent | 8a18cfa3869555389e7e9ff8f104d83f9c54cb43 (diff) | |
download | px4-firmware-1cc5f3778dd6c18f3a14c64959d28f978b0305ef.tar.gz px4-firmware-1cc5f3778dd6c18f3a14c64959d28f978b0305ef.tar.bz2 px4-firmware-1cc5f3778dd6c18f3a14c64959d28f978b0305ef.zip |
Merge branch 'master' into takeoff_fix
Diffstat (limited to 'src/modules/navigator/mission_feasibility_checker.cpp')
-rw-r--r-- | src/modules/navigator/mission_feasibility_checker.cpp | 20 |
1 files changed, 15 insertions, 5 deletions
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() |