bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRotarywing,
dm_item_t dm_current, size_t nMissionItems, Geofence &geofence,
float home_alt, bool home_valid, double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued,
float default_acceptance_rad,
bool condition_landed)
{
bool failed = false;
bool warned = false;
/* Init if not done yet */
init();
_mavlink_fd = mavlink_fd;
// first check if we have a valid position
if (!home_valid /* can later use global / local pos for finer granularity */) {
failed = true;
warned = true;
mavlink_log_info(_mavlink_fd, "Not yet ready for mission, no position lock.");
} else {
failed = failed || !check_dist_1wp(dm_current, nMissionItems, curr_lat, curr_lon, max_waypoint_distance, warning_issued);
}
// check if all mission item commands are supported
failed = failed || !checkMissionItemValidity(dm_current, nMissionItems, condition_landed);
failed = failed || !checkGeofence(dm_current, nMissionItems, geofence);
failed = failed || !checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid, warned);
if (isRotarywing) {
failed = failed || !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid, default_acceptance_rad);
} else {
failed = failed || !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt, home_valid);
}
return !failed;
}
bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float 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::checkMissionFeasible(const mission_s &mission,
float max_distance_to_1st_waypoint, float max_distance_between_waypoints,
bool land_start_req)
{
dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(mission.dataman_id);
const size_t nMissionItems = mission.count;
const bool isRotarywing = (_navigator->get_vstatus()->is_rotary_wing || _navigator->get_vstatus()->is_vtol);
Geofence &geofence = _navigator->get_geofence();
fw_pos_ctrl_status_s *fw_pos_ctrl_status = _navigator->get_fw_pos_ctrl_status();
const float home_alt = _navigator->get_home_position()->alt;
const bool home_valid = _navigator->home_position_valid();
const bool home_alt_valid = _navigator->home_alt_valid();
bool &warning_issued = _navigator->get_mission_result()->warning;
const float default_acceptance_rad = _navigator->get_default_acceptance_radius();
const float default_altitude_acceptance_rad = _navigator->get_altitude_acceptance_radius();
const bool landed = _navigator->get_land_detected()->landed;
bool failed = false;
bool warned = false;
// first check if we have a valid position
if (!home_alt_valid) {
failed = true;
warned = true;
mavlink_log_info(_navigator->get_mavlink_log_pub(), "Not yet ready for mission, no position lock.");
} else {
failed = failed ||
!checkDistanceToFirstWaypoint(dm_current, nMissionItems, max_distance_to_1st_waypoint, warning_issued);
}
// check if all mission item commands are supported
failed = failed || !checkMissionItemValidity(dm_current, nMissionItems, landed);
failed = failed
|| !checkDistancesBetweenWaypoints(dm_current, nMissionItems, max_distance_between_waypoints, warning_issued);
failed = failed || !checkGeofence(dm_current, nMissionItems, geofence, home_alt, home_valid);
failed = failed || !checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_alt_valid, warned);
if (isRotarywing) {
failed = failed
|| !checkRotarywing(dm_current, nMissionItems, home_alt, home_alt_valid, default_altitude_acceptance_rad);
} else {
failed = failed
|| !checkFixedwing(dm_current, nMissionItems, fw_pos_ctrl_status, home_alt, home_alt_valid,
default_acceptance_rad, land_start_req);
}
return !failed;
}
bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
{
/* Update fixed wing navigation capabilites */
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);
/* 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::checkMissionFeasible(const mission_s &mission,
float max_distance_to_1st_waypoint, float max_distance_between_waypoints,
bool land_start_req)
{
bool failed = false;
bool warned = false;
// first check if we have a valid position
const bool home_valid = _navigator->home_position_valid();
const bool home_alt_valid = _navigator->home_alt_valid();
if (!home_alt_valid) {
failed = true;
warned = true;
mavlink_log_info(_navigator->get_mavlink_log_pub(), "Not yet ready for mission, no position lock.");
} else {
failed = failed || !checkDistanceToFirstWaypoint(mission, max_distance_to_1st_waypoint);
}
const float home_alt = _navigator->get_home_position()->alt;
// check if all mission item commands are supported
failed = failed || !checkMissionItemValidity(mission);
failed = failed || !checkDistancesBetweenWaypoints(mission, max_distance_between_waypoints);
failed = failed || !checkGeofence(mission, home_alt, home_valid);
failed = failed || !checkHomePositionAltitude(mission, home_alt, home_alt_valid, warned);
// VTOL always respects rotary wing feasibility
if (_navigator->get_vstatus()->is_rotary_wing || _navigator->get_vstatus()->is_vtol) {
failed = failed || !checkRotarywing(mission, home_alt, home_alt_valid);
} else {
failed = failed || !checkFixedwing(mission, home_alt, home_alt_valid, land_start_req);
}
return !failed;
}
bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
{
/* Update fixed wing navigation capabilites */
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));
}
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));
}
