IridiumSBD: Clean up MavLink message parsing

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

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