mavlink: Add handling of esc_status message

This is preparation for moving esc_status sending into gazebo mavlink plugin Signed-off-by: Jukka Laitinen <[email protected]>
tiiuae · Aug 20, 2024 · 2b38774 · 2b38774
1 parent 466fcc5
commit 2b38774
Show file tree

Hide file tree

Showing 2 changed files with 30 additions and 0 deletions.
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
@@ -313,6 +313,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_gimbal_device_attitude_status(msg);
 		break;
 
+	case MAVLINK_MSG_ID_ESC_STATUS:
+		handle_message_esc_status(msg);
+		break;
+
 	default:
 		break;
 	}
@@ -3114,6 +3118,29 @@ MavlinkReceiver::handle_message_gimbal_device_attitude_status(mavlink_message_t
 	_gimbal_device_attitude_status_pub.publish(gimbal_attitude_status);
 }
 
+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
@@ -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>
@@ -203,6 +204,7 @@ class MavlinkReceiver : public ModuleParams
 	void handle_message_gimbal_manager_set_manual_control(mavlink_message_t *msg);
 	void handle_message_gimbal_device_information(mavlink_message_t *msg);
 	void handle_message_gimbal_device_attitude_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);
@@ -325,6 +327,7 @@ class MavlinkReceiver : public ModuleParams
 	uORB::Publication<vehicle_trajectory_waypoint_s>	_trajectory_waypoint_pub{ORB_ID(vehicle_trajectory_waypoint)};
 	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<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)};