From ffd670b54cf33fe2eb3ed3f97adc40790ea05235 Mon Sep 17 00:00:00 2001 From: Silvan Date: Thu, 19 Mar 2026 17:03:44 +0100 Subject: [PATCH] chore(navigator): move return to point of last link from RTL to Hold Give the operator the optiont to configure a "Hold at position where the data link was still coming through" by setting NAV_DLL_ACT to Hold and the new param NAV_LTR_LAST_DL to 1. Signed-off-by: Silvan --- src/modules/navigator/loiter.cpp | 25 +++++++++++--- src/modules/navigator/loiter.h | 6 ++++ src/modules/navigator/mission_block.cpp | 13 ++++++++ src/modules/navigator/mission_block.h | 1 + src/modules/navigator/navigator.h | 8 +++++ src/modules/navigator/navigator_main.cpp | 13 ++++++++ src/modules/navigator/navigator_params.yaml | 10 ++++++ src/modules/navigator/rtl.cpp | 36 +++------------------ src/modules/navigator/rtl.h | 7 +--- 9 files changed, 78 insertions(+), 41 deletions(-) diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index 832a7d6f9a..d0064a483e 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -73,6 +73,22 @@ Loiter::on_active() && hrt_elapsed_time(&_navigator->get_reposition_triplet()->current.timestamp) < 500_ms) { reposition(); } + + if (_param_nav_ltr_last_dl.get() && _navigator->get_vstatus()->failsafe && _navigator->get_vstatus()->gcs_connection_lost) { + if (!_loiter_at_last_link_position_executed) { + set_loiter_position(); // if we already were in hold (e.g. GoTo), we need to reset the position setpoint + _loiter_at_last_link_position_executed = true; + } + + } else { + _loiter_at_last_link_position_executed = false; + } +} + +void +Loiter::on_inactive() +{ + _loiter_at_last_link_position_executed = false; } void @@ -87,7 +103,6 @@ Loiter::set_loiter_position() _navigator->get_position_setpoint_triplet()->current.type = position_setpoint_s::SETPOINT_TYPE_IDLE; _navigator->set_position_setpoint_triplet_updated(); return; - } position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); @@ -104,10 +119,13 @@ Loiter::set_loiter_position() const float d_current = get_distance_to_next_waypoint(pos_sp_triplet->current.lat, pos_sp_triplet->current.lon, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); on_loiter = d_current <= (_navigator->get_acceptance_radius() + pos_sp_triplet->current.loiter_radius); - } - if (on_loiter) { + if (_navigator->get_vstatus()->failsafe && _navigator->get_vstatus()->gcs_connection_lost && _param_nav_ltr_last_dl.get()) { + + setLoiterFromLastLink(&_mission_item); + + } else if (on_loiter) { setLoiterItemFromCurrentPositionSetpoint(&_mission_item); } else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { @@ -116,7 +134,6 @@ Loiter::set_loiter_position() } else { setLoiterItemFromCurrentPosition(&_mission_item); } - } // convert mission item to current setpoint diff --git a/src/modules/navigator/loiter.h b/src/modules/navigator/loiter.h index 3af6265e1c..47441cb45c 100644 --- a/src/modules/navigator/loiter.h +++ b/src/modules/navigator/loiter.h @@ -53,6 +53,7 @@ public: void on_activation() override; void on_active() override; + void on_inactive() override; private: /** @@ -66,4 +67,9 @@ private: */ void set_loiter_position(); + bool _loiter_at_last_link_position_executed{false}; + + DEFINE_PARAMETERS( + (ParamInt) _param_nav_ltr_last_dl + ) }; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index b7dc54f420..d56259cb62 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -760,6 +760,19 @@ MissionBlock::setLoiterItemCommonFields(struct mission_item_s *item) item->origin = ORIGIN_ONBOARD; } +void +MissionBlock::setLoiterFromLastLink(struct mission_item_s *item) +{ + setLoiterItemCommonFields(item); + + const PositionYawSetpoint &last_heartbeat_pos = _navigator->get_last_pos_with_gcs_heartbeat(); + + item->lat = PX4_ISFINITE(last_heartbeat_pos.lat) ? last_heartbeat_pos.lat : _navigator->get_global_position()->lat; + item->lon = PX4_ISFINITE(last_heartbeat_pos.lon) ? last_heartbeat_pos.lon : _navigator->get_global_position()->lon; + item->altitude = PX4_ISFINITE(last_heartbeat_pos.alt) ? last_heartbeat_pos.alt : _navigator->get_global_position()->alt; + item->yaw = PX4_ISFINITE(last_heartbeat_pos.yaw) ? last_heartbeat_pos.yaw : NAN; +} + void MissionBlock::set_takeoff_item(struct mission_item_s *item, float abs_altitude) { diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 793e0e6bb3..a96864b8f2 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -165,6 +165,7 @@ protected: void setLoiterItemFromCurrentPosition(struct mission_item_s *item); void setLoiterItemFromCurrentPositionWithBraking(struct mission_item_s *item); + void setLoiterFromLastLink(struct mission_item_s *item); void setLoiterItemCommonFields(struct mission_item_s *item); diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 1b00b5700c..2a50d7a1c6 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -57,6 +57,9 @@ #include "GeofenceBreachAvoidance/geofence_breach_avoidance.h" +#include +#include + #if CONFIG_NAVIGATOR_ADSB #include #endif // CONFIG_NAVIGATOR_ADSB @@ -174,6 +177,8 @@ public: PrecLand *get_precland() { return &_precland; } /**< allow others, e.g. Mission, to use the precision land block */ + const PositionYawSetpoint &get_last_pos_with_gcs_heartbeat() const { return _last_pos_with_gcs_heartbeat; } + const vehicle_roi_s &get_vroi() { return _vroi; } void reset_vroi() { _vroi = {}; } @@ -402,6 +407,9 @@ private: bool _is_capturing_images{false}; // keep track if we need to stop capturing images + uORB::SubscriptionMultiArray _telemetry_status_subs{ORB_ID::telemetry_status}; + PositionYawSetpoint _last_pos_with_gcs_heartbeat{(double)NAN, (double)NAN, NAN, NAN}; + // timer to trigger a delayed set gimbal neutral command hrt_abstime _gimbal_neutral_activation_time{UINT64_MAX}; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 497486fcae..82319c638c 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -225,6 +225,19 @@ void Navigator::run() _global_pos_sub.copy(&_global_pos); } + /* update last known position with GCS heartbeat */ + for (auto &telemetry_sub : _telemetry_status_subs) { + telemetry_status_s telemetry; + + if (telemetry_sub.update(&telemetry) && telemetry.heartbeat_type_gcs) { + _last_pos_with_gcs_heartbeat.lat = _global_pos.lat; + _last_pos_with_gcs_heartbeat.lon = _global_pos.lon; + _last_pos_with_gcs_heartbeat.alt = _global_pos.alt; + _last_pos_with_gcs_heartbeat.yaw = _local_pos.heading; + break; + } + } + /* check for parameter updates */ if (_parameter_update_sub.updated()) { // clear update diff --git a/src/modules/navigator/navigator_params.yaml b/src/modules/navigator/navigator_params.yaml index ed7325de58..18bb1146a0 100644 --- a/src/modules/navigator/navigator_params.yaml +++ b/src/modules/navigator/navigator_params.yaml @@ -140,3 +140,13 @@ parameters: min: -1 decimal: 1 increment: 1 + NAV_LTR_LAST_DL: + description: + short: Loiter at last GCS heartbeat position on data link loss + long: |- + When the data link is lost and this setting is enabled, + the vehicle will loiter at the position where the last GCS + heartbeat was received rather than at its current position. + Only applies to Hold mode during failsafe actions. + type: boolean + default: 0 diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index cf32daabb0..1a2597d877 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -536,32 +536,11 @@ void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpo destination_type = DestinationType::DESTINATION_TYPE_SAFE_POINT; } else if (_param_rtl_type.get() == 5) { - // Safe points only but no valid safe point, fallback to last position with valid data link - for (auto &telemetry_status : _telemetry_status_subs) { - telemetry_status_s telemetry; - - if (telemetry_status.update(&telemetry)) { - - if (telemetry.heartbeat_type_gcs) { - _last_position_before_link_loss.alt = _global_pos_sub.get().alt; - _last_position_before_link_loss.lat = _global_pos_sub.get().lat; - _last_position_before_link_loss.lon = _global_pos_sub.get().lon; - break; - } - } - } - - if (PX4_ISFINITE(_last_position_before_link_loss.lat) && PX4_ISFINITE(_last_position_before_link_loss.lon)) { - destination = _last_position_before_link_loss; - - } else { - // If no valid data link position, fallback to current position - destination.alt = _global_pos_sub.get().alt; - destination.lat = _global_pos_sub.get().lat; - destination.lon = _global_pos_sub.get().lon; - } - - destination_type = DestinationType::DESTINATION_TYPE_LAST_LINK_POSITION; + // for RTL_TYPE=5: if no rally point is found fallback to current position + destination.alt = _global_pos_sub.get().alt; + destination.lat = _global_pos_sub.get().lat; + destination.lon = _global_pos_sub.get().lon; + destination_type = DestinationType::DESTINATION_TYPE_SAFE_POINT; } } @@ -600,11 +579,6 @@ void RTL::setSafepointAsDestination(PositionYawSetpoint &rtl_position, const mis float RTL::computeReturnAltitude(const PositionYawSetpoint &rtl_position, DestinationType destination_type, float cone_half_angle_deg) const { - if (destination_type == DestinationType::DESTINATION_TYPE_LAST_LINK_POSITION) { - // when returning to last known link position, do not modify altitude - return rtl_position.alt; - } - if (_param_rtl_cone_ang.get() > 0 && _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { // horizontal distance to destination const float destination_dist = diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index d6c5a9afb6..35cff4bdcb 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -55,13 +55,11 @@ #include #include #include -#include #include #include #include #include #include -#include class Navigator; @@ -96,8 +94,7 @@ private: enum class DestinationType { DESTINATION_TYPE_HOME, DESTINATION_TYPE_MISSION_LAND, - DESTINATION_TYPE_SAFE_POINT, - DESTINATION_TYPE_LAST_LINK_POSITION + DESTINATION_TYPE_SAFE_POINT }; private: @@ -229,7 +226,6 @@ private: mutable DatamanCache _dataman_cache_landItem{"rtl_dm_cache_miss_land", 2}; uint32_t _mission_id = 0u; uint32_t _safe_points_id = 0u; - PositionYawSetpoint _last_position_before_link_loss{(double)NAN, (double)NAN, NAN, NAN}; mission_stats_entry_s _stats; @@ -253,7 +249,6 @@ private: uORB::SubscriptionData _mission_sub{ORB_ID(mission)}; uORB::SubscriptionData _home_pos_sub{ORB_ID(home_position)}; uORB::SubscriptionData _wind_sub{ORB_ID(wind)}; - uORB::SubscriptionMultiArray _telemetry_status_subs{ORB_ID::telemetry_status}; uORB::Publication _rtl_time_estimate_pub{ORB_ID(rtl_time_estimate)}; uORB::Publication _rtl_status_pub{ORB_ID(rtl_status)};