diff --git a/Tools/mavlink_ulog_streaming.py b/Tools/mavlink_ulog_streaming.py index 9ce94b83ce..369303c3f7 100755 --- a/Tools/mavlink_ulog_streaming.py +++ b/Tools/mavlink_ulog_streaming.py @@ -23,7 +23,10 @@ from argparse import ArgumentParser class MavlinkLogStreaming(): - '''Streams log data via MAVLink''' + '''Streams log data via MAVLink. + Assumptions: + - the sender only sends one acked message at a time + - the data is in the ULog format ''' def __init__(self, portname, baudrate, output_filename, debug=0): self.baudrate = 0 self._debug = debug @@ -42,6 +45,7 @@ class MavlinkLogStreaming(): self.last_sequence = -1 self.logging_started = False self.num_dropouts = 0 + self.target_component = 1 def debug(self, s, level=1): '''write some debug text''' @@ -50,13 +54,13 @@ class MavlinkLogStreaming(): def start_log(self): self.mav.mav.command_long_send(self.mav.target_system, - self.mav.target_component, + self.target_component, mavutil.mavlink.MAV_CMD_LOGGING_START, 0, 0, 0, 0, 0, 0, 0, 0) def stop_log(self): self.mav.mav.command_long_send(self.mav.target_system, - self.mav.target_component, + self.target_component, mavutil.mavlink.MAV_CMD_LOGGING_STOP, 0, 0, 0, 0, 0, 0, 0, 0) @@ -112,7 +116,8 @@ class MavlinkLogStreaming(): self.num_dropouts += num_drops if m.get_type() == 'LOGGING_DATA_ACKED': - self.mav.mav.logging_ack_send(m.sequence) + self.mav.mav.logging_ack_send(self.mav.target_system, + self.target_component, m.sequence) else: if not self.got_header_section: print('Header received in {:0.2f}s'.format(timer()-self.start_time)) diff --git a/msg/ulog_stream.msg b/msg/ulog_stream.msg index 0fe647a6ce..446d8c1d70 100644 --- a/msg/ulog_stream.msg +++ b/msg/ulog_stream.msg @@ -12,5 +12,5 @@ uint8 first_message_offset # offset into data where first message starts. This # can be used for recovery, when a previous message got lost uint16 sequence # allows determine drops uint8 flags # see FLAGS_* -uint8[251] data # ulog data +uint8[249] data # ulog data diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 90127ee14d..637d03786d 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -438,9 +438,9 @@ public: /** get ulog streaming if active, nullptr otherwise */ MavlinkULog *get_ulog_streaming() { return _mavlink_ulog; } - void try_start_ulog_streaming() { + void try_start_ulog_streaming(uint8_t target_system, uint8_t target_component) { if (_mavlink_ulog) { return; } - _mavlink_ulog = MavlinkULog::try_start(); + _mavlink_ulog = MavlinkULog::try_start(target_system, target_component); } void request_stop_ulog_streaming() { if (_mavlink_ulog) { _mavlink_ulog_stop_requested = true; } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 02e1048dfe..493842f95e 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -389,7 +389,7 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) // 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(); + _mavlink->try_start_ulog_streaming(msg->sysid, msg->compid); } else if (cmd_mavlink.command == MAV_CMD_LOGGING_STOP) { _mavlink->request_stop_ulog_streaming(); } diff --git a/src/modules/mavlink/mavlink_ulog.cpp b/src/modules/mavlink/mavlink_ulog.cpp index 0157746acc..75ae9902b7 100644 --- a/src/modules/mavlink/mavlink_ulog.cpp +++ b/src/modules/mavlink/mavlink_ulog.cpp @@ -47,7 +47,8 @@ MavlinkULog *MavlinkULog::_instance = nullptr; sem_t MavlinkULog::_lock; -MavlinkULog::MavlinkULog() +MavlinkULog::MavlinkULog(uint8_t target_system, uint8_t target_component) + : _target_system(target_system), _target_component(target_component) { _ulog_stream_sub = orb_subscribe(ORB_ID(ulog_stream)); @@ -107,6 +108,8 @@ int MavlinkULog::handle_update(mavlink_channel_t channel) msg.sequence = _ulog_data.sequence; msg.length = _ulog_data.length; msg.first_message_offset = _ulog_data.first_message_offset; + msg.target_system = _target_system; + msg.target_component = _target_component; memcpy(msg.data, _ulog_data.data, sizeof(msg.data)); mavlink_msg_logging_data_acked_send_struct(channel, &msg); } @@ -134,6 +137,8 @@ int MavlinkULog::handle_update(mavlink_channel_t channel) msg.sequence = _ulog_data.sequence; msg.length = _ulog_data.length; msg.first_message_offset = _ulog_data.first_message_offset; + msg.target_system = _target_system; + msg.target_component = _target_component; memcpy(msg.data, _ulog_data.data, sizeof(msg.data)); mavlink_msg_logging_data_acked_send_struct(channel, &msg); @@ -142,6 +147,8 @@ int MavlinkULog::handle_update(mavlink_channel_t channel) msg.sequence = _ulog_data.sequence; msg.length = _ulog_data.length; msg.first_message_offset = _ulog_data.first_message_offset; + msg.target_system = _target_system; + msg.target_component = _target_component; memcpy(msg.data, _ulog_data.data, sizeof(msg.data)); mavlink_msg_logging_data_send_struct(channel, &msg); } @@ -160,13 +167,13 @@ void MavlinkULog::initialize() _init = true; } -MavlinkULog* MavlinkULog::try_start() +MavlinkULog* MavlinkULog::try_start(uint8_t target_system, uint8_t target_component) { MavlinkULog *ret = nullptr; bool failed = false; lock(); if (!_instance) { - ret = _instance = new MavlinkULog(); + ret = _instance = new MavlinkULog(target_system, target_component); if (!_instance) { failed = true; } diff --git a/src/modules/mavlink/mavlink_ulog.h b/src/modules/mavlink/mavlink_ulog.h index f584f06038..032f7e4796 100644 --- a/src/modules/mavlink/mavlink_ulog.h +++ b/src/modules/mavlink/mavlink_ulog.h @@ -68,7 +68,7 @@ public: * thread-safe * @return instance, or nullptr */ - static MavlinkULog *try_start(); + static MavlinkULog *try_start(uint8_t target_system, uint8_t target_component); /** * stop the stream. It also deletes the singleton object, so make sure cleanup @@ -91,7 +91,7 @@ public: private: - MavlinkULog(); + MavlinkULog(uint8_t target_system, uint8_t target_component); ~MavlinkULog(); @@ -119,6 +119,9 @@ private: 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; + const uint8_t _target_system; + const uint8_t _target_component; + /* do not allow copying this class */ MavlinkULog(const MavlinkULog &) = delete;