From b8eed3fc9a5d008c2988cdd37b2395f03f023e19 Mon Sep 17 00:00:00 2001
From: Jukka Laitinen <jukkax@ssrc.tii.ae>
Date: Fri, 16 Aug 2024 09:41:40 +0300
Subject: [PATCH] mavlink: Add handling of esc_status message

This is preparation for moving esc_status sending into gazebo mavlink plugin

Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>
---
 src/modules/mavlink/mavlink_receiver.cpp | 27 ++++++++++++++++++++++++
 src/modules/mavlink/mavlink_receiver.h   |  3 +++
 2 files changed, 30 insertions(+)

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index bf1ef409c019..3bbbeb59a74b 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -317,6 +317,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_actuator_output_status(msg);
 		break;
 
+	case MAVLINK_MSG_ID_ESC_STATUS:
+		handle_message_esc_status(msg);
+		break;
+
 	default:
 		break;
 	}
@@ -3141,6 +3145,29 @@ MavlinkReceiver::handle_message_actuator_output_status(mavlink_message_t *msg)
 	}
 }
 
+void
+MavlinkReceiver::handle_message_esc_status(mavlink_message_t *msg)
+{
+	static constexpr int batch_size = MAVLINK_MSG_ESC_STATUS_FIELD_RPM_LEN;
+	mavlink_esc_status_t esc_status_msg;
+	mavlink_msg_esc_status_decode(msg, &esc_status_msg);
+	esc_status_s esc_status{};
+
+	esc_status.timestamp = hrt_absolute_time();
+
+	/* Status is sent in batches, esc_idx is the actual esc index, idx is the index within the received batch */
+
+	size_t esc_idx = esc_status_msg.index;
+
+	for (int idx = 0; idx < batch_size && esc_idx < sizeof(esc_status.esc) / sizeof(esc_status.esc[0]); idx++, esc_idx++) {
+		esc_status.esc[esc_idx].esc_rpm = esc_status_msg.rpm[idx];
+		esc_status.esc[esc_idx].esc_voltage = esc_status_msg.voltage[idx];
+		esc_status.esc[esc_idx].esc_current = esc_status_msg.current[idx];
+	}
+
+	_esc_status_pub.publish(esc_status);
+}
+
 void
 MavlinkReceiver::run()
 {
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 86d039036964..91379552c9f7 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -70,6 +70,7 @@
 #include <uORB/topics/collision_report.h>
 #include <uORB/topics/differential_pressure.h>
 #include <uORB/topics/distance_sensor.h>
+#include <uORB/topics/esc_status.h>
 #include <uORB/topics/follow_target.h>
 #include <uORB/topics/generator_status.h>
 #include <uORB/topics/gimbal_manager_set_attitude.h>
@@ -204,6 +205,7 @@ class MavlinkReceiver : public ModuleParams
 	void handle_message_gimbal_device_information(mavlink_message_t *msg);
 	void handle_message_gimbal_device_attitude_status(mavlink_message_t *msg);
 	void handle_message_actuator_output_status(mavlink_message_t *msg);
+	void handle_message_esc_status(mavlink_message_t *msg);
 
 #if !defined(CONSTRAINED_FLASH)
 	void handle_message_debug(mavlink_message_t *msg);
@@ -327,6 +329,7 @@ class MavlinkReceiver : public ModuleParams
 	uORB::Publication<vehicle_status_s>			_redundant_status_pub[vehicle_status_s::MAX_REDUNDANT_CONTROLLERS] {ORB_ID(redundant_status0), ORB_ID(redundant_status1)};
 	vehicle_status_s					_redundant_status[vehicle_status_s::MAX_REDUNDANT_CONTROLLERS] {};
 	uORB::Publication<actuator_outputs_s>			_redundant_actuator_outputs_pub[vehicle_status_s::MAX_REDUNDANT_CONTROLLERS] {ORB_ID(redundant_actuator_outputs0), ORB_ID(redundant_actuator_outputs1)};
+	uORB::Publication<esc_status_s>				_esc_status_pub{ORB_ID(esc_status)};
 
 #if !defined(CONSTRAINED_FLASH)
 	uORB::Publication<debug_array_s>			_debug_array_pub {ORB_ID(debug_array)};