fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

voxl2_io: Added UART passthru

Browse Source
This commit is contained in:
Eric Katzfey 2025-03-04 13:52:52 -08:00 committed by Eric Katzfey
parent 5e54d727fc
commit eb43d21730
5 changed files with 42 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
msg/MavlinkTunnel.msg
View File

@ -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

32
src/drivers/voxl2_io/voxl2_io.cpp
View File

@ -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);
}

4
src/drivers/voxl2_io/voxl2_io.hpp
View File

@ -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();
};

4
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -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;

1
src/modules/mavlink/mavlink_receiver.h
View File

@ -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)};