mavlink: integrate MavlinkULog into the main Mavlink task & receiver

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

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;
 

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

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