diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 8684920..44736fc 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -228,22 +228,21 @@ else # Waypoint storage. # REBOOTWORK this needs to start in parallel. # - - #if param compare -s SYS_DM_BACKEND 1 - #then -# dataman start -r -# else -# if param compare SYS_DM_BACKEND 0 -# then -# # dataman start default -# dataman start -# fi -# fi + if param compare -s SYS_DM_BACKEND 1 + then + dataman start -r + else + if param compare SYS_DM_BACKEND 0 + then + # dataman start default + dataman start + fi + fi # # Start the socket communication send_event handler. # -# send_event start + send_event start # # Start the resource load monitor. @@ -558,4 +557,4 @@ unset VEHICLE_TYPE # # Boot is complete, inform MAVLink app(s) that the system is now fully up and running. # -# mavlink boot_complete +mavlink boot_complete diff --git a/boards/px4/fts-can/src/board_config.h b/boards/px4/fts-can/src/board_config.h index 598e81f..d0cf2a3 100644 --- a/boards/px4/fts-can/src/board_config.h +++ b/boards/px4/fts-can/src/board_config.h @@ -149,6 +149,7 @@ #define GPIO_FTS_ARM /* PE0 FTS CAN ARM NEGATED */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN0) #define GPIO_FTS_CAN_OUTPUT /* PE2 FTS CAN CUT */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN2) #define GPIO_FTS_CHUTE_OUTPUT /* PE4 FTS CHUTE FIRE */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4) +#define FTS_ARM_NEGATED(on_true) px4_arch_gpiowrite(GPIO_FTS_ARM, (on_true)) #define FTS_TERMINATE_CAN(on_true) px4_arch_gpiowrite(GPIO_FTS_CAN_OUTPUT, (on_true)) #define FTS_TRIGGER_CHUTE(on_true) px4_arch_gpiowrite(GPIO_FTS_CHUTE_OUTPUT, (on_true)) diff --git a/build_6c.sh b/build_v6c.sh similarity index 100% rename from build_6c.sh rename to build_v6c.sh diff --git a/cmake/kconfig.cmake b/cmake/kconfig.cmake index 6dfae4d..ea5e488 100644 --- a/cmake/kconfig.cmake +++ b/cmake/kconfig.cmake @@ -159,6 +159,7 @@ if(EXISTS ${BOARD_DEFCONFIG}) # Find variable name string(REGEX MATCH "^CONFIG_MODULES[^=]+" Modules ${NameAndValue}) + if(Modules) # Find the value string(REPLACE "${Name}=" "" Value ${NameAndValue}) diff --git a/flash_6c.sh b/flash_v6c.sh similarity index 89% rename from flash_6c.sh rename to flash_v6c.sh index 2191b77..3574b9f 100755 --- a/flash_6c.sh +++ b/flash_v6c.sh @@ -1,6 +1,6 @@ #!/bin/bash -xe # build firmware -./build_6c.sh +./build_v6c.sh # flash firmware st-flash write build/px4_fmu-v6c_default/px4_fmu-v6c_default.bin 0x08020000 diff --git a/src/drivers/fts/fts.cpp b/src/drivers/fts/fts.cpp index 1eda06a..1b07188 100644 --- a/src/drivers/fts/fts.cpp +++ b/src/drivers/fts/fts.cpp @@ -36,6 +36,15 @@ int FTS::custom_command(int argc, char *argv[]) return PX4_ERROR; } + if (!strcmp(argv[0], "arm")) { + get_instance()->fts_arm(); + return 0; + } + if (!strcmp(argv[0], "disarm")) { + get_instance()->fts_disarm(); + return 0; + } + if (!strcmp(argv[0], "fire")) { get_instance()->fts_on(); return 0; @@ -64,6 +73,16 @@ px4_arch_configgpio(GPIO_FTS_CAN_OUTPUT); px4_arch_configgpio(GPIO_FTS_CHUTE_OUTPUT); } +void FTS::fts_arm() +{ + FTS_ARM_NEGATED(false); +} + +void FTS::fts_disarm() +{ + FTS_ARM_NEGATED(true); +} + void FTS::fts_off() { FTS_TERMINATE_CAN(false); diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index f6bcc92..817a68c 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -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) @@ -70,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/ringbuffer/CMakeLists.txt b/src/lib/ringbuffer/CMakeLists.txt new file mode 100644 index 0000000..78a26de --- /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 0000000..64fc33d --- /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 0000000..541f322 --- /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 0000000..e5e0a7a --- /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); + } +} diff --git a/src/lib/variable_length_ringbuffer/CMakeLists.txt b/src/lib/variable_length_ringbuffer/CMakeLists.txt new file mode 100644 index 0000000..2d2bf47 --- /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 0000000..9b607e2 --- /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 0000000..89f92eb --- /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 0000000..958f369 --- /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); + } +} diff --git a/src/modules/mavlink/CMakeLists.txt b/src/modules/mavlink/CMakeLists.txt index ec9d4c3..b63ea6c 100644 --- a/src/modules/mavlink/CMakeLists.txt +++ b/src/modules/mavlink/CMakeLists.txt @@ -144,6 +144,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 40f6cbf..36aec9e 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 85c0361..1ba71c6 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 @@ public: 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 @@ private: 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 @@ private: 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 @@ private: */ 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(); diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index e89f0f1..ec6bdea 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -1587,6 +1587,7 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s * mavlink_mission_item->frame = mission_item->frame; mavlink_mission_item->command = mission_item->nav_cmd; mavlink_mission_item->autocontinue = mission_item->autocontinue; + mavlink_mission_item->mission_type = _mission_type; /* default mappings for generic commands */ if (mission_item->frame == MAV_FRAME_MISSION) { diff --git a/src/modules/mavlink/streams/HEARTBEAT.hpp b/src/modules/mavlink/streams/HEARTBEAT.hpp index 48032b4..20e3974 100644 --- a/src/modules/mavlink/streams/HEARTBEAT.hpp +++ b/src/modules/mavlink/streams/HEARTBEAT.hpp @@ -130,14 +130,14 @@ private: system_status = MAV_STATE_CALIBRATING; } // custom heartbeat compliant to Nimbus Ground FTS - custom_mode = custom_mode; - base_mode = base_mode; - system_status = system_status; - mavlink_msg_heartbeat_send(_mavlink->get_channel(), _mavlink->get_system_type(), 20, - 0, 0, 3); + //custom_mode = custom_mode; + //base_mode = base_mode; + //system_status = system_status; + //mavlink_msg_heartbeat_send(_mavlink->get_channel(), _mavlink->get_system_type(), 20, + // 0, 0, 3); // standard heartbeat - //mavlink_msg_heartbeat_send(_mavlink->get_channel(), _mavlink->get_system_type(), 20, - // base_mode, custom_mode.data, system_status); + mavlink_msg_heartbeat_send(_mavlink->get_channel(), _mavlink->get_system_type(), MAV_AUTOPILOT_PX4, + base_mode, custom_mode.data, system_status); return true; }