From af0107ae0a1d65f8b62549f76291c088ffbb240e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 5 Jul 2017 16:13:42 +0200 Subject: [PATCH] mavlink: add class for command retransmission This adds a class to allow for retransmission of outgoing commands. The sent commands are kept in a timestamped list to check if they are acked as required by the mavlink protocol. If they are not acked within a timeout, they can be retransmitted. --- src/modules/mavlink/CMakeLists.txt | 3 +- .../mavlink/mavlink_command_sender.cpp | 230 ++++++++++++++++++ src/modules/mavlink/mavlink_command_sender.h | 125 ++++++++++ src/modules/mavlink/mavlink_main.cpp | 2 + src/modules/mavlink/mavlink_messages.cpp | 24 +- src/modules/mavlink/mavlink_receiver.cpp | 3 + src/modules/mavlink/timestamped_list.h | 158 ++++++++++++ 7 files changed, 527 insertions(+), 18 deletions(-) create mode 100644 src/modules/mavlink/mavlink_command_sender.cpp create mode 100644 src/modules/mavlink/mavlink_command_sender.h create mode 100644 src/modules/mavlink/timestamped_list.h diff --git a/src/modules/mavlink/CMakeLists.txt b/src/modules/mavlink/CMakeLists.txt index a647a46eff..0fb2205143 100644 --- a/src/modules/mavlink/CMakeLists.txt +++ b/src/modules/mavlink/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# Copyright (c) 2015-2017 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 @@ -53,6 +53,7 @@ px4_add_module( mavlink_log_handler.cpp mavlink_shell.cpp mavlink_ulog.cpp + mavlink_command_sender.cpp DEPENDS platforms__common ) diff --git a/src/modules/mavlink/mavlink_command_sender.cpp b/src/modules/mavlink/mavlink_command_sender.cpp new file mode 100644 index 0000000000..2a0107f986 --- /dev/null +++ b/src/modules/mavlink/mavlink_command_sender.cpp @@ -0,0 +1,230 @@ +/**************************************************************************** + * + * Copyright (c) 2017 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. + * + ****************************************************************************/ + +/** + * @file mavlink_command_sender.cpp + * Mavlink commands sender with support for retransmission. + * + * @author Julian Oes + */ + +#include "mavlink_command_sender.h" +#include +#include + +bool MavlinkCommandSender::_init = false; +MavlinkCommandSender *MavlinkCommandSender::_instance = nullptr; +px4_sem_t MavlinkCommandSender::_lock; + +void MavlinkCommandSender::initialize() +{ + if (_init) { + return; + } + + px4_sem_init(&_lock, 1, 1); + _init = true; +} + +MavlinkCommandSender &MavlinkCommandSender::instance() +{ + if (_instance == nullptr) { + _instance = new MavlinkCommandSender(); + } + + return *_instance; +} + +MavlinkCommandSender::MavlinkCommandSender() : + _commands(5) +{ +} + +MavlinkCommandSender::~MavlinkCommandSender() +{ + px4_sem_destroy(&_lock); +} + +int MavlinkCommandSender::handle_vehicle_command(struct vehicle_command_s &command, mavlink_channel_t channel) +{ + assert(0 <= channel); + assert(channel < MAX_MAVLINK_CHANNEL); + + lock(); + PX4_INFO("getting vehicle command with timestamp %" PRIu64 ", channel: %d", command.timestamp, channel); + + mavlink_command_long_t msg = {}; + msg.target_system = command.target_system; + msg.target_component = command.target_component; + msg.command = command.command; + msg.confirmation = command.confirmation; + msg.param1 = command.param1; + msg.param2 = command.param2; + msg.param3 = command.param3; + msg.param4 = command.param4; + msg.param5 = command.param5; + msg.param6 = command.param6; + msg.param7 = command.param7; + mavlink_msg_command_long_send_struct(channel, &msg); + + bool already_existing = false; + _commands.reset_to_start(); + + while (command_item_t *item = _commands.get_next()) { + if (item->timestamp_us == command.timestamp) { + + // We should activate the channel by setting num_sent_per_channel from -1 to 0. + item->num_sent_per_channel[channel] = 0; + + PX4_INFO("already existing"); + already_existing = true; + + break; + } + } + + if (!already_existing) { + + command_item_t new_item; + new_item.command = msg; + new_item.timestamp_us = command.timestamp; + new_item.num_sent_per_channel[channel] = 0; + new_item.last_time_sent_us = hrt_absolute_time(); + _commands.put(new_item); + } + + unlock(); + return 0; +} + +void MavlinkCommandSender::handle_mavlink_command_ack(const mavlink_command_ack_t &ack, + uint8_t from_sysid, uint8_t from_compid) +{ + PX4_INFO("handling result %d for command %d: %d from %d, %d", + ack.result, ack.command, from_sysid, from_compid); + lock(); + + _commands.reset_to_start(); + + while (command_item_t *item = _commands.get_next()) { + // Check if the incoming ack matches any of the commands that we have sent. + if (item->command.command == ack.command && + from_sysid == item->command.target_system && + from_compid == item->command.target_component) { + PX4_INFO("handling result %d for command %d: %d", ack.result, ack.command); + // Drop it anyway because the command seems to have arrived at the destination, even if we + // receive IN_PROGRESS because we trust that it will be handled after that. + _commands.drop_current(); + break; + } + } + + unlock(); +} + +void MavlinkCommandSender::check_timeout(mavlink_channel_t channel) +{ + assert(0 <= channel); + assert(channel < MAX_MAVLINK_CHANNEL); + + lock(); + + _commands.reset_to_start(); + + while (command_item_t *item = _commands.get_next()) { + if (hrt_elapsed_time(&item->last_time_sent_us) <= TIMEOUT_US) { + // We keep waiting for the timeout. + continue; + } + + // The goal of this is to retry from all channels. Therefore, we keep + // track of the retry count for each channel. + // + // When the first channel does a retry, the timeout is reset. + // (e.g. all channel have done 2 retries, then channel 0 is called + // and does retry number 3, and also resets the timeout timestamp). + + // First, we need to determine what the current max and min retry level + // are because we can only level up, if all have caught up. + // If num_sent_per_channel is at -1, the channel is inactive. + int8_t max_sent = 0; + int8_t min_sent = INT8_MAX; + + for (unsigned i = 0; i < MAX_MAVLINK_CHANNEL; ++i) { + if (item->num_sent_per_channel[i] > max_sent) { + max_sent = item->num_sent_per_channel[i]; + } + + if ((item->num_sent_per_channel[i] != -1) && + (item->num_sent_per_channel[i] < min_sent)) { + min_sent = item->num_sent_per_channel[i]; + } + } + + if (item->num_sent_per_channel[channel] < max_sent) { + // We are behind and need to do a retransmission. + mavlink_msg_command_long_send_struct(channel, &item->command); + item->num_sent_per_channel[channel]++; + + PX4_INFO("%x timeout (behind), retries: %d/%d, channel: %d", + item, item->num_sent_per_channel[channel], max_sent, channel); + + } else if (item->num_sent_per_channel[channel] == max_sent && + min_sent == max_sent) { + + // If the next retry would be above the needed retries anyway, we can + // drop the item, and continue with other items. + if (item->num_sent_per_channel[channel] + 1 > RETRIES) { + _commands.drop_current(); + PX4_INFO("%x, timeout dropped", item); + continue; + } + + // We are the first of a new retransmission series. + mavlink_msg_command_long_send_struct(channel, &item->command); + item->num_sent_per_channel[channel]++; + // Therefore, we are the ones setting the timestamp of this retry round. + item->last_time_sent_us = hrt_absolute_time(); + + PX4_INFO("%x timeout (first), retries: %d/%d, channel: %d", + item, item->num_sent_per_channel[channel], max_sent, channel); + + } else { + // We are already ahead, so this should not happen. + // If it ever does, just ignore it. It will timeout eventually. + continue; + } + } + + unlock(); +} diff --git a/src/modules/mavlink/mavlink_command_sender.h b/src/modules/mavlink/mavlink_command_sender.h new file mode 100644 index 0000000000..9f5cdd0cb8 --- /dev/null +++ b/src/modules/mavlink/mavlink_command_sender.h @@ -0,0 +1,125 @@ +/**************************************************************************** + * + * Copyright (c) 2017 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. + * + ****************************************************************************/ + +/** + * @file mavlink_command_sender.h + * Mavlink commands sender with support for retransmission. + * + * @author Julian Oes + */ + +#pragma once + +#include +#include +#include + +#include +#include + +#include "timestamped_list.h" +#include "mavlink_bridge_header.h" +#include + +/** + * @class MavlinkCommandSender + */ +class MavlinkCommandSender +{ +public: + /** + * initialize: call this once on startup (this function is not thread-safe!) + */ + static void initialize(); + + static MavlinkCommandSender &instance(); + + /** + * Send a command on a channel and keep it in a queue for retransmission. + * thread-safe + * @return 0 on success, <0 otherwise + */ + int handle_vehicle_command(struct vehicle_command_s &command, mavlink_channel_t channel); + + /** + * Check timeouts to verify if an commands need retransmission. + * thread-safe + */ + void check_timeout(mavlink_channel_t channel); + + /** + * Handle mavlink command_ack. + * thread-safe + */ + void handle_mavlink_command_ack(const mavlink_command_ack_t &ack, uint8_t from_sysid, uint8_t from_compid); + +private: + MavlinkCommandSender(); + + ~MavlinkCommandSender(); + + static void lock() + { + do {} while (px4_sem_wait(&_lock) != 0); + } + + static void unlock() + { + px4_sem_post(&_lock); + } + + + static bool _init; + 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; + + typedef struct { + 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}; + } command_item_t; + + TimestampedList _commands; + + static const uint8_t RETRIES = 3; + static const uint64_t TIMEOUT_US = 500000; + + /* do not allow copying or assigning this class */ + MavlinkCommandSender(const MavlinkCommandSender &) = delete; + MavlinkCommandSender operator=(const MavlinkCommandSender &) = delete; +}; diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 4185e5967f..6239f13283 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -85,6 +85,7 @@ #include "mavlink_messages.h" #include "mavlink_receiver.h" #include "mavlink_rate_limiter.h" +#include "mavlink_command_sender.h" // Guard against MAVLink misconfiguration #ifndef MAVLINK_CRC_EXTRA @@ -2450,6 +2451,7 @@ int Mavlink::start(int argc, char *argv[]) { MavlinkULog::initialize(); + MavlinkCommandSender::initialize(); // Wait for the instance count to go up one // before returning to the shell diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 89942c6329..09677faf51 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -44,6 +44,7 @@ #include "mavlink_main.h" #include "mavlink_messages.h" +#include "mavlink_command_sender.h" #include #include @@ -453,25 +454,14 @@ protected: struct vehicle_command_s cmd; if (_cmd_sub->update(&_cmd_time, &cmd)) { + /* only send commands for other systems/components */ - if (cmd.target_system != mavlink_system.sysid || cmd.target_component != mavlink_system.compid) { - mavlink_command_long_t msg = {}; - - msg.target_system = cmd.target_system; - msg.target_component = cmd.target_component; - msg.command = cmd.command; - msg.confirmation = cmd.confirmation; - msg.param1 = cmd.param1; - msg.param2 = cmd.param2; - msg.param3 = cmd.param3; - msg.param4 = cmd.param4; - msg.param5 = cmd.param5; - msg.param6 = cmd.param6; - msg.param7 = cmd.param7; - - mavlink_msg_command_long_send_struct(_mavlink->get_channel(), &msg); - } + //if (cmd.target_system != mavlink_system.sysid || cmd.target_component != mavlink_system.compid) { + MavlinkCommandSender::instance().handle_vehicle_command(cmd, _mavlink->get_channel()); + //} } + + MavlinkCommandSender::instance().check_timeout(_mavlink->get_channel()); } }; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 8071a5b4e1..cc353305f3 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -88,6 +88,7 @@ #include "mavlink_bridge_header.h" #include "mavlink_receiver.h" #include "mavlink_main.h" +#include "mavlink_command_sender.h" static const float mg2ms2 = CONSTANTS_ONE_G / 1000.0f; @@ -628,6 +629,8 @@ MavlinkReceiver::handle_message_command_ack(mavlink_message_t *msg) mavlink_msg_command_ack_decode(msg, &ack); if (ack.result != MAV_RESULT_ACCEPTED && ack.result != MAV_RESULT_IN_PROGRESS) { + MavlinkCommandSender::instance().handle_mavlink_command_ack(ack, msg->sysid, msg->compid); + if (msg->compid == MAV_COMP_ID_CAMERA) { PX4_WARN("Got unsuccessful result %d from camera", ack.result); } diff --git a/src/modules/mavlink/timestamped_list.h b/src/modules/mavlink/timestamped_list.h new file mode 100644 index 0000000000..769ca40599 --- /dev/null +++ b/src/modules/mavlink/timestamped_list.h @@ -0,0 +1,158 @@ +/**************************************************************************** + * + * Copyright (c) 2017 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. + * + ****************************************************************************/ + +/** + * @file timestamped list.h + * Fixed size list with timestamps. + * + * The list has a fixed size that is set at instantiation and is based + * on timestamps. If a new value is put into a full list, the oldest value + * is overwritten. + * + * @author Julian Oes + */ + +#pragma once + +#include + +/** + * @class TimestampedList + */ +template +class TimestampedList +{ +public: + TimestampedList(int num_items) + { + _list = new item_t[num_items]; + _list_len = num_items; + } + ~TimestampedList() + { + delete[] _list; + } + + /* + * Insert a value into the list, overwrite the oldest entry if full. + */ + void put(T new_value) + { + hrt_abstime now = hrt_absolute_time(); + + // Insert it wherever there is a free space. + for (int i = 0; i < _list_len; ++i) { + if (_list[i].timestamp_us == 0) { + _list[i].timestamp_us = now; + _list[i].value = new_value; + return; + } + } + + // Find oldest entry. + int oldest_i = 0; + + for (int i = 1; i < _list_len; ++i) { + if (_list[i].timestamp_us < _list[oldest_i].timestamp_us) { + oldest_i = i; + } + } + + // And overwrite oldest. + _list[oldest_i].timestamp_us = now; + _list[oldest_i].value = new_value; + } + + /* + * Before iterating using get_next(), reset to start. + */ + void reset_to_start() + { + _current_i = -1; + } + + /* + * Iterate through all active values (not sorted). + * Return nullptr if at end of list. + * + * This is basically a poor man's iterator. + */ + T *get_next() + { + // Increment first, then leave it until called again. + ++_current_i; + + for (int i = _current_i; i < _list_len; ++i) { + if (_list[i].timestamp_us != 0) { + _current_i = i; + return &_list[i].value; + } + } + + return nullptr; + } + + /* + * Disable the last item that we have gotten. + */ + void drop_current() + { + if (_current_i < _list_len) { + _list[_current_i].timestamp_us = 0; + } + } + + /* + * Update the timestamp of the item we have gotten. + */ + void update_current() + { + if (_current_i < _list_len) { + _list[_current_i].timestamp = hrt_absolute_time(); + } + } + + /* do not allow copying or assigning this class */ + TimestampedList(const TimestampedList &) = delete; + TimestampedList operator=(const TimestampedList &) = delete; + +private: + typedef struct { + hrt_abstime timestamp_us = 0; // 0 signals inactive. + T value; + } item_t; + + item_t *_list = nullptr; + int _list_len = 0; + int _current_i = -1; +};