gg/fts-can
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

afts backport mavlink ringbuffer from 1.14.3 to solve qgc incompatibility (could upload but not download mission)

Browse Source
This commit is contained in:
neo 2024-06-06 16:00:34 +02:00
parent f6a24c9f83
commit ee273063b8
20 changed files with 1178 additions and 207 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
ROMFS/px4fmu_common/init.d/rcS
View File

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

1
boards/px4/fts-can/src/board_config.h
View File

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

0
build_6c.sh → build_v6c.sh
View File

1
cmake/kconfig.cmake
View File

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

2
flash_6c.sh → flash_v6c.sh
View File

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

19
src/drivers/fts/fts.cpp
View File

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

2
src/lib/CMakeLists.txt
View File

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

40
src/lib/ringbuffer/CMakeLists.txt Normal file
View File

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

180
src/lib/ringbuffer/Ringbuffer.cpp Normal file
View File

@ -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 <mathlib/mathlib.h>
#include <assert.h>
#include <string.h>
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;
}
}

120
src/lib/ringbuffer/Ringbuffer.hpp Normal file
View File

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

247
src/lib/ringbuffer/RingbufferTest.cpp Normal file
View File

@ -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 <gtest/gtest.h>
#include <stdint.h>
#include <string.h>
#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);
}
}

42
src/lib/variable_length_ringbuffer/CMakeLists.txt Normal file
View File

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

106
src/lib/variable_length_ringbuffer/VariableLengthRingbuffer.cpp Normal file
View File

@ -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 <assert.h>
#include <string.h>
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<uint32_t>(packet_len)};
bool result = _ringbuffer.push_back(reinterpret_cast<const uint8_t * >(&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<uint8_t *>(&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<uint32_t>(header.len) <= buf_max_len);
size_t bytes_read = _ringbuffer.pop_front(buf, header.len);
assert(bytes_read == header.len);
return bytes_read;
}

111
src/lib/variable_length_ringbuffer/VariableLengthRingbuffer.hpp Normal file
View File

@ -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 <stdint.h>
#include <lib/ringbuffer/Ringbuffer.hpp>
// 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 {};
};

257
src/lib/variable_length_ringbuffer/VariableLengthRingbufferTest.cpp Normal file
View File

@ -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 <gtest/gtest.h>
#include <stdint.h>
#include <string.h>
#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);
}
}

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

@ -144,6 +144,7 @@ px4_add_module(
mavlink_c
timesync
tunes
variable_length_ringbuffer
version
UNITY_BUILD
)

180
src/modules/mavlink/mavlink_main.cpp
View File

@ -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<const uint8_t *>(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);
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<uint8_t *>(&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.
}
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);
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);

32
src/modules/mavlink/mavlink_main.h
View File

@ -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 <containers/List.hpp>
#include <parameters/param.h>
#include <lib/variable_length_ringbuffer/VariableLengthRingbuffer.hpp>
#include <perf/perf_counter.h>
#include <px4_platform_common/cli.h>
#include <px4_platform_common/px4_config.h>
@ -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;
};
mavlink_message_buffer _message_buffer {};
pthread_mutex_t _message_buffer_mutex{};
VariableLengthRingbuffer _message_buffer{};
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();

1
src/modules/mavlink/mavlink_mission.cpp
View File

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

14
src/modules/mavlink/streams/HEARTBEAT.hpp
View File

@ -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);
// standard heartbeat
//custom_mode = custom_mode;
//base_mode = base_mode;
//system_status = system_status;
//mavlink_msg_heartbeat_send(_mavlink->get_channel(), _mavlink->get_system_type(), 20,
// base_mode, custom_mode.data, system_status);
// 0, 0, 3);
// standard heartbeat
mavlink_msg_heartbeat_send(_mavlink->get_channel(), _mavlink->get_system_type(), MAV_AUTOPILOT_PX4,
base_mode, custom_mode.data, system_status);
return true;
}