mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-23 02:03:33 +08:00
bresch
3f3a4ac5c1
This is to make clear that the relevant part of the home position message for navigator is the global one. Local home position isn't required as everything is done in global coordinates. The specific home_alt_valid is used when lat/lon are not used
816 lines
34 KiB
C++
816 lines
34 KiB
C++
/****************************************************************************
|
|
*
|
|
* Copyright (c) 2013-2019 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.
|
|
*
|
|
****************************************************************************/
|
|
/**
|
|
* @file mission_feasibility_checker.cpp
|
|
* Provides checks if mission is feasible given the navigation capabilities
|
|
*
|
|
* @author Lorenz Meier <lm@inf.ethz.ch>
|
|
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
|
|
* @author Sander Smeets <sander@droneslab.com>
|
|
* @author Nuno Marques <nuno.marques@dronesolutions.io>
|
|
*/
|
|
|
|
#include "mission_feasibility_checker.h"
|
|
|
|
#include "mission_block.h"
|
|
#include "navigator.h"
|
|
|
|
#include <drivers/drv_pwm_output.h>
|
|
#include <lib/geo/geo.h>
|
|
#include <lib/mathlib/mathlib.h>
|
|
#include <lib/landing_slope/Landingslope.hpp>
|
|
#include <systemlib/mavlink_log.h>
|
|
#include <uORB/Subscription.hpp>
|
|
#include <uORB/topics/position_controller_landing_status.h>
|
|
#include <px4_platform_common/events.h>
|
|
|
|
bool
|
|
MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission,
|
|
float max_distance_to_1st_waypoint, float max_distance_between_waypoints,
|
|
bool land_start_req)
|
|
{
|
|
// Reset warning flag
|
|
_navigator->get_mission_result()->warning = false;
|
|
|
|
// trivial case: A mission with length zero cannot be valid
|
|
if ((int)mission.count <= 0) {
|
|
return false;
|
|
}
|
|
|
|
bool failed = false;
|
|
|
|
// first check if we have a valid position
|
|
const bool home_valid = _navigator->home_global_position_valid();
|
|
const bool home_alt_valid = _navigator->home_alt_valid();
|
|
|
|
if (!home_alt_valid) {
|
|
failed = true;
|
|
mavlink_log_info(_navigator->get_mavlink_log_pub(), "Not yet ready for mission, no position lock.\t");
|
|
events::send(events::ID("navigator_mis_no_pos_lock"), events::Log::Info, "Not yet ready for mission, no position lock");
|
|
|
|
} else {
|
|
failed = failed || !checkDistanceToFirstWaypoint(mission, max_distance_to_1st_waypoint);
|
|
}
|
|
|
|
const float home_alt = _navigator->get_home_position()->alt;
|
|
|
|
// check if all mission item commands are supported
|
|
failed = failed || !checkMissionItemValidity(mission);
|
|
failed = failed || !checkDistancesBetweenWaypoints(mission, max_distance_between_waypoints);
|
|
failed = failed || !checkGeofence(mission, home_alt, home_valid);
|
|
failed = failed || !checkHomePositionAltitude(mission, home_alt, home_alt_valid);
|
|
|
|
if (_navigator->get_vstatus()->is_vtol) {
|
|
failed = failed || !checkVTOL(mission, home_alt, false);
|
|
|
|
} else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
|
|
failed = failed || !checkRotarywing(mission, home_alt);
|
|
|
|
} else {
|
|
failed = failed || !checkFixedwing(mission, home_alt, land_start_req);
|
|
}
|
|
|
|
return !failed;
|
|
}
|
|
|
|
bool
|
|
MissionFeasibilityChecker::checkRotarywing(const mission_s &mission, float home_alt)
|
|
{
|
|
/*
|
|
* Perform check and issue feedback to the user
|
|
* Mission is only marked as feasible if takeoff check passes
|
|
*/
|
|
return checkTakeoff(mission, home_alt);
|
|
}
|
|
|
|
bool
|
|
MissionFeasibilityChecker::checkFixedwing(const mission_s &mission, float home_alt, bool land_start_req)
|
|
{
|
|
/* Perform checks and issue feedback to the user for all checks */
|
|
bool resTakeoff = checkTakeoff(mission, home_alt);
|
|
bool resLanding = checkFixedWingLanding(mission, land_start_req);
|
|
|
|
/* Mission is only marked as feasible if all checks return true */
|
|
return (resTakeoff && resLanding);
|
|
}
|
|
|
|
bool
|
|
MissionFeasibilityChecker::checkVTOL(const mission_s &mission, float home_alt, bool land_start_req)
|
|
{
|
|
/* Perform checks and issue feedback to the user for all checks */
|
|
bool resTakeoff = checkTakeoff(mission, home_alt);
|
|
bool resLanding = checkVTOLLanding(mission, land_start_req);
|
|
|
|
/* Mission is only marked as feasible if all checks return true */
|
|
return (resTakeoff && resLanding);
|
|
}
|
|
|
|
bool
|
|
MissionFeasibilityChecker::checkGeofence(const mission_s &mission, float home_alt, bool home_valid)
|
|
{
|
|
if (_navigator->get_geofence().isHomeRequired() && !home_valid) {
|
|
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence requires valid home position\t");
|
|
events::send(events::ID("navigator_mis_geofence_no_home"), {events::Log::Error, events::LogInternal::Info},
|
|
"Geofence requires a valid home position");
|
|
return false;
|
|
}
|
|
|
|
/* Check if all mission items are inside the geofence (if we have a valid geofence) */
|
|
if (_navigator->get_geofence().valid()) {
|
|
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.altitude_is_relative && !home_valid) {
|
|
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence requires valid home position\t");
|
|
events::send(events::ID("navigator_mis_geofence_no_home2"), {events::Log::Error, events::LogInternal::Info},
|
|
"Geofence requires a valid home position");
|
|
return false;
|
|
}
|
|
|
|
// Geofence function checks against home altitude amsl
|
|
missionitem.altitude = missionitem.altitude_is_relative ? missionitem.altitude + home_alt : missionitem.altitude;
|
|
|
|
if (MissionBlock::item_contains_position(missionitem) && !_navigator->get_geofence().check(missionitem)) {
|
|
|
|
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence violation for waypoint %zu\t", i + 1);
|
|
events::send<int16_t>(events::ID("navigator_mis_geofence_violation"), {events::Log::Error, events::LogInternal::Info},
|
|
"Geofence violation for waypoint {1}",
|
|
i + 1);
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
|
|
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_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) {
|
|
|
|
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 has_takeoff = false;
|
|
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) {
|
|
// 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 (_navigator->get_takeoff_required() && _navigator->get_land_detected()->landed) {
|
|
// check for a takeoff waypoint, after the above conditions have been met
|
|
// MIS_TAKEOFF_REQ param has to be set and the vehicle has to be landed - one can load a mission
|
|
// while the vehicle is flying and it does not require a takeoff waypoint
|
|
if (!has_takeoff) {
|
|
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;
|
|
|
|
} else if (!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::checkFixedWingLanding(const mission_s &mission, bool land_start_req)
|
|
{
|
|
/* 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;
|
|
|
|
bool land_start_found = 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 (land_start_found) {
|
|
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 {
|
|
land_start_found = true;
|
|
do_land_start_index = i;
|
|
}
|
|
}
|
|
|
|
if (missionitem.nav_cmd == NAV_CMD_LAND) {
|
|
mission_item_s missionitem_previous {};
|
|
|
|
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)) {
|
|
|
|
uORB::SubscriptionData<position_controller_landing_status_s> landing_status{ORB_ID(position_controller_landing_status)};
|
|
|
|
const bool landing_status_valid = (landing_status.get().timestamp > 0);
|
|
const float wp_distance = get_distance_to_next_waypoint(missionitem_previous.lat, missionitem_previous.lon,
|
|
missionitem.lat, missionitem.lon);
|
|
|
|
if (landing_status_valid && (wp_distance > landing_status.get().flare_length)) {
|
|
/* Last wp is before flare region */
|
|
|
|
const float delta_altitude = missionitem.altitude - missionitem_previous.altitude;
|
|
|
|
if (delta_altitude < 0) {
|
|
|
|
const float horizontal_slope_displacement = landing_status.get().horizontal_slope_displacement;
|
|
const float slope_angle_rad = landing_status.get().slope_angle_rad;
|
|
const float slope_alt_req = Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, missionitem.altitude,
|
|
horizontal_slope_displacement, slope_angle_rad);
|
|
|
|
if (missionitem_previous.altitude > slope_alt_req + 1.0f) {
|
|
/* Landing waypoint is above altitude of slope at the given waypoint distance (with small tolerance for floating point discrepancies) */
|
|
const float wp_distance_req = Landingslope::getLandingSlopeWPDistance(missionitem_previous.altitude,
|
|
missionitem.altitude, horizontal_slope_displacement, slope_angle_rad);
|
|
|
|
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: adjust landing approach.\t");
|
|
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Move down %d m or move further away by %d m.\t",
|
|
(int)ceilf(slope_alt_req - missionitem_previous.altitude),
|
|
(int)ceilf(wp_distance_req - wp_distance));
|
|
/* EVENT
|
|
* @description
|
|
* The landing waypoint must be above the altitude of slope at the given waypoint distance.
|
|
* Move it down {1m_v} or move it further away by {2m}.
|
|
*/
|
|
events::send<int32_t, int32_t>(events::ID("navigator_mis_land_approach"), {events::Log::Error, events::LogInternal::Info},
|
|
"Mission rejected: adjust landing approach",
|
|
(int)ceilf(slope_alt_req - missionitem_previous.altitude),
|
|
(int)ceilf(wp_distance_req - wp_distance));
|
|
|
|
return false;
|
|
}
|
|
|
|
} else {
|
|
/* Landing waypoint is above last waypoint */
|
|
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: landing above last waypoint.\t");
|
|
events::send(events::ID("navigator_mis_land_too_high"), {events::Log::Error, events::LogInternal::Info},
|
|
"Mission rejected: landing waypoint is above the last waypoint");
|
|
return false;
|
|
}
|
|
|
|
} else {
|
|
/* Last wp is in flare region */
|
|
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: waypoint within landing flare.\t");
|
|
events::send(events::ID("navigator_mis_land_within_flare"), {events::Log::Error, events::LogInternal::Info},
|
|
"Mission rejected: waypoint is within landing flare");
|
|
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 (land_start_found && 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 (land_start_req && !land_start_found) {
|
|
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: landing pattern required.\t");
|
|
events::send(events::ID("navigator_mis_land_missing"), {events::Log::Error, events::LogInternal::Info},
|
|
"Mission rejected: landing pattern required");
|
|
return false;
|
|
}
|
|
|
|
if (land_start_found && (!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, bool land_start_req)
|
|
{
|
|
/* 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 land_start_found = 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 (land_start_found) {
|
|
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 {
|
|
land_start_found = 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 {};
|
|
|
|
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 (land_start_found && 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 (land_start_req && !land_start_found) {
|
|
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: landing pattern required.\t");
|
|
events::send(events::ID("navigator_mis_land_missing2"), {events::Log::Error, events::LogInternal::Info},
|
|
"Mission rejected: landing pattern required");
|
|
return false;
|
|
}
|
|
|
|
if (land_start_found && (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::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;
|
|
}
|