logger: add support for mavlink backend in Logger class, handle start/stop

msg/vehicle_command.msg

@@ -61,6 +61,8 @@ uint32 VEHICLE_CMD_DO_VTOL_TRANSITION = 3000    # Command VTOL transition
 uint32 VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY = 30001	# Prepare a payload deployment in the flight plan
 uint32 VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002	# Control a pre-programmed payload deployment
 uint32 VEHICLE_CMD_PREFLIGHT_UAVCAN = 243		# UAVCAN configuration. If param 1 == 1 actuator mapping and direction assignment should be started
+uint32 VEHICLE_CMD_LOGGING_START = 2510		# start streaming ULog data
+uint32 VEHICLE_CMD_LOGGING_STOP = 2511			# stop streaming ULog data
 
 uint32 VEHICLE_CMD_RESULT_ACCEPTED = 0			# Command ACCEPTED and EXECUTED |
 uint32 VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED = 1	# Command TEMPORARY REJECTED/DENIED |

src/modules/commander/commander.cpp

@@ -1122,6 +1122,8 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 	case vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED:
 	case vehicle_command_s::VEHICLE_CMD_DO_GO_AROUND:
 	case vehicle_command_s::VEHICLE_CMD_START_RX_PAIR:
+	case vehicle_command_s::VEHICLE_CMD_LOGGING_START:
+	case vehicle_command_s::VEHICLE_CMD_LOGGING_STOP:
 		/* ignore commands that handled in low prio loop */
 		break;
 

src/modules/logger/logger.cpp

@@ -47,6 +47,8 @@
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_command_ack.h>
 
 #include <drivers/drv_hrt.h>
 #include <px4_includes.h>
@@ -655,6 +657,13 @@ void Logger::run()
 		add_default_topics();
 	}
 
+	int vehicle_command_sub = -1;
+	orb_advert_t vehicle_command_ack_pub = nullptr;
+
+	if (_writer.backend() & LogWriter::BackendMavlink) {
+		vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));
+	}
+
 	//all topics added. Get required message buffer size
 	int max_msg_size = 0;
 
@@ -738,6 +747,39 @@ void Logger::run()
 			}
 		}
 
+		/* check for logging command from MAVLink */
+		if (vehicle_command_sub != -1) {
+			bool command_updated = false;
+			int ret = orb_check(vehicle_command_sub, &command_updated);
+
+			if (ret == 0 && command_updated) {
+				vehicle_command_s command;
+				orb_copy(ORB_ID(vehicle_command), vehicle_command_sub, &command);
+
+				if (command.command == vehicle_command_s::VEHICLE_CMD_LOGGING_START) {
+					if ((int)(command.param1 + 0.5f) != 0) {
+						ack_vehicle_command(vehicle_command_ack_pub, command.command,
+								    vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED);
+
+					} else if (can_start_mavlink_log()) {
+						ack_vehicle_command(vehicle_command_ack_pub, command.command,
+								    vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
+						start_log_mavlink();
+
+					} else {
+						ack_vehicle_command(vehicle_command_ack_pub, command.command,
+								    vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
+					}
+
+				} else if (command.command == vehicle_command_s::VEHICLE_CMD_LOGGING_STOP) {
+					stop_log_mavlink();
+					ack_vehicle_command(vehicle_command_ack_pub, command.command,
+							    vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
+				}
+			}
+		}
+
+
 		if (_writer.is_started()) {
 
 			bool data_written = false;
@@ -871,6 +913,14 @@ void Logger::run()
 		orb_unadvertise(_mavlink_log_pub);
 		_mavlink_log_pub = nullptr;
 	}
+
+	if (vehicle_command_ack_pub) {
+		orb_unadvertise(vehicle_command_ack_pub);
+	}
+
+	if (vehicle_command_sub != -1) {
+		orb_unsubscribe(vehicle_command_sub);
+	}
 }
 
 bool Logger::write_message(void *ptr, size_t size)
@@ -1101,6 +1151,7 @@ void Logger::start_log_file()
 	mavlink_log_info(&_mavlink_log_pub, "[logger] file: %s", file_name);
 
 	_writer.start_log_file(file_name);
+	_writer.select_write_backend(LogWriter::BackendFile);
 	_writer.set_need_reliable_transfer(true);
 	write_header();
 	write_version();
@@ -1108,6 +1159,7 @@ void Logger::start_log_file()
 	write_parameters();
 	write_all_add_logged_msg();
 	_writer.set_need_reliable_transfer(false);
+	_writer.unselect_write_backend();
 	_writer.notify();
 
 	_start_time_file = hrt_absolute_time();
@@ -1118,6 +1170,33 @@ void Logger::stop_log_file()
 	_writer.stop_log_file();
 }
 
+void Logger::start_log_mavlink()
+{
+	if (!can_start_mavlink_log()) {
+		return;
+	}
+
+	PX4_INFO("Start mavlink log");
+
+	_writer.start_log_mavlink();
+	_writer.select_write_backend(LogWriter::BackendMavlink);
+	_writer.set_need_reliable_transfer(true);
+	write_header();
+	write_version();
+	write_formats();
+	write_parameters();
+	write_all_add_logged_msg();
+	_writer.set_need_reliable_transfer(false);
+	_writer.unselect_write_backend();
+	_writer.notify();
+}
+
+void Logger::stop_log_mavlink()
+{
+	PX4_INFO("Stop mavlink log");
+	_writer.stop_log_mavlink();
+}
+
 void Logger::write_formats()
 {
 	_writer.lock();
@@ -1393,5 +1472,22 @@ int Logger::check_free_space()
 	return PX4_OK;
 }
 
+void Logger::ack_vehicle_command(orb_advert_t &vehicle_command_ack_pub, uint16_t command, uint32_t result)
+{
+	vehicle_command_ack_s vehicle_command_ack;
+	vehicle_command_ack.timestamp = hrt_absolute_time();
+	vehicle_command_ack.command = command;
+	vehicle_command_ack.result = result;
+
+	if (vehicle_command_ack_pub == nullptr) {
+		vehicle_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &vehicle_command_ack,
+					  vehicle_command_ack_s::ORB_QUEUE_LENGTH);
+
+	} else {
+		orb_publish(ORB_ID(vehicle_command_ack), vehicle_command_ack_pub, &vehicle_command_ack);
+	}
+
+}
+
 }
 }

src/modules/logger/logger.h

@@ -160,6 +160,15 @@ private:
 
 	void stop_log_file();
 
+	void start_log_mavlink();
+
+	void stop_log_mavlink();
+
+	/** check if mavlink logging can be started */
+	bool can_start_mavlink_log() const
+	{
+		return !_writer.is_started(LogWriter::BackendMavlink) && (_writer.backend() & LogWriter::BackendMavlink) != 0;
+	}
 
 	/** get the configured backend as string */
 	const char *configured_backend_mode() const;
@@ -206,6 +215,8 @@ private:
 
 	void add_default_topics();
 
+	void ack_vehicle_command(orb_advert_t &vehicle_command_ack_pub, uint16_t command, uint32_t result);
+
 	static constexpr size_t 	MAX_TOPICS_NUM = 64; /**< Maximum number of logged topics */
 	static constexpr unsigned	MAX_NO_LOGFOLDER = 999;	/**< Maximum number of log dirs */
 	static constexpr unsigned	MAX_NO_LOGFILE = 999;	/**< Maximum number of log files */