dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mf checker: fix landing check, ensure feedback from all checks is sent

This commit is contained in:
Thomas Gubler 2014-09-22 17:20:50 +02:00
parent feb848c12e
commit ec09f08008
1 changed files with 15 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
src/modules/navigator/mission_feasibility_checker.cpp
View File

@ -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()