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

iridiumsbd: update logic for detecting if the modem is not responsive

Browse Source
This commit is contained in:
Igor Mišić
2023-02-15 10:25:24 +01:00
committed by Beat Küng
parent 5be0adc779
commit 4f8de500af
5 changed files with 23 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+7 -7
View File
@@ -1109,7 +1109,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
case vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY: {
// if no high latency telemetry exists send a failed acknowledge
if (_high_latency_datalink_heartbeat < _boot_timestamp) {
if (_high_latency_datalink_timestamp < _boot_timestamp) {
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED;
mavlink_log_critical(&_mavlink_log_pub, "Control high latency failed! Telemetry unavailable\t");
events::send(events::ID("commander_ctrl_high_latency_failed"), {events::Log::Critical, events::LogInternal::Info},
@@ -2676,13 +2676,13 @@ void Commander::dataLinkCheck()
iridiumsbd_status_s iridium_status;
if (_iridiumsbd_status_sub.update(&iridium_status)) {
_high_latency_datalink_heartbeat = iridium_status.last_heartbeat;
_high_latency_datalink_timestamp = iridium_status.last_successful_at_cmd;
if (_vehicle_status.high_latency_data_link_lost &&
(_high_latency_datalink_heartbeat > _high_latency_datalink_lost) &&
(_high_latency_datalink_timestamp > _high_latency_datalink_lost) &&
(_high_latency_datalink_regained == 0)
) {
_high_latency_datalink_regained = _high_latency_datalink_heartbeat;
_high_latency_datalink_regained = _high_latency_datalink_timestamp;
}
if (_vehicle_status.high_latency_data_link_lost &&
@@ -2708,10 +2708,10 @@ void Commander::dataLinkCheck()
case telemetry_status_s::LINK_TYPE_IRIDIUM: {
if ((_high_latency_datalink_heartbeat > 0) &&
(hrt_elapsed_time(&_high_latency_datalink_heartbeat) > (_param_com_hldl_loss_t.get() * 1_s))) {
if ((_high_latency_datalink_timestamp > 0) &&
(hrt_elapsed_time(&_high_latency_datalink_timestamp) > (_param_com_hldl_loss_t.get() * 1_s))) {
_high_latency_datalink_lost = _high_latency_datalink_heartbeat;
_high_latency_datalink_lost = _high_latency_datalink_timestamp;
_high_latency_datalink_regained = 0;
if (!_vehicle_status.high_latency_data_link_lost) {
src/modules/commander/Commander.hpp
+1 -1
View File
@@ -246,7 +246,7 @@ private:
hrt_abstime _last_print_mode_reject_time{0}; ///< To remember when last notification was sent
hrt_abstime _high_latency_datalink_heartbeat{0};
hrt_abstime _high_latency_datalink_timestamp{0};
hrt_abstime _high_latency_datalink_lost{0};
hrt_abstime _high_latency_datalink_regained{0};