From d4f703bde8b644689dd4a77870975dc419c3f2b3 Mon Sep 17 00:00:00 2001 From: acfloria Date: Tue, 13 Mar 2018 11:16:26 +0100 Subject: [PATCH] IridiumSBD: Improve TX buffer handling - Allow using the full buffer size (340 bytes) - Add tx mutex for sbd session to avoid dropping message which are written during an sbd session --- .../telemetry/iridiumsbd/IridiumSBD.cpp | 30 ++++++++++++------- src/drivers/telemetry/iridiumsbd/IridiumSBD.h | 2 +- 2 files changed, 20 insertions(+), 12 deletions(-) diff --git a/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp b/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp index 41c1bc4e1d..665d1ad09b 100644 --- a/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp +++ b/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp @@ -170,7 +170,7 @@ int IridiumSBD::ioctl(struct file *filp, int cmd, unsigned long arg) } case FIONSPACE: { - int count = SATCOM_TX_BUF_LEN - tx_buf_write_idx; + int count = SATCOM_TX_BUF_LEN - tx_buf_write_idx + SATCOM_MAX_MESSAGE_LENGTH; *(int *)arg = count; return OK; @@ -408,6 +408,8 @@ void IridiumSBD::sbdsession_loop(void) PX4_WARN("SBD SESSION: TIMEOUT!"); new_state = SATCOM_STATE_STANDBY; + pthread_mutex_unlock(&tx_buf_mutex); + } return; @@ -417,6 +419,7 @@ void IridiumSBD::sbdsession_loop(void) VERBOSE_INFO("SBD SESSION: ERROR. RESULT: %d", res); new_state = SATCOM_STATE_STANDBY; + pthread_mutex_unlock(&tx_buf_mutex); return; } @@ -425,6 +428,7 @@ void IridiumSBD::sbdsession_loop(void) VERBOSE_INFO("SBD SESSION: WRONG ANSWER: %s", rx_command_buf); new_state = SATCOM_STATE_STANDBY; + pthread_mutex_unlock(&tx_buf_mutex); return; } @@ -494,6 +498,7 @@ void IridiumSBD::sbdsession_loop(void) } new_state = SATCOM_STATE_STANDBY; + pthread_mutex_unlock(&tx_buf_mutex); } void IridiumSBD::test_loop(void) @@ -545,6 +550,7 @@ void IridiumSBD::start_sbd_session(void) } new_state = SATCOM_STATE_SBDSESSION; + pthread_mutex_lock(&tx_buf_mutex); session_start_time = hrt_absolute_time(); } @@ -586,30 +592,31 @@ void IridiumSBD::start_test(void) ssize_t IridiumSBD::write(struct file *filp, const char *buffer, size_t buflen) { - // check and reset the remaining buffer space for incoming MavLink messages + // general check if the incoming message would be too large (the buffer should not reset in that case) + if ((ssize_t)buflen > SATCOM_TX_BUF_LEN) { + return PX4_ERROR; + } + + // check if there is enough space for the incoming mavlink message if (buflen == 6) { if (*buffer == MAVLINK_PACKAGE_START) { - if (SATCOM_TX_BUF_LEN - tx_buf_write_idx - SATCOM_MIN_TX_BUF_SPACE - (*(buffer + 1) + 8) < 0) { + if (SATCOM_TX_BUF_LEN - tx_buf_write_idx - (*(buffer + 1) + 8) < 0) { tx_buf_write_idx = 0; mavlink_log_critical(&_mavlink_log_pub, "Deleting full TX buffer before writing new message"); } } } - // check and reset the remaining buffer space for any non mavlink messages - if (SATCOM_TX_BUF_LEN - tx_buf_write_idx - SATCOM_MIN_TX_BUF_SPACE < 0) { + // check if there is enough space for the incoming non mavlink message + if ((ssize_t)buflen > SATCOM_TX_BUF_LEN - tx_buf_write_idx) { tx_buf_write_idx = 0; mavlink_log_critical(&_mavlink_log_pub, "Deleting full TX buffer"); } - VERBOSE_INFO("WRITE: LEN %d, TX WRITTEN: %d", buflen, tx_buf_write_idx); - - if ((ssize_t)buflen > SATCOM_TX_BUF_LEN - tx_buf_write_idx) { - return PX4_ERROR; - } - pthread_mutex_lock(&tx_buf_mutex); + VERBOSE_INFO("WRITE: LEN %d, TX WRITTEN: %d", buflen, tx_buf_write_idx); + memcpy(tx_buf + tx_buf_write_idx, buffer, buflen); tx_buf_write_idx += buflen; @@ -658,6 +665,7 @@ void IridiumSBD::write_tx_buf() if (read_at_command() != SATCOM_RESULT_READY) { VERBOSE_INFO("WRITE SBD: MODEM NOT RESPONDING!"); + pthread_mutex_unlock(&tx_buf_mutex); return; } diff --git a/src/drivers/telemetry/iridiumsbd/IridiumSBD.h b/src/drivers/telemetry/iridiumsbd/IridiumSBD.h index 92f8afa40d..2d7acc96f5 100644 --- a/src/drivers/telemetry/iridiumsbd/IridiumSBD.h +++ b/src/drivers/telemetry/iridiumsbd/IridiumSBD.h @@ -84,7 +84,7 @@ typedef enum { extern "C" __EXPORT int iridiumsbd_main(int argc, char *argv[]); #define SATCOM_TX_BUF_LEN 340 // TX buffer size - maximum for a SBD MO message is 340, but billed per 50 -#define SATCOM_MIN_TX_BUF_SPACE 50 // Minimum remaining buffer space required, should be equal to the largerst message intended to send +#define SATCOM_MAX_MESSAGE_LENGTH 50 // Maximum length of the expected messages sent over this link #define SATCOM_RX_MSG_BUF_LEN 270 // RX buffer size for MT messages #define SATCOM_RX_COMMAND_BUF_LEN 50 // RX buffer size for other commands #define SATCOM_SIGNAL_REFRESH_DELAY 20000000 // update signal quality every 20s