fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

MissionFeasibility: add combined takeoff/landing requirement check

Browse Source 
Replace the existing check for the availability of a takeoff mission item with a combined
check for takeoff and landing item (or landing pattern). New param MIS_TKO_LAND_REQ
can be set to require only a takeoff, only a landing, both takeoff and landing, and
both or none. The latter is meant to be set if is e.g. deemed unsafe to start a flight
through a Takeoff WP without though defining a Landing - as then in case of a RTL the
vehicle doesn't follow a pre-defined path but instead only can do default RTL that especially
for FW and VTOL isn't always safe.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2022-11-10 17:11:20 +01:00
parent cce3b43b4f
commit 4cd7dfa162
7 changed files with 144 additions and 115 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ROMFS/px4fmu_common/init.d/rc.fw_defaults
View File

@ -45,6 +45,7 @@ param set-default NAV_ACC_RAD 10
param set-default MIS_DIST_WPS 5000
param set-default MIS_LTRMIN_ALT 25
param set-default MIS_TAKEOFF_ALT 25
param set-default MIS_TKO_LAND_REQ 2
#
# FW takeoff acceleration can easily exceed ublox GPS 2G default.

1
ROMFS/px4fmu_common/init.d/rc.vtol_defaults
View File

@ -19,6 +19,7 @@ param set-default EKF2_FUSE_BETA 1
param set-default HTE_VXY_THR 2.0
param set-default MIS_DIST_WPS 5000
param set-default MIS_TKO_LAND_REQ 2
param set-default MPC_ACC_HOR_MAX 2
param set-default MPC_VEL_MANUAL 5

3
src/modules/navigator/mission.cpp
View File

@ -1743,8 +1743,7 @@ Mission::check_mission_valid(bool force)
_navigator->get_mission_result()->valid =
_missionFeasibilityChecker.checkMissionFeasible(_mission,
_param_mis_dist_1wp.get(),
_param_mis_dist_wps.get(),
_navigator->mission_landing_required());
_param_mis_dist_wps.get());
_navigator->get_mission_result()->seq_total = _mission.count;
_navigator->increment_mission_instance_count();

210
src/modules/navigator/mission_feasibility_checker.cpp
View File

@ -54,8 +54,7 @@
bool
MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission,
float max_distance_to_1st_waypoint, float max_distance_between_waypoints,
bool land_start_req)
float max_distance_to_1st_waypoint, float max_distance_between_waypoints)
{
// Reset warning flag
_navigator->get_mission_result()->warning = false;
@ -82,57 +81,30 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission,
const float home_alt = _navigator->get_home_position()->alt;
// reset for next check
_has_takeoff = false;
_has_landing = false;
// 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);
failed = failed || !checkTakeoff(mission, home_alt);
if (_navigator->get_vstatus()->is_vtol) {
failed = failed || !checkVTOL(mission, home_alt, false);
failed |= (!checkTakeoff(mission, home_alt) || !checkVTOLLanding(mission) || !checkTakeoffLandAvailable());
} else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
failed = failed || !checkRotarywing(mission, home_alt);
failed |= (!checkTakeoff(mission, home_alt) || !checkRotaryWingLanding(mission) || !checkTakeoffLandAvailable());
} else {
failed = failed || !checkFixedwing(mission, home_alt, land_start_req);
} else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
failed |= (!checkTakeoff(mission, home_alt) || !checkFixedWingLanding(mission) || !checkTakeoffLandAvailable());
}
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)
{
@ -327,7 +299,6 @@ MissionFeasibilityChecker::checkMissionItemValidity(const mission_s &mission)
bool
MissionFeasibilityChecker::checkTakeoff(const mission_s &mission, float home_alt)
{
bool has_takeoff = false;
bool takeoff_first = false;
int takeoff_index = -1;
@ -341,7 +312,7 @@ MissionFeasibilityChecker::checkTakeoff(const mission_s &mission, float home_alt
}
// look for a takeoff waypoint
if (missionitem.nav_cmd == NAV_CMD_TAKEOFF) {
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
@ -367,7 +338,7 @@ MissionFeasibilityChecker::checkTakeoff(const mission_s &mission, float home_alt
}
// tell that mission has a takeoff waypoint
has_takeoff = true;
_has_takeoff = true;
// tell that a takeoff waypoint is the first "waypoint"
// mission item
@ -427,25 +398,14 @@ MissionFeasibilityChecker::checkTakeoff(const mission_s &mission, float home_alt
}
}
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;
}
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
@ -453,14 +413,36 @@ MissionFeasibilityChecker::checkTakeoff(const mission_s &mission, float home_alt
}
bool
MissionFeasibilityChecker::checkFixedWingLanding(const mission_s &mission, bool land_start_req)
MissionFeasibilityChecker::checkRotaryWingLanding(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.
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) {
_has_landing = true;
}
}
return true;
}
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;
bool land_start_found = false;
size_t do_land_start_index = 0;
size_t landing_approach_index = 0;
@ -475,14 +457,14 @@ MissionFeasibilityChecker::checkFixedWingLanding(const mission_s &mission, bool
// if DO_LAND_START found then require valid landing AFTER
if (missionitem.nav_cmd == NAV_CMD_DO_LAND_START) {
if (land_start_found) {
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 {
land_start_found = true;
_has_landing = true;
do_land_start_index = i;
}
}
@ -615,7 +597,7 @@ MissionFeasibilityChecker::checkFixedWingLanding(const mission_s &mission, bool
}
} else if (missionitem.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
if (land_start_found && do_land_start_index < i) {
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},
@ -625,14 +607,7 @@ MissionFeasibilityChecker::checkFixedWingLanding(const mission_s &mission, bool
}
}
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))) {
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");
@ -644,12 +619,10 @@ MissionFeasibilityChecker::checkFixedWingLanding(const mission_s &mission, bool
}
bool
MissionFeasibilityChecker::checkVTOLLanding(const mission_s &mission, bool land_start_req)
MissionFeasibilityChecker::checkVTOLLanding(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 */
// Go through all mission items and search for a landing waypoint, then run some checks on it
bool land_start_found = false;
size_t do_land_start_index = 0;
size_t landing_approach_index = 0;
@ -664,14 +637,14 @@ MissionFeasibilityChecker::checkVTOLLanding(const mission_s &mission, bool land_
// if DO_LAND_START found then require valid landing AFTER
if (missionitem.nav_cmd == NAV_CMD_DO_LAND_START) {
if (land_start_found) {
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 {
land_start_found = true;
_has_landing = true;
do_land_start_index = i;
}
}
@ -696,7 +669,7 @@ MissionFeasibilityChecker::checkVTOLLanding(const mission_s &mission, bool land_
}
} else if (missionitem.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
if (land_start_found && do_land_start_index < i) {
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},
@ -706,14 +679,7 @@ MissionFeasibilityChecker::checkVTOLLanding(const mission_s &mission, bool land_
}
}
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)) {
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");
@ -724,6 +690,74 @@ MissionFeasibilityChecker::checkVTOLLanding(const mission_s &mission, bool land_
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)
{

25
src/modules/navigator/mission_feasibility_checker.h
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013-2017 PX4 Development Team. All rights reserved.
* Copyright (c) 2013-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
@ -56,26 +56,18 @@ private:
/* Checks for all airframes */
bool checkGeofence(const mission_s &mission, float home_alt, bool home_valid);
bool checkHomePositionAltitude(const mission_s &mission, float home_alt, bool home_alt_valid);
bool checkMissionItemValidity(const mission_s &mission);
bool checkDistanceToFirstWaypoint(const mission_s &mission, float max_distance);
bool checkDistancesBetweenWaypoints(const mission_s &mission, float max_distance);
bool checkTakeoff(const mission_s &mission, float home_alt);
bool checkTakeoffLandAvailable();
bool checkRotaryWingLanding(const mission_s &mission);
bool checkFixedWingLanding(const mission_s &mission);
bool checkVTOLLanding(const mission_s &mission);
/* Checks specific to fixedwing airframes */
bool checkFixedwing(const mission_s &mission, float home_alt, bool land_start_req);
bool checkFixedWingLanding(const mission_s &mission, bool land_start_req);
/* Checks specific to rotarywing airframes */
bool checkRotarywing(const mission_s &mission, float home_alt);
/* Checks specific to VTOL airframes */
bool checkVTOL(const mission_s &mission, float home_alt, bool land_start_req);
bool checkVTOLLanding(const mission_s &mission, bool land_start_req);
bool _has_takeoff{false};
bool _has_landing{false};
public:
MissionFeasibilityChecker(Navigator *navigator) : ModuleParams(nullptr), _navigator(navigator) {}
@ -88,6 +80,5 @@ public:
* Returns true if mission is feasible and false otherwise
*/
bool checkMissionFeasible(const mission_s &mission,
float max_distance_to_1st_waypoint, float max_distance_between_waypoints,
bool land_start_req);
float max_distance_to_1st_waypoint, float max_distance_between_waypoints);
};

13
src/modules/navigator/mission_params.c
View File

@ -58,14 +58,19 @@
PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 2.5f);
/**
* Take-off waypoint required
* Mission takeoff/landing required
*
* If set, the mission feasibility checker will check for a takeoff waypoint on the mission.
* Specifies if a mission has to contain a takeoff and/or mission landing.
* Validity of configured takeoffs/landings is checked independently of the setting here.
*
* @boolean
* @value 0 No requirements
* @value 1 Require a takeoff
* @value 2 Require a landing
* @value 3 Require a takeoff and a landing
* @value 4 Require a takeoff and a landing, or neither of both
* @group Mission
*/
PARAM_DEFINE_INT32(MIS_TAKEOFF_REQ, 0);
PARAM_DEFINE_INT32(MIS_TKO_LAND_REQ, 0);
/**
* Minimum Loiter altitude

6
src/modules/navigator/navigator.h
View File

@ -297,8 +297,6 @@ public:
float get_mission_landing_loiter_radius() { return _mission.get_landing_loiter_rad(); }
// RTL
bool mission_landing_required() { return _rtl.get_rtl_type() == RTL::RTL_TYPE_MISSION_LANDING; }
bool in_rtl_state() const { return _vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; }
bool abort_landing();
@ -308,7 +306,7 @@ public:
// Param access
float get_loiter_min_alt() const { return _param_mis_ltrmin_alt.get(); }
float get_takeoff_min_alt() const { return _param_mis_takeoff_alt.get(); }
bool get_takeoff_required() const { return _param_mis_takeoff_req.get(); }
int get_takeoff_land_required() const { return _para_mis_takeoff_land_req.get(); }
float get_yaw_timeout() const { return _param_mis_yaw_tmt.get(); }
float get_yaw_threshold() const { return math::radians(_param_mis_yaw_err.get()); }
float get_lndmc_alt_max() const { return _param_lndmc_alt_max.get(); }
@ -451,7 +449,7 @@ private:
// non-navigator parameters: Mission (MIS_*)
(ParamFloat<px4::params::MIS_LTRMIN_ALT>) _param_mis_ltrmin_alt,
(ParamFloat<px4::params::MIS_TAKEOFF_ALT>) _param_mis_takeoff_alt,
(ParamBool<px4::params::MIS_TAKEOFF_REQ>) _param_mis_takeoff_req,
(ParamInt<px4::params::MIS_TKO_LAND_REQ>) _para_mis_takeoff_land_req,
(ParamFloat<px4::params::MIS_YAW_TMT>) _param_mis_yaw_tmt,
(ParamFloat<px4::params::MIS_YAW_ERR>) _param_mis_yaw_err,
(ParamFloat<px4::params::MIS_PD_TO>) _param_mis_payload_delivery_timeout,