From 9e6dcb1f60d052452e7ce2c00792111314ae1efc Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 29 Apr 2024 10:33:11 +0200
Subject: [PATCH] fix mavlink: cmd_logging_{start,stop}_acknowledgement flags
 were not reset

Regression from https://github.com/PX4/PX4-Autopilot/pull/23043

Also avoids a race condition by making sure the command ack is handled
before sending out the mavlink message (in case an external component
reacts immediately to the mavlink message).
---
 src/modules/mavlink/mavlink_main.cpp | 52 ++++++++++++----------------
 src/modules/mavlink/mavlink_main.h   |  2 +-
 2 files changed, 24 insertions(+), 30 deletions(-)

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 727360df8ff5..722c1254f65c 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -2255,9 +2255,6 @@ Mavlink::task_main(int argc, char *argv[])
 
 	_task_running.store(true);
 
-	bool cmd_logging_start_acknowledgement = false;
-	bool cmd_logging_stop_acknowledgement = false;
-
 	while (!should_exit()) {
 		/* main loop */
 		px4_usleep(_main_loop_delay);
@@ -2266,7 +2263,7 @@ Mavlink::task_main(int argc, char *argv[])
 			check_requested_subscriptions();
 			handleStatus();
 			handleCommands();
-			handleAndGetCurrentCommandAck(cmd_logging_start_acknowledgement, cmd_logging_stop_acknowledgement);
+			handleAndGetCurrentCommandAck();
 			continue;
 		}
 
@@ -2291,7 +2288,7 @@ Mavlink::task_main(int argc, char *argv[])
 
 		handleStatus();
 		handleCommands();
-		handleAndGetCurrentCommandAck(cmd_logging_start_acknowledgement, cmd_logging_stop_acknowledgement);
+		handleAndGetCurrentCommandAck();
 
 		/* check for shell output */
 		if (_mavlink_shell && _mavlink_shell->available() > 0) {
@@ -2330,25 +2327,15 @@ Mavlink::task_main(int argc, char *argv[])
 
 		/* check for ulog streaming messages */
 		if (_mavlink_ulog) {
-			if (cmd_logging_stop_acknowledgement) {
-				_mavlink_ulog->stop();
-				_mavlink_ulog = nullptr;
+			const int ret = _mavlink_ulog->handle_update(get_channel());
 
-			} else {
-				if (cmd_logging_start_acknowledgement) {
-					_mavlink_ulog->start_ack_received();
+			if (ret < 0) { // abort the streaming on error
+				if (ret != -1) {
+					PX4_WARN("mavlink ulog stream update failed, stopping (%i)", ret);
 				}
 
-				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;
-				}
+				_mavlink_ulog->stop();
+				_mavlink_ulog = nullptr;
 			}
 		}
 
@@ -2571,7 +2558,7 @@ void Mavlink::handleCommands()
 	}
 }
 
-void Mavlink::handleAndGetCurrentCommandAck(bool &logging_start_ack, bool &logging_stop_ack)
+void Mavlink::handleAndGetCurrentCommandAck()
 {
 	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;
@@ -2601,6 +2588,20 @@ void Mavlink::handleAndGetCurrentCommandAck(bool &logging_start_ack, bool &loggi
 					msg.target_system = command_ack.target_system;
 					msg.target_component = command_ack.target_component;
 
+					// Handle logging acks before sending out the mavlink message to prevent a race condition
+					if (command_ack.command == vehicle_command_s::VEHICLE_CMD_LOGGING_START) {
+						if (_mavlink_ulog) {
+							_mavlink_ulog->start_ack_received();
+						}
+
+					} else if (command_ack.command == vehicle_command_s::VEHICLE_CMD_LOGGING_STOP
+						   && command_ack.result == vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED) {
+						if (_mavlink_ulog) {
+							_mavlink_ulog->stop();
+							_mavlink_ulog = nullptr;
+						}
+					}
+
 					if (_mode == MAVLINK_MODE_IRIDIUM) {
 						if (command_ack.from_external) {
 							// for MAVLINK_MODE_IRIDIUM send only if external
@@ -2611,13 +2612,6 @@ void Mavlink::handleAndGetCurrentCommandAck(bool &logging_start_ack, bool &loggi
 						mavlink_msg_command_ack_send_struct(get_channel(), &msg);
 					}
 
-					if (command_ack.command == vehicle_command_s::VEHICLE_CMD_LOGGING_START) {
-						logging_start_ack = true;
-
-					} else if (command_ack.command == vehicle_command_s::VEHICLE_CMD_LOGGING_STOP
-						   && command_ack.result == vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED) {
-						logging_stop_ack = true;
-					}
 				}
 			}
 		}
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 5bf47913274b..9444b8058a48 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -710,7 +710,7 @@ class Mavlink final : public ModuleParams
 
 	void handleCommands();
 
-	void handleAndGetCurrentCommandAck(bool &logging_start_ack, bool &logging_stop_ack);
+	void handleAndGetCurrentCommandAck();
 
 	void handleStatus();