#126 Use custom MAVLink serialization for ISBD

CMakeLists.txt

@@ -44,11 +44,13 @@ add_executable(tltest tests/TimeLibTest.cc)
 add_executable(cbtest tests/CircularBufferTest.cc)
 add_executable(tcptest tests/MAVLinkTCPChannelTest.cc)
 add_executable(isbdtest tests/MAVLinkISBDChannelTest.cc)
+add_executable(srtest tests/CustomSerializationTest.cc)
 
 target_link_libraries(aptest mavio)
 target_link_libraries(cbtest mavio)
 target_link_libraries(tcptest mavio)
 target_link_libraries(isbdtest mavio)
+target_link_libraries(srtest mavio)
 
 install(TARGETS radioroom DESTINATION "/usr/sbin")
 

libs/mavio/include/MAVLinkISBD.h

@@ -97,6 +97,7 @@ class MAVLinkISBD {
   bool get_signal_quality(int& quality);
 
  private:
+
   Serial stream;
   IridiumSBD isbd;
 };

libs/mavio/src/MAVLinkISBD.cc

@@ -31,6 +31,10 @@ namespace mavio {
 using std::string;
 using std::vector;
 
+// Custom serialization/deerialization helpers
+uint16_t message_to_send_buffer(uint8_t *buf, const mavlink_message_t *msg);
+bool parse_message(const uint8_t *buf, size_t buf_size, mavlink_message_t *msg);
+
 MAVLinkISBD::MAVLinkISBD() : stream(), isbd(stream) {}
 
 MAVLinkISBD::~MAVLinkISBD() {}
@@ -158,7 +162,7 @@ bool MAVLinkISBD::send_receive_message(const mavlink_message_t& mo_msg,
   uint16_t len = 0;
 
   if (mo_msg.len != 0 && mo_msg.msgid != 0) {
-    len = mavlink_msg_to_send_buffer(buf, &mo_msg);
+    len = message_to_send_buffer(buf, &mo_msg);
   }
 
   received = false;
@@ -179,19 +183,9 @@ bool MAVLinkISBD::send_receive_message(const mavlink_message_t& mo_msg,
   }
 
   if (buf_size > 0) {
-    mavlink_status_t mavlink_status;
-
-    for (size_t i = 0; i < buf_size; i++) {
-      if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &mt_msg,
-                             &mavlink_status)) {
-        received = true;
-
-        MAVLinkLogger::log(LOG_INFO, "SBD >>", mt_msg);
-        break;
-      }
-    }
-
-    if (!received) {
+    if (parse_message(buf, buf_size, &mt_msg)) {
+      MAVLinkLogger::log(LOG_INFO, "SBD >>", mt_msg);
+    } else {
       mavio::log(LOG_WARNING,
                  "Failed to parse MAVLink message received from ISBD.");
     }
@@ -202,4 +196,110 @@ bool MAVLinkISBD::send_receive_message(const mavlink_message_t& mo_msg,
   return true;
 }
 
+/**
+ * Custom serialization of MAVLink messages.
+ * 
+ * With standard mavlink_msg_to_send_buffer() function, HIGH_LATENCY2 takes 54
+ * bytes, which requires 2 RockBLOCK credits. With the custom serialization 
+ * sending HIGH_LATENCY2 takes 50 bytes.
+ * 
+ * For MAVLink 2.0 the serialization does not include compatibility, 
+ * incompatibility flags, and the checksum.
+ */
+uint16_t message_to_send_buffer(uint8_t* buf, const mavlink_message_t* msg) {
+  uint8_t header_len;
+  uint8_t length = msg->len;
+
+  buf[0] = msg->magic;
+  buf[1] = length;
+  buf[2] = msg->seq;
+  buf[3] = msg->sysid;
+  buf[4] = msg->compid;
+
+  if (msg->magic == MAVLINK_STX_MAVLINK1) {
+    header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN;
+    buf[5] = msg->msgid & 0xFF;
+    memcpy(buf + 6, _MAV_PAYLOAD(msg), msg->len);
+    uint8_t* ck = buf + header_len + 1 + (uint16_t)msg->len;
+    ck[0] = (uint8_t)(msg->checksum & 0xFF);
+    ck[1] = (uint8_t)(msg->checksum >> 8);
+    return header_len + 1 + (uint16_t)length + 2;
+  } else {  // MAVLink 2.0
+    length = _mav_trim_payload(_MAV_PAYLOAD(msg), length);
+    header_len = MAVLINK_CORE_HEADER_LEN - 2;
+    buf[5] = msg->msgid & 0xFF;
+    buf[6] = (msg->msgid >> 8) & 0xFF;
+    buf[7] = (msg->msgid >> 16) & 0xFF;
+    memcpy(buf + 8, _MAV_PAYLOAD(msg), length);
+    return header_len + 1 + (uint16_t)length;
+  }
+}
+
+/* 
+ * Custom deserialization of MAVLink messages.
+ */
+bool parse_message(const uint8_t* buf, size_t buf_size,
+                   mavlink_message_t* msg) {
+  if (buf == NULL || buf_size < 8 || msg == NULL)
+    return false;
+
+  const mavlink_msg_entry_t *msg_entry;
+
+  msg->magic = buf[0];
+  msg->compat_flags = 0;
+  msg->incompat_flags = 0;
+  msg->len = buf[1];
+  msg->seq = buf[2];
+  msg->sysid = buf[3];
+  msg->compid = buf[4];
+
+  switch (msg->magic) {
+    case MAVLINK_STX_MAVLINK1:
+
+      msg->msgid = buf[5];
+      memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf + 6, msg->len);
+
+      // Compute checksum
+      crc_init(&(msg->checksum));
+      crc_accumulate_buffer(&(msg->checksum), (const char *)(buf + 1),
+                            msg->len + 5);
+
+      msg_entry = mavlink_get_msg_entry(msg->msgid);
+
+      if (msg_entry == NULL) {
+        return false;
+      }
+
+      crc_accumulate(msg_entry->crc_extra, &(msg->checksum));
+
+      msg->ck[0] = (uint8_t)(msg->checksum & 0xFF);
+      msg->ck[1] = (uint8_t)(msg->checksum >> 8);
+      break;
+    case MAVLINK_STX:
+      msg->msgid = buf[5] + (buf[6] << 8) + (buf[7] << 16);
+      memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf + 8, msg->len);
+
+      // Compute checksum
+      crc_init(&(msg->checksum));
+      crc_accumulate(buf[1], &(msg->checksum));
+      crc_accumulate(0, &(msg->checksum));  // Compat flags
+      crc_accumulate(0, &(msg->checksum));  // Incompat flags
+      crc_accumulate_buffer(&(msg->checksum), (const char *)(buf + 2),
+                            buf_size - 2);
+
+      msg_entry = mavlink_get_msg_entry(msg->msgid);
+
+      if (msg_entry == NULL) {
+        return false;
+      }
+
+      crc_accumulate(msg_entry->crc_extra, &(msg->checksum));
+      break;
+    default:
+      return false;
+  }
+
+  return true;
+}
+
 }  // namespace mavio

tests/CustomSerializationTest.cc

@@ -0,0 +1,110 @@
+/*
+ CustomSerializationTest.h
+
+ MAVIO MAVLink I/O library
+
+ (C) Copyright 2021 Envirover.
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of the GNU Lesser General Public
+ License as published by the Free Software Foundation; either
+ version 2.1 of the License, or (at your option) any later version.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with this library; if not, write to the Free Software
+ Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+*/
+#include "MAVLinkLib.h"
+
+#include <cassert>
+#include <iostream>
+
+using std::cout;
+using std::endl;
+
+namespace mavio {
+// Custom serialization/deerialization helpers
+uint16_t message_to_send_buffer(uint8_t *buf, const mavlink_message_t *msg);
+bool parse_message(const uint8_t *buf, size_t buf_size, mavlink_message_t *msg);
+}  // namespace mavio
+
+int main() {
+  cout << "Custom serialization test started." << endl;
+
+  // Test MAVLink 2.0 serialization using HIGH_LATENCY2 message
+  mavlink_high_latency2_t high_latency2;
+  memset(&high_latency2, 0, sizeof(high_latency2));
+  high_latency2.timestamp = 1;
+  high_latency2.airspeed = 1;
+  high_latency2.custom2 = 1;
+
+  uint8_t buf[MAVLINK_MAX_PACKET_LEN];
+  mavlink_message_t msg;
+  memset(&msg, 0, sizeof(mavlink_message_t));
+  mavlink_msg_high_latency2_encode(1, 0, &msg, &high_latency2);
+
+  uint8_t len = mavlink_msg_to_send_buffer(buf, &msg);
+
+  cout << "Original message length: " << (long)len << endl;
+
+  len = mavio::message_to_send_buffer(buf, &msg);
+
+  cout << "Custom message length: " << (long)len << endl;
+
+  mavlink_message_t parsed_msg;
+  memset(&parsed_msg, 0, sizeof(mavlink_message_t));
+  if (mavio::parse_message(buf, len, &parsed_msg)) {
+    assert(msg.checksum == parsed_msg.checksum);
+    assert(memcmp(&msg.payload64, &parsed_msg.payload64, msg.len) == 0);
+
+    mavlink_high_latency2_t high_latency2_parsed;
+    memset(&high_latency2_parsed, 0, sizeof(high_latency2_parsed));
+    mavlink_msg_high_latency2_decode(&parsed_msg, &high_latency2_parsed);
+    assert(memcmp(&high_latency2_parsed, &high_latency2,
+                  sizeof(high_latency2)) == 0);
+  } else {
+    cout << "Failed to parse MAVLink message." << endl;
+    return 1;
+  }
+
+  // Test MAVLink 1.0 Serialization using HEARTBEAT message.
+  mavlink_heartbeat_t heartbeat;
+  heartbeat.custom_mode = 1;
+  heartbeat.type = 0;
+  heartbeat.autopilot = 0;
+  heartbeat.base_mode = 0;
+  heartbeat.system_status = 0;
+  heartbeat.mavlink_version = 1;
+
+  mavlink_message_t tmp_msg;
+  mavlink_msg_heartbeat_encode(1, 0, &tmp_msg, &heartbeat);
+
+  memset(&msg, 0, sizeof(mavlink_message_t));
+  msg.checksum = 52057;
+  msg.magic = MAVLINK_STX_MAVLINK1;
+  msg.len = MAVLINK_MSG_ID_HEARTBEAT_LEN;
+  msg.sysid = 1;
+  msg.compid = 0;
+  msg.msgid = MAVLINK_MSG_ID_HEARTBEAT;
+  memcpy(_MAV_PAYLOAD_NON_CONST(&msg), _MAV_PAYLOAD(&tmp_msg), msg.len);
+
+
+  len = mavio::message_to_send_buffer(buf, &msg);
+  memset(&parsed_msg, 0, sizeof(mavlink_message_t));
+
+  if (mavio::parse_message(buf, len, &parsed_msg)) {
+    assert(msg.checksum == parsed_msg.checksum);
+    assert(memcmp(&msg.payload64, &parsed_msg.payload64, msg.len) == 0);
+  } else {
+    cout << "Failed to parse MAVLink message." << endl;
+    return 1;
+  }
+
+  cout << "Custom serialization test succeeded." << endl;
+  return 0;
+}