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::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;
}
