fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-29 21:00:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander: improve handling high latency link lost/regain

Browse Source
This commit is contained in:
Igor Mišić
2023-02-15 09:45:13 +01:00
committed by Beat Küng
parent d3b853a7f9
commit de23c10b68
2 changed files with 33 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+32 -21
View File
@@ -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()
src/modules/commander/Commander.hpp
+1
View File
@@ -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};