fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
msg/IridiumsbdStatus.msg
View File

@ -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

41
src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp
View File

@ -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) {

4
src/drivers/telemetry/iridiumsbd/IridiumSBD.h
View File

@ -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;

14
src/modules/commander/Commander.cpp
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) {

2
src/modules/commander/Commander.hpp
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};