voxl_esc: Added Mavlink tunnel UART pass-through mechanism

PX4 · Feb 26, 2025 · 1c2dd88 · 1c2dd88
1 parent 57bb4bd
commit 1c2dd88
Show file tree

Hide file tree

Showing 5 changed files with 26 additions and 1 deletion.
diff --git a/msg/MavlinkTunnel.msg b/msg/MavlinkTunnel.msg
@@ -18,3 +18,6 @@ uint8 target_system      # System ID (can be 0 for broadcast, but this is discou
 uint8 target_component   # Component ID (can be 0 for broadcast, but this is discouraged)
 uint8 payload_length     # Length of the data transported in payload
 uint8[128] payload       # Data itself
+
+# Topic aliases for known payload types
+# TOPICS mavlink_tunnel esc_serial_passthru
diff --git a/src/drivers/actuators/voxl_esc/voxl_esc.cpp b/src/drivers/actuators/voxl_esc/voxl_esc.cpp
@@ -1334,6 +1334,16 @@ bool VoxlEsc::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
 
 	_esc_status_pub.publish(_esc_status);
 
+	while (_esc_serial_passthru_sub.updated()) {
+		mavlink_tunnel_s uart_passthru{};
+		_esc_serial_passthru_sub.copy(&uart_passthru);
+
+		if (_uart_port.write(uart_passthru.payload, uart_passthru.payload_length) != uart_passthru.payload_length) {
+			PX4_ERR("Failed to send mavlink tunnel data to esc");
+			return false;
+		}
+	}
+
 	perf_count(_output_update_perf);
 
 	return true;

diff --git a/src/drivers/actuators/voxl_esc/voxl_esc.hpp b/src/drivers/actuators/voxl_esc/voxl_esc.hpp
@@ -49,6 +49,7 @@
 #include <uORB/topics/led_control.h>
 #include <uORB/topics/esc_status.h>
 #include <uORB/topics/actuator_test.h>
+#include <uORB/topics/mavlink_tunnel.h>
 
 #include <px4_platform_common/Serial.hpp>
 
@@ -212,6 +213,7 @@ class VoxlEsc : public ModuleBase<VoxlEsc>, public OutputModuleInterface
 	uORB::Subscription	_parameter_update_sub{ORB_ID(parameter_update)};
 	uORB::Subscription	_actuator_test_sub{ORB_ID(actuator_test)};
 	uORB::Subscription	_led_update_sub{ORB_ID(led_control)};
+	uORB::Subscription	_esc_serial_passthru_sub{ORB_ID(esc_serial_passthru)};
 
 	uORB::Publication<actuator_outputs_s> _outputs_debug_pub{ORB_ID(actuator_outputs_debug)};
 	uORB::Publication<esc_status_s> _esc_status_pub{ORB_ID(esc_status)};

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
@@ -1976,7 +1976,16 @@ MavlinkReceiver::handle_message_tunnel(mavlink_message_t *msg)
 	memcpy(tunnel.payload, mavlink_tunnel.payload, sizeof(tunnel.payload));
 	static_assert(sizeof(tunnel.payload) == sizeof(mavlink_tunnel.payload), "mavlink_tunnel.payload size mismatch");
 
-	_mavlink_tunnel_pub.publish(tunnel);
+	switch (mavlink_tunnel.payload_type) {
+	case MAV_TUNNEL_PAYLOAD_TYPE_MODALAI_ESC_UART_PASSTHRU:
+		_esc_serial_passthru_pub.publish(tunnel);
+		break;
+
+	default:
+		_mavlink_tunnel_pub.publish(tunnel);
+		break;
+	}
+
 
 }
 

diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
@@ -307,6 +307,7 @@ class MavlinkReceiver : public ModuleParams
 	uORB::Publication<landing_target_pose_s>		_landing_target_pose_pub{ORB_ID(landing_target_pose)};
 	uORB::Publication<log_message_s>			_log_message_pub{ORB_ID(log_message)};
 	uORB::Publication<mavlink_tunnel_s>			_mavlink_tunnel_pub{ORB_ID(mavlink_tunnel)};
+	uORB::Publication<mavlink_tunnel_s>			_esc_serial_passthru_pub{ORB_ID(esc_serial_passthru)};
 	uORB::Publication<obstacle_distance_s>			_obstacle_distance_pub{ORB_ID(obstacle_distance)};
 	uORB::Publication<offboard_control_mode_s>		_offboard_control_mode_pub{ORB_ID(offboard_control_mode)};
 	uORB::Publication<onboard_computer_status_s>		_onboard_computer_status_pub{ORB_ID(onboard_computer_status)};