diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index fc26a052ff..03a16c9951 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2672,6 +2672,28 @@ void Commander::enable_hil() void Commander::dataLinkCheck() { + // high latency data link + iridiumsbd_status_s iridium_status; + + if (_iridiumsbd_status_sub.update(&iridium_status)) { + _high_latency_datalink_heartbeat = iridium_status.last_heartbeat; + + if (_vehicle_status.high_latency_data_link_lost && + (_high_latency_datalink_heartbeat > _high_latency_datalink_lost) && + (_high_latency_datalink_regained == 0) + ) { + _high_latency_datalink_regained = _high_latency_datalink_heartbeat; + } + + if (_vehicle_status.high_latency_data_link_lost && + (_high_latency_datalink_regained != 0) && + (hrt_elapsed_time(&_high_latency_datalink_regained) > (_param_com_hldl_reg_t.get() * 1_s)) + ) { + _vehicle_status.high_latency_data_link_lost = false; + _status_changed = true; + } + } + for (auto &telemetry_status : _telemetry_status_subs) { telemetry_status_s telemetry; @@ -2685,16 +2707,18 @@ void Commander::dataLinkCheck() break; case telemetry_status_s::LINK_TYPE_IRIDIUM: { - iridiumsbd_status_s iridium_status; - if (_iridiumsbd_status_sub.update(&iridium_status)) { - _high_latency_datalink_heartbeat = iridium_status.last_heartbeat; + if ((_high_latency_datalink_heartbeat > 0) && + (hrt_elapsed_time(&_high_latency_datalink_heartbeat) > (_param_com_hldl_loss_t.get() * 1_s))) { - if (_vehicle_status.high_latency_data_link_lost) { - if (hrt_elapsed_time(&_high_latency_datalink_lost) > (_param_com_hldl_reg_t.get() * 1_s)) { - _vehicle_status.high_latency_data_link_lost = false; - _status_changed = true; - } + _high_latency_datalink_lost = _high_latency_datalink_heartbeat; + _high_latency_datalink_regained = 0; + + if (!_vehicle_status.high_latency_data_link_lost) { + _vehicle_status.high_latency_data_link_lost = true; + mavlink_log_critical(&_mavlink_log_pub, "High latency data link lost\t"); + events::send(events::ID("commander_high_latency_lost"), events::Log::Critical, "High latency data link lost"); + _status_changed = true; } } @@ -2836,19 +2860,6 @@ void Commander::dataLinkCheck() _vehicle_status.avoidance_system_valid = false; } } - - // high latency data link loss failsafe - if (_high_latency_datalink_heartbeat > 0 - && hrt_elapsed_time(&_high_latency_datalink_heartbeat) > (_param_com_hldl_loss_t.get() * 1_s)) { - _high_latency_datalink_lost = hrt_absolute_time(); - - if (!_vehicle_status.high_latency_data_link_lost) { - _vehicle_status.high_latency_data_link_lost = true; - mavlink_log_critical(&_mavlink_log_pub, "High latency data link lost\t"); - events::send(events::ID("commander_high_latency_lost"), events::Log::Critical, "High latency data link lost"); - _status_changed = true; - } - } } void Commander::battery_status_check() diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index d477585c03..e98132227d 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -248,6 +248,7 @@ private: hrt_abstime _high_latency_datalink_heartbeat{0}; hrt_abstime _high_latency_datalink_lost{0}; + hrt_abstime _high_latency_datalink_regained{0}; hrt_abstime _boot_timestamp{0}; hrt_abstime _last_disarmed_timestamp{0};