FeasibilityChecker: Add new TakeoffLandAvailable option

ADd a new misison feasiblity checker option to check if a proper landing approach is defined when in air. There must be at least a mission landing or a VTOL approach defined in order for the mission to be accepted. Else, use the same logic as in MIS_TKO_LAND_REQ=4
This commit is contained in:
Konrad 2024-02-14 10:16:52 +01:00 committed by Silvan Fuhrer
parent 68100650da
commit 424c3cd2cb
3 changed files with 45 additions and 9 deletions

View File

@ -125,6 +125,12 @@ void FeasibilityChecker::updateData()
_current_position_lat_lon = matrix::Vector2d(vehicle_global_position.lat, vehicle_global_position.lon);
}
if (_rtl_status_sub.updated()) {
rtl_status_s rtl_status = {};
_rtl_status_sub.copy(&rtl_status);
_has_vtol_approach = rtl_status.has_vtol_approach;
}
param_t handle = param_find("FW_LND_ANG");
if (handle != PARAM_INVALID) {
@ -577,17 +583,22 @@ bool FeasibilityChecker::checkTakeoffLandAvailable()
break;
case 4:
result = _has_takeoff == _landing_valid;
result = hasMissionBothOrNeitherTakeoffAndLanding();
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");
break;
} 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");
case 5:
if (!_is_landed && !_has_vtol_approach) {
result = _landing_valid;
if (!result) {
mavlink_log_critical(_mavlink_log_pub, "Mission rejected: Landing waypoint/pattern required.");
events::send(events::ID("feasibility_mis_in_air_landing_req"), {events::Log::Error, events::LogInternal::Info},
"Mission rejected: Landing waypoint/pattern required");
}
} else {
result = hasMissionBothOrNeitherTakeoffAndLanding();
}
break;
@ -600,6 +611,23 @@ bool FeasibilityChecker::checkTakeoffLandAvailable()
return result;
}
bool FeasibilityChecker::hasMissionBothOrNeitherTakeoffAndLanding()
{
bool 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");
}
return result;
}
bool FeasibilityChecker::checkHorizontalDistanceToFirstWaypoint(mission_item_s &mission_item)
{

View File

@ -36,6 +36,7 @@
#include "../navigation.h"
#include <mathlib/mathlib.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/rtl_status.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_land_detected.h>
@ -97,6 +98,7 @@ private:
uORB::Subscription _status_sub{ORB_ID(vehicle_status)};
uORB::Subscription _land_detector_sub{ORB_ID(vehicle_land_detected)};
uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
uORB::Subscription _rtl_status_sub{ORB_ID(rtl_status)};
// parameters
float _param_fw_lnd_ang{0.f};
@ -106,6 +108,7 @@ private:
bool _is_landed{false};
float _home_alt_msl{NAN};
bool _has_vtol_approach{false};
matrix::Vector2d _home_lat_lon = matrix::Vector2d((double)NAN, (double)NAN);
matrix::Vector2d _current_position_lat_lon = matrix::Vector2d((double)NAN, (double)NAN);
VehicleType _vehicle_type{VehicleType::RotaryWing};
@ -247,4 +250,8 @@ private:
* @return False if the check failed.
*/
void doMulticopterChecks(mission_item_s &mission_item, const int current_index);
// Helper functions
bool hasMissionBothOrNeitherTakeoffAndLanding();
};

View File

@ -68,6 +68,7 @@ PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 2.5f);
* @value 2 Require a landing
* @value 3 Require a takeoff and a landing
* @value 4 Require both a takeoff and a landing, or neither
* @value 5 Same as previous, but require a landing if in air and no valid VTOL landing approach is present
* @group Mission
*/
PARAM_DEFINE_INT32(MIS_TKO_LAND_REQ, 0);