From 68100650da9ae3676db24a673cdf29881205f831 Mon Sep 17 00:00:00 2001 From: Konrad Date: Wed, 14 Feb 2024 10:40:15 +0100 Subject: [PATCH] RTL: publish a status message on currently chosen RTL point --- msg/CMakeLists.txt | 1 + msg/RtlStatus.msg | 15 +++++++++ src/modules/logger/logged_topics.cpp | 1 + src/modules/navigator/navigator_main.cpp | 2 +- src/modules/navigator/rtl.cpp | 39 +++++++++++++++++++++--- src/modules/navigator/rtl.h | 11 +++++-- 6 files changed, 62 insertions(+), 7 deletions(-) create mode 100644 msg/RtlStatus.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index eaad5510bb..62544cffc0 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -173,6 +173,7 @@ set(msg_files RegisterExtComponentReply.msg RegisterExtComponentRequest.msg Rpm.msg + RtlStatus.msg RtlTimeEstimate.msg SatelliteInfo.msg SensorAccel.msg diff --git a/msg/RtlStatus.msg b/msg/RtlStatus.msg new file mode 100644 index 0000000000..94c90d513e --- /dev/null +++ b/msg/RtlStatus.msg @@ -0,0 +1,15 @@ +uint64 timestamp # time since system start (microseconds) + +uint32 safe_points_id # unique ID of active set of safe_point_items +bool is_evaluation_pending # flag if the RTL point needs reevaluation (e.g. new safe points available, but need loading). + +bool has_vtol_approach # flag if approaches are defined for current RTL_TYPE parameter setting + +uint8 rtl_type # Type of RTL chosen +uint8 safe_point_index # index of the chosen safe point, if in RTL_STATUS_TYPE_DIRECT_SAFE_POINT mode + +uint8 RTL_STATUS_TYPE_NONE=0 # RTL type is pending if evaluation can't pe performed currently e.g. when it is still loading the safe points +uint8 RTL_STATUS_TYPE_DIRECT_SAFE_POINT=1 # RTL type is chosen to directly go to a safe point or home position +uint8 RTL_STATUS_TYPE_DIRECT_MISSION_LAND=2 # RTL type is going straight to the beginning of the mission landing +uint8 RTL_STATUS_TYPE_FOLLOW_MISSION=3 # RTL type is following the mission from closest point to mission landing +uint8 RTL_STATUS_TYPE_FOLLOW_MISSION_REVERSE=4 # RTL type is following the mission in reverse to the start position diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index c19f31ee13..68b3d27b42 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -102,6 +102,7 @@ void LoggedTopics::add_default_topics() add_optional_topic("px4io_status"); add_topic("radio_status"); add_topic("rtl_time_estimate", 1000); + add_optional_topic("rtl_status", 5000); add_optional_topic("sensor_airflow", 100); add_topic("sensor_combined"); add_optional_topic("sensor_correction"); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 0dead8ddd5..30cf417c48 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -208,7 +208,7 @@ void Navigator::run() if (mission.safe_points_id != safe_points_id) { safe_points_id = mission.safe_points_id; - _rtl.updateSafePoints(); + _rtl.updateSafePoints(safe_points_id); } } diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index af30abab1e..db20f8cf74 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -294,12 +294,14 @@ void RTL::setRtlTypeAndDestination() init_rtl_mission_type(); + uint8_t safe_point_index{0U}; + if (_param_rtl_type.get() != 2) { // check the closest allowed destination. DestinationType destination_type{DestinationType::DESTINATION_TYPE_HOME}; PositionYawSetpoint rtl_position; float rtl_alt; - findRtlDestination(destination_type, rtl_position, rtl_alt); + findRtlDestination(destination_type, rtl_position, rtl_alt, safe_point_index); switch (destination_type) { case DestinationType::DESTINATION_TYPE_MISSION_LAND: @@ -331,9 +333,29 @@ void RTL::setRtlTypeAndDestination() break; } } + + // Publish rtl status + _rtl_status_pub.get().timestamp = hrt_absolute_time(); + _rtl_status_pub.get().safe_points_id = _safe_points_id; + _rtl_status_pub.get().is_evaluation_pending = _dataman_state != DatamanState::UpdateRequestWait; + _rtl_status_pub.get().has_vtol_approach = false; + + if ((_param_rtl_type.get() == 0) || (_param_rtl_type.get() == 3)) { + _rtl_status_pub.get().has_vtol_approach = _home_has_land_approach || _one_rally_point_has_land_approach; + + } else if (_param_rtl_type.get() == 1) { + _rtl_status_pub.get().has_vtol_approach = _one_rally_point_has_land_approach; + } + + _rtl_status_pub.get().rtl_type = static_cast(_rtl_type); + _rtl_status_pub.get().safe_point_index = safe_point_index; + + _rtl_status_pub.update(); + } -void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpoint &rtl_position, float &rtl_alt) +void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpoint &rtl_position, float &rtl_alt, + uint8_t &safe_point_index) { // set destination to home per default, then check if other valid landing spot is closer rtl_position.alt = _home_pos_sub.get().alt; @@ -352,8 +374,10 @@ void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpo float home_dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, rtl_position.lat, rtl_position.lon)}; float min_dist; + _home_has_land_approach = hasVtolLandApproach(rtl_position); + if (((_param_rtl_type.get() == 1) && !vtol_in_rw_mode) || (vtol_in_fw_mode && (_param_rtl_approach_force.get() == 1) - && !hasVtolLandApproach(rtl_position))) { + && !_home_has_land_approach)) { // Set minimum distance to maximum value when RTL_TYPE is set to 1 and we are not in RW mode or we forces approach landing for vtol in fw and it is not defined for home. min_dist = FLT_MAX; @@ -394,6 +418,8 @@ void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpo if (_safe_points_updated) { + _one_rally_point_has_land_approach = false; + for (int current_seq = 0; current_seq < _dataman_cache_safepoint.size(); ++current_seq) { mission_item_s mission_safe_point; @@ -416,11 +442,16 @@ void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpo PositionYawSetpoint safepoint_position; setSafepointAsDestination(safepoint_position, mission_safe_point); + bool current_safe_point_has_approaches{hasVtolLandApproach(safepoint_position)}; + + _one_rally_point_has_land_approach |= current_safe_point_has_approaches; + if (((dist + MIN_DIST_THRESHOLD) < min_dist) && (!vtol_in_fw_mode || (_param_rtl_approach_force.get() == 0) - || hasVtolLandApproach(safepoint_position))) { + || current_safe_point_has_approaches)) { min_dist = dist; rtl_position = safepoint_position; destination_type = DestinationType::DESTINATION_TYPE_SAFE_POINT; + safe_point_index = current_seq; } } } diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 525a4583a7..ab61aebe29 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -58,6 +58,7 @@ #include #include #include +#include #include class Navigator; @@ -86,7 +87,7 @@ public: void set_return_alt_min(bool min) { _enforce_rtl_alt = min; } - void updateSafePoints() { _initiate_safe_points_updated = true; } + void updateSafePoints(uint32_t new_safe_point_id) { _initiate_safe_points_updated = true; _safe_points_id = new_safe_point_id; } private: enum class DestinationType { @@ -109,7 +110,8 @@ private: * @brief Find RTL destination. * */ - void findRtlDestination(DestinationType &destination_type, PositionYawSetpoint &rtl_position, float &rtl_alt); + void findRtlDestination(DestinationType &destination_type, PositionYawSetpoint &rtl_position, float &rtl_alt, + uint8_t &safe_point_index); /** * @brief Set the position of the land start marker in the planned mission as destination. @@ -188,6 +190,9 @@ private: RtlType _rtl_type{RtlType::RTL_DIRECT}; + bool _home_has_land_approach; ///< Flag if the home position has a land approach defined + bool _one_rally_point_has_land_approach; ///< Flag if a rally point has a land approach defined + DatamanState _dataman_state{DatamanState::UpdateRequestWait}; DatamanState _error_state{DatamanState::UpdateRequestWait}; uint32_t _opaque_id{0}; ///< dataman safepoint id: if it does not match, safe points data was updated @@ -197,6 +202,7 @@ private: bool _initiate_safe_points_updated{true}; ///< flag indicating if safe points update is needed mutable DatamanCache _dataman_cache_landItem{"rtl_dm_cache_miss_land", 2}; uint32_t _mission_id = 0u; + uint32_t _safe_points_id = 0u; mission_stats_entry_s _stats; @@ -222,4 +228,5 @@ private: uORB::SubscriptionData _wind_sub{ORB_ID(wind)}; uORB::Publication _rtl_time_estimate_pub{ORB_ID(rtl_time_estimate)}; + uORB::PublicationData _rtl_status_pub{ORB_ID(rtl_status)}; };