From 7d72f31a295ad0dde3600581110326e87924cab2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Wed, 12 Oct 2016 16:52:28 +0200 Subject: [PATCH] mavlink: integrate MavlinkULog into the main Mavlink task & receiver --- src/modules/mavlink/mavlink_main.cpp | 27 ++++++++++++++++++++++++ src/modules/mavlink/mavlink_main.h | 13 ++++++++++++ src/modules/mavlink/mavlink_receiver.cpp | 26 +++++++++++++++++++++++ src/modules/mavlink/mavlink_receiver.h | 1 + 4 files changed, 67 insertions(+) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index c136a209ba..b1499c612a 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -187,6 +187,8 @@ Mavlink::Mavlink() : _mavlink_ftp(nullptr), _mavlink_log_handler(nullptr), _mavlink_shell(nullptr), + _mavlink_ulog(nullptr), + _mavlink_ulog_stop_requested(false), _mode(MAVLINK_MODE_NORMAL), _channel(MAVLINK_COMM_0), _radio_id(0), @@ -2150,10 +2152,12 @@ Mavlink::task_main(int argc, char *argv[]) } /* send command ACK */ + uint16_t current_command_ack = 0; if (ack_sub->update(&ack_time, &command_ack)) { mavlink_command_ack_t msg; msg.result = command_ack.result; msg.command = command_ack.command; + current_command_ack = command_ack.command; mavlink_msg_command_ack_send_struct(get_channel(), &msg); } @@ -2175,6 +2179,27 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_msg_serial_control_send_struct(get_channel(), &msg); } + /* check for ulog streaming messages */ + if (_mavlink_ulog) { + if (_mavlink_ulog_stop_requested) { + _mavlink_ulog->stop(); + _mavlink_ulog = nullptr; + _mavlink_ulog_stop_requested = false; + } else { + if (current_command_ack == vehicle_command_s::VEHICLE_CMD_LOGGING_START) { + _mavlink_ulog->start_ack_received(); + } + int ret = _mavlink_ulog->handle_update(get_channel()); + if (ret < 0) { //abort the streaming on error + if (ret != -1) { + PX4_WARN("mavlink ulog stream update failed, stopping (%i)", ret); + } + _mavlink_ulog->stop(); + _mavlink_ulog = nullptr; + } + } + } + /* check for requested subscriptions */ if (_subscribe_to_stream != nullptr) { if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) { @@ -2360,6 +2385,8 @@ int Mavlink::start_helper(int argc, char *argv[]) int Mavlink::start(int argc, char *argv[]) { + MavlinkULog::initialize(); + // Wait for the instance count to go up one // before returning to the shell int ic = Mavlink::instance_count(); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index c69286ac9c..90127ee14d 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -70,6 +70,7 @@ #include "mavlink_ftp.h" #include "mavlink_log_handler.h" #include "mavlink_shell.h" +#include "mavlink_ulog.h" enum Protocol { SERIAL = 0, @@ -435,6 +436,16 @@ public: /** close the Mavlink shell if it is open */ void close_shell(); + /** get ulog streaming if active, nullptr otherwise */ + MavlinkULog *get_ulog_streaming() { return _mavlink_ulog; } + void try_start_ulog_streaming() { + if (_mavlink_ulog) { return; } + _mavlink_ulog = MavlinkULog::try_start(); + } + void request_stop_ulog_streaming() { + if (_mavlink_ulog) { _mavlink_ulog_stop_requested = true; } + } + protected: Mavlink *next; @@ -467,6 +478,8 @@ private: MavlinkFTP *_mavlink_ftp; MavlinkLogHandler *_mavlink_log_handler; MavlinkShell *_mavlink_shell; + MavlinkULog *_mavlink_ulog; + volatile bool _mavlink_ulog_stop_requested; MAVLINK_MODE _mode; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index d9fc66e7fa..02e1048dfe 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -266,6 +266,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_serial_control(msg); break; + case MAVLINK_MSG_ID_LOGGING_ACK: + handle_message_logging_ack(msg); + break; + default: break; } @@ -380,6 +384,16 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) return; } + if (cmd_mavlink.command == MAV_CMD_LOGGING_START) { + // we already instanciate the streaming object, because at this point we know on which + // mavlink channel streaming was requested. But in fact it's possible that the logger is + // not even running. The main mavlink thread takes care of this by waiting for an ack + // from the logger. + _mavlink->try_start_ulog_streaming(); + } else if (cmd_mavlink.command == MAV_CMD_LOGGING_STOP) { + _mavlink->request_stop_ulog_streaming(); + } + struct vehicle_command_s vcmd; memset(&vcmd, 0, sizeof(vcmd)); @@ -1220,6 +1234,18 @@ MavlinkReceiver::handle_message_serial_control(mavlink_message_t *msg) } } +void +MavlinkReceiver::handle_message_logging_ack(mavlink_message_t *msg) +{ + mavlink_logging_ack_t logging_ack; + mavlink_msg_logging_ack_decode(msg, &logging_ack); + + MavlinkULog *ulog_streaming = _mavlink->get_ulog_streaming(); + if (ulog_streaming) { + ulog_streaming->handle_ack(logging_ack); + } +} + switch_pos_t MavlinkReceiver::decode_switch_pos(uint16_t buttons, unsigned sw) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 7cc293a05c..da006c3ce7 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -144,6 +144,7 @@ private: void handle_message_gps_rtcm_data(mavlink_message_t *msg); void handle_message_battery_status(mavlink_message_t *msg); void handle_message_serial_control(mavlink_message_t *msg); + void handle_message_logging_ack(mavlink_message_t *msg); void *receive_thread(void *arg);