diff --git a/src/modules/navigator/CMakeLists.txt b/src/modules/navigator/CMakeLists.txt index ce5abd72bd..08eb898a74 100644 --- a/src/modules/navigator/CMakeLists.txt +++ b/src/modules/navigator/CMakeLists.txt @@ -32,6 +32,7 @@ ############################################################################ add_subdirectory(GeofenceBreachAvoidance) +add_subdirectory(MissionFeasibility) px4_add_module( MODULE modules__navigator @@ -53,4 +54,5 @@ px4_add_module( geo geofence_breach_avoidance motion_planning + mission_feasibility_checker ) diff --git a/src/modules/navigator/MissionFeasibility/CMakeLists.txt b/src/modules/navigator/MissionFeasibility/CMakeLists.txt new file mode 100644 index 0000000000..d2dc9ad1d9 --- /dev/null +++ b/src/modules/navigator/MissionFeasibility/CMakeLists.txt @@ -0,0 +1,4 @@ +px4_add_library(mission_feasibility_checker + FeasibilityChecker.hpp + FeasibilityChecker.cpp +) diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp new file mode 100644 index 0000000000..86e5a56533 --- /dev/null +++ b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp @@ -0,0 +1,692 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "FeasibilityChecker.hpp" +#include +#include +#include +#include "../mission_block.h" +#include +#include + +FeasibilityChecker::FeasibilityChecker() : + ModuleParams(nullptr) +{ +} + +void FeasibilityChecker::reset() +{ + _mission_validity_failed = false; + _takeoff_failed = false; + _fixed_wing_landing_failed = false; + _distance_first_waypoint_failed = false; + _distance_between_waypoints_failed = false; + _below_home_alt_failed = false; + _fixed_wing_land_approach_failed = false; + _takeoff_land_available_failed = false; + + _found_item_with_position = false; + _has_vtol_takeoff = false; + _has_takeoff = false; + + _landing_valid = false; + _do_land_start_index = -1; + _landing_approach_index = -1; + _mission_item_previous = {}; + + _first_waypoint_found = false; + + _last_lat = NAN; + _last_lon = NAN; + _last_cmd = -1; +} + +void FeasibilityChecker::updateData() +{ + home_position_s home = {}; + + if (_home_pos_sub.copy(&home)) { + if (home.valid_hpos) { + _home_lat_lon = matrix::Vector2d(home.lat, home.lon); + } + + if (home.valid_alt) { + _home_alt_msl = home.alt; + } + } + + vehicle_status_s status = {}; + + if (_status_sub.copy(&status)) { + _vehicle_type = status.vehicle_type; + _is_vtol = status.is_vtol; + } + + vehicle_land_detected_s land_detected = {}; + + if (_land_detector_sub.copy(&land_detected)) { + _is_landed = land_detected.landed; + } + +} + +void FeasibilityChecker::processNextItem(mission_item_s &mission_item, const int current_index, const int total_count) +{ + if (current_index == 0) { + reset(); + updateData(); + } + + doCommonChecks(mission_item, current_index); + + if (_is_vtol) { + doVtolChecks(mission_item, current_index); + + } else if (_vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + doFixedWingChecks(mission_item, current_index); + + } else { + doMulticopterChecks(mission_item, current_index); + } + + if (current_index == total_count) { + _takeoff_land_available_failed = !checkTakeoffLandAvailable(); + } + + _mission_item_previous = mission_item; +} + +void FeasibilityChecker::doCommonChecks(mission_item_s &mission_item, const int current_index) +{ + if (!_mission_validity_failed) { + _mission_validity_failed = !checkMissionItemValidity(mission_item, current_index); + } + + if (!_distance_between_waypoints_failed) { + _distance_between_waypoints_failed = !checkDistancesBetweenWaypoints(mission_item); + } + + if (!_distance_first_waypoint_failed) { + _distance_first_waypoint_failed = !checkDistanceToFirstWaypoint(mission_item); + } + + if (!_below_home_alt_failed) { + _below_home_alt_failed = !checkIfBelowHomeAltitude(mission_item, current_index); + } + + if (!_takeoff_failed) { + _takeoff_failed = !checkTakeoff(mission_item); + } +} + +void FeasibilityChecker::doVtolChecks(mission_item_s &mission_item, const int current_index) +{ + if (!_fixed_wing_landing_failed) { + _fixed_wing_landing_failed = !checkFixedWingLanding(mission_item, current_index); + } +} + +void FeasibilityChecker::doFixedWingChecks(mission_item_s &mission_item, const int current_index) +{ + if (!_fixed_wing_landing_failed) { + _fixed_wing_landing_failed = !checkFixedWingLanding(mission_item, current_index); + } + + if (_fixed_wing_land_approach_failed) { + _fixed_wing_land_approach_failed = !checkFixedWindLandApproach(mission_item, current_index); + } + +} + +void FeasibilityChecker::doMulticopterChecks(mission_item_s &mission_item, const int current_index) +{ + // this flag is used for the checkTakeoffLandAvailable check at the very end + _landing_valid |= mission_item.nav_cmd == NAV_CMD_LAND; +} + +bool FeasibilityChecker::checkMissionItemValidity(mission_item_s &mission_item, const int current_index) +{ + // check if we find unsupported items and reject mission if so + if (mission_item.nav_cmd != NAV_CMD_IDLE && + mission_item.nav_cmd != NAV_CMD_WAYPOINT && + mission_item.nav_cmd != NAV_CMD_LOITER_UNLIMITED && + mission_item.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT && + mission_item.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH && + mission_item.nav_cmd != NAV_CMD_LAND && + mission_item.nav_cmd != NAV_CMD_TAKEOFF && + mission_item.nav_cmd != NAV_CMD_LOITER_TO_ALT && + mission_item.nav_cmd != NAV_CMD_VTOL_TAKEOFF && + mission_item.nav_cmd != NAV_CMD_VTOL_LAND && + mission_item.nav_cmd != NAV_CMD_DELAY && + mission_item.nav_cmd != NAV_CMD_CONDITION_GATE && + mission_item.nav_cmd != NAV_CMD_DO_WINCH && + mission_item.nav_cmd != NAV_CMD_DO_GRIPPER && + mission_item.nav_cmd != NAV_CMD_DO_JUMP && + mission_item.nav_cmd != NAV_CMD_DO_CHANGE_SPEED && + mission_item.nav_cmd != NAV_CMD_DO_SET_HOME && + mission_item.nav_cmd != NAV_CMD_DO_SET_SERVO && + mission_item.nav_cmd != NAV_CMD_DO_LAND_START && + mission_item.nav_cmd != NAV_CMD_DO_TRIGGER_CONTROL && + mission_item.nav_cmd != NAV_CMD_DO_DIGICAM_CONTROL && + mission_item.nav_cmd != NAV_CMD_IMAGE_START_CAPTURE && + mission_item.nav_cmd != NAV_CMD_IMAGE_STOP_CAPTURE && + mission_item.nav_cmd != NAV_CMD_VIDEO_START_CAPTURE && + mission_item.nav_cmd != NAV_CMD_VIDEO_STOP_CAPTURE && + mission_item.nav_cmd != NAV_CMD_DO_CONTROL_VIDEO && + mission_item.nav_cmd != NAV_CMD_DO_MOUNT_CONFIGURE && + mission_item.nav_cmd != NAV_CMD_DO_MOUNT_CONTROL && + mission_item.nav_cmd != NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW && + mission_item.nav_cmd != NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE && + mission_item.nav_cmd != NAV_CMD_DO_SET_ROI && + mission_item.nav_cmd != NAV_CMD_DO_SET_ROI_LOCATION && + mission_item.nav_cmd != NAV_CMD_DO_SET_ROI_WPNEXT_OFFSET && + mission_item.nav_cmd != NAV_CMD_DO_SET_ROI_NONE && + mission_item.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_DIST && + mission_item.nav_cmd != NAV_CMD_OBLIQUE_SURVEY && + mission_item.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL && + mission_item.nav_cmd != NAV_CMD_SET_CAMERA_MODE && + mission_item.nav_cmd != NAV_CMD_SET_CAMERA_ZOOM && + mission_item.nav_cmd != NAV_CMD_SET_CAMERA_FOCUS && + mission_item.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION) { + + mavlink_log_critical(_mavlink_log_pub, "Mission rejected: item %i: unsupported cmd: %d\t", + (int)(current_index + 1), + (int)mission_item.nav_cmd); + events::send(events::ID("navigator_mis_unsup_cmd"), {events::Log::Error, events::LogInternal::Warning}, + "Mission rejected: item {1}: unsupported command: {2}", current_index + 1, mission_item.nav_cmd); + return false; + } + + /* Check non navigation item */ + if (mission_item.nav_cmd == NAV_CMD_DO_SET_SERVO) { + + /* check actuator number */ + if (mission_item.params[0] < 0 || mission_item.params[0] > 5) { + mavlink_log_critical(_mavlink_log_pub, "Actuator number %d is out of bounds 0..5\t", + (int)mission_item.params[0]); + events::send(events::ID("navigator_mis_act_index"), {events::Log::Error, events::LogInternal::Warning}, + "Actuator number {1} is out of bounds 0..5", (int)mission_item.params[0]); + return false; + } + + /* check actuator value */ + if (mission_item.params[1] < -PWM_DEFAULT_MAX || mission_item.params[1] > PWM_DEFAULT_MAX) { + mavlink_log_critical(_mavlink_log_pub, + "Actuator value %d is out of bounds -PWM_DEFAULT_MAX..PWM_DEFAULT_MAX\t", (int)mission_item.params[1]); + events::send(events::ID("navigator_mis_act_range"), {events::Log::Error, events::LogInternal::Warning}, + "Actuator value {1} is out of bounds -{2}..{2}", (int)mission_item.params[1], PWM_DEFAULT_MAX); + return false; + } + } + + // check if the mission starts with a land command while the vehicle is landed + if ((current_index == 0) && mission_item.nav_cmd == NAV_CMD_LAND && _is_landed) { + + mavlink_log_critical(_mavlink_log_pub, "Mission rejected: starts with landing\t"); + events::send(events::ID("navigator_mis_starts_w_landing"), {events::Log::Error, events::LogInternal::Info}, + "Mission rejected: starts with landing"); + return false; + } + + return true; +} + +bool FeasibilityChecker::checkTakeoff(mission_item_s &mission_item) +{ + + // look for a takeoff waypoint + if (mission_item.nav_cmd == NAV_CMD_TAKEOFF || mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF) { + // make sure that the altitude of the waypoint is at least one meter larger than the acceptance radius + // this makes sure that the takeoff waypoint is not reached before we are at least one meter in the air + + float takeoff_alt = mission_item.altitude_is_relative + ? mission_item.altitude + : mission_item.altitude - _home_alt_msl; + + float acceptance_radius = _param_nav_acc_rad.get(); + + if (mission_item.acceptance_radius > NAV_EPSILON_POSITION) { + acceptance_radius = mission_item.acceptance_radius; + } + + if (takeoff_alt - 1.0f < acceptance_radius) { + mavlink_log_critical(_mavlink_log_pub, "Mission rejected: Takeoff altitude too low!\t"); + events::send(events::ID("navigator_mis_takeoff_too_low"), {events::Log::Error, events::LogInternal::Info}, + "Mission rejected: takeoff altitude too low! Minimum: {1:.1m_v}", acceptance_radius + 1.f); + return false; + } + + if (!_has_vtol_takeoff) { + _has_vtol_takeoff = mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF; + } + + if (!_has_takeoff) { + _has_takeoff = true; + } + + + if (_found_item_with_position) { + mavlink_log_critical(_mavlink_log_pub, "Mission rejected: takeoff not first waypoint item\t"); + events::send(events::ID("navigator_mis_takeoff_not_first"), {events::Log::Error, events::LogInternal::Info}, + "Mission rejected: takeoff is not the first waypoint item"); + return false; + } + } + + if (!_found_item_with_position) { + _found_item_with_position = (mission_item.nav_cmd != NAV_CMD_IDLE && + mission_item.nav_cmd != NAV_CMD_DELAY && + mission_item.nav_cmd != NAV_CMD_DO_JUMP && + mission_item.nav_cmd != NAV_CMD_DO_CHANGE_SPEED && + mission_item.nav_cmd != NAV_CMD_DO_SET_HOME && + mission_item.nav_cmd != NAV_CMD_DO_SET_SERVO && + mission_item.nav_cmd != NAV_CMD_DO_LAND_START && + mission_item.nav_cmd != NAV_CMD_DO_TRIGGER_CONTROL && + mission_item.nav_cmd != NAV_CMD_DO_DIGICAM_CONTROL && + mission_item.nav_cmd != NAV_CMD_IMAGE_START_CAPTURE && + mission_item.nav_cmd != NAV_CMD_IMAGE_STOP_CAPTURE && + mission_item.nav_cmd != NAV_CMD_VIDEO_START_CAPTURE && + mission_item.nav_cmd != NAV_CMD_VIDEO_STOP_CAPTURE && + mission_item.nav_cmd != NAV_CMD_DO_CONTROL_VIDEO && + mission_item.nav_cmd != NAV_CMD_DO_MOUNT_CONFIGURE && + mission_item.nav_cmd != NAV_CMD_DO_MOUNT_CONTROL && + mission_item.nav_cmd != NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW && + mission_item.nav_cmd != NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE && + mission_item.nav_cmd != NAV_CMD_DO_SET_ROI && + mission_item.nav_cmd != NAV_CMD_DO_SET_ROI_LOCATION && + mission_item.nav_cmd != NAV_CMD_DO_SET_ROI_WPNEXT_OFFSET && + mission_item.nav_cmd != NAV_CMD_DO_SET_ROI_NONE && + mission_item.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_DIST && + mission_item.nav_cmd != NAV_CMD_OBLIQUE_SURVEY && + mission_item.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL && + mission_item.nav_cmd != NAV_CMD_SET_CAMERA_MODE && + mission_item.nav_cmd != NAV_CMD_SET_CAMERA_ZOOM && + mission_item.nav_cmd != NAV_CMD_SET_CAMERA_FOCUS && + mission_item.nav_cmd != NAV_CMD_SET_CAMERA_FOCUS && + mission_item.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION); + } + + return true; +} + +bool FeasibilityChecker::checkFixedWindLandApproach(mission_item_s &mission_item, const int current_index) +{ + if (mission_item.nav_cmd == NAV_CMD_LAND && current_index > 0) { + + if (MissionBlock::item_contains_position(_mission_item_previous)) { + + const float land_alt_amsl = mission_item.altitude_is_relative ? mission_item.altitude + + _home_alt_msl : mission_item.altitude; + const float entrance_alt_amsl = _mission_item_previous.altitude_is_relative ? _mission_item_previous.altitude + + _home_alt_msl : _mission_item_previous.altitude; + const float relative_approach_altitude = entrance_alt_amsl - land_alt_amsl; + + if (relative_approach_altitude < FLT_EPSILON) { + mavlink_log_critical(_mavlink_log_pub, + "Mission rejected: the approach waypoint must be above the landing point.\t"); + events::send(events::ID("navigator_mis_approach_wp_below_land"), {events::Log::Error, events::LogInternal::Info}, + "Mission rejected: the approach waypoint must be above the landing point"); + return false; + } + + float landing_approach_distance; + + if (_mission_item_previous.nav_cmd == NAV_CMD_LOITER_TO_ALT) { + // assume this is a fixed-wing landing pattern with orbit to alt followed + // by tangent exit to landing approach and touchdown at landing waypoint + + const float distance_orbit_center_to_land = get_distance_to_next_waypoint(_mission_item_previous.lat, + _mission_item_previous.lon, mission_item.lat, mission_item.lon); + const float orbit_radius = fabsf(_mission_item_previous.loiter_radius); + + if (distance_orbit_center_to_land <= orbit_radius) { + mavlink_log_critical(_mavlink_log_pub, + "Mission rejected: the landing point must be outside the orbit radius.\t"); + events::send(events::ID("navigator_mis_land_wp_inside_orbit_radius"), {events::Log::Error, events::LogInternal::Info}, + "Mission rejected: the landing point must be outside the orbit radius"); + return false; + } + + landing_approach_distance = sqrtf(distance_orbit_center_to_land * distance_orbit_center_to_land - orbit_radius * + orbit_radius); + + } else if (_mission_item_previous.nav_cmd == NAV_CMD_WAYPOINT) { + // approaching directly from waypoint position + + const float waypoint_distance = get_distance_to_next_waypoint(_mission_item_previous.lat, _mission_item_previous.lon, + mission_item.lat, mission_item.lon); + landing_approach_distance = waypoint_distance; + + } else { + mavlink_log_critical(_mavlink_log_pub, + "Mission rejected: unsupported landing approach entrance waypoint type. Only ORBIT_TO_ALT or WAYPOINT allowed.\t"); + events::send(events::ID("navigator_mis_unsupported_landing_approach_wp"), {events::Log::Error, events::LogInternal::Info}, + "Mission rejected: unsupported landing approach entrance waypoint type. Only ORBIT_TO_ALT or WAYPOINT allowed"); + return false; + } + + const float glide_slope = relative_approach_altitude / landing_approach_distance; + + // respect user setting as max glide slope, but account for floating point + // rounding on next check with small (arbitrary) 0.1 deg buffer, as the + // landing angle parameter is what is typically used for steepest glide + // in landing config + const float max_glide_slope = tanf(math::radians(_param_fw_lnd_ang.get() + 0.1f)); + + if (glide_slope > max_glide_slope) { + + const uint8_t land_angle_left_of_decimal = (uint8_t)_param_fw_lnd_ang.get(); + const uint8_t land_angle_first_after_decimal = (uint8_t)((_param_fw_lnd_ang.get() - floorf( + _param_fw_lnd_ang.get())) * 10.0f); + + mavlink_log_critical(_mavlink_log_pub, + "Mission rejected: the landing glide slope is steeper than the vehicle setting of %d.%d degrees.\t", + (int)land_angle_left_of_decimal, (int)land_angle_first_after_decimal); + events::send(events::ID("navigator_mis_glide_slope_too_steep"), {events::Log::Error, events::LogInternal::Info}, + "Mission rejected: the landing glide slope is steeper than the vehicle setting of {1}.{2} degrees", + land_angle_left_of_decimal, land_angle_first_after_decimal); + + const uint32_t acceptable_entrance_alt = (uint32_t)(max_glide_slope * landing_approach_distance); + const uint32_t acceptable_landing_dist = (uint32_t)ceilf(relative_approach_altitude / max_glide_slope); + + mavlink_log_critical(_mavlink_log_pub, + "Reduce the glide slope, lower the entrance altitude %d meters, or increase the landing approach distance %d meters.\t", + (int)acceptable_entrance_alt, (int)acceptable_landing_dist); + events::send(events::ID("navigator_mis_correct_glide_slope"), {events::Log::Error, events::LogInternal::Info}, + "Reduce the glide slope, lower the entrance altitude {1} meters, or increase the landing approach distance {2} meters", + acceptable_entrance_alt, acceptable_landing_dist); + + return false; + } + + _landing_valid = true; + + } else { + // mission item before land doesn't have a position + mavlink_log_critical(_mavlink_log_pub, "Mission rejected: need landing approach.\t"); + events::send(events::ID("navigator_mis_req_landing_approach"), {events::Log::Error, events::LogInternal::Info}, + "Mission rejected: landing approach is required"); + return false; + } + } + + return true; +} + +bool FeasibilityChecker::checkFixedWingLanding(mission_item_s &mission_item, const int current_index) +{ + const bool land_start_found = _do_land_start_index > 0; + + // if DO_LAND_START found then require valid landing AFTER + if (mission_item.nav_cmd == NAV_CMD_DO_LAND_START) { + if (_do_land_start_index >= 0) { + mavlink_log_critical(_mavlink_log_pub, "Mission rejected: more than one land start.\t"); + events::send(events::ID("navigator_mis_multiple_land"), {events::Log::Error, events::LogInternal::Info}, + "Mission rejected: more than one land start commands"); + return false; + + } + + _do_land_start_index = current_index; + } + + if (mission_item.nav_cmd == NAV_CMD_LAND || mission_item.nav_cmd == NAV_CMD_VTOL_LAND) { + + if (current_index > 0) { + _landing_approach_index = current_index - 1; + + } else { + mavlink_log_critical(_mavlink_log_pub, "Mission rejected: starts with land waypoint.\t"); + events::send(events::ID("navigator_mis_starts_w_landing2"), {events::Log::Error, events::LogInternal::Info}, + "Mission rejected: starts with landing"); + return false; + } + + } else if (mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { + if (land_start_found && _do_land_start_index < current_index) { + mavlink_log_critical(_mavlink_log_pub, + "Mission rejected: land start item before RTL item not possible.\t"); + events::send(events::ID("navigator_mis_land_before_rtl"), {events::Log::Error, events::LogInternal::Info}, + "Mission rejected: land start item before RTL item is not possible"); + return false; + } + } + + if (land_start_found && (!_landing_valid || (_do_land_start_index > _landing_approach_index))) { + mavlink_log_critical(_mavlink_log_pub, "Mission rejected: invalid land start.\t"); + events::send(events::ID("navigator_mis_invalid_land"), {events::Log::Error, events::LogInternal::Info}, + "Mission rejected: invalid land start"); + return false; + } + + /* No landing waypoints or no waypoints */ + return true; +} + +bool FeasibilityChecker::checkTakeoffLandAvailable() +{ + bool result = true; + + switch (_param_mis_takeoff_land_req.get()) { + case 0: + result = true; + break; + + case 1: + result = _has_takeoff; + + if (!result) { + mavlink_log_critical(_mavlink_log_pub, "Mission rejected: Takeoff waypoint required.\t"); + events::send(events::ID("navigator_mis_takeoff_missing"), {events::Log::Error, events::LogInternal::Info}, + "Mission rejected: Takeoff waypoint required"); + return false; + } + + break; + + case 2: + result = _landing_valid; + + if (!result) { + mavlink_log_critical(_mavlink_log_pub, "Mission rejected: Landing waypoint/pattern required.\t"); + events::send(events::ID("navigator_mis_land_missing"), {events::Log::Error, events::LogInternal::Info}, + "Mission rejected: Landing waypoint/pattern required"); + } + + break; + + case 3: + result = _has_takeoff && _landing_valid; + + if (!result) { + mavlink_log_critical(_mavlink_log_pub, "Mission rejected: Takeoff or Landing item missing.\t"); + events::send(events::ID("navigator_mis_takeoff_or_land_missing"), {events::Log::Error, events::LogInternal::Info}, + "Mission rejected: Takeoff or Landing item missing"); + } + + break; + + case 4: + result = _has_takeoff == _landing_valid; + + if (!result && (_has_takeoff)) { + mavlink_log_critical(_mavlink_log_pub, "Mission rejected: Add Landing item or remove Takeoff.\t"); + events::send(events::ID("navigator_mis_add_land_or_rm_to"), {events::Log::Error, events::LogInternal::Info}, + "Mission rejected: Add Landing item or remove Takeoff"); + + } else if (!result && (_landing_valid)) { + mavlink_log_critical(_mavlink_log_pub, "Mission rejected: Add Takeoff item or remove Landing.\t"); + events::send(events::ID("navigator_mis_add_to_or_rm_land"), {events::Log::Error, events::LogInternal::Info}, + "Mission rejected: Add Takeoff item or remove Landing"); + } + + break; + + default: + result = true; + break; + } + + return result; +} + + +bool FeasibilityChecker::checkDistanceToFirstWaypoint(mission_item_s &mission_item) +{ + if (_param_mis_dist_1wp.get() <= 0.0f) { + /* param not set, check is ok */ + return true; + } + + if (!_first_waypoint_found && MissionBlock::item_contains_position(mission_item)) { + _first_waypoint_found = true; + + + /* check distance from current position to item */ + float dist_to_1wp = get_distance_to_next_waypoint( + mission_item.lat, mission_item.lon, + _home_lat_lon(0), _home_lat_lon(1)); + + if (dist_to_1wp < _param_mis_dist_1wp.get()) { + + return true; + + } else { + /* item is too far from home */ + mavlink_log_critical(_mavlink_log_pub, + "First waypoint too far away: %dm, %d max\t", + (int)dist_to_1wp, (int)_param_mis_dist_1wp.get()); + events::send(events::ID("navigator_mis_first_wp_too_far"), {events::Log::Error, events::LogInternal::Info}, + "First waypoint too far away: {1m} (maximum: {2m})", (uint32_t)dist_to_1wp, (uint32_t)_param_mis_dist_1wp.get()); + + return false; + } + } + + /* no waypoints found in mission, then we will not fly far away */ + return true; +} + +bool FeasibilityChecker::checkDistancesBetweenWaypoints(const mission_item_s &mission_item) +{ + if (_param_mis_dist_wps.get() <= 0.0f) { + /* param not set, check is ok */ + return true; + } + + + /* check only items with valid lat/lon */ + if (!MissionBlock::item_contains_position(mission_item)) { + return true; + } + + /* Compare it to last waypoint if already available. */ + if (PX4_ISFINITE(_last_lat) && PX4_ISFINITE(_last_lon)) { + /* check distance from current position to item */ + const float dist_between_waypoints = get_distance_to_next_waypoint( + mission_item.lat, mission_item.lon, + _last_lat, _last_lon); + + + if (dist_between_waypoints > _param_mis_dist_wps.get()) { + /* distance between waypoints is too high */ + mavlink_log_critical(_mavlink_log_pub, + "Distance between waypoints too far: %d meters, %d max.\t", + (int)dist_between_waypoints, (int)_param_mis_dist_wps.get()); + events::send(events::ID("navigator_mis_wp_dist_too_far"), {events::Log::Error, events::LogInternal::Info}, + "Distance between waypoints too far: {1m}, (maximum: {2m})", (uint32_t)dist_between_waypoints, + (uint32_t)_param_mis_dist_wps.get()); + + + return false; + + /* do not allow waypoints that are literally on top of each other */ + + /* and do not allow condition gates that are at the same position as a navigation waypoint */ + + } else if (dist_between_waypoints < 0.05f && + (mission_item.nav_cmd == NAV_CMD_CONDITION_GATE || _last_cmd == NAV_CMD_CONDITION_GATE)) { + + /* Waypoints and gate are at the exact same position, which indicates an + * invalid mission and makes calculating the direction from one waypoint + * to another impossible. */ + mavlink_log_critical(_mavlink_log_pub, + "Distance between waypoint and gate too close: %d meters\t", + (int)dist_between_waypoints); + events::send(events::ID("navigator_mis_wp_gate_too_close"), {events::Log::Error, events::LogInternal::Info}, + "Distance between waypoint and gate too close: {1:.3m} (minimum: {2:.3m})", dist_between_waypoints, 0.05f); + + + return false; + } + } + + _last_lat = mission_item.lat; + _last_lon = mission_item.lon; + _last_cmd = mission_item.nav_cmd; + + /* We ran through all waypoints and have not found any distances between waypoints that are too far. */ + return true; +} + +bool FeasibilityChecker::checkIfBelowHomeAltitude(const mission_item_s &mission_item, const int current_index) +{ + + /* reject relative alt without home set */ + if (mission_item.altitude_is_relative && !PX4_ISFINITE(_home_alt_msl) + && MissionBlock::item_contains_position(mission_item)) { + + + + mavlink_log_critical(_mavlink_log_pub, "Mission rejected: No home pos, WP %d uses rel alt\t", current_index + 1); + events::send(events::ID("navigator_mis_no_home_rel_alt"), {events::Log::Error, events::LogInternal::Info}, + "Mission rejected: No home position, waypoint {1} uses relative altitude", + current_index + 1); + return false; + + } + + /* calculate the global waypoint altitude */ + float wp_alt = (mission_item.altitude_is_relative) ? mission_item.altitude + _home_alt_msl : mission_item.altitude; + + if (PX4_ISFINITE(_home_alt_msl) && _home_alt_msl > wp_alt && MissionBlock::item_contains_position(mission_item)) { + + + + mavlink_log_critical(_mavlink_log_pub, "Warning: Waypoint %d below home\t", current_index + 1); + events::send(events::ID("navigator_mis_wp_below_home"), {events::Log::Warning, events::LogInternal::Info}, + "Waypoint {1} below home", current_index + 1); + } + + return true; +} diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp new file mode 100644 index 0000000000..5ae2bf0ce4 --- /dev/null +++ b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp @@ -0,0 +1,139 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "../navigation.h" +#include +#include +#include +#include +#include +#include + +class FeasibilityChecker : public ModuleParams +{ +public: + + FeasibilityChecker(); + + void processNextItem(mission_item_s &mission_item, const int current_index, const int total_count); + + void setMavlinkLogPub(orb_advert_t *mavlink_log_pub) + { + _mavlink_log_pub = mavlink_log_pub; + } + + bool someCheckFailed() + { + return _takeoff_failed || + _distance_first_waypoint_failed || + _distance_between_waypoints_failed || + _fixed_wing_landing_failed || + _below_home_alt_failed || + _mission_validity_failed || + _takeoff_land_available_failed; + } + + void reset(); + +private: + orb_advert_t *_mavlink_log_pub{nullptr}; + + uORB::Subscription _home_pos_sub{ORB_ID(home_position)}; + uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _land_detector_sub{ORB_ID(vehicle_land_detected)}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_fw_lnd_ang, + (ParamFloat) _param_mis_dist_1wp, + (ParamFloat) _param_mis_dist_wps, + (ParamFloat) _param_nav_acc_rad, + (ParamInt) _param_mis_takeoff_land_req + ) + + // parameter that need to be set from the outside + bool _is_landed{false}; + float _home_alt_msl{NAN}; + matrix::Vector2d _home_lat_lon = matrix::Vector2d(NAN, NAN); + int _vehicle_type{0}; + bool _is_vtol{false}; + + // internal flags to keep track of which checks failed + bool _mission_validity_failed{false}; + bool _takeoff_failed{false}; + bool _fixed_wing_landing_failed{false}; + bool _distance_first_waypoint_failed{false}; + bool _distance_between_waypoints_failed{false}; + bool _below_home_alt_failed{false}; + bool _fixed_wing_land_approach_failed{false}; + bool _takeoff_land_available_failed{false}; + + // internal checkTakeoff related variables + bool _found_item_with_position{false}; + bool _has_vtol_takeoff{false}; + bool _has_takeoff{false}; + + // internal checkFixedWingLanding related variables + bool _landing_valid{false}; + int _do_land_start_index{-1}; + int _landing_approach_index{-1}; + mission_item_s _mission_item_previous = {}; + + // internal checkDistanceToFirstWaypoint variables + bool _first_waypoint_found{false}; + + // internal checkDistancesBetweenWaypoints variables + double _last_lat{NAN}; + double _last_lon{NAN}; + int _last_cmd{-1}; + + void updateData(); + + // methods which are called for each mission item + bool checkMissionItemValidity(mission_item_s &mission_item, const int current_index); + bool checkTakeoff(mission_item_s &mission_item); + bool checkFixedWingLanding(mission_item_s &mission_item, const int current_index); + bool checkDistanceToFirstWaypoint(mission_item_s &mission_item); + bool checkDistancesBetweenWaypoints(const mission_item_s &mission_item); + bool checkIfBelowHomeAltitude(const mission_item_s &mission_item, const int current_index); + bool checkFixedWindLandApproach(mission_item_s &mission_item, const int current_index); + + // methods which are called once at the end + bool checkTakeoffLandAvailable(); + + void doCommonChecks(mission_item_s &mission_item, const int current_index); + void doVtolChecks(mission_item_s &mission_item, const int current_index); + void doFixedWingChecks(mission_item_s &mission_item, const int current_index); + void doMulticopterChecks(mission_item_s &mission_item, const int current_index); +}; diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index b1f7a9bd80..013d331a0b 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -41,6 +41,7 @@ */ #include "mission_feasibility_checker.h" +#include "MissionFeasibility/FeasibilityChecker.hpp" #include "mission_block.h" #include "navigator.h" @@ -59,52 +60,42 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission, // Reset warning flag _navigator->get_mission_result()->warning = false; + // first check if we have a valid position + const bool home_valid = _navigator->home_global_position_valid(); + const bool home_alt_valid = _navigator->home_alt_valid(); + // trivial case: A mission with length zero cannot be valid if ((int)mission.count <= 0) { return false; } - bool failed = false; - - // first check if we have a valid position - const bool home_valid = _navigator->home_global_position_valid(); - const bool home_alt_valid = _navigator->home_alt_valid(); - if (!home_alt_valid) { - failed = true; mavlink_log_info(_navigator->get_mavlink_log_pub(), "Not yet ready for mission, no position lock.\t"); events::send(events::ID("navigator_mis_no_pos_lock"), events::Log::Info, "Not yet ready for mission, no position lock"); - - } else { - failed |= !checkDistanceToFirstWaypoint(mission, max_distance_to_1st_waypoint); + return false; } - const float home_alt = _navigator->get_home_position()->alt; + bool failed = false; - // reset for next check - _has_takeoff = false; - _has_landing = false; + for (size_t i = 0; i < mission.count; i++) { + struct mission_item_s missionitem = {}; + const ssize_t len = sizeof(struct mission_item_s); - // run generic (for all vehicle types) checks - failed |= !checkMissionItemValidity(mission); - failed |= !checkDistancesBetweenWaypoints(mission, max_distance_between_waypoints); - failed |= !checkGeofence(mission, home_alt, home_valid); - failed |= !checkHomePositionAltitude(mission, home_alt, home_alt_valid); - failed |= !checkTakeoff(mission, home_alt); + if (dm_read((dm_item_t)mission.dataman_id, i, &missionitem, len) != len) { + _navigator->get_mission_result()->warning = true; + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + return false; + } - // run type-specifc landing checks, which also include seting _has_landing that is used in checkTakeoffLandAvailable() - if (_navigator->get_vstatus()->is_vtol) { - failed |= !checkVTOLLanding(mission); + _feasibility_checker.processNextItem(missionitem, i, mission.count); - } else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { - failed |= !checkFixedWingLanding(mission); - - } else { - // if neither VTOL nor FW, only check if mission has landing but don't check that one for validity - _has_landing = hasMissionLanding(mission); } - failed |= !checkTakeoffLandAvailable(); + failed |= _feasibility_checker.someCheckFailed(); + + failed |= !checkGeofence(mission, _navigator->get_home_position()->alt, home_valid); + + _navigator->get_mission_result()->warning = failed; return !failed; } @@ -153,749 +144,3 @@ MissionFeasibilityChecker::checkGeofence(const mission_s &mission, float home_al return true; } - -bool -MissionFeasibilityChecker::checkHomePositionAltitude(const mission_s &mission, float home_alt, bool home_alt_valid) -{ - /* Check if all waypoints are above the home altitude */ - for (size_t i = 0; i < mission.count; i++) { - struct mission_item_s missionitem = {}; - const ssize_t len = sizeof(struct mission_item_s); - - if (dm_read((dm_item_t)mission.dataman_id, i, &missionitem, len) != len) { - _navigator->get_mission_result()->warning = true; - /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - return false; - } - - /* reject relative alt without home set */ - if (missionitem.altitude_is_relative && !home_alt_valid && MissionBlock::item_contains_position(missionitem)) { - - _navigator->get_mission_result()->warning = true; - - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: No home pos, WP %zu uses rel alt\t", i + 1); - events::send(events::ID("navigator_mis_no_home_rel_alt"), {events::Log::Error, events::LogInternal::Info}, - "Mission rejected: No home position, waypoint {1} uses relative altitude", - i + 1); - return false; - - } - - /* calculate the global waypoint altitude */ - float wp_alt = (missionitem.altitude_is_relative) ? missionitem.altitude + home_alt : missionitem.altitude; - - if (home_alt_valid && home_alt > wp_alt && MissionBlock::item_contains_position(missionitem)) { - - _navigator->get_mission_result()->warning = true; - - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Warning: Waypoint %zu below home\t", i + 1); - events::send(events::ID("navigator_mis_wp_below_home"), {events::Log::Warning, events::LogInternal::Info}, - "Waypoint {1} below home", i + 1); - } - } - - return true; -} - -bool -MissionFeasibilityChecker::checkMissionItemValidity(const mission_s &mission) -{ - // do not allow mission if we find unsupported item - for (size_t i = 0; i < mission.count; i++) { - struct mission_item_s missionitem; - const ssize_t len = sizeof(struct mission_item_s); - - if (dm_read((dm_item_t)mission.dataman_id, i, &missionitem, len) != len) { - // not supposed to happen unless the datamanager can't access the SD card, etc. - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Cannot access SD card\t"); - events::send(events::ID("navigator_mis_sd_failure"), events::Log::Error, - "Mission rejected: Cannot access mission storage"); - return false; - } - - // check if we find unsupported items and reject mission if so - if (missionitem.nav_cmd != NAV_CMD_IDLE && - missionitem.nav_cmd != NAV_CMD_WAYPOINT && - missionitem.nav_cmd != NAV_CMD_LOITER_UNLIMITED && - missionitem.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT && - missionitem.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH && - missionitem.nav_cmd != NAV_CMD_LAND && - missionitem.nav_cmd != NAV_CMD_TAKEOFF && - missionitem.nav_cmd != NAV_CMD_LOITER_TO_ALT && - missionitem.nav_cmd != NAV_CMD_VTOL_TAKEOFF && - missionitem.nav_cmd != NAV_CMD_VTOL_LAND && - missionitem.nav_cmd != NAV_CMD_DELAY && - missionitem.nav_cmd != NAV_CMD_CONDITION_GATE && - missionitem.nav_cmd != NAV_CMD_DO_WINCH && - missionitem.nav_cmd != NAV_CMD_DO_GRIPPER && - missionitem.nav_cmd != NAV_CMD_DO_JUMP && - missionitem.nav_cmd != NAV_CMD_DO_CHANGE_SPEED && - missionitem.nav_cmd != NAV_CMD_DO_SET_HOME && - missionitem.nav_cmd != NAV_CMD_DO_SET_SERVO && - missionitem.nav_cmd != NAV_CMD_DO_SET_ACTUATOR && - missionitem.nav_cmd != NAV_CMD_DO_LAND_START && - missionitem.nav_cmd != NAV_CMD_DO_TRIGGER_CONTROL && - missionitem.nav_cmd != NAV_CMD_DO_DIGICAM_CONTROL && - missionitem.nav_cmd != NAV_CMD_IMAGE_START_CAPTURE && - missionitem.nav_cmd != NAV_CMD_IMAGE_STOP_CAPTURE && - missionitem.nav_cmd != NAV_CMD_VIDEO_START_CAPTURE && - missionitem.nav_cmd != NAV_CMD_VIDEO_STOP_CAPTURE && - missionitem.nav_cmd != NAV_CMD_DO_CONTROL_VIDEO && - missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONFIGURE && - missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONTROL && - missionitem.nav_cmd != NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW && - missionitem.nav_cmd != NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE && - missionitem.nav_cmd != NAV_CMD_DO_SET_ROI && - missionitem.nav_cmd != NAV_CMD_DO_SET_ROI_LOCATION && - missionitem.nav_cmd != NAV_CMD_DO_SET_ROI_WPNEXT_OFFSET && - missionitem.nav_cmd != NAV_CMD_DO_SET_ROI_NONE && - missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_DIST && - missionitem.nav_cmd != NAV_CMD_OBLIQUE_SURVEY && - missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL && - missionitem.nav_cmd != NAV_CMD_SET_CAMERA_MODE && - missionitem.nav_cmd != NAV_CMD_SET_CAMERA_ZOOM && - missionitem.nav_cmd != NAV_CMD_SET_CAMERA_FOCUS && - missionitem.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION) { - - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: item %i: unsupported cmd: %d\t", - (int)(i + 1), - (int)missionitem.nav_cmd); - events::send(events::ID("navigator_mis_unsup_cmd"), {events::Log::Error, events::LogInternal::Warning}, - "Mission rejected: item {1}: unsupported command: {2}", i + 1, missionitem.nav_cmd); - return false; - } - - /* Check non navigation item */ - if (missionitem.nav_cmd == NAV_CMD_DO_SET_SERVO) { - - /* check actuator number */ - if (missionitem.params[0] < 0 || missionitem.params[0] > 5) { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Actuator number %d is out of bounds 0..5\t", - (int)missionitem.params[0]); - events::send(events::ID("navigator_mis_act_index"), {events::Log::Error, events::LogInternal::Warning}, - "Actuator number {1} is out of bounds 0..5", (int)missionitem.params[0]); - return false; - } - - /* check actuator value */ - if (missionitem.params[1] < -PWM_DEFAULT_MAX || missionitem.params[1] > PWM_DEFAULT_MAX) { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), - "Actuator value %d is out of bounds -PWM_DEFAULT_MAX..PWM_DEFAULT_MAX\t", (int)missionitem.params[1]); - events::send(events::ID("navigator_mis_act_range"), {events::Log::Error, events::LogInternal::Warning}, - "Actuator value {1} is out of bounds -{2}..{2}", (int)missionitem.params[1], PWM_DEFAULT_MAX); - return false; - } - } - - // check if the mission starts with a land command while the vehicle is landed - if ((i == 0) && missionitem.nav_cmd == NAV_CMD_LAND && _navigator->get_land_detected()->landed) { - - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: starts with landing\t"); - events::send(events::ID("navigator_mis_starts_w_landing"), {events::Log::Error, events::LogInternal::Info}, - "Mission rejected: starts with landing"); - return false; - } - } - - return true; -} - -bool -MissionFeasibilityChecker::checkTakeoff(const mission_s &mission, float home_alt) -{ - bool takeoff_first = false; - int takeoff_index = -1; - - for (size_t i = 0; i < mission.count; i++) { - struct mission_item_s missionitem = {}; - const ssize_t len = sizeof(struct mission_item_s); - - if (dm_read((dm_item_t)mission.dataman_id, i, &missionitem, len) != len) { - /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - return false; - } - - // look for a takeoff waypoint - if (missionitem.nav_cmd == NAV_CMD_TAKEOFF || missionitem.nav_cmd == NAV_CMD_VTOL_TAKEOFF) { - // make sure that the altitude of the waypoint is at least one meter larger than the acceptance radius - // this makes sure that the takeoff waypoint is not reached before we are at least one meter in the air - - float takeoff_alt = missionitem.altitude_is_relative - ? missionitem.altitude - : missionitem.altitude - home_alt; - - // check if we should use default acceptance radius - float acceptance_radius = _navigator->get_default_acceptance_radius(); - - if (missionitem.acceptance_radius > NAV_EPSILON_POSITION) { - acceptance_radius = missionitem.acceptance_radius; - } - - if (takeoff_alt - 1.0f < acceptance_radius) { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Takeoff altitude too low!\t"); - /* EVENT - * @description The minimum takeoff altitude is the acceptance radius plus 1m. - */ - events::send(events::ID("navigator_mis_takeoff_too_low"), {events::Log::Error, events::LogInternal::Info}, - "Mission rejected: takeoff altitude too low! Minimum: {1:.1m_v}", acceptance_radius + 1.f); - return false; - } - - // tell that mission has a takeoff waypoint - _has_takeoff = true; - - // tell that a takeoff waypoint is the first "waypoint" - // mission item - if (i == 0) { - takeoff_first = true; - - } else if (takeoff_index == -1) { - // stores the index of the first takeoff waypoint - takeoff_index = i; - } - } - } - - if (takeoff_index != -1) { - // checks if all the mission items before the first takeoff waypoint - // are not waypoints or position-related items; - // this means that, before a takeoff waypoint, one can set - // one of the bellow mission items - for (size_t i = 0; i < (size_t)takeoff_index; i++) { - struct mission_item_s missionitem = {}; - const ssize_t len = sizeof(struct mission_item_s); - - if (dm_read((dm_item_t)mission.dataman_id, i, &missionitem, len) != len) { - /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - return false; - } - - takeoff_first = !(missionitem.nav_cmd != NAV_CMD_IDLE && - missionitem.nav_cmd != NAV_CMD_DELAY && - missionitem.nav_cmd != NAV_CMD_DO_JUMP && - missionitem.nav_cmd != NAV_CMD_DO_CHANGE_SPEED && - missionitem.nav_cmd != NAV_CMD_DO_SET_HOME && - missionitem.nav_cmd != NAV_CMD_DO_SET_SERVO && - missionitem.nav_cmd != NAV_CMD_DO_LAND_START && - missionitem.nav_cmd != NAV_CMD_DO_TRIGGER_CONTROL && - missionitem.nav_cmd != NAV_CMD_DO_DIGICAM_CONTROL && - missionitem.nav_cmd != NAV_CMD_IMAGE_START_CAPTURE && - missionitem.nav_cmd != NAV_CMD_IMAGE_STOP_CAPTURE && - missionitem.nav_cmd != NAV_CMD_VIDEO_START_CAPTURE && - missionitem.nav_cmd != NAV_CMD_VIDEO_STOP_CAPTURE && - missionitem.nav_cmd != NAV_CMD_DO_CONTROL_VIDEO && - missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONFIGURE && - missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONTROL && - missionitem.nav_cmd != NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW && - missionitem.nav_cmd != NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE && - missionitem.nav_cmd != NAV_CMD_DO_SET_ROI && - missionitem.nav_cmd != NAV_CMD_DO_SET_ROI_LOCATION && - missionitem.nav_cmd != NAV_CMD_DO_SET_ROI_WPNEXT_OFFSET && - missionitem.nav_cmd != NAV_CMD_DO_SET_ROI_NONE && - missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_DIST && - missionitem.nav_cmd != NAV_CMD_OBLIQUE_SURVEY && - missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL && - missionitem.nav_cmd != NAV_CMD_SET_CAMERA_MODE && - missionitem.nav_cmd != NAV_CMD_SET_CAMERA_ZOOM && - missionitem.nav_cmd != NAV_CMD_SET_CAMERA_FOCUS && - missionitem.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION); - } - } - - if (_has_takeoff && !takeoff_first) { - // check if the takeoff waypoint is the first waypoint item on the mission - // i.e, an item with position/attitude change modification - // if it is not, the mission should be rejected - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: takeoff not first waypoint item\t"); - events::send(events::ID("navigator_mis_takeoff_not_first"), {events::Log::Error, events::LogInternal::Info}, - "Mission rejected: takeoff is not the first waypoint item"); - return false; - } - - // all checks have passed - return true; -} - -bool -MissionFeasibilityChecker::hasMissionLanding(const mission_s &mission) -{ - // Go through all mission items and search for a landing waypoint. - // For MC we currently do not run any checks on the validity of the planned landing. - - bool mission_landing_found = false; - - for (size_t i = 0; i < mission.count; i++) { - struct mission_item_s missionitem; - const ssize_t len = sizeof(missionitem); - - if (dm_read((dm_item_t)mission.dataman_id, i, &missionitem, len) != len) { - /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - return false; - } - - if (missionitem.nav_cmd == NAV_CMD_LAND) { - mission_landing_found = true; - } - } - - return mission_landing_found; -} - -bool -MissionFeasibilityChecker::checkFixedWingLanding(const mission_s &mission) -{ - /* Go through all mission items and search for a landing waypoint - * if landing waypoint is found: the previous waypoint is checked to be at a feasible distance and altitude given the landing slope */ - - bool landing_valid = false; - - size_t do_land_start_index = 0; - size_t landing_approach_index = 0; - - for (size_t i = 0; i < mission.count; i++) { - struct mission_item_s missionitem; - const ssize_t len = sizeof(missionitem); - - if (dm_read((dm_item_t)mission.dataman_id, i, &missionitem, len) != len) { - /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - return false; - } - - // if DO_LAND_START found then require valid landing AFTER - if (missionitem.nav_cmd == NAV_CMD_DO_LAND_START) { - if (_has_landing) { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: more than one land start.\t"); - events::send(events::ID("navigator_mis_multiple_land"), {events::Log::Error, events::LogInternal::Info}, - "Mission rejected: more than one land start commands"); - return false; - - } else { - _has_landing = true; - do_land_start_index = i; - } - } - - if (missionitem.nav_cmd == NAV_CMD_LAND) { - mission_item_s missionitem_previous {}; - - _has_landing = true; - - float param_fw_lnd_ang = 0.0f; - const param_t param_handle_fw_lnd_ang = param_find("FW_LND_ANG"); - - if (param_handle_fw_lnd_ang == PARAM_INVALID) { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: FW_LND_ANG parameter is missing.\t"); - events::send(events::ID("navigator_mis_land_angle_param_missing"), {events::Log::Error, events::LogInternal::Info}, - "Mission rejected: FW_LND_ANG parameter is missing"); - return false; - - } else { - param_get(param_handle_fw_lnd_ang, ¶m_fw_lnd_ang); - } - - if (i > 0) { - landing_approach_index = i - 1; - - if (dm_read((dm_item_t)mission.dataman_id, landing_approach_index, &missionitem_previous, len) != len) { - /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - return false; - } - - if (MissionBlock::item_contains_position(missionitem_previous)) { - - const float land_alt_amsl = missionitem.altitude_is_relative ? missionitem.altitude + - _navigator->get_home_position()->alt : missionitem.altitude; - const float entrance_alt_amsl = missionitem_previous.altitude_is_relative ? missionitem_previous.altitude + - _navigator->get_home_position()->alt : missionitem_previous.altitude; - const float relative_approach_altitude = entrance_alt_amsl - land_alt_amsl; - - if (relative_approach_altitude < FLT_EPSILON) { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), - "Mission rejected: the approach waypoint must be above the landing point.\t"); - events::send(events::ID("navigator_mis_approach_wp_below_land"), {events::Log::Error, events::LogInternal::Info}, - "Mission rejected: the approach waypoint must be above the landing point"); - return false; - } - - float landing_approach_distance; - - if (missionitem_previous.nav_cmd == NAV_CMD_LOITER_TO_ALT) { - // assume this is a fixed-wing landing pattern with orbit to alt followed - // by tangent exit to landing approach and touchdown at landing waypoint - - const float distance_orbit_center_to_land = get_distance_to_next_waypoint(missionitem_previous.lat, - missionitem_previous.lon, missionitem.lat, missionitem.lon); - const float orbit_radius = fabsf(missionitem_previous.loiter_radius); - - if (distance_orbit_center_to_land <= orbit_radius) { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), - "Mission rejected: the landing point must be outside the orbit radius.\t"); - events::send(events::ID("navigator_mis_land_wp_inside_orbit_radius"), {events::Log::Error, events::LogInternal::Info}, - "Mission rejected: the landing point must be outside the orbit radius"); - return false; - } - - landing_approach_distance = sqrtf(distance_orbit_center_to_land * distance_orbit_center_to_land - orbit_radius * - orbit_radius); - - } else if (missionitem_previous.nav_cmd == NAV_CMD_WAYPOINT) { - // approaching directly from waypoint position - - const float waypoint_distance = get_distance_to_next_waypoint(missionitem_previous.lat, missionitem_previous.lon, - missionitem.lat, missionitem.lon); - landing_approach_distance = waypoint_distance; - - } else { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), - "Mission rejected: unsupported landing approach entrance waypoint type. Only ORBIT_TO_ALT or WAYPOINT allowed.\t"); - events::send(events::ID("navigator_mis_unsupported_landing_approach_wp"), {events::Log::Error, events::LogInternal::Info}, - "Mission rejected: unsupported landing approach entrance waypoint type. Only ORBIT_TO_ALT or WAYPOINT allowed"); - return false; - } - - const float glide_slope = relative_approach_altitude / landing_approach_distance; - - // respect user setting as max glide slope, but account for floating point - // rounding on next check with small (arbitrary) 0.1 deg buffer, as the - // landing angle parameter is what is typically used for steepest glide - // in landing config - const float max_glide_slope = tanf(math::radians(param_fw_lnd_ang + 0.1f)); - - if (glide_slope > max_glide_slope) { - - const uint8_t land_angle_left_of_decimal = (uint8_t)param_fw_lnd_ang; - const uint8_t land_angle_first_after_decimal = (uint8_t)((param_fw_lnd_ang - floorf( - param_fw_lnd_ang)) * 10.0f); - - mavlink_log_critical(_navigator->get_mavlink_log_pub(), - "Mission rejected: the landing glide slope is steeper than the vehicle setting of %d.%d degrees.\t", - (int)land_angle_left_of_decimal, (int)land_angle_first_after_decimal); - events::send(events::ID("navigator_mis_glide_slope_too_steep"), {events::Log::Error, events::LogInternal::Info}, - "Mission rejected: the landing glide slope is steeper than the vehicle setting of {1}.{2} degrees", - land_angle_left_of_decimal, land_angle_first_after_decimal); - - const uint32_t acceptable_entrance_alt = (uint32_t)(max_glide_slope * landing_approach_distance); - const uint32_t acceptable_landing_dist = (uint32_t)ceilf(relative_approach_altitude / max_glide_slope); - - mavlink_log_critical(_navigator->get_mavlink_log_pub(), - "Reduce the glide slope, lower the entrance altitude %d meters, or increase the landing approach distance %d meters.\t", - (int)acceptable_entrance_alt, (int)acceptable_landing_dist); - events::send(events::ID("navigator_mis_correct_glide_slope"), {events::Log::Error, events::LogInternal::Info}, - "Reduce the glide slope, lower the entrance altitude {1} meters, or increase the landing approach distance {2} meters", - acceptable_entrance_alt, acceptable_landing_dist); - - return false; - } - - landing_valid = true; - - } else { - // mission item before land doesn't have a position - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: need landing approach.\t"); - events::send(events::ID("navigator_mis_req_landing_approach"), {events::Log::Error, events::LogInternal::Info}, - "Mission rejected: landing approach is required"); - return false; - } - - } else { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: starts with land waypoint.\t"); - events::send(events::ID("navigator_mis_starts_w_landing2"), {events::Log::Error, events::LogInternal::Info}, - "Mission rejected: starts with landing"); - return false; - } - - } else if (missionitem.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { - if (_has_landing && do_land_start_index < i) { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), - "Mission rejected: land start item before RTL item not possible.\t"); - events::send(events::ID("navigator_mis_land_before_rtl"), {events::Log::Error, events::LogInternal::Info}, - "Mission rejected: land start item before RTL item is not possible"); - return false; - } - } - } - - if (_has_landing && (!landing_valid || (do_land_start_index > landing_approach_index))) { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: invalid land start.\t"); - events::send(events::ID("navigator_mis_invalid_land"), {events::Log::Error, events::LogInternal::Info}, - "Mission rejected: invalid land start"); - return false; - } - - /* No landing waypoints or no waypoints */ - return true; -} - -bool -MissionFeasibilityChecker::checkVTOLLanding(const mission_s &mission) -{ - // Go through all mission items and search for a landing waypoint, then run some checks on it - - size_t do_land_start_index = 0; - size_t landing_approach_index = 0; - - for (size_t i = 0; i < mission.count; i++) { - struct mission_item_s missionitem; - const ssize_t len = sizeof(missionitem); - - if (dm_read((dm_item_t)mission.dataman_id, i, &missionitem, len) != len) { - /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - return false; - } - - // if DO_LAND_START found then require valid landing AFTER - if (missionitem.nav_cmd == NAV_CMD_DO_LAND_START) { - if (_has_landing) { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: more than one land start.\t"); - events::send(events::ID("navigator_mis_multi_land"), {events::Log::Error, events::LogInternal::Info}, - "Mission rejected: more than one land start commands"); - return false; - - } else { - _has_landing = true; - do_land_start_index = i; - } - } - - if (missionitem.nav_cmd == NAV_CMD_LAND || missionitem.nav_cmd == NAV_CMD_VTOL_LAND) { - mission_item_s missionitem_previous {}; - - _has_landing = true; - - if (i > 0) { - landing_approach_index = i - 1; - - if (dm_read((dm_item_t)mission.dataman_id, landing_approach_index, &missionitem_previous, len) != len) { - /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - return false; - } - - - } else { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: starts with land waypoint.\t"); - events::send(events::ID("navigator_mis_starts_w_land"), {events::Log::Error, events::LogInternal::Info}, - "Mission rejected: starts with land waypoint"); - return false; - } - - } else if (missionitem.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { - if (_has_landing && do_land_start_index < i) { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), - "Mission rejected: land start item before RTL item not possible.\t"); - events::send(events::ID("navigator_mis_land_before_rtl2"), {events::Log::Error, events::LogInternal::Info}, - "Mission rejected: land start item before RTL item is not possible"); - return false; - } - } - } - - if (_has_landing && (do_land_start_index > landing_approach_index)) { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: invalid land start.\t"); - events::send(events::ID("navigator_mis_invalid_land2"), {events::Log::Error, events::LogInternal::Info}, - "Mission rejected: invalid land start"); - return false; - } - - /* No landing waypoints or no waypoints */ - return true; -} - -bool -MissionFeasibilityChecker::checkTakeoffLandAvailable() -{ - bool result = true; - - switch (_navigator->get_takeoff_land_required()) { - case 0: - result = true; - break; - - case 1: - result = _has_takeoff; - - if (!result) { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Takeoff waypoint required.\t"); - events::send(events::ID("navigator_mis_takeoff_missing"), {events::Log::Error, events::LogInternal::Info}, - "Mission rejected: Takeoff waypoint required"); - return false; - } - - break; - - case 2: - result = _has_landing; - - if (!result) { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Landing waypoint/pattern required.\t"); - events::send(events::ID("navigator_mis_land_missing"), {events::Log::Error, events::LogInternal::Info}, - "Mission rejected: Landing waypoint/pattern required"); - } - - break; - - case 3: - result = _has_takeoff && _has_landing; - - if (!result) { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Takeoff or Landing item missing.\t"); - events::send(events::ID("navigator_mis_takeoff_or_land_missing"), {events::Log::Error, events::LogInternal::Info}, - "Mission rejected: Takeoff or Landing item missing"); - } - - break; - - case 4: - result = _has_takeoff == _has_landing; - - if (!result && (_has_takeoff)) { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Add Landing item or remove Takeoff.\t"); - events::send(events::ID("navigator_mis_add_land_or_rm_to"), {events::Log::Error, events::LogInternal::Info}, - "Mission rejected: Add Landing item or remove Takeoff"); - - } else if (!result && (_has_landing)) { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Add Takeoff item or remove Landing.\t"); - events::send(events::ID("navigator_mis_add_to_or_rm_land"), {events::Log::Error, events::LogInternal::Info}, - "Mission rejected: Add Takeoff item or remove Landing"); - } - - break; - - default: - result = true; - break; - } - - return result; -} - -bool -MissionFeasibilityChecker::checkDistanceToFirstWaypoint(const mission_s &mission, float max_distance) -{ - if (max_distance <= 0.0f) { - /* param not set, check is ok */ - return true; - } - - /* find first waypoint (with lat/lon) item in datamanager */ - for (size_t i = 0; i < mission.count; i++) { - - struct mission_item_s mission_item {}; - - if (!(dm_read((dm_item_t)mission.dataman_id, i, &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s))) { - /* error reading, mission is invalid */ - mavlink_log_info(_navigator->get_mavlink_log_pub(), "Error reading offboard mission.\t"); - events::send(events::ID("navigator_mis_storage_failure"), events::Log::Error, - "Error reading mission storage"); - return false; - } - - /* check only items with valid lat/lon */ - if (!MissionBlock::item_contains_position(mission_item)) { - continue; - } - - /* check distance from current position to item */ - float dist_to_1wp = get_distance_to_next_waypoint( - mission_item.lat, mission_item.lon, - _navigator->get_home_position()->lat, _navigator->get_home_position()->lon); - - if (dist_to_1wp < max_distance) { - - return true; - - } else { - /* item is too far from home */ - mavlink_log_critical(_navigator->get_mavlink_log_pub(), - "First waypoint too far away: %dm, %d max\t", - (int)dist_to_1wp, (int)max_distance); - events::send(events::ID("navigator_mis_first_wp_too_far"), {events::Log::Error, events::LogInternal::Info}, - "First waypoint too far away: {1m} (maximum: {2m})", (uint32_t)dist_to_1wp, (uint32_t)max_distance); - - _navigator->get_mission_result()->warning = true; - return false; - } - } - - /* no waypoints found in mission, then we will not fly far away */ - return true; -} - -bool -MissionFeasibilityChecker::checkDistancesBetweenWaypoints(const mission_s &mission, float max_distance) -{ - if (max_distance <= 0.0f) { - /* param not set, check is ok */ - return true; - } - - double last_lat = (double)NAN; - double last_lon = (double)NAN; - int last_cmd = 0; - - /* Go through all waypoints */ - for (size_t i = 0; i < mission.count; i++) { - - struct mission_item_s mission_item {}; - - if (!(dm_read((dm_item_t)mission.dataman_id, i, &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s))) { - /* error reading, mission is invalid */ - mavlink_log_info(_navigator->get_mavlink_log_pub(), "Error reading offboard mission.\t"); - events::send(events::ID("navigator_mis_storage_failure2"), events::Log::Error, - "Error reading mission storage"); - return false; - } - - /* check only items with valid lat/lon */ - if (!MissionBlock::item_contains_position(mission_item)) { - continue; - } - - /* Compare it to last waypoint if already available. */ - if (PX4_ISFINITE(last_lat) && PX4_ISFINITE(last_lon)) { - - /* check distance from current position to item */ - const float dist_between_waypoints = get_distance_to_next_waypoint( - mission_item.lat, mission_item.lon, - last_lat, last_lon); - - - if (dist_between_waypoints > max_distance) { - /* distance between waypoints is too high */ - mavlink_log_critical(_navigator->get_mavlink_log_pub(), - "Distance between waypoints too far: %d meters, %d max.\t", - (int)dist_between_waypoints, (int)max_distance); - events::send(events::ID("navigator_mis_wp_dist_too_far"), {events::Log::Error, events::LogInternal::Info}, - "Distance between waypoints too far: {1m}, (maximum: {2m})", (uint32_t)dist_between_waypoints, (uint32_t)max_distance); - - _navigator->get_mission_result()->warning = true; - return false; - - /* do not allow waypoints that are literally on top of each other */ - - /* and do not allow condition gates that are at the same position as a navigation waypoint */ - - } else if (dist_between_waypoints < 0.05f && - (mission_item.nav_cmd == NAV_CMD_CONDITION_GATE || last_cmd == NAV_CMD_CONDITION_GATE)) { - - /* Waypoints and gate are at the exact same position, which indicates an - * invalid mission and makes calculating the direction from one waypoint - * to another impossible. */ - mavlink_log_critical(_navigator->get_mavlink_log_pub(), - "Distance between waypoint and gate too close: %d meters\t", - (int)dist_between_waypoints); - events::send(events::ID("navigator_mis_wp_gate_too_close"), {events::Log::Error, events::LogInternal::Info}, - "Distance between waypoint and gate too close: {1:.3m} (minimum: {2:.3m})", dist_between_waypoints, 0.05f); - - _navigator->get_mission_result()->warning = true; - return false; - } - } - - last_lat = mission_item.lat; - last_lon = mission_item.lon; - last_cmd = mission_item.nav_cmd; - } - - /* We ran through all waypoints and have not found any distances between waypoints that are too far. */ - return true; -} diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index 948f6f1553..90b87952fa 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -45,6 +45,7 @@ #include #include #include +#include "MissionFeasibility/FeasibilityChecker.hpp" class Geofence; class Navigator; @@ -53,106 +54,21 @@ class MissionFeasibilityChecker: public ModuleParams { private: Navigator *_navigator{nullptr}; - - /** - * @brief Check geofence validity (if available) - * - * Check includes: - * - home position is valid if required - * - home position is valid if geofence is set - * - all mission items don't violate set geofence - * - * @param mission Mission struct - * @param home_alt Home altitude [m AMSL] - * @param home_valid Home valid - * @return True if checks passed, False if not - */ - bool checkGeofence(const mission_s &mission, float home_alt, bool home_valid); - - /** - * @brief Check if Home altitude is valid if waypoints have relative altitude, and that waypoints are above it - * - * @param mission Mission struct - * @param home_alt Home altitude [m AMSL] - * @param home_alt_valid Home altitude valid - * @return True if checks passed, False if not - */ - bool checkHomePositionAltitude(const mission_s &mission, float home_alt, bool home_alt_valid); - - /** - * @brief Check if all mission items are supported - * - * @param mission Mission struct - * @return True if all set mission items are supported, False if not - */ - bool checkMissionItemValidity(const mission_s &mission); - - /** - * @brief Check if distance to first waypoint is below threshold - * - * @param mission Mission struct - * @param max_distance Maximally allowed distance to first waypoint [m] - * @return True if distance is below threshold, False if not - */ - bool checkDistanceToFirstWaypoint(const mission_s &mission, float max_distance); - - /** - * @brief Check if distance between consecutive waypoints is below threshold - * - * @param mission Mission struct - * @param max_distance Maximally allowed distance between consecutive waypoints [m] - * @return True if all distances between waypoints are below threshold, False if not - */ - bool checkDistancesBetweenWaypoints(const mission_s &mission, float max_distance); - - /** - * @brief Check if mission contains takeoff (safe in _has_takeoff), and if that takeoff is valid - * - * @param mission Mission struct - * @param home_alt Home altitude [m AMSL] - * @return True if the planned takeoff is valid, or no takeoff planned, False otherwise - */ - bool checkTakeoff(const mission_s &mission, float home_alt); - - /** - * @brief Check if requirement for availability of planned takeoff and/or landing is fullfilled - * - * The requirement is controlled through parameter MIS_TKO_LAND_REQ. - * Only checks for availability of takeoff/landing, not their validity - * - * @return True if check passes, False if not - */ - bool checkTakeoffLandAvailable(); - - /** - * @brief Check if mission contains landing - * - * @param mission Mission struct - * @return True if mission contains a landing, False if otherwise - */ - bool hasMissionLanding(const mission_s &mission); - - /** - * @brief Fixed-wing vehicle: Check if mission contains landing (safe in _has_landing), and if that landing is valid - * - * @param mission Mission struct - * @return True if the planned landing is valid, or no landing planned, False otherwise - */ - bool checkFixedWingLanding(const mission_s &mission); - - /** - * @brief VTOL vehicle: Check if mission contains landing (safe in _has_landing), and if that landing is valid - * - * @param mission Mission struct - * @return True if the planned landing is valid, or no landing planned, False otherwise - */ - bool checkVTOLLanding(const mission_s &mission); + FeasibilityChecker _feasibility_checker; bool _has_takeoff{false}; bool _has_landing{false}; + bool checkGeofence(const mission_s &mission, float home_alt, bool home_valid); + public: - MissionFeasibilityChecker(Navigator *navigator) : ModuleParams(nullptr), _navigator(navigator) {} + MissionFeasibilityChecker(Navigator *navigator) : + ModuleParams(nullptr), + _navigator(navigator), + _feasibility_checker() + { + + } ~MissionFeasibilityChecker() = default; MissionFeasibilityChecker(const MissionFeasibilityChecker &) = delete;