From eb43d2173053849a4c35ec2270f092fc424d3bb6 Mon Sep 17 00:00:00 2001
From: Eric Katzfey <eric.katzfey@modalai.com>
Date: Tue, 4 Mar 2025 13:52:52 -0800
Subject: [PATCH] voxl2_io: Added UART passthru

---
 msg/MavlinkTunnel.msg                    |  2 +-
 src/drivers/voxl2_io/voxl2_io.cpp        | 32 ++++++++++++++++++++++++
 src/drivers/voxl2_io/voxl2_io.hpp        |  4 +++
 src/modules/mavlink/mavlink_receiver.cpp |  4 +++
 src/modules/mavlink/mavlink_receiver.h   |  1 +
 5 files changed, 42 insertions(+), 1 deletion(-)

diff --git a/msg/MavlinkTunnel.msg b/msg/MavlinkTunnel.msg
index 140bca5a1b..bc9949f139 100644
--- a/msg/MavlinkTunnel.msg
+++ b/msg/MavlinkTunnel.msg
@@ -20,4 +20,4 @@ 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
+# TOPICS mavlink_tunnel esc_serial_passthru io_serial_passthru
diff --git a/src/drivers/voxl2_io/voxl2_io.cpp b/src/drivers/voxl2_io/voxl2_io.cpp
index 66551e468a..07b29ce7d4 100644
--- a/src/drivers/voxl2_io/voxl2_io.cpp
+++ b/src/drivers/voxl2_io/voxl2_io.cpp
@@ -276,6 +276,31 @@ int Voxl2IO::get_version_info()
 	return (got_response == true ? 0 : -1);
 }
 
+int Voxl2IO::handle_uart_passthru()
+{
+	int num_writes = 0;
+
+	// Don't do these faster than 20Hz
+	if (hrt_elapsed_time(&_last_uart_passthru) > 50_ms) {
+		_last_uart_passthru = hrt_absolute_time();
+
+		// Don't do more than a few writes each check
+		while (_io_serial_passthru_sub.updated() && (num_writes < 4)) {
+			mavlink_tunnel_s uart_passthru{};
+			_io_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");
+				return false;
+			}
+
+			num_writes++;
+		}
+	}
+
+	return num_writes;
+}
+
 bool Voxl2IO::updateOutputs(uint16_t outputs[input_rc_s::RC_INPUT_MAX_CHANNELS],
 			    unsigned num_outputs, unsigned num_control_groups_updated)
 {
@@ -561,6 +586,13 @@ void Voxl2IO::Run()
 
 	/* check at end of cycle (updateSubscriptions() can potentially change to a different WorkQueue thread) */
 	_mixing_output.updateSubscriptions(true);
+
+	int num_writes = handle_uart_passthru();
+
+	if (_debug && num_writes) {
+		PX4_INFO("UART Passthru wrote %d packets", num_writes);
+	}
+
 	perf_end(_cycle_perf);
 }
 
diff --git a/src/drivers/voxl2_io/voxl2_io.hpp b/src/drivers/voxl2_io/voxl2_io.hpp
index 31f9cac9da..1f9c6c2eaa 100644
--- a/src/drivers/voxl2_io/voxl2_io.hpp
+++ b/src/drivers/voxl2_io/voxl2_io.hpp
@@ -55,6 +55,7 @@
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/input_rc.h>
+#include <uORB/topics/mavlink_tunnel.h>
 
 #include "voxl2_io_packet.h"
 #include "voxl2_io_packet_types.h"
@@ -188,9 +189,11 @@ private:
 
 	/* Subscriptions */
 	uORB::Subscription 	_parameter_update_sub{ORB_ID(parameter_update)};
+	uORB::Subscription	_io_serial_passthru_sub{ORB_ID(io_serial_passthru)};
 
 	bool		_pwm_on{false};
 	bool		_outputs_disabled{false};
+	hrt_abstime	_last_uart_passthru{0};
 
 	perf_counter_t		_cycle_perf;
 	perf_counter_t		_output_update_perf;
@@ -208,4 +211,5 @@ private:
 	int update_params();
 	int calibrate_escs();
 	std::string board_id_to_name(int board_id);
+	int handle_uart_passthru();
 };
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 6143c0a0dd..d34074cba2 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -1989,6 +1989,10 @@ MavlinkReceiver::handle_message_tunnel(mavlink_message_t *msg)
 		_esc_serial_passthru_pub.publish(tunnel);
 		break;
 
+	case MAV_TUNNEL_PAYLOAD_TYPE_MODALAI_IO_UART_PASSTHRU:
+		_io_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
index d1c477e1c7..5023c7a7df 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -310,6 +310,7 @@ private:
 	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<mavlink_tunnel_s>			_io_serial_passthru_pub{ORB_ID(io_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)};