diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-09-28 12:36:26 +0200 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-09-28 12:36:26 +0200 |
commit | 3efffb68e7c816dbf21d143a1c39de93f1b5b400 (patch) | |
tree | ac0545f1e2598092e22be0c2f0ce783bb760e763 /src/modules/navigator/mission_feasibility_checker.cpp | |
parent | d113fcfc54c246f3d5ac22ad5485f7103aecab41 (diff) | |
parent | 8a18cfa3869555389e7e9ff8f104d83f9c54cb43 (diff) | |
download | px4-firmware-3efffb68e7c816dbf21d143a1c39de93f1b5b400.tar.gz px4-firmware-3efffb68e7c816dbf21d143a1c39de93f1b5b400.tar.bz2 px4-firmware-3efffb68e7c816dbf21d143a1c39de93f1b5b400.zip |
Merge remote-tracking branch 'upstream/master' into HEAD
Conflicts:
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
src/modules/navigator/geofence.cpp
src/modules/navigator/mission.cpp
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 937e4fa5a..389cdd0d2 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() |