From 2a0ddf9f441d956fcb7ebf32dfa24c76238c9dde Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 9 Oct 2020 18:14:42 +0200 Subject: [PATCH] mavlink: enable 6 instead of 4 instance --- src/modules/mavlink/mavlink_bridge_header.h | 6 ++++++ src/modules/mavlink/mavlink_command_sender.cpp | 4 ++-- src/modules/mavlink/mavlink_command_sender.h | 13 +++++++++---- src/modules/mavlink/mavlink_log_handler.h | 2 +- src/modules/mavlink/mavlink_main.cpp | 2 +- src/modules/mavlink/mavlink_main.h | 2 +- 6 files changed, 20 insertions(+), 9 deletions(-) diff --git a/src/modules/mavlink/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h index d9c1a66958..a3774dbb06 100644 --- a/src/modules/mavlink/mavlink_bridge_header.h +++ b/src/modules/mavlink/mavlink_bridge_header.h @@ -55,6 +55,12 @@ #define MAVLINK_GET_CHANNEL_BUFFER mavlink_get_channel_buffer #define MAVLINK_GET_CHANNEL_STATUS mavlink_get_channel_status +#if defined(__PX4_POSIX) +#define MAVLINK_COMM_NUM_BUFFERS 6 +#define MAVLINK_COMM_4 static_cast(4) +#define MAVLINK_COMM_5 static_cast(5) +#endif + #include #include diff --git a/src/modules/mavlink/mavlink_command_sender.cpp b/src/modules/mavlink/mavlink_command_sender.cpp index b076115e8e..85578b1335 100644 --- a/src/modules/mavlink/mavlink_command_sender.cpp +++ b/src/modules/mavlink/mavlink_command_sender.cpp @@ -151,7 +151,7 @@ void MavlinkCommandSender::check_timeout(mavlink_channel_t channel) // as some channels might be lagging behind and will end up putting the same command into the buffer. bool dropped_command = false; - for (unsigned i = 0; i < MAX_MAVLINK_CHANNEL; ++i) { + for (unsigned i = 0; i < MAVLINK_COMM_NUM_BUFFERS; ++i) { if (item->num_sent_per_channel[i] == -2) { _commands.drop_current(); dropped_command = true; @@ -176,7 +176,7 @@ void MavlinkCommandSender::check_timeout(mavlink_channel_t channel) int8_t max_sent = 0; int8_t min_sent = INT8_MAX; - for (unsigned i = 0; i < MAX_MAVLINK_CHANNEL; ++i) { + for (unsigned i = 0; i < MAVLINK_COMM_NUM_BUFFERS; ++i) { if (item->num_sent_per_channel[i] > max_sent) { max_sent = item->num_sent_per_channel[i]; } diff --git a/src/modules/mavlink/mavlink_command_sender.h b/src/modules/mavlink/mavlink_command_sender.h index 2454055802..b029ab33a7 100644 --- a/src/modules/mavlink/mavlink_command_sender.h +++ b/src/modules/mavlink/mavlink_command_sender.h @@ -102,14 +102,19 @@ private: static MavlinkCommandSender *_instance; static px4_sem_t _lock; - // There are MAVLINK_COMM_0 to MAVLINK_COMM_3, so it should be 4. - static const unsigned MAX_MAVLINK_CHANNEL = 4; - struct command_item_s { mavlink_command_long_t command = {}; hrt_abstime timestamp_us = 0; hrt_abstime last_time_sent_us = 0; - int8_t num_sent_per_channel[MAX_MAVLINK_CHANNEL] = {-1, -1, -1, -1}; // -1: channel did not request this command to be sent, -2: channel got an ack for this command + // -1: channel did not request this command to be sent, -2: channel got an ack for this command +#if MAVLINK_COMM_NUM_BUFFERS == 4 + int8_t num_sent_per_channel[MAVLINK_COMM_NUM_BUFFERS] = {-1, -1, -1, -1}; +#elif MAVLINK_COMM_NUM_BUFFERS == 6 + int8_t num_sent_per_channel[MAVLINK_COMM_NUM_BUFFERS] = {-1, -1, -1, -1, -1, -1}; +#else +#error Unknown number of MAVLINK_COMM_NUM_BUFFERS +#endif + }; TimestampedList _commands{3}; diff --git a/src/modules/mavlink/mavlink_log_handler.h b/src/modules/mavlink/mavlink_log_handler.h index dc4843eee4..a882ee1d95 100644 --- a/src/modules/mavlink/mavlink_log_handler.h +++ b/src/modules/mavlink/mavlink_log_handler.h @@ -41,7 +41,7 @@ #include #include #include -#include +#include "mavlink_bridge_header.h" #include class Mavlink; diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 0814e14b19..8da68e42e9 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -278,7 +278,7 @@ Mavlink::set_channel() #endif default: - PX4_WARN("instance ID is out of range"); + PX4_WARN("instance ID %d is out of range", _instance_id); px4_task_exit(1); break; } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index afa4743eea..096bb6e46e 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -540,7 +540,7 @@ private: bool _task_running{true}; static bool _boot_complete; - static constexpr int MAVLINK_MAX_INSTANCES{4}; + static constexpr int MAVLINK_MAX_INSTANCES{6}; static constexpr int MAVLINK_MIN_INTERVAL{1500}; static constexpr int MAVLINK_MAX_INTERVAL{10000}; static constexpr float MAVLINK_MIN_MULTIPLIER{0.0005f};