diff --git a/CMakeLists.txt b/CMakeLists.txt index cda4da3..e28348e 100644 --- a/CMakeLists.txt +++ b/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") diff --git a/libs/mavio/include/MAVLinkISBD.h b/libs/mavio/include/MAVLinkISBD.h index d36206a..21a7161 100644 --- a/libs/mavio/include/MAVLinkISBD.h +++ b/libs/mavio/include/MAVLinkISBD.h @@ -97,6 +97,7 @@ class MAVLinkISBD { bool get_signal_quality(int& quality); private: + Serial stream; IridiumSBD isbd; }; diff --git a/libs/mavio/src/MAVLinkISBD.cc b/libs/mavio/src/MAVLinkISBD.cc index c9c1612..db35493 100644 --- a/libs/mavio/src/MAVLinkISBD.cc +++ b/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 diff --git a/libs/mavlink/include b/libs/mavlink/include index 19c3a5f..636fae5 160000 --- a/libs/mavlink/include +++ b/libs/mavlink/include @@ -1 +1 @@ -Subproject commit 19c3a5ffff0e8571b6b0e9c3d3c2c9d571c840b2 +Subproject commit 636fae5ca34eff6007344718755d78fcdf94238e diff --git a/tests/CustomSerializationTest.cc b/tests/CustomSerializationTest.cc new file mode 100644 index 0000000..4fe9d78 --- /dev/null +++ b/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 +#include + +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; +}