From ad7032df27d4769351ae236bc3bf09ef48fece40 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 6 Oct 2023 08:09:58 +1300 Subject: [PATCH 1/3] lib: add FIFO ringbuffer class This adds a reusable class for a simple FIFO ringbuffer. Signed-off-by: Julian Oes --- src/lib/CMakeLists.txt | 3 +- src/lib/ringbuffer/CMakeLists.txt | 40 +++++ src/lib/ringbuffer/Ringbuffer.cpp | 180 +++++++++++++++++++ src/lib/ringbuffer/Ringbuffer.hpp | 120 +++++++++++++ src/lib/ringbuffer/RingbufferTest.cpp | 247 ++++++++++++++++++++++++++ 5 files changed, 589 insertions(+), 1 deletion(-) create mode 100644 src/lib/ringbuffer/CMakeLists.txt create mode 100644 src/lib/ringbuffer/Ringbuffer.cpp create mode 100644 src/lib/ringbuffer/Ringbuffer.hpp create mode 100644 src/lib/ringbuffer/RingbufferTest.cpp diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index f6bcc9260b6a..43694f5e9a77 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2017 PX4 Development Team. All rights reserved. +# Copyright (c) 2017-2023 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 @@ -61,6 +61,7 @@ add_subdirectory(pid EXCLUDE_FROM_ALL) add_subdirectory(pid_design EXCLUDE_FROM_ALL) add_subdirectory(rate_control EXCLUDE_FROM_ALL) add_subdirectory(rc EXCLUDE_FROM_ALL) +add_subdirectory(ringbuffer EXCLUDE_FROM_ALL) add_subdirectory(sensor_calibration EXCLUDE_FROM_ALL) add_subdirectory(slew_rate EXCLUDE_FROM_ALL) add_subdirectory(systemlib EXCLUDE_FROM_ALL) diff --git a/src/lib/ringbuffer/CMakeLists.txt b/src/lib/ringbuffer/CMakeLists.txt new file mode 100644 index 000000000000..78a26de16664 --- /dev/null +++ b/src/lib/ringbuffer/CMakeLists.txt @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2023 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. +# +############################################################################ + +px4_add_library(ringbuffer + Ringbuffer.cpp +) + +target_include_directories(ringbuffer PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +px4_add_unit_gtest(SRC RingbufferTest.cpp LINKLIBS ringbuffer) diff --git a/src/lib/ringbuffer/Ringbuffer.cpp b/src/lib/ringbuffer/Ringbuffer.cpp new file mode 100644 index 000000000000..64fc33d8361d --- /dev/null +++ b/src/lib/ringbuffer/Ringbuffer.cpp @@ -0,0 +1,180 @@ +/**************************************************************************** + * + * Copyright (C) 2023 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 "Ringbuffer.hpp" + +#include +#include +#include + + +Ringbuffer::~Ringbuffer() +{ + deallocate(); +} + +bool Ringbuffer::allocate(size_t buffer_size) +{ + assert(_ringbuffer == nullptr); + + _size = buffer_size; + _ringbuffer = new uint8_t[_size]; + return _ringbuffer != nullptr; +} + +void Ringbuffer::deallocate() +{ + delete[] _ringbuffer; + _ringbuffer = nullptr; + _size = 0; +} + +size_t Ringbuffer::space_available() const +{ + if (_start > _end) { + return _start - _end - 1; + + } else { + return _start - _end - 1 + _size; + } +} + +size_t Ringbuffer::space_used() const +{ + if (_start <= _end) { + return _end - _start; + + } else { + // Potential wrap around. + return _end - _start + _size; + } +} + + +bool Ringbuffer::push_back(const uint8_t *buf, size_t buf_len) +{ + if (buf_len == 0 || buf == nullptr) { + // Nothing to add, we better don't try. + return false; + } + + if (_start > _end) { + // Add after end up to start, no wrap around. + + // Leave one byte free so that start don't end up the same + // which signals empty. + const size_t available = _start - _end - 1; + + if (available < buf_len) { + return false; + } + + memcpy(&_ringbuffer[_end], buf, buf_len); + _end += buf_len; + + } else { + // Add after end, maybe wrap around. + const size_t available = _start - _end - 1 + _size; + + if (available < buf_len) { + return false; + } + + const size_t remaining_packet_len = _size - _end; + + if (buf_len > remaining_packet_len) { + memcpy(&_ringbuffer[_end], buf, remaining_packet_len); + _end = 0; + + memcpy(&_ringbuffer[_end], buf + remaining_packet_len, buf_len - remaining_packet_len); + _end += buf_len - remaining_packet_len; + + } else { + memcpy(&_ringbuffer[_end], buf, buf_len); + _end += buf_len; + } + } + + return true; +} + +size_t Ringbuffer::pop_front(uint8_t *buf, size_t buf_max_len) +{ + if (buf == nullptr) { + // User needs to supply a valid pointer. + return 0; + } + + if (_start == _end) { + // Empty + return 0; + } + + if (_start < _end) { + + // No wrap around. + size_t to_copy_len = math::min(_end - _start, buf_max_len); + + memcpy(buf, &_ringbuffer[_start], to_copy_len); + _start += to_copy_len; + + return to_copy_len; + + } else { + // Potential wrap around. + size_t to_copy_len = _end - _start + _size; + + if (to_copy_len > buf_max_len) { + to_copy_len = buf_max_len; + } + + const size_t remaining_buf_len = _size - _start; + + if (to_copy_len > remaining_buf_len) { + + memcpy(buf, &_ringbuffer[_start], remaining_buf_len); + _start = 0; + memcpy(buf + remaining_buf_len, &_ringbuffer[_start], to_copy_len - remaining_buf_len); + _start += to_copy_len - remaining_buf_len; + + } else { + memcpy(buf, &_ringbuffer[_start], to_copy_len); + _start += to_copy_len; + } + + return to_copy_len; + } +} diff --git a/src/lib/ringbuffer/Ringbuffer.hpp b/src/lib/ringbuffer/Ringbuffer.hpp new file mode 100644 index 000000000000..541f322db947 --- /dev/null +++ b/src/lib/ringbuffer/Ringbuffer.hpp @@ -0,0 +1,120 @@ +/**************************************************************************** + * + * Copyright (C) 2023 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 +#include + + +// FIFO ringbuffer implementation. +// +// The ringbuffer can store up 1 byte less than allocated as +// start and end marker need to be one byte apart when the buffer +// is full, otherwise it would suddenly be empty. +// +// The buffer is not thread-safe. + +class Ringbuffer +{ +public: + /* @brief Constructor + * + * @note Does not allocate automatically. + */ + Ringbuffer() = default; + + /* + * @brief Destructor + * + * Automatically calls deallocate. + */ + ~Ringbuffer(); + + /* @brief Allocate ringbuffer + * + * @param buffer_size Number of bytes to allocate on heap. + * + * @returns false if allocation fails. + */ + bool allocate(size_t buffer_size); + + /* + * @brief Deallocate ringbuffer + * + * @note only required to deallocate and reallocate again. + */ + void deallocate(); + + /* + * @brief Space available to copy bytes into + * + * @returns number of free bytes. + */ + size_t space_available() const; + + /* + * @brief Space used to copy data from + * + * @returns number of used bytes. + */ + size_t space_used() const; + + /* + * @brief Copy data into ringbuffer + * + * @param buf Pointer to buffer to copy from. + * @param buf_len Number of bytes to copy. + * + * @returns true if packet could be copied into buffer. + */ + bool push_back(const uint8_t *buf, size_t buf_len); + + /* + * @brief Get data from ringbuffer + * + * @param buf Pointer to buffer where data can be copied into. + * @param max_buf_len Max number of bytes to copy. + * + * @returns 0 if buffer is empty. + */ + size_t pop_front(uint8_t *buf, size_t max_buf_len); + +private: + size_t _size {0}; + uint8_t *_ringbuffer {nullptr}; + size_t _start{0}; + size_t _end{0}; +}; diff --git a/src/lib/ringbuffer/RingbufferTest.cpp b/src/lib/ringbuffer/RingbufferTest.cpp new file mode 100644 index 000000000000..e5e0a7a57f8a --- /dev/null +++ b/src/lib/ringbuffer/RingbufferTest.cpp @@ -0,0 +1,247 @@ +/**************************************************************************** + * + * Copyright (C) 2023 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 +#include +#include + + +#include "Ringbuffer.hpp" + +class TempData +{ +public: + TempData(size_t len) + { + _size = len; + _buf = new uint8_t[_size]; + } + + ~TempData() + { + delete[] _buf; + _buf = nullptr; + } + + uint8_t *buf() const + { + return _buf; + } + + size_t size() const + { + return _size; + } + + void paint(unsigned offset = 0) + { + for (size_t i = 0; i < _size; ++i) { + _buf[i] = (uint8_t)((i + offset) % UINT8_MAX); + } + } + +private: + uint8_t *_buf {nullptr}; + size_t _size{0}; + +}; + +bool operator==(const TempData &lhs, const TempData &rhs) +{ + if (lhs.size() != rhs.size()) { + return false; + } + + return memcmp(lhs.buf(), rhs.buf(), lhs.size()) == 0; +} + + +TEST(Ringbuffer, AllocateAndDeallocate) +{ + Ringbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + buf.deallocate(); + + ASSERT_TRUE(buf.allocate(1000)); + // The second time we forget to clean up, but we expect no leak. +} + +TEST(Ringbuffer, PushATooBigMessage) +{ + Ringbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + TempData data{200}; + + // A message that doesn't fit should get rejected. + EXPECT_FALSE(buf.push_back(data.buf(), data.size())); +} + +TEST(Ringbuffer, PushAndPopOne) +{ + Ringbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + TempData data{20}; + data.paint(); + + EXPECT_TRUE(buf.push_back(data.buf(), data.size())); + + EXPECT_EQ(buf.space_used(), 20); + EXPECT_EQ(buf.space_available(), 79); + + // Get everything + TempData out{20}; + EXPECT_EQ(buf.pop_front(out.buf(), out.size()), 20); + EXPECT_EQ(data, out); + + // Nothing remaining + EXPECT_EQ(buf.pop_front(out.buf(), out.size()), 0); +} + +TEST(Ringbuffer, PushAndPopSeveral) +{ + Ringbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + TempData data{90}; + data.paint(); + + // 9 little chunks in + for (unsigned i = 0; i < 9; ++i) { + EXPECT_TRUE(buf.push_back(data.buf() + i * 10, 10)); + } + + // 10 won't because of overhead inside the buffer + EXPECT_FALSE(buf.push_back(data.buf(), 10)); + + TempData out{90}; + // Take it back out in 2 big steps + EXPECT_EQ(buf.pop_front(out.buf(), 50), 50); + EXPECT_EQ(buf.pop_front(out.buf() + 50, 40), 40); + EXPECT_EQ(data, out); +} + +TEST(Ringbuffer, PushAndTryToPopMore) +{ + Ringbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + TempData data1{50}; + data1.paint(); + EXPECT_TRUE(buf.push_back(data1.buf(), data1.size())); + + TempData out1{80}; + EXPECT_EQ(buf.pop_front(out1.buf(), out1.size()), data1.size()); +} + +TEST(Ringbuffer, PushAndPopSeveralInterleaved) +{ + Ringbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + TempData data1{50}; + data1.paint(); + EXPECT_TRUE(buf.push_back(data1.buf(), data1.size())); + + TempData data2{30}; + data2.paint(50); + EXPECT_TRUE(buf.push_back(data2.buf(), data2.size())); + + TempData out12{80}; + EXPECT_EQ(buf.pop_front(out12.buf(), out12.size()), out12.size()); + + TempData out12_ref{80}; + out12_ref.paint(); + EXPECT_EQ(out12_ref, out12); + + TempData data3{50}; + data3.paint(33); + EXPECT_TRUE(buf.push_back(data3.buf(), data3.size())); + + TempData out3{50}; + EXPECT_EQ(buf.pop_front(out3.buf(), out3.size()), data3.size()); + EXPECT_EQ(data3, out3); +} + +TEST(Ringbuffer, PushEmpty) +{ + Ringbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + EXPECT_FALSE(buf.push_back(nullptr, 0)); +} + +TEST(Ringbuffer, PopWithoutBuffer) +{ + Ringbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + EXPECT_FALSE(buf.push_back(nullptr, 0)); + + TempData data{50}; + data.paint(); + + EXPECT_TRUE(buf.push_back(data.buf(), data.size())); + + + EXPECT_EQ(buf.pop_front(nullptr, 50), 0); +} + +TEST(Ringbuffer, EmptyAndNoSpaceForHeader) +{ + // Addressing a corner case where start and end are at the end + // and the same. + + Ringbuffer buf; + // Allocate 1 bytes more than the packet, 1 for the start/end logic. + ASSERT_TRUE(buf.allocate(21)); + + { + TempData data{20}; + data.paint(); + EXPECT_TRUE(buf.push_back(data.buf(), data.size())); + TempData out{20}; + EXPECT_EQ(buf.pop_front(out.buf(), out.size()), out.size()); + EXPECT_EQ(data, out); + } + + { + TempData data{10}; + data.paint(); + EXPECT_TRUE(buf.push_back(data.buf(), data.size())); + TempData out{10}; + EXPECT_EQ(buf.pop_front(out.buf(), out.size()), out.size()); + EXPECT_EQ(data, out); + } +} From 318d93e6637d1830ac60a03c1739107dade35625 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 6 Oct 2023 08:10:54 +1300 Subject: [PATCH 2/3] lib: add variable length ringbuffer This adds a reusable class for a FIFO ringbuffer that accepts variable length packets. It is using the Ringbuffer class internally. Signed-off-by: Julian Oes --- src/lib/CMakeLists.txt | 1 + .../variable_length_ringbuffer/CMakeLists.txt | 42 +++ .../VariableLengthRingbuffer.cpp | 106 ++++++++ .../VariableLengthRingbuffer.hpp | 111 ++++++++ .../VariableLengthRingbufferTest.cpp | 257 ++++++++++++++++++ 5 files changed, 517 insertions(+) create mode 100644 src/lib/variable_length_ringbuffer/CMakeLists.txt create mode 100644 src/lib/variable_length_ringbuffer/VariableLengthRingbuffer.cpp create mode 100644 src/lib/variable_length_ringbuffer/VariableLengthRingbuffer.hpp create mode 100644 src/lib/variable_length_ringbuffer/VariableLengthRingbufferTest.cpp diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 43694f5e9a77..27f926acc295 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -71,6 +71,7 @@ add_subdirectory(terrain_estimation EXCLUDE_FROM_ALL) add_subdirectory(timesync EXCLUDE_FROM_ALL) add_subdirectory(tinybson EXCLUDE_FROM_ALL) add_subdirectory(tunes EXCLUDE_FROM_ALL) +add_subdirectory(variable_length_ringbuffer EXCLUDE_FROM_ALL) add_subdirectory(version EXCLUDE_FROM_ALL) add_subdirectory(weather_vane EXCLUDE_FROM_ALL) add_subdirectory(wind_estimator EXCLUDE_FROM_ALL) diff --git a/src/lib/variable_length_ringbuffer/CMakeLists.txt b/src/lib/variable_length_ringbuffer/CMakeLists.txt new file mode 100644 index 000000000000..2d2bf4706731 --- /dev/null +++ b/src/lib/variable_length_ringbuffer/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2023 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. +# +############################################################################ + +px4_add_library(variable_length_ringbuffer + VariableLengthRingbuffer.cpp +) + +target_link_libraries(variable_length_ringbuffer PRIVATE ringbuffer) + +target_include_directories(variable_length_ringbuffer PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +px4_add_unit_gtest(SRC VariableLengthRingbufferTest.cpp LINKLIBS variable_length_ringbuffer) diff --git a/src/lib/variable_length_ringbuffer/VariableLengthRingbuffer.cpp b/src/lib/variable_length_ringbuffer/VariableLengthRingbuffer.cpp new file mode 100644 index 000000000000..9b607e208b07 --- /dev/null +++ b/src/lib/variable_length_ringbuffer/VariableLengthRingbuffer.cpp @@ -0,0 +1,106 @@ +/**************************************************************************** + * + * Copyright (C) 2023 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 "VariableLengthRingbuffer.hpp" + +#include +#include + + +VariableLengthRingbuffer::~VariableLengthRingbuffer() +{ + deallocate(); +} + +bool VariableLengthRingbuffer::allocate(size_t buffer_size) +{ + return _ringbuffer.allocate(buffer_size); +} + +void VariableLengthRingbuffer::deallocate() +{ + _ringbuffer.deallocate(); +} + +bool VariableLengthRingbuffer::push_back(const uint8_t *packet, size_t packet_len) +{ + if (packet_len == 0 || packet == nullptr) { + // Nothing to add, we better don't try. + return false; + } + + size_t space_required = packet_len + sizeof(Header); + + if (space_required > _ringbuffer.space_available()) { + return false; + } + + Header header{static_cast(packet_len)}; + bool result = _ringbuffer.push_back(reinterpret_cast(&header), sizeof(header)); + assert(result); + + result = _ringbuffer.push_back(packet, packet_len); + assert(result); + + // In case asserts are commented out to prevent unused warnings. + (void)result; + + return true; +} + +size_t VariableLengthRingbuffer::pop_front(uint8_t *buf, size_t buf_max_len) +{ + if (buf == nullptr) { + // User needs to supply a valid pointer. + return 0; + } + + // Check next header + Header header; + + if (_ringbuffer.pop_front(reinterpret_cast(&header), sizeof(header)) < sizeof(header)) { + return 0; + } + + // We can't fit the packet into the user supplied buffer. + // This should never happen as the user has to supply a big // enough buffer. + assert(static_cast(header.len) <= buf_max_len); + + size_t bytes_read = _ringbuffer.pop_front(buf, header.len); + assert(bytes_read == header.len); + + return bytes_read; +} diff --git a/src/lib/variable_length_ringbuffer/VariableLengthRingbuffer.hpp b/src/lib/variable_length_ringbuffer/VariableLengthRingbuffer.hpp new file mode 100644 index 000000000000..89f92eb24670 --- /dev/null +++ b/src/lib/variable_length_ringbuffer/VariableLengthRingbuffer.hpp @@ -0,0 +1,111 @@ +/**************************************************************************** + * + * Copyright (C) 2023 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 +#include + + +// FIFO ringbuffer implementation for packets of variable length. +// +// The variable length is implemented using a 4 byte header +// containing a the length. +// +// The buffer is not thread-safe. + +class VariableLengthRingbuffer +{ +public: + /* @brief Constructor + * + * @note Does not allocate automatically. + */ + VariableLengthRingbuffer() = default; + + /* + * @brief Destructor + * + * Automatically calls deallocate. + */ + ~VariableLengthRingbuffer(); + + /* @brief Allocate ringbuffer + * + * @note The variable length requires 4 bytes + * of overhead per packet. + * + * @param buffer_size Number of bytes to allocate on heap. + * + * @returns false if allocation fails. + */ + bool allocate(size_t buffer_size); + + /* + * @brief Deallocate ringbuffer + * + * @note only required to deallocate and reallocate again. + */ + void deallocate(); + + /* + * @brief Copy packet into ringbuffer + * + * @param packet Pointer to packet to copy from. + * @param packet_len Length of packet. + * + * @returns true if packet could be copied into buffer. + */ + bool push_back(const uint8_t *packet, size_t packet_len); + + /* + * @brief Get packet from ringbuffer + * + * @note max_buf_len needs to be bigger equal to any pushed packet. + * + * @param buf Pointer to where next packet can be copied into. + * @param max_buf_len Max size of buf + * + * @returns 0 if packet is bigger than max_len or buffer is empty. + */ + size_t pop_front(uint8_t *buf, size_t max_buf_len); + +private: + struct Header { + uint32_t len; + }; + + Ringbuffer _ringbuffer {}; +}; diff --git a/src/lib/variable_length_ringbuffer/VariableLengthRingbufferTest.cpp b/src/lib/variable_length_ringbuffer/VariableLengthRingbufferTest.cpp new file mode 100644 index 000000000000..958f36947eda --- /dev/null +++ b/src/lib/variable_length_ringbuffer/VariableLengthRingbufferTest.cpp @@ -0,0 +1,257 @@ +/**************************************************************************** + * + * Copyright (C) 2023 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 +#include +#include + + +#include "VariableLengthRingbuffer.hpp" + +class TempData +{ +public: + TempData(size_t len) + { + _size = len; + _buf = new uint8_t[_size]; + } + + ~TempData() + { + delete[] _buf; + _buf = nullptr; + } + + uint8_t *buf() const + { + return _buf; + } + + size_t size() const + { + return _size; + } + + void paint(unsigned offset = 0) + { + for (size_t i = 0; i < _size; ++i) { + _buf[i] = (uint8_t)((i + offset) % UINT8_MAX); + } + } + +private: + uint8_t *_buf {nullptr}; + size_t _size{0}; + +}; + +bool operator==(const TempData &lhs, const TempData &rhs) +{ + if (lhs.size() != rhs.size()) { + return false; + } + + return memcmp(lhs.buf(), rhs.buf(), lhs.size()) == 0; +} + + +TEST(VariableLengthRingbuffer, AllocateAndDeallocate) +{ + VariableLengthRingbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + buf.deallocate(); + + ASSERT_TRUE(buf.allocate(1000)); + // The second time we forget to clean up, but we expect no leak. +} + +TEST(VariableLengthRingbuffer, PushATooBigMessage) +{ + VariableLengthRingbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + TempData data{200}; + + // A message that doesn't fit should get rejected. + EXPECT_FALSE(buf.push_back(data.buf(), data.size())); +} + +TEST(VariableLengthRingbuffer, PushAndPopOne) +{ + VariableLengthRingbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + TempData data{20}; + data.paint(); + + EXPECT_TRUE(buf.push_back(data.buf(), data.size())); + + + // Out buffer is the same size + TempData out{20}; + EXPECT_EQ(buf.pop_front(out.buf(), out.size()), 20); + EXPECT_EQ(data, out); + + EXPECT_TRUE(buf.push_back(data.buf(), data.size())); + // Out buffer is supposedly bigger + TempData out2{20}; + EXPECT_EQ(buf.pop_front(out2.buf(), 21), 20); + EXPECT_EQ(data, out2); + + EXPECT_TRUE(buf.push_back(data.buf(), data.size())); + + // Disabled because it doesn't work reliably. + // For some reason the abort works when tests are filtered using TESTFILTER + // but not when all tests are run. + // + // Out buffer is too small + // Asserts are disabled in release build + //TempData out3{19}; + //EXPECT_DEATH(buf.pop_front(out3.buf(), out3.size()), ".*"); +} + +TEST(VariableLengthRingbuffer, PushAndPopSeveral) +{ + VariableLengthRingbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + TempData data{20}; + data.paint(); + + // 4 should fit + for (unsigned i = 0; i < 4; ++i) { + EXPECT_TRUE(buf.push_back(data.buf(), data.size())); + } + + // 5 won't because of overhead + EXPECT_FALSE(buf.push_back(data.buf(), data.size())); + + // Take 4 back out + for (unsigned i = 0; i < 4; ++i) { + TempData out{20}; + EXPECT_EQ(buf.pop_front(out.buf(), out.size()), data.size()); + EXPECT_EQ(data, out); + } + + TempData out{20}; + EXPECT_EQ(buf.pop_front(out.buf(), out.size()), 0); +} + +TEST(VariableLengthRingbuffer, PushAndPopSeveralVariableSize) +{ + VariableLengthRingbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + TempData data1{50}; + data1.paint(); + EXPECT_TRUE(buf.push_back(data1.buf(), data1.size())); + + TempData data2{30}; + data2.paint(42); + EXPECT_TRUE(buf.push_back(data2.buf(), data2.size())); + + // Supposedly more space + TempData out1{50}; + EXPECT_EQ(buf.pop_front(out1.buf(), 100), data1.size()); + EXPECT_EQ(data1, out1); + + TempData data3{50}; + data3.paint(33); + EXPECT_TRUE(buf.push_back(data3.buf(), data3.size())); + + // Supposedly more space + TempData out2{30}; + EXPECT_EQ(buf.pop_front(out2.buf(), 100), data2.size()); + EXPECT_EQ(data2, out2); + + // Supposedly more space + TempData out3{50}; + EXPECT_EQ(buf.pop_front(out3.buf(), 100), data3.size()); + EXPECT_EQ(data3, out3); + + TempData out4{100}; + EXPECT_EQ(buf.pop_front(out4.buf(), out4.size()), 0); +} + +TEST(VariableLengthRingbuffer, PushEmpty) +{ + VariableLengthRingbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + EXPECT_FALSE(buf.push_back(nullptr, 0)); +} + +TEST(VariableLengthRingbuffer, PopWithoutBuffer) +{ + VariableLengthRingbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + EXPECT_FALSE(buf.push_back(nullptr, 0)); + + TempData data{50}; + data.paint(); + + EXPECT_TRUE(buf.push_back(data.buf(), data.size())); + + + EXPECT_EQ(buf.pop_front(nullptr, 50), 0); +} + +TEST(VariableLengthRingbuffer, EmptyAndNoSpaceForHeader) +{ + // Addressing a corner case where start and end are at the end + // and the same. + + VariableLengthRingbuffer buf; + // Allocate 4+1 bytes more than the packet, 4 for the header, 1 for the start/end logic. + ASSERT_TRUE(buf.allocate(25)); + + { + TempData data{20}; + data.paint(); + EXPECT_TRUE(buf.push_back(data.buf(), data.size())); + TempData out{20}; + EXPECT_EQ(buf.pop_front(out.buf(), out.size()), out.size()); + EXPECT_EQ(data, out); + } + + { + TempData data{10}; + data.paint(); + EXPECT_TRUE(buf.push_back(data.buf(), data.size())); + TempData out{10}; + EXPECT_EQ(buf.pop_front(out.buf(), out.size()), out.size()); + EXPECT_EQ(data, out); + } +} From 6520fea839685de623dee19730d4d6ba04f24533 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 6 Oct 2023 08:12:26 +1300 Subject: [PATCH 3/3] mavlink: fix MAVLink message forwarding This switches from the horribly intertwined ringbuffer implementation to the new VariableLengthRingbuffer implementation. By ditching the previous implementation, we fix MAVLink message forwarding, which didn't work reliably. The reason it didn't work is that multiple mavlink messages could be added but only one of them was sent out because the buffer didn't keep track of the messages properly and only read the first one. Signed-off-by: Julian Oes --- src/modules/mavlink/CMakeLists.txt | 3 +- src/modules/mavlink/mavlink_main.cpp | 184 ++++----------------------- src/modules/mavlink/mavlink_main.h | 32 +---- 3 files changed, 32 insertions(+), 187 deletions(-) diff --git a/src/modules/mavlink/CMakeLists.txt b/src/modules/mavlink/CMakeLists.txt index 45f7737e6e03..8f5f6231af30 100644 --- a/src/modules/mavlink/CMakeLists.txt +++ b/src/modules/mavlink/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2015-2021 PX4 Development Team. All rights reserved. +# Copyright (c) 2015-2023 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 @@ -140,6 +140,7 @@ px4_add_module( mavlink_c timesync tunes + variable_length_ringbuffer version UNITY_BUILD ) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 40f6cbf8191b..36aec9ec1dbd 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2023 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 @@ -178,6 +178,7 @@ Mavlink::~Mavlink() perf_free(_loop_perf); perf_free(_loop_interval_perf); perf_free(_send_byte_error_perf); + perf_free(_forwarding_error_perf); } void @@ -1218,117 +1219,16 @@ Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate) } } -int -Mavlink::message_buffer_init(int size) -{ - _message_buffer.size = size; - _message_buffer.write_ptr = 0; - _message_buffer.read_ptr = 0; - _message_buffer.data = (char *)malloc(_message_buffer.size); - - int ret; - - if (_message_buffer.data == nullptr) { - ret = PX4_ERROR; - _message_buffer.size = 0; - - } else { - ret = OK; - } - - return ret; -} - -void -Mavlink::message_buffer_destroy() -{ - _message_buffer.size = 0; - _message_buffer.write_ptr = 0; - _message_buffer.read_ptr = 0; - free(_message_buffer.data); -} - -int -Mavlink::message_buffer_count() -{ - int n = _message_buffer.write_ptr - _message_buffer.read_ptr; - - if (n < 0) { - n += _message_buffer.size; - } - - return n; -} - -bool -Mavlink::message_buffer_write(const void *ptr, int size) -{ - // bytes available to write - int available = _message_buffer.read_ptr - _message_buffer.write_ptr - 1; - - if (available < 0) { - available += _message_buffer.size; - } - - if (size > available) { - // buffer overflow - return false; - } - - char *c = (char *) ptr; - int n = _message_buffer.size - _message_buffer.write_ptr; // bytes to end of the buffer - - if (n < size) { - // message goes over end of the buffer - memcpy(&(_message_buffer.data[_message_buffer.write_ptr]), c, n); - _message_buffer.write_ptr = 0; - - } else { - n = 0; - } - - // now: n = bytes already written - int p = size - n; // number of bytes to write - memcpy(&(_message_buffer.data[_message_buffer.write_ptr]), &(c[n]), p); - _message_buffer.write_ptr = (_message_buffer.write_ptr + p) % _message_buffer.size; - return true; -} - -int -Mavlink::message_buffer_get_ptr(void **ptr, bool *is_part) -{ - // bytes available to read - int available = _message_buffer.write_ptr - _message_buffer.read_ptr; - - if (available == 0) { - return 0; // buffer is empty - } - - int n = 0; - - if (available > 0) { - // read pointer is before write pointer, all available bytes can be read - n = available; - *is_part = false; - - } else { - // read pointer is after write pointer, read bytes from read_ptr to end of the buffer - n = _message_buffer.size - _message_buffer.read_ptr; - *is_part = _message_buffer.write_ptr > 0; - } - - *ptr = &(_message_buffer.data[_message_buffer.read_ptr]); - return n; -} - void Mavlink::pass_message(const mavlink_message_t *msg) { - /* size is 8 bytes plus variable payload */ + /* size is 12 bytes plus variable payload */ int size = MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len; - pthread_mutex_lock(&_message_buffer_mutex); - message_buffer_write(msg, size); - pthread_mutex_unlock(&_message_buffer_mutex); + LockGuard lg{_message_buffer_mutex}; + + if (!_message_buffer.push_back(reinterpret_cast(msg), size)) { + perf_count(_forwarding_error_perf); + } } MavlinkShell * @@ -2211,7 +2111,7 @@ Mavlink::task_main(int argc, char *argv[]) return PX4_ERROR; } - /* initialize send mutex */ + pthread_mutex_init(&_message_buffer_mutex, nullptr); pthread_mutex_init(&_send_mutex, nullptr); pthread_mutex_init(&_radio_status_mutex, nullptr); @@ -2221,13 +2121,12 @@ Mavlink::task_main(int argc, char *argv[]) * make space for two messages plus off-by-one space as we use the empty element * marker ring buffer approach. */ - if (OK != message_buffer_init(2 * sizeof(mavlink_message_t) + 1)) { + LockGuard lg{_message_buffer_mutex}; + + if (!_message_buffer.allocate(2 * sizeof(mavlink_message_t) + 1)) { PX4_ERR("msg buf alloc fail"); return PX4_ERROR; } - - /* initialize message buffer mutex */ - pthread_mutex_init(&_message_buffer_mutex, nullptr); } /* Activate sending the data by default (for the IRIDIUM mode it will be disabled after the first round of packages is sent)*/ @@ -2569,50 +2468,21 @@ Mavlink::task_main(int argc, char *argv[]) _events.update(t); - /* pass messages from other UARTs */ + /* pass messages from other instances */ if (get_forwarding_on()) { - bool is_part; - uint8_t *read_ptr; - uint8_t *write_ptr; - - pthread_mutex_lock(&_message_buffer_mutex); - int available = message_buffer_get_ptr((void **)&read_ptr, &is_part); - pthread_mutex_unlock(&_message_buffer_mutex); - - if (available > 0) { - // Reconstruct message from buffer - - mavlink_message_t msg; - write_ptr = (uint8_t *)&msg; - - // Pull a single message from the buffer - size_t read_count = available; - - if (read_count > sizeof(mavlink_message_t)) { - read_count = sizeof(mavlink_message_t); - } - - memcpy(write_ptr, read_ptr, read_count); - - // We hold the mutex until after we complete the second part of the buffer. If we don't - // we may end up breaking the empty slot overflow detection semantics when we mark the - // possibly partial read below. - pthread_mutex_lock(&_message_buffer_mutex); - - message_buffer_mark_read(read_count); - - /* write second part of buffer if there is some */ - if (is_part && read_count < sizeof(mavlink_message_t)) { - write_ptr += read_count; - available = message_buffer_get_ptr((void **)&read_ptr, &is_part); - read_count = sizeof(mavlink_message_t) - read_count; - memcpy(write_ptr, read_ptr, read_count); - message_buffer_mark_read(available); - } - - pthread_mutex_unlock(&_message_buffer_mutex); + mavlink_message_t msg; + size_t available_bytes; + { + // We only send one message at a time, not to put too much strain on a + // link from forwarded messages. + LockGuard lg{_message_buffer_mutex}; + available_bytes = _message_buffer.pop_front(reinterpret_cast(&msg), sizeof(msg)); + // We need to make sure to release the lock here before sending the + // bytes out via IP or UART which could potentially take longer. + } + if (available_bytes > 0) { resend_message(&msg); } } @@ -2662,11 +2532,6 @@ Mavlink::task_main(int argc, char *argv[]) _socket_fd = -1; } - if (get_forwarding_on()) { - message_buffer_destroy(); - pthread_mutex_destroy(&_message_buffer_mutex); - } - if (_mavlink_ulog) { _mavlink_ulog->stop(); _mavlink_ulog = nullptr; @@ -2674,6 +2539,7 @@ Mavlink::task_main(int argc, char *argv[]) pthread_mutex_destroy(&_send_mutex); pthread_mutex_destroy(&_radio_status_mutex); + pthread_mutex_destroy(&_message_buffer_mutex); PX4_INFO("exiting channel %i", (int)_channel); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 85c0361de3ad..1ba71c6a2ba1 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2023 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 @@ -60,6 +60,7 @@ #include #include +#include #include #include #include @@ -419,11 +420,6 @@ class Mavlink final : public ModuleParams bool get_wait_to_transmit() { return _wait_to_transmit; } bool should_transmit() { return (_transmitting_enabled && (!_wait_to_transmit || (_wait_to_transmit && _received_messages))); } - bool message_buffer_write(const void *ptr, int size); - - void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); } - void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); } - /** * Count transmitted bytes */ @@ -651,16 +647,9 @@ class Mavlink final : public ModuleParams ping_statistics_s _ping_stats {}; - struct mavlink_message_buffer { - int write_ptr; - int read_ptr; - int size; - char *data; - }; + pthread_mutex_t _message_buffer_mutex{}; + VariableLengthRingbuffer _message_buffer{}; - mavlink_message_buffer _message_buffer {}; - - pthread_mutex_t _message_buffer_mutex {}; pthread_mutex_t _send_mutex {}; pthread_mutex_t _radio_status_mutex {}; @@ -682,6 +671,7 @@ class Mavlink final : public ModuleParams perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": tx run elapsed")}; /**< loop performance counter */ perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": tx run interval")}; /**< loop interval performance counter */ perf_counter_t _send_byte_error_perf{perf_alloc(PC_COUNT, MODULE_NAME": send_bytes error")}; /**< send bytes error count */ + perf_counter_t _forwarding_error_perf{perf_alloc(PC_COUNT, MODULE_NAME": forwarding error")}; /**< forwarding messages error count */ void mavlink_update_parameters(); @@ -711,18 +701,6 @@ class Mavlink final : public ModuleParams */ int configure_streams_to_default(const char *configure_single_stream = nullptr); - int message_buffer_init(int size); - - void message_buffer_destroy(); - - int message_buffer_count(); - - int message_buffer_is_empty() const { return (_message_buffer.read_ptr == _message_buffer.write_ptr); } - - int message_buffer_get_ptr(void **ptr, bool *is_part); - - void message_buffer_mark_read(int n) { _message_buffer.read_ptr = (_message_buffer.read_ptr + n) % _message_buffer.size; } - void pass_message(const mavlink_message_t *msg); void publish_telemetry_status();