diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp index 68dbd2509d..ef829d62f5 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp @@ -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) { diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp index d6c859932b..fd18cbcfb6 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp @@ -36,6 +36,7 @@ #include "../navigation.h" #include #include +#include #include #include #include @@ -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(); }; diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index edfbac54bd..051f3c816b 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -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);