From 975d7ddcb2b8e0959fb042882050f7f3d6a64b03 Mon Sep 17 00:00:00 2001 From: acfloria Date: Fri, 6 Apr 2018 17:00:59 +0200 Subject: [PATCH] IridiumSBD: Clean up MavLink message parsing --- .../telemetry/iridiumsbd/IridiumSBD.cpp | 38 +++++++++++++++---- src/drivers/telemetry/iridiumsbd/IridiumSBD.h | 9 +++-- 2 files changed, 35 insertions(+), 12 deletions(-) diff --git a/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp b/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp index f07b03c958..88a37e1a94 100644 --- a/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp +++ b/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp @@ -627,22 +627,44 @@ ssize_t IridiumSBD::write(struct file *filp, const char *buffer, size_t buflen) pthread_mutex_lock(&_tx_buf_mutex); - // 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 - (*(buffer + 1) + 8) < 0) { - _tx_buf_write_idx = 0; - publish_telemetry_status(); + // parsing the size of the message to write + if (!_writing_mavlink_packet) { + if (buflen < 3) { + _packet_length = buflen; + + } else if ((unsigned char)buffer[0] == 253 && (buflen == 10)) { // mavlink 2 + const uint8_t payload_len = buffer[1]; + const uint8_t incompat_flags = buffer[2]; + _packet_length = payload_len + 12; + _writing_mavlink_packet = true; + + if (incompat_flags & 0x1) { //signing + _packet_length += 13; } + + } else if ((unsigned char)buffer[0] == 254 && (buflen == 6)) { // mavlink 1 + const uint8_t payload_len = buffer[1]; + _packet_length = payload_len + 8; + _writing_mavlink_packet = true; + + } else { + _packet_length = buflen; } } - // check if there is enough space for the incoming non mavlink message - if ((ssize_t)buflen > SATCOM_TX_BUF_LEN - _tx_buf_write_idx) { + // check if there is enough space to write the message + if (SATCOM_TX_BUF_LEN - _tx_buf_write_idx - _packet_length < 0) { _tx_buf_write_idx = 0; publish_telemetry_status(); } + // keep track of the remaining packet length and if the full message is written + _packet_length -= buflen; + + if (_packet_length == 0) { + _writing_mavlink_packet = false; + } + VERBOSE_INFO("WRITE: LEN %d, TX WRITTEN: %d", buflen, _tx_buf_write_idx); memcpy(_tx_buf + _tx_buf_write_idx, buffer, buflen); diff --git a/src/drivers/telemetry/iridiumsbd/IridiumSBD.h b/src/drivers/telemetry/iridiumsbd/IridiumSBD.h index a7eca71547..6d9ee82f42 100644 --- a/src/drivers/telemetry/iridiumsbd/IridiumSBD.h +++ b/src/drivers/telemetry/iridiumsbd/IridiumSBD.h @@ -91,12 +91,12 @@ extern "C" __EXPORT int iridiumsbd_main(int argc, char *argv[]); #define MAVLINK_PACKAGE_START 254 // The value of the first byte of the mavlink header /** - * The driver for the Rockblock 9602 and 9603 RockBlock module for satellite communication over the Iridium satellite system + * The driver for the Rockblock 9602 and 9603 RockBlock module for satellite communication over the Iridium satellite system. + * The MavLink 1 protocol should be used to ensure that the status message is 50 bytes (RockBlock bills every 50 bytes per transmission). * * TODO: * - Improve TX buffer handling: * - Do not reset the full TX buffer but delete the oldest HIGH_LATENCY2 message if one is in the buffer or delete the oldest message in general - * - Keep CDev active even if the driver is stopped to avoid a hard fault caused by MavLink. */ class IridiumSBD : public device::CDev { @@ -299,6 +299,9 @@ private: uint8_t _signal_quality = 0; uint16_t _failed_sbd_sessions = 0; + bool _writing_mavlink_packet = false; + uint16_t _packet_length = 0; + orb_advert_t _telemetry_status_pub = nullptr; bool _test_pending = false; @@ -333,6 +336,4 @@ private: pthread_mutex_t _tx_buf_mutex = pthread_mutex_t(); bool _verbose = false; - - orb_advert_t _mavlink_log_pub{nullptr}; };