From e1ac6fe297ce258df76ace6b5a21f87518aa75a0 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 15 Feb 2021 11:09:04 +0100
Subject: [PATCH] logger: send post-flight perf data when stopping mavlink log
 streaming

- send out VEHICLE_CMD_RESULT_IN_PROGRESS
- delete the ulog object when receiving the ACK from logger, instead of
  the stop command
---
 src/modules/logger/logger.cpp            | 17 +++++++++++++++--
 src/modules/mavlink/mavlink_main.cpp     |  8 ++++++--
 src/modules/mavlink/mavlink_main.h       |  6 ------
 src/modules/mavlink/mavlink_receiver.cpp |  3 ---
 4 files changed, 21 insertions(+), 13 deletions(-)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index c1d4906b15..8ba403e41d 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -1124,7 +1124,11 @@ void Logger::handle_vehicle_command_update()
 			}
 
 		} else if (command.command == vehicle_command_s::VEHICLE_CMD_LOGGING_STOP) {
-			stop_log_mavlink();
+			if (_writer.is_started(LogType::Full, LogWriter::BackendMavlink)) {
+				ack_vehicle_command(&command, vehicle_command_s::VEHICLE_CMD_RESULT_IN_PROGRESS);
+				stop_log_mavlink();
+			}
+
 			ack_vehicle_command(&command, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
 		}
 	}
@@ -1388,7 +1392,16 @@ void Logger::stop_log_mavlink()
 {
 	// don't write perf data since a client does not expect more data after a stop command
 	PX4_INFO("Stop mavlink log");
-	_writer.stop_log_mavlink();
+
+	if (_writer.is_started(LogType::Full, LogWriter::BackendMavlink)) {
+		_writer.select_write_backend(LogWriter::BackendMavlink);
+		_writer.set_need_reliable_transfer(true);
+		write_perf_data(false);
+		_writer.set_need_reliable_transfer(false);
+		_writer.unselect_write_backend();
+		_writer.notify();
+		_writer.stop_log_mavlink();
+	}
 }
 
 struct perf_callback_data_t {
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index e382790f27..1beac0c6f8 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -2381,6 +2381,7 @@ Mavlink::task_main(int argc, char *argv[])
 
 		/* send command ACK */
 		bool cmd_logging_start_acknowledgement = false;
+		bool cmd_logging_stop_acknowledgement = false;
 
 		if (_vehicle_command_ack_sub.updated()) {
 			static constexpr size_t COMMAND_ACK_TOTAL_LEN = MAVLINK_MSG_ID_COMMAND_ACK_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
@@ -2412,6 +2413,10 @@ Mavlink::task_main(int argc, char *argv[])
 
 						if (command_ack.command == vehicle_command_s::VEHICLE_CMD_LOGGING_START) {
 							cmd_logging_start_acknowledgement = true;
+
+						} else if (command_ack.command == vehicle_command_s::VEHICLE_CMD_LOGGING_STOP
+							   && command_ack.result == vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED) {
+							cmd_logging_stop_acknowledgement = true;
 						}
 					}
 				}
@@ -2453,10 +2458,9 @@ Mavlink::task_main(int argc, char *argv[])
 
 		/* check for ulog streaming messages */
 		if (_mavlink_ulog) {
-			if (_mavlink_ulog_stop_requested.load()) {
+			if (cmd_logging_stop_acknowledgement) {
 				_mavlink_ulog->stop();
 				_mavlink_ulog = nullptr;
-				_mavlink_ulog_stop_requested.store(false);
 
 			} else {
 				if (cmd_logging_start_acknowledgement) {
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index e22d6b5ce9..402c89d982 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -498,10 +498,6 @@ public:
 
 		_mavlink_ulog = MavlinkULog::try_start(_datarate, 0.7f, target_system, target_component);
 	}
-	void			request_stop_ulog_streaming()
-	{
-		if (_mavlink_ulog) { _mavlink_ulog_stop_requested.store(true); }
-	}
 
 	const events::SendProtocol &get_events_protocol() const { return _events; };
 	bool ftp_enabled() const { return _ftp_on; }
@@ -575,8 +571,6 @@ private:
 	static events::EventBuffer	*_event_buffer;
 	events::SendProtocol		_events{*_event_buffer, *this};
 
-	px4::atomic_bool	_mavlink_ulog_stop_requested{false};
-
 	MAVLINK_MODE 		_mode{MAVLINK_MODE_NORMAL};
 
 	mavlink_channel_t	_channel{MAVLINK_COMM_0};
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 6264dd7797..da97e95de4 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -594,9 +594,6 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
 				// from the logger.
 				_mavlink->try_start_ulog_streaming(msg->sysid, msg->compid);
 			}
-
-		} else if (cmd_mavlink.command == MAV_CMD_LOGGING_STOP) {
-			_mavlink->request_stop_ulog_streaming();
 		}
 
 		if (!send_ack) {