From 57d85de4d19aa83cf229c4aa6d86b8fdf667733d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Wed, 12 Oct 2016 16:47:11 +0200 Subject: [PATCH] mavlink: add MavlinkULog class to receive ulog data from the logger --- src/modules/mavlink/CMakeLists.txt | 1 + src/modules/mavlink/mavlink_ulog.cpp | 216 +++++++++++++++++++++++++++ src/modules/mavlink/mavlink_ulog.h | 126 ++++++++++++++++ 3 files changed, 343 insertions(+) create mode 100644 src/modules/mavlink/mavlink_ulog.cpp create mode 100644 src/modules/mavlink/mavlink_ulog.h diff --git a/src/modules/mavlink/CMakeLists.txt b/src/modules/mavlink/CMakeLists.txt index cec012e92e..83e0118af3 100644 --- a/src/modules/mavlink/CMakeLists.txt +++ b/src/modules/mavlink/CMakeLists.txt @@ -49,6 +49,7 @@ px4_add_module( mavlink_ftp.cpp mavlink_log_handler.cpp mavlink_shell.cpp + mavlink_ulog.cpp DEPENDS platforms__common ) diff --git a/src/modules/mavlink/mavlink_ulog.cpp b/src/modules/mavlink/mavlink_ulog.cpp new file mode 100644 index 0000000000..0157746acc --- /dev/null +++ b/src/modules/mavlink/mavlink_ulog.cpp @@ -0,0 +1,216 @@ +/**************************************************************************** + * + * Copyright (c) 2016 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_ulog.cpp + * ULog data streaming via MAVLink + * + * @author Beat Küng + */ + +#include "mavlink_ulog.h" +#include +#include + +bool MavlinkULog::_init = false; +MavlinkULog *MavlinkULog::_instance = nullptr; +sem_t MavlinkULog::_lock; + + +MavlinkULog::MavlinkULog() +{ + _ulog_stream_sub = orb_subscribe(ORB_ID(ulog_stream)); + + if (_ulog_stream_sub < 0) { + PX4_ERR("orb_subscribe failed (%i)", errno); + } + _waiting_for_initial_ack = true; + _last_sent_time = hrt_absolute_time(); //(ab)use this timestamp during initialization +} + +MavlinkULog::~MavlinkULog() +{ + if (_ulog_stream_ack_pub) { + orb_unadvertise(_ulog_stream_ack_pub); + } + if (_ulog_stream_sub >= 0) { + orb_unsubscribe(_ulog_stream_sub); + } +} + +void MavlinkULog::start_ack_received() +{ + if (_waiting_for_initial_ack) { + _last_sent_time = 0; + _waiting_for_initial_ack = false; + PX4_DEBUG("got logger ack"); + } +} + +int MavlinkULog::handle_update(mavlink_channel_t channel) +{ + static_assert(sizeof(ulog_stream_s::data) == MAVLINK_MSG_LOGGING_DATA_FIELD_DATA_LEN, "Invalid uorb ulog_stream.data length"); + static_assert(sizeof(ulog_stream_s::data) == MAVLINK_MSG_LOGGING_DATA_ACKED_FIELD_DATA_LEN, "Invalid uorb ulog_stream.data length"); + + if (_waiting_for_initial_ack) { + if (hrt_elapsed_time(&_last_sent_time) > 3e5) { + PX4_WARN("no ack from logger (is it running?)"); + return -1; + } + } + + // check if we're waiting for an ACK + if (_last_sent_time) { + bool check_for_updates = false; + if (_ack_received) { + _last_sent_time = 0; + check_for_updates = true; + } else { + + if (hrt_elapsed_time(&_last_sent_time) > ulog_stream_ack_s::ACK_TIMEOUT * 1000) { + if (++_sent_tries > ulog_stream_ack_s::ACK_MAX_TRIES) { + return -ETIMEDOUT; + } else { + PX4_DEBUG("re-sending ulog mavlink message (try=%i)", _sent_tries); + _last_sent_time = hrt_absolute_time(); + mavlink_logging_data_acked_t msg; + msg.sequence = _ulog_data.sequence; + msg.length = _ulog_data.length; + msg.first_message_offset = _ulog_data.first_message_offset; + memcpy(msg.data, _ulog_data.data, sizeof(msg.data)); + mavlink_msg_logging_data_acked_send_struct(channel, &msg); + } + } + } + + if (!check_for_updates) { + return 0; + } + } + + bool updated = false; + int ret = orb_check(_ulog_stream_sub, &updated); + while (updated && !ret) { + orb_copy(ORB_ID(ulog_stream), _ulog_stream_sub, &_ulog_data); + if (_ulog_data.flags & ulog_stream_s::FLAGS_NEED_ACK) { + _sent_tries = 1; + _last_sent_time = hrt_absolute_time(); + lock(); + _wait_for_ack_sequence = _ulog_data.sequence; + _ack_received = false; + unlock(); + + mavlink_logging_data_acked_t msg; + msg.sequence = _ulog_data.sequence; + msg.length = _ulog_data.length; + msg.first_message_offset = _ulog_data.first_message_offset; + memcpy(msg.data, _ulog_data.data, sizeof(msg.data)); + mavlink_msg_logging_data_acked_send_struct(channel, &msg); + + } else { + mavlink_logging_data_t msg; + msg.sequence = _ulog_data.sequence; + msg.length = _ulog_data.length; + msg.first_message_offset = _ulog_data.first_message_offset; + memcpy(msg.data, _ulog_data.data, sizeof(msg.data)); + mavlink_msg_logging_data_send_struct(channel, &msg); + } + ret = orb_check(_ulog_stream_sub, &updated); + } + + return 0; +} + +void MavlinkULog::initialize() +{ + if (_init) { + return; + } + sem_init(&_lock, 1, 1); + _init = true; +} + +MavlinkULog* MavlinkULog::try_start() +{ + MavlinkULog *ret = nullptr; + bool failed = false; + lock(); + if (!_instance) { + ret = _instance = new MavlinkULog(); + if (!_instance) { + failed = true; + } + } + unlock(); + + if (failed) { + PX4_ERR("alloc failed"); + } + return ret; +} + +void MavlinkULog::stop() +{ + lock(); + if (_instance) { + delete _instance; + _instance = nullptr; + } + unlock(); +} + +void MavlinkULog::handle_ack(mavlink_logging_ack_t ack) +{ + lock(); + if (_instance) { // make sure stop() was not called right before + if (_wait_for_ack_sequence == ack.sequence) { + _ack_received = true; + publish_ack(ack.sequence); + } + } + unlock(); +} + +void MavlinkULog::publish_ack(uint16_t sequence) +{ + ulog_stream_ack_s ack; + ack.timestamp = hrt_absolute_time(); + ack.sequence = sequence; + + if (_ulog_stream_ack_pub == nullptr) { + _ulog_stream_ack_pub = orb_advertise_queue(ORB_ID(ulog_stream_ack), &ack, 3); + + } else { + orb_publish(ORB_ID(ulog_stream_ack), _ulog_stream_ack_pub, &ack); + } +} diff --git a/src/modules/mavlink/mavlink_ulog.h b/src/modules/mavlink/mavlink_ulog.h new file mode 100644 index 0000000000..f584f06038 --- /dev/null +++ b/src/modules/mavlink/mavlink_ulog.h @@ -0,0 +1,126 @@ +/**************************************************************************** + * + * Copyright (c) 2016 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_ulog.h + * ULog data streaming via MAVLink + * + * @author Beat Küng + */ + +#pragma once + +#include +#include +#include +#include +#include + +#include +#include + +#include "mavlink_bridge_header.h" + +/** + * @class MavlinkULog + * ULog streaming class. At most one instance (stream) can exist, assigned to a specific mavlink channel. + */ +class MavlinkULog +{ +public: + /** + * initialize: call this once on startup (this function is not thread-safe!) + */ + static void initialize(); + + /** + * try to start a new stream. This fails if a stream is already running. + * thread-safe + * @return instance, or nullptr + */ + static MavlinkULog *try_start(); + + /** + * stop the stream. It also deletes the singleton object, so make sure cleanup + * all pointers to it. + * thread-safe + */ + void stop(); + + /** + * periodic update method: check for ulog stream messages and handle retransmission. + * @return 0 on success, <0 otherwise + */ + int handle_update(mavlink_channel_t channel); + + /** ack from mavlink for a data message */ + void handle_ack(mavlink_logging_ack_t ack); + + /** this is called when we got an vehicle_command_ack from the logger */ + void start_ack_received(); + +private: + + MavlinkULog(); + + ~MavlinkULog(); + + static void lock() + { + do {} while (sem_wait(&_lock) != 0); + } + + static void unlock() + { + sem_post(&_lock); + } + + void publish_ack(uint16_t sequence); + + static sem_t _lock; + static bool _init; + static MavlinkULog *_instance; + + int _ulog_stream_sub = -1; + orb_advert_t _ulog_stream_ack_pub = nullptr; + uint16_t _wait_for_ack_sequence; + uint8_t _sent_tries = 0; + volatile bool _ack_received = false; ///< set to true if a matching ack received + hrt_abstime _last_sent_time = 0; ///< last time we sent a message that requires an ack + ulog_stream_s _ulog_data; + bool _waiting_for_initial_ack = false; + + /* do not allow copying this class */ + MavlinkULog(const MavlinkULog &) = delete; + MavlinkULog operator=(const MavlinkULog &) = delete; +};