diff --git a/msg/MavlinkTunnel.msg b/msg/MavlinkTunnel.msg index 16934a9522..140bca5a1b 100644 --- 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 index 740211c4e8..67aa579eda 100644 --- a/src/drivers/actuators/voxl_esc/voxl_esc.cpp +++ b/src/drivers/actuators/voxl_esc/voxl_esc.cpp @@ -1334,6 +1334,20 @@ bool VoxlEsc::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], _esc_status_pub.publish(_esc_status); + uint8_t num_writes = 0; + + while (_esc_serial_passthru_sub.updated() && (num_writes < 4)) { + 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; + } + + num_writes++; + } + 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 index 0fe6f9fd42..c28d1e1ab9 100644 --- a/src/drivers/actuators/voxl_esc/voxl_esc.hpp +++ b/src/drivers/actuators/voxl_esc/voxl_esc.hpp @@ -49,6 +49,7 @@ #include #include #include +#include #include @@ -212,6 +213,7 @@ private: 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 _outputs_debug_pub{ORB_ID(actuator_outputs_debug)}; uORB::Publication _esc_status_pub{ORB_ID(esc_status)}; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 0b06a24b76..cc20e9f355 100644 --- 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 index 3d617a7731..6c7b3065e2 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -307,6 +307,7 @@ private: uORB::Publication _landing_target_pose_pub{ORB_ID(landing_target_pose)}; uORB::Publication _log_message_pub{ORB_ID(log_message)}; uORB::Publication _mavlink_tunnel_pub{ORB_ID(mavlink_tunnel)}; + uORB::Publication _esc_serial_passthru_pub{ORB_ID(esc_serial_passthru)}; uORB::Publication _obstacle_distance_pub{ORB_ID(obstacle_distance)}; uORB::Publication _offboard_control_mode_pub{ORB_ID(offboard_control_mode)}; uORB::Publication _onboard_computer_status_pub{ORB_ID(onboard_computer_status)};