mavlink ulog: add target sys & component ids (update to changed mavlink message)

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))

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
 

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; }

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();
 			}

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;
 		}

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;