diff --git a/src/modules/navigator/CMakeLists.txt b/src/modules/navigator/CMakeLists.txt index 08eb898a74..ce5abd72bd 100644 --- a/src/modules/navigator/CMakeLists.txt +++ b/src/modules/navigator/CMakeLists.txt @@ -32,7 +32,6 @@ ############################################################################ add_subdirectory(GeofenceBreachAvoidance) -add_subdirectory(MissionFeasibility) px4_add_module( MODULE modules__navigator @@ -54,5 +53,4 @@ 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 deleted file mode 100644 index d2dc9ad1d9..0000000000 --- a/src/modules/navigator/MissionFeasibility/CMakeLists.txt +++ /dev/null @@ -1,4 +0,0 @@ -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 deleted file mode 100644 index 86e5a56533..0000000000 --- a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp +++ /dev/null @@ -1,692 +0,0 @@ -/**************************************************************************** - * - * 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 deleted file mode 100644 index 5ae2bf0ce4..0000000000 --- a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp +++ /dev/null @@ -1,139 +0,0 @@ -/**************************************************************************** - * - * 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 013d331a0b..b1f7a9bd80 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -41,7 +41,6 @@ */ #include "mission_feasibility_checker.h" -#include "MissionFeasibility/FeasibilityChecker.hpp" #include "mission_block.h" #include "navigator.h" @@ -60,42 +59,52 @@ 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; } - if (!home_alt_valid) { - 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"); - return false; - } - bool failed = false; - for (size_t i = 0; i < mission.count; i++) { - struct mission_item_s missionitem = {}; - const ssize_t len = sizeof(struct mission_item_s); + // 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 (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; - } - - _feasibility_checker.processNextItem(missionitem, i, mission.count); + 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); } - failed |= _feasibility_checker.someCheckFailed(); + const float home_alt = _navigator->get_home_position()->alt; - failed |= !checkGeofence(mission, _navigator->get_home_position()->alt, home_valid); + // reset for next check + _has_takeoff = false; + _has_landing = false; - _navigator->get_mission_result()->warning = failed; + // 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); + + // 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); + + } 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(); return !failed; } @@ -144,3 +153,749 @@ 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 90b87952fa..948f6f1553 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -45,7 +45,6 @@ #include #include #include -#include "MissionFeasibility/FeasibilityChecker.hpp" class Geofence; class Navigator; @@ -54,21 +53,106 @@ class MissionFeasibilityChecker: public ModuleParams { private: Navigator *_navigator{nullptr}; - FeasibilityChecker _feasibility_checker; + + /** + * @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); 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), - _feasibility_checker() - { - - } + MissionFeasibilityChecker(Navigator *navigator) : ModuleParams(nullptr), _navigator(navigator) {} ~MissionFeasibilityChecker() = default; MissionFeasibilityChecker(const MissionFeasibilityChecker &) = delete;