forked from Archive/PX4-Autopilot
mf checker: fix landing check, ensure feedback from all checks is sent
This commit is contained in:
parent
feb848c12e
commit
ec09f08008
|
@ -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)
|
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)
|
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();
|
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);
|
// 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)
|
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
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* No landing waypoints or no waypoints */
|
||||||
// float slope_alt = wp_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_distance)/_flare_constant) - _H1_virt;
|
return true;
|
||||||
return false;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void MissionFeasibilityChecker::updateNavigationCapabilities()
|
void MissionFeasibilityChecker::updateNavigationCapabilities()
|
||||||
|
|
Loading…
Reference in New Issue