From 2e0286e6bb597a9cb2ee5b22a54b0c3fa9147600 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 12 Apr 2021 13:29:16 +0200 Subject: [PATCH] mavlink: support receiving multi-chunk statustexts This adds support for mavlink statustext messages arriving in multiple chunks. --- src/modules/mavlink/CMakeLists.txt | 8 + .../mavlink/MavlinkStatustextHandler.cpp | 121 ++++++++++ .../mavlink/MavlinkStatustextHandler.hpp | 52 ++++ .../mavlink/MavlinkStatustextHandlerTest.cpp | 222 ++++++++++++++++++ src/modules/mavlink/mavlink_receiver.cpp | 15 +- src/modules/mavlink/mavlink_receiver.h | 2 + 6 files changed, 411 insertions(+), 9 deletions(-) create mode 100644 src/modules/mavlink/MavlinkStatustextHandler.cpp create mode 100644 src/modules/mavlink/MavlinkStatustextHandler.hpp create mode 100644 src/modules/mavlink/MavlinkStatustextHandlerTest.cpp diff --git a/src/modules/mavlink/CMakeLists.txt b/src/modules/mavlink/CMakeLists.txt index 947abd39ce..34e3640017 100644 --- a/src/modules/mavlink/CMakeLists.txt +++ b/src/modules/mavlink/CMakeLists.txt @@ -93,6 +93,7 @@ px4_add_module( mavlink_stream.cpp mavlink_timesync.cpp mavlink_ulog.cpp + MavlinkStatustextHandler.cpp tune_publisher.cpp MODULE_CONFIG module.yaml @@ -115,3 +116,10 @@ px4_add_module( if(PX4_TESTING) add_subdirectory(mavlink_tests) endif() + +px4_add_unit_gtest(SRC MavlinkStatustextHandlerTest.cpp + INCLUDES ${MAVLINK_LIBRARY_DIR}/${MAVLINK_DIALECT} + COMPILE_FLAGS + -Wno-address-of-packed-member # TODO: fix in c_library_v2 + LINKLIBS modules__mavlink + ) diff --git a/src/modules/mavlink/MavlinkStatustextHandler.cpp b/src/modules/mavlink/MavlinkStatustextHandler.cpp new file mode 100644 index 0000000000..b69df9a881 --- /dev/null +++ b/src/modules/mavlink/MavlinkStatustextHandler.cpp @@ -0,0 +1,121 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "MavlinkStatustextHandler.hpp" +#include + +bool MavlinkStatustextHandler::should_publish_previous(const mavlink_statustext_t &msg_statustext) +{ + // Check if the previous message has not been published yet. This can + // happen if the last chunk has been dropped, let's publish what we have. + if (_last_log_id > 0 && msg_statustext.id != _last_log_id) { + // We add a note that a part is missing at the end. + const size_t offset = strlen(_log_msg.text); + strncpy(_log_msg.text + offset, "[ missing ... ]", + sizeof(_log_msg.text) - offset); + _log_msg.text[sizeof(_log_msg.text) - 1] = '\0'; + + return true; + } + + return false; +} + +bool MavlinkStatustextHandler::should_publish_current(const mavlink_statustext_t &msg_statustext, const uint64_t &now) +{ + if (msg_statustext.id > 0) { + // Multi statustext message. + if (_last_log_id != msg_statustext.id) { + // On the first one arriving, we save the timestamp and severity. + _log_msg.timestamp = now; + _log_msg.severity = msg_statustext.severity; + } + + if (msg_statustext.chunk_seq == 0) { + // We start from 0 with a new message. + strncpy(_log_msg.text, msg_statustext.text, + math::min(sizeof(_log_msg.text), sizeof(msg_statustext.text))); + _log_msg.text[sizeof(msg_statustext.text)] = '\0'; + + } else { + if (msg_statustext.chunk_seq != _last_log_chunk_seq + 1) { + const size_t offset = strlen(_log_msg.text); + strncpy(_log_msg.text + offset, "[ missing ... ]", + sizeof(_log_msg.text) - offset); + _log_msg.text[sizeof(_log_msg.text) - offset - 1] = '\0'; + } + + // We add a consecutive chunk. + const size_t offset = strlen(_log_msg.text); + const size_t max_to_add = math::min(sizeof(_log_msg.text) - offset - 1, sizeof(msg_statustext.text)); + strncpy(_log_msg.text + offset, msg_statustext.text, max_to_add); + _log_msg.text[math::min(offset + max_to_add, sizeof(_log_msg.text) - 1)] = '\0'; + } + + _last_log_chunk_seq = msg_statustext.chunk_seq; + + const bool publication_message_is_full = sizeof(_log_msg.text) - 1 == strlen(_log_msg.text); + const bool found_zero_termination = strnlen(msg_statustext.text, + sizeof(msg_statustext.text)) < sizeof(msg_statustext.text); + + if (publication_message_is_full || found_zero_termination) { + _last_log_id = 0; + _last_log_chunk_seq = -1; + return true; + + } else { + _last_log_id = msg_statustext.id; + return false; + } + + } else { + // Single statustext message. + _log_msg.timestamp = now; + _log_msg.severity = msg_statustext.severity; + + static_assert(sizeof(_log_msg.text) > sizeof(msg_statustext.text), + "_log_msg.text not big enough to hold msg_statustext.text"); + + strncpy(_log_msg.text, msg_statustext.text, + math::min(sizeof(_log_msg.text), + sizeof(msg_statustext.text))); + + // We need to 0-terminate after the copied text which does not have to be + // 0-terminated on the wire. + _log_msg.text[sizeof(msg_statustext.text) - 1] = '\0'; + + _last_log_id = 0; + return true; + + } +} diff --git a/src/modules/mavlink/MavlinkStatustextHandler.hpp b/src/modules/mavlink/MavlinkStatustextHandler.hpp new file mode 100644 index 0000000000..d9e375ec8d --- /dev/null +++ b/src/modules/mavlink/MavlinkStatustextHandler.hpp @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "mavlink_bridge_header.h" +#include +#include + +class MavlinkStatustextHandler +{ +public: + bool should_publish_previous(const mavlink_statustext_t &msg_statustext); + bool should_publish_current(const mavlink_statustext_t &msg_statustext, const uint64_t &now); + + const log_message_s &log_message() const { return _log_msg; }; + +private: + log_message_s _log_msg{}; // required global for multi chunk statustexts + int _last_log_chunk_seq{-1}; + uint16_t _last_log_id{0}; // non-zero means message has not been published +}; diff --git a/src/modules/mavlink/MavlinkStatustextHandlerTest.cpp b/src/modules/mavlink/MavlinkStatustextHandlerTest.cpp new file mode 100644 index 0000000000..3f7d6a74e7 --- /dev/null +++ b/src/modules/mavlink/MavlinkStatustextHandlerTest.cpp @@ -0,0 +1,222 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "MavlinkStatustextHandler.hpp" +#include + +static constexpr uint64_t some_time = 12345678; +static constexpr uint64_t some_other_time = 99999999; +static constexpr auto some_text = "Some short text"; +static constexpr auto some_other_text = "Some more short text"; +static constexpr uint8_t some_severity = MAV_SEVERITY_CRITICAL; +static constexpr uint8_t some_other_severity = MAV_SEVERITY_INFO; + +TEST(MavlinkStatustextHandler, Singles) +{ + MavlinkStatustextHandler handler; + + mavlink_statustext_t statustext1 {}; + statustext1.severity = some_severity; + strncpy(statustext1.text, some_text, sizeof(statustext1.text)); + + EXPECT_FALSE(handler.should_publish_previous(statustext1)); + EXPECT_TRUE(handler.should_publish_current(statustext1, some_time)); + EXPECT_STREQ(handler.log_message().text, some_text); + EXPECT_EQ(handler.log_message().severity, some_severity); + EXPECT_EQ(handler.log_message().timestamp, some_time); + + mavlink_statustext_t statustext2 {}; + statustext2.severity = some_other_severity; + strncpy(statustext2.text, some_other_text, sizeof(statustext2.text)); + + EXPECT_FALSE(handler.should_publish_previous(statustext2)); + EXPECT_TRUE(handler.should_publish_current(statustext2, some_other_time)); + EXPECT_STREQ(handler.log_message().text, some_other_text); + EXPECT_EQ(handler.log_message().severity, some_other_severity); + EXPECT_EQ(handler.log_message().timestamp, some_other_time); +} + +TEST(MavlinkStatustextHandler, Multiple) +{ + MavlinkStatustextHandler handler; + + mavlink_statustext_t statustext1 {}; + mavlink_statustext_t statustext2 {}; + + statustext1.severity = some_severity; + statustext2.severity = some_severity; + + statustext1.id = 1; + statustext2.id = 1; + + statustext1.chunk_seq = 0; + statustext2.chunk_seq = 1; + + memset(statustext1.text, 'a', 50); + memset(statustext2.text, 'b', 25); + + // The first message is just stored, we don't have to publish yet. + EXPECT_FALSE(handler.should_publish_previous(statustext1)); + EXPECT_FALSE(handler.should_publish_current(statustext1, some_time)); + // Now we should be able to publish. + EXPECT_FALSE(handler.should_publish_previous(statustext2)); + EXPECT_TRUE(handler.should_publish_current(statustext2, some_other_time)); + + // Check overall text length. + EXPECT_EQ(strlen(handler.log_message().text), 75); + + // And a few samples. + EXPECT_EQ(handler.log_message().text[30], 'a'); + EXPECT_EQ(handler.log_message().text[60], 'b'); + + EXPECT_EQ(handler.log_message().severity, some_severity); + EXPECT_EQ(handler.log_message().timestamp, some_time); +} + +TEST(MavlinkStatustextHandler, TooMany) +{ + // If we receive too many, we need to cap it. + MavlinkStatustextHandler handler; + + mavlink_statustext_t statustext1 {}; + mavlink_statustext_t statustext2 {}; + mavlink_statustext_t statustext3 {}; + + statustext1.id = 1; + statustext2.id = 1; + statustext3.id = 1; + + statustext1.chunk_seq = 0; + statustext2.chunk_seq = 1; + statustext3.chunk_seq = 2; + + memset(statustext1.text, 'a', 50); + memset(statustext2.text, 'b', 50); + memset(statustext3.text, 'c', 49); + + // The first messages are just stored, we don't have to publish yet. + EXPECT_FALSE(handler.should_publish_previous(statustext1)); + EXPECT_FALSE(handler.should_publish_current(statustext1, some_time)); + EXPECT_FALSE(handler.should_publish_previous(statustext2)); + EXPECT_FALSE(handler.should_publish_current(statustext2, some_time + 1)); + EXPECT_FALSE(handler.should_publish_previous(statustext3)); + EXPECT_TRUE(handler.should_publish_current(statustext3, some_time + 2)); + + EXPECT_EQ(strlen(handler.log_message().text), sizeof(log_message_s::text) - 1); +} + +TEST(MavlinkStatustextHandler, LastMissing) +{ + // Here we fail to send the last multi-chunk but we still want to + // publish the incomplete message. + + MavlinkStatustextHandler handler; + + mavlink_statustext_t statustext1 {}; + statustext1.id = 1; + statustext1.chunk_seq = 0; + + memset(statustext1.text, 'a', 50); + + // The first message is just stored, we don't have to publish yet. + EXPECT_FALSE(handler.should_publish_previous(statustext1)); + EXPECT_FALSE(handler.should_publish_current(statustext1, some_time)); + + // Now we send a single and we expect the previous one to be published. + mavlink_statustext_t statustext2 {}; + memset(statustext2.text, 'b', 10); + statustext2.id = 0; + + // We expect 50 a and then a missing bracket. + EXPECT_TRUE(handler.should_publish_previous(statustext2)); + EXPECT_EQ(strlen(handler.log_message().text), 50 + 15); + EXPECT_EQ(handler.log_message().text[25], 'a'); + + EXPECT_TRUE(handler.should_publish_previous(statustext2)); + EXPECT_TRUE(handler.should_publish_current(statustext2, some_other_time)); + // And we expect the single message to get through as well. + EXPECT_EQ(strlen(handler.log_message().text), 10); + EXPECT_EQ(handler.log_message().text[5], 'b'); +} + +TEST(MavlinkStatustextHandler, FirstMissing) +{ + // Here we fail to send the first multi-chunk but we still want to + // publish the incomplete message. + + MavlinkStatustextHandler handler; + + mavlink_statustext_t statustext2 {}; + statustext2.id = 1; + statustext2.chunk_seq = 1; + + // By chosing 35 we might exactly trigger an overflow if we don't pay attention. + memset(statustext2.text, 'b', 35); + + // The first message is just stored, we don't have to publish yet. + EXPECT_FALSE(handler.should_publish_previous(statustext2)); + EXPECT_TRUE(handler.should_publish_current(statustext2, some_time)); + + // We expect a missing bracket and our 10 b. + EXPECT_EQ(strlen(handler.log_message().text), 15 + 35); + EXPECT_EQ(handler.log_message().text[20], 'b'); +} + +TEST(MavlinkStatustextHandler, MiddleMissing) +{ + // This time one message in the middle goes missing. + MavlinkStatustextHandler handler; + + mavlink_statustext_t statustext1 {}; + mavlink_statustext_t statustext3 {}; + + statustext1.id = 1; + statustext3.id = 1; + + statustext1.chunk_seq = 0; + statustext3.chunk_seq = 2; + + memset(statustext1.text, 'a', 50); + memset(statustext3.text, 'c', 25); + + // The first messages are just stored, we don't have to publish yet. + EXPECT_FALSE(handler.should_publish_previous(statustext1)); + EXPECT_FALSE(handler.should_publish_current(statustext1, some_time)); + EXPECT_FALSE(handler.should_publish_previous(statustext3)); + EXPECT_TRUE(handler.should_publish_current(statustext3, some_time + 2)); + + // The two texts plus the missing bracket. + EXPECT_EQ(strlen(handler.log_message().text), 50 + 25 + 15); + EXPECT_EQ(handler.log_message().text[20], 'a'); + EXPECT_EQ(handler.log_message().text[70], 'c'); +} diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 6852f85036..b8409bc517 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2925,16 +2925,13 @@ void MavlinkReceiver::handle_message_statustext(mavlink_message_t *msg) mavlink_statustext_t statustext; mavlink_msg_statustext_decode(msg, &statustext); - log_message_s log_message{}; + if (_mavlink_statustext_handler.should_publish_previous(statustext)) { + _log_message_pub.publish(_mavlink_statustext_handler.log_message()); + } - log_message.severity = statustext.severity; - log_message.timestamp = hrt_absolute_time(); - - snprintf(log_message.text, sizeof(log_message.text), - "[mavlink: component %" PRIu8 "] %." STRINGIFY(MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN) "s", msg->compid, - statustext.text); - - _log_message_pub.publish(log_message); + if (_mavlink_statustext_handler.should_publish_current(statustext, hrt_absolute_time())) { + _log_message_pub.publish(_mavlink_statustext_handler.log_message()); + } } } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 48f97aba7a..55fb16df96 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -45,6 +45,7 @@ #include "mavlink_log_handler.h" #include "mavlink_mission.h" #include "mavlink_parameters.h" +#include "MavlinkStatustextHandler.hpp" #include "mavlink_timesync.h" #include "tune_publisher.h" @@ -243,6 +244,7 @@ private: MavlinkMissionManager _mission_manager; MavlinkParametersManager _parameters_manager; MavlinkTimesync _mavlink_timesync; + MavlinkStatustextHandler _mavlink_statustext_handler; mavlink_status_t _status{}; ///< receiver status, used for mavlink_parse_char()