From bf1266af11ea7996ad0eba4f9455df73821f687b Mon Sep 17 00:00:00 2001
From: oravla5 <alvaro@auterion.com>
Date: Tue, 23 Apr 2024 16:32:48 +0200
Subject: [PATCH] mavlink: added back gimbal v1 protocol command

---
 src/modules/mavlink/mavlink_main.cpp | 26 +++++++++++++++++++++++++-
 src/modules/mavlink/mavlink_main.h   |  2 +-
 2 files changed, 26 insertions(+), 2 deletions(-)

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index c6ecb1c538..681c911488 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -2525,9 +2525,33 @@ void Mavlink::handleCommands()
 			}
 		}
 	}
+
+	// For legacy gimbals using the mavlink gimbal v1 protocol, we need to send out commands.
+	// We don't care about acks for these though.
+	if (_gimbal_v1_command_sub.updated()) {
+		vehicle_command_s cmd;
+		_gimbal_v1_command_sub.copy(&cmd);
+
+		// FIXME: filter for target system/component
+
+		mavlink_command_long_t msg{};
+		msg.param1 = cmd.param1;
+		msg.param2 = cmd.param2;
+		msg.param3 = cmd.param3;
+		msg.param4 = cmd.param4;
+		msg.param5 = cmd.param5;
+		msg.param6 = cmd.param6;
+		msg.param7 = cmd.param7;
+		msg.command = cmd.command;
+		msg.target_system = cmd.target_system;
+		msg.target_component = cmd.target_component;
+		msg.confirmation = 0;
+
+		mavlink_msg_command_long_send_struct(get_channel(), &msg);
+	}
 }
 
-void Mavlink::handleAndGetCurrentCommandAck(bool &start_ack, bool &stop_ack)
+void Mavlink::handleAndGetCurrentCommandAck(bool &logging_start_ack, bool &logging_stop_ack)
 {
 	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;
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 9d105dcb41..498e083c41 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -709,7 +709,7 @@ private:
 
 	void handleCommands();
 
-	void handleAndGetCurrentCommandAck(bool &start_ack, bool &stop_ack);
+	void handleAndGetCurrentCommandAck(bool &logging_start_ack, bool &logging_stop_ack);
 
 	void handleStatus();