fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-03 06:01:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

started with feasibility checks

Browse Source 
Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst
2023-02-16 11:28:41 +01:00
committed by Roman Bapst
parent d6bb19e11b
commit ceb8f6e1d5
6 changed files with 869 additions and 871 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/CMakeLists.txt
+2
View File
@@ -32,6 +32,7 @@
############################################################################ ############################################################################
add_subdirectory(GeofenceBreachAvoidance) add_subdirectory(GeofenceBreachAvoidance)
add_subdirectory(MissionFeasibility)
px4_add_module( px4_add_module(
MODULE modules__navigator MODULE modules__navigator
@@ -53,4 +54,5 @@ px4_add_module(
geo geo
geofence_breach_avoidance geofence_breach_avoidance
motion_planning motion_planning
mission_feasibility_checker
) )
src/modules/navigator/MissionFeasibility/CMakeLists.txt
+4
View File
@@ -0,0 +1,4 @@
px4_add_library(mission_feasibility_checker
FeasibilityChecker.hpp
FeasibilityChecker.cpp
)
src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp
+692
View File
@@ -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 <systemlib/mavlink_log.h>
#include <px4_platform_common/events.h>
#include <drivers/drv_pwm_output.h>
#include "../mission_block.h"
#include <lib/mathlib/mathlib.h>
#include <lib/geo/geo.h>
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<uint16_t, uint16_t>(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<uint32_t>(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<uint32_t, uint32_t>(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<float>(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<uint8_t, uint8_t>(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<uint32_t, uint32_t>(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<uint32_t, uint32_t>(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<uint32_t, uint32_t>(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<float, float>(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<int16_t>(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<int16_t>(events::ID("navigator_mis_wp_below_home"), {events::Log::Warning, events::LogInternal::Info},
"Waypoint {1} below home", current_index + 1);
}
return true;
}
src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp
+139
View File
@@ -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 <mathlib/mathlib.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/Subscription.hpp>
#include <px4_platform_common/module_params.h>
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<px4::params::FW_LND_ANG>) _param_fw_lnd_ang,
(ParamFloat<px4::params::MIS_DIST_1WP>) _param_mis_dist_1wp,
(ParamFloat<px4::params::MIS_DIST_WPS>) _param_mis_dist_wps,
(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad,
(ParamInt<px4::params::MIS_TKO_LAND_REQ>) _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);
};
src/modules/navigator/mission_feasibility_checker.cpp
+21 -776
View File
@@ -41,6 +41,7 @@
*/ */
#include "mission_feasibility_checker.h" #include "mission_feasibility_checker.h"
#include "MissionFeasibility/FeasibilityChecker.hpp"
#include "mission_block.h" #include "mission_block.h"
#include "navigator.h" #include "navigator.h"
@@ -59,52 +60,42 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission,
// Reset warning flag // Reset warning flag
_navigator->get_mission_result()->warning = false; _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 // trivial case: A mission with length zero cannot be valid
if ((int)mission.count <= 0) { if ((int)mission.count <= 0) {
return false; 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) { if (!home_alt_valid) {
failed = true;
mavlink_log_info(_navigator->get_mavlink_log_pub(), "Not yet ready for mission, no position lock.\t"); 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"); events::send(events::ID("navigator_mis_no_pos_lock"), events::Log::Info, "Not yet ready for mission, no position lock");
return false;
} else {
failed |= !checkDistanceToFirstWaypoint(mission, max_distance_to_1st_waypoint);
} }
const float home_alt = _navigator->get_home_position()->alt; bool failed = false;
// reset for next check for (size_t i = 0; i < mission.count; i++) {
_has_takeoff = false; struct mission_item_s missionitem = {};
_has_landing = false; const ssize_t len = sizeof(struct mission_item_s);
// run generic (for all vehicle types) checks if (dm_read((dm_item_t)mission.dataman_id, i, &missionitem, len) != len) {
failed |= !checkMissionItemValidity(mission); _navigator->get_mission_result()->warning = true;
failed |= !checkDistancesBetweenWaypoints(mission, max_distance_between_waypoints); /* not supposed to happen unless the datamanager can't access the SD card, etc. */
failed |= !checkGeofence(mission, home_alt, home_valid); return false;
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() _feasibility_checker.processNextItem(missionitem, i, mission.count);
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(); failed |= _feasibility_checker.someCheckFailed();
failed |= !checkGeofence(mission, _navigator->get_home_position()->alt, home_valid);
_navigator->get_mission_result()->warning = failed;
return !failed; return !failed;
} }
@@ -153,749 +144,3 @@ MissionFeasibilityChecker::checkGeofence(const mission_s &mission, float home_al
return true; 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<int16_t>(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<int16_t>(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<uint16_t, uint16_t>(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<uint32_t>(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<uint32_t, uint32_t>(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<float>(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, &param_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<uint8_t, uint8_t>(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<uint32_t, uint32_t>(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<uint32_t, uint32_t>(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<uint32_t, uint32_t>(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<float, float>(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;
}
src/modules/navigator/mission_feasibility_checker.h
+11 -95
View File
@@ -45,6 +45,7 @@
#include <dataman/dataman.h> #include <dataman/dataman.h>
#include <uORB/topics/mission.h> #include <uORB/topics/mission.h>
#include <px4_platform_common/module_params.h> #include <px4_platform_common/module_params.h>
#include "MissionFeasibility/FeasibilityChecker.hpp"
class Geofence; class Geofence;
class Navigator; class Navigator;
@@ -53,106 +54,21 @@ class MissionFeasibilityChecker: public ModuleParams
{ {
private: private:
Navigator *_navigator{nullptr}; 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_takeoff{false};
bool _has_landing{false}; bool _has_landing{false};
bool checkGeofence(const mission_s &mission, float home_alt, bool home_valid);
public: public:
MissionFeasibilityChecker(Navigator *navigator) : ModuleParams(nullptr), _navigator(navigator) {} MissionFeasibilityChecker(Navigator *navigator) :
ModuleParams(nullptr),
_navigator(navigator),
_feasibility_checker()
{
}
~MissionFeasibilityChecker() = default; ~MissionFeasibilityChecker() = default;
MissionFeasibilityChecker(const MissionFeasibilityChecker &) = delete; MissionFeasibilityChecker(const MissionFeasibilityChecker &) = delete;