diff --git a/msg/IridiumsbdStatus.msg b/msg/IridiumsbdStatus.msg index 39e3de780a..14ea1d30a5 100644 --- a/msg/IridiumsbdStatus.msg +++ b/msg/IridiumsbdStatus.msg @@ -1,5 +1,5 @@ uint64 timestamp # time since system start (microseconds) -uint64 last_heartbeat # timestamp of the last successful sbd session +uint64 last_successful_at_cmd # timestamp of the last successful AT command uint16 tx_buf_write_index # current size of the tx buffer uint16 rx_buf_read_index # the rx buffer is parsed up to that index uint16 rx_buf_end_index # current size of the rx buffer diff --git a/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp b/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp index 5fddd4ac46..1ee9d2c627 100644 --- a/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp +++ b/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp @@ -235,7 +235,7 @@ int IridiumSBD::print_status() PX4_INFO("RX session pending: %d", _rx_session_pending); PX4_INFO("RX read pending: %d", _rx_read_pending); PX4_INFO("Time since last signal check: %" PRId64, hrt_absolute_time() - _last_signal_check); - PX4_INFO("Last heartbeat: %" PRId64, _last_heartbeat); + PX4_INFO("Last heartbeat: %" PRId64, _last_at_ok_timestamp); return 0; } @@ -333,6 +333,11 @@ void IridiumSBD::standby_loop(void) } } + if (!is_modem_responsive()) { + VERBOSE_INFO("MODEM IS NOT RENSPONSIVE"); + return; + } + // check for incoming SBDRING, handled inside read_at_command() read_at_command(); @@ -477,7 +482,6 @@ void IridiumSBD::sbdsession_loop(void) _ring_pending = false; _tx_session_pending = false; _last_read_time = hrt_absolute_time(); - _last_heartbeat = _last_read_time; ++_successful_sbd_sessions; if (mt_queued > 0) { @@ -498,8 +502,6 @@ void IridiumSBD::sbdsession_loop(void) case 1: VERBOSE_INFO("SBD SESSION: MO SUCCESS, MT FAIL"); - _last_heartbeat = hrt_absolute_time(); - // after a successful session reset the tx buffer _tx_buf_write_idx = 0; ++_successful_sbd_sessions; @@ -552,11 +554,6 @@ void IridiumSBD::start_csq(void) _last_signal_check = hrt_absolute_time(); - if (!is_modem_ready()) { - VERBOSE_INFO("UPDATE SIGNAL QUALITY: MODEM NOT READY!"); - return; - } - write_at("AT+CSQ"); _new_state = SATCOM_STATE_CSQ; } @@ -571,11 +568,6 @@ void IridiumSBD::start_sbd_session(void) VERBOSE_INFO("STARTING SBD SESSION"); } - if (!is_modem_ready()) { - VERBOSE_INFO("SBD SESSION: MODEM NOT READY!"); - return; - } - if (_ring_pending) { write_at("AT+SBDIXA"); @@ -610,8 +602,8 @@ void IridiumSBD::start_test(void) printf("\n"); } - if (!is_modem_ready()) { - PX4_WARN("MODEM NOT READY!"); + if (!is_modem_responsive()) { + PX4_WARN("MODEM NOT RENSPONSIVE!"); return; } @@ -718,11 +710,6 @@ ssize_t IridiumSBD::read(struct file *filp, char *buffer, size_t buflen) void IridiumSBD::write_tx_buf() { - if (!is_modem_ready()) { - VERBOSE_INFO("WRITE SBD: MODEM NOT READY!"); - return; - } - pthread_mutex_lock(&_tx_buf_mutex); char command[13]; @@ -779,11 +766,6 @@ void IridiumSBD::write_tx_buf() void IridiumSBD::read_rx_buf(void) { - if (!is_modem_ready()) { - VERBOSE_INFO("READ SBD: MODEM NOT READY!"); - return; - } - pthread_mutex_lock(&_rx_buf_mutex); @@ -949,11 +931,12 @@ satcom_uart_status IridiumSBD::open_uart(char *uart_name) return SATCOM_UART_OK; } -bool IridiumSBD::is_modem_ready(void) +bool IridiumSBD::is_modem_responsive(void) { write_at("AT"); if (read_at_command() == SATCOM_RESULT_OK) { + _last_at_ok_timestamp = hrt_absolute_time(); return true; } else { @@ -980,9 +963,9 @@ void IridiumSBD::publish_iridium_status() { bool need_to_publish = false; - if (_status.last_heartbeat != _last_heartbeat) { + if (_status.last_at_ok_timestamp != _last_at_ok_timestamp) { need_to_publish = true; - _status.last_heartbeat = _last_heartbeat; + _status.last_at_ok_timestamp = _last_at_ok_timestamp; } if (_status.tx_buf_write_index != _tx_buf_write_idx) { diff --git a/src/drivers/telemetry/iridiumsbd/IridiumSBD.h b/src/drivers/telemetry/iridiumsbd/IridiumSBD.h index 3d628440e6..897201cf71 100644 --- a/src/drivers/telemetry/iridiumsbd/IridiumSBD.h +++ b/src/drivers/telemetry/iridiumsbd/IridiumSBD.h @@ -245,7 +245,7 @@ private: /* * Checks if the modem responds to the "AT" command */ - bool is_modem_ready(void); + bool is_modem_responsive(void); /* * Get the poll state @@ -321,7 +321,7 @@ private: hrt_abstime _last_write_time = 0; hrt_abstime _last_read_time = 0; - hrt_abstime _last_heartbeat = 0; + hrt_abstime _last_at_ok_timestamp = 0; hrt_abstime _session_start_time = 0; satcom_state _state = SATCOM_STATE_STANDBY; diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 03a16c9951..f7ac58e961 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -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) { diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index e98132227d..3108d3386a 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -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};