diff options
| author | Julian Oes <julian@oes.ch> | 2014-09-23 12:13:30 +1000 |
|---|---|---|
| committer | Julian Oes <julian@oes.ch> | 2014-09-23 12:13:30 +1000 |
| commit | 15eee418a035a0109bc0a33fa5889352aa3a1799 (patch) | |
| tree | 5c47bd4ebe09989c44a8f12d7cfb2789f56bdd9a | |
| parent | 17a29dba92f83ba055ccd8c84b8c2d5dfd260bbc (diff) | |
| parent | 73aa7d81e31e18a6d1405697856e389a7a7b89af (diff) | |
| download | px4-firmware-15eee418a035a0109bc0a33fa5889352aa3a1799.tar.gz px4-firmware-15eee418a035a0109bc0a33fa5889352aa3a1799.tar.bz2 px4-firmware-15eee418a035a0109bc0a33fa5889352aa3a1799.zip | |
Merge pull request #20 from swissfang/swissfang_mfcheck
mf checker: fix landing check, ensure feedback from all checks is sent
| -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() |