From 61dabca4c87df97a3f27cc63961b869970aba0bd Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Fri, 1 Oct 2021 09:59:02 +0200
Subject: [PATCH] mavlink_receiver: Add mavlink handler with progress report

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

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 7b8cc0f3d2..f539b122d4 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -97,7 +97,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 }
 
 void
-MavlinkReceiver::acknowledge(uint8_t sysid, uint8_t compid, uint16_t command, uint8_t result)
+MavlinkReceiver::acknowledge(uint8_t sysid, uint8_t compid, uint16_t command, uint8_t result, uint8_t progress)
 {
 	vehicle_command_ack_s command_ack{};
 
@@ -106,6 +106,7 @@ MavlinkReceiver::acknowledge(uint8_t sysid, uint8_t compid, uint16_t command, ui
 	command_ack.result = result;
 	command_ack.target_system = sysid;
 	command_ack.target_component = compid;
+	command_ack.result_param1 = progress;
 
 	_cmd_ack_pub.publish(command_ack);
 }
@@ -469,6 +470,7 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
 	bool target_ok = evaluate_target_ok(cmd_mavlink.command, cmd_mavlink.target_system, cmd_mavlink.target_component);
 	bool send_ack = true;
 	uint8_t result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
+	uint8_t progress = 0; // TODO: should be 255, 0 for backwards compatibility
 
 	if (!target_ok) {
 		// Reject alien commands only if there is no forwarding or we've never seen target component before
@@ -630,48 +632,48 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
 			switch (status.state) {
 			case autotune_attitude_control_status_s::STATE_IDLE:
 			case autotune_attitude_control_status_s::STATE_INIT:
-				/* progress = 0; */
+				progress = 0;
 				break;
 
 			case autotune_attitude_control_status_s::STATE_ROLL:
 			case autotune_attitude_control_status_s::STATE_ROLL_PAUSE:
-				/* progress = 20; */
+				progress = 20;
 				break;
 
 			case autotune_attitude_control_status_s::STATE_PITCH:
 			case autotune_attitude_control_status_s::STATE_PITCH_PAUSE:
-				/* progress = 40; */
+				progress = 40;
 				break;
 
 			case autotune_attitude_control_status_s::STATE_YAW:
 			case autotune_attitude_control_status_s::STATE_YAW_PAUSE:
-				/* progress = 60; */
+				progress = 60;
 				break;
 
 			case autotune_attitude_control_status_s::STATE_VERIFICATION:
-				/* progress = 80; */
+				progress = 80;
 				break;
 
 			case autotune_attitude_control_status_s::STATE_APPLY:
-				/* progress = 85; */
+				progress = 85;
 				break;
 
 			case autotune_attitude_control_status_s::STATE_TEST:
-				/* progress = 90; */
+				progress = 90;
 				break;
 
 			case autotune_attitude_control_status_s::STATE_WAIT_FOR_DISARM:
-				/* progress = 95; */
+				progress = 95;
 				break;
 
 			case autotune_attitude_control_status_s::STATE_COMPLETE:
-				/* progress = 100; */
+				progress = 100;
 				// ack it properly with an ACCEPTED once we're done
 				result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
 				break;
 
 			case autotune_attitude_control_status_s::STATE_FAIL:
-				/* progress = 0; */
+				progress = 0;
 				result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
 				break;
 			}
@@ -716,7 +718,7 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
 	}
 
 	if (send_ack) {
-		acknowledge(msg->sysid, msg->compid, cmd_mavlink.command, result);
+		acknowledge(msg->sysid, msg->compid, cmd_mavlink.command, result, progress);
 	}
 }
 
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 46d6399ca9..8b30e1cd72 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -135,7 +135,7 @@ private:
 	static void *start_trampoline(void *context);
 	void run();
 
-	void acknowledge(uint8_t sysid, uint8_t compid, uint16_t command, uint8_t result);
+	void acknowledge(uint8_t sysid, uint8_t compid, uint16_t command, uint8_t result, uint8_t progress = 0);
 
 	/**
 	 * Common method to handle both mavlink command types. T is one of mavlink_command_int_t or mavlink_command_long_t.