dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mavlink: support receiving multi-chunk statustexts 

This adds support for mavlink statustext messages arriving in multiple
chunks.
This commit is contained in:
Julian Oes 2021-04-12 13:29:16 +02:00 committed by Daniel Agar
parent 0029a75ab0
commit 2e0286e6bb
6 changed files with 411 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/modules/mavlink/CMakeLists.txt
View File

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

121
src/modules/mavlink/MavlinkStatustextHandler.cpp Normal file
View File

@ -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 <mathlib/mathlib.h>
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;
}
}

52
src/modules/mavlink/MavlinkStatustextHandler.hpp Normal file
View File

@ -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 <uORB/topics/log_message.h>
#include <stdint.h>
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
};

222
src/modules/mavlink/MavlinkStatustextHandlerTest.cpp Normal file
View File

@ -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 <gtest/gtest.h>
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');
}

15
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -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());
}
}
}

2
src/modules/mavlink/mavlink_receiver.h
View File

@ -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()