diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h
index 1bdb5f3e35220..085a4263f8a8c 100644
--- a/Tools/AP_Periph/AP_Periph.h
+++ b/Tools/AP_Periph/AP_Periph.h
@@ -77,6 +77,9 @@ extern "C" {
void can_printf(const char *fmt, ...) FMT_PRINTF(1,2);
}
+struct CanardInstance;
+struct CanardRxTransfer;
+
class AP_Periph_FW {
public:
AP_Periph_FW();
@@ -340,6 +343,23 @@ class AP_Periph_FW {
uint8_t priority,
const void* payload,
uint16_t payload_len);
+
+#if AP_UART_MONITOR_ENABLED
+ void handle_tunnel_Targetted(CanardInstance* ins, CanardRxTransfer* transfer);
+ void send_serial_monitor_data();
+ int8_t get_default_tunnel_serial_port(void) const;
+
+ struct {
+ ByteBuffer *buffer;
+ uint32_t last_request_ms;
+ AP_HAL::UARTDriver *uart;
+ int8_t uart_num;
+ uint8_t node_id;
+ uint8_t protocol;
+ uint32_t baudrate;
+ bool locked;
+ } uart_monitor;
+#endif
};
namespace AP
diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp
index 1a4fd91eea05a..e4cccc9a68f05 100644
--- a/Tools/AP_Periph/can.cpp
+++ b/Tools/AP_Periph/can.cpp
@@ -968,8 +968,14 @@ static void onTransferReceived(CanardInstance* ins,
handle_MovingBaselineData(ins, transfer);
break;
#endif
+#endif // HAL_PERIPH_ENABLE_GPS
+
+#if AP_UART_MONITOR_ENABLED
+ case UAVCAN_TUNNEL_TARGETTED_ID:
+ periph.handle_tunnel_Targetted(ins, transfer);
+ break;
#endif
-
+
#if defined(AP_PERIPH_HAVE_LED_WITHOUT_NOTIFY) || defined(HAL_PERIPH_ENABLE_NOTIFY)
case UAVCAN_EQUIPMENT_INDICATION_LIGHTSCOMMAND_ID:
handle_lightscommand(ins, transfer);
@@ -1068,7 +1074,14 @@ static bool shouldAcceptTransfer(const CanardInstance* ins,
*out_data_type_signature = ARDUPILOT_GNSS_MOVINGBASELINEDATA_SIGNATURE;
return true;
#endif
+#endif // HAL_PERIPH_ENABLE_GPS
+
+#if AP_UART_MONITOR_ENABLED
+ case UAVCAN_TUNNEL_TARGETTED_ID:
+ *out_data_type_signature = UAVCAN_TUNNEL_TARGETTED_SIGNATURE;
+ return true;
#endif
+
#ifdef HAL_PERIPH_ENABLE_RC_OUT
case UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_ID:
*out_data_type_signature = UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_SIGNATURE;
@@ -1845,6 +1858,9 @@ void AP_Periph_FW::can_update()
{
can_mag_update();
can_gps_update();
+#if AP_UART_MONITOR_ENABLED
+ send_serial_monitor_data();
+#endif
can_battery_update();
can_baro_update();
can_airspeed_update();
diff --git a/Tools/AP_Periph/serial_tunnel.cpp b/Tools/AP_Periph/serial_tunnel.cpp
new file mode 100644
index 0000000000000..be38a35125839
--- /dev/null
+++ b/Tools/AP_Periph/serial_tunnel.cpp
@@ -0,0 +1,211 @@
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+/*
+ handle tunnelling of serial data over DroneCAN
+ */
+
+#include
+#include "AP_Periph.h"
+
+#if AP_UART_MONITOR_ENABLED
+
+#include
+
+extern const AP_HAL::HAL &hal;
+
+#define TUNNEL_LOCK_KEY 0xf2e460e4U
+
+#ifndef TUNNEL_DEBUG
+#define TUNNEL_DEBUG 0
+#endif
+
+#if TUNNEL_DEBUG
+# define debug(fmt, args...) can_printf(fmt "\n", ##args)
+#else
+# define debug(fmt, args...)
+#endif
+
+/*
+ get the default port to tunnel if the client requests port -1
+ */
+int8_t AP_Periph_FW::get_default_tunnel_serial_port(void) const
+{
+ int8_t uart_num = -1;
+#ifdef HAL_PERIPH_ENABLE_GPS
+ if (uart_num == -1) {
+ uart_num = g.gps_port;
+ }
+#endif
+#ifdef HAL_PERIPH_ENABLE_RANGEFINDER
+ if (uart_num == -1) {
+ uart_num = g.rangefinder_port;
+ }
+#endif
+#ifdef HAL_PERIPH_ENABLE_ADSB
+ if (uart_num == -1) {
+ uart_num = g.adsb_port;
+ }
+#endif
+#if HAL_PROXIMITY_ENABLED
+ if (uart_num == -1) {
+ uart_num = g.proximity_port;
+ }
+#endif
+ return uart_num;
+}
+
+/*
+ handle tunnel data
+ */
+void AP_Periph_FW::handle_tunnel_Targetted(CanardInstance* ins, CanardRxTransfer* transfer)
+{
+ uavcan_tunnel_Targetted pkt;
+ if (uavcan_tunnel_Targetted_decode(transfer, &pkt)) {
+ return;
+ }
+ if (pkt.target_node != canardGetLocalNodeID(ins)) {
+ return;
+ }
+ if (uart_monitor.buffer == nullptr) {
+ uart_monitor.buffer = new ByteBuffer(1024);
+ if (uart_monitor.buffer == nullptr) {
+ return;
+ }
+ }
+ int8_t uart_num = pkt.serial_id;
+ if (uart_num == -1) {
+ uart_num = get_default_tunnel_serial_port();
+ }
+ if (uart_num < 0) {
+ return;
+ }
+ auto *uart = hal.serial(uart_num);
+ if (uart == nullptr) {
+ return;
+ }
+ if (uart_monitor.uart_num != uart_num && uart_monitor.uart != nullptr) {
+ // remove monitor from previous uart
+ hal.serial(uart_monitor.uart_num)->set_monitor_read_buffer(nullptr);
+ }
+ uart_monitor.uart_num = uart_num;
+ if (uart != uart_monitor.uart) {
+ // change of uart or expired, clear old data
+ uart_monitor.buffer->clear();
+ uart_monitor.uart = uart;
+ uart_monitor.baudrate = 0;
+ }
+ if (uart_monitor.uart == nullptr) {
+ return;
+ }
+ /*
+ allow for locked state to change at any time, so users can
+ switch between locked and unlocked while connected
+ */
+ uart_monitor.locked = (pkt.options & UAVCAN_TUNNEL_TARGETTED_OPTION_LOCK_PORT) != 0;
+ if (uart_monitor.locked) {
+ uart_monitor.uart->lock_port(TUNNEL_LOCK_KEY, TUNNEL_LOCK_KEY);
+ } else {
+ uart_monitor.uart->lock_port(0,0);
+ }
+ uart_monitor.node_id = transfer->source_node_id;
+ uart_monitor.protocol = pkt.protocol.protocol;
+ if (pkt.baudrate != uart_monitor.baudrate) {
+ if (uart_monitor.locked && pkt.baudrate != 0) {
+ uart_monitor.uart->begin_locked(pkt.baudrate, 0, 0, TUNNEL_LOCK_KEY);
+ debug("begin_locked %u", unsigned(pkt.baudrate));
+ }
+ uart_monitor.baudrate = pkt.baudrate;
+ }
+ uart_monitor.uart->set_monitor_read_buffer(uart_monitor.buffer);
+ uart_monitor.last_request_ms = AP_HAL::millis();
+
+ // write to device
+ if (pkt.buffer.len > 0) {
+ if (uart_monitor.locked) {
+ debug("write_locked %u", unsigned(pkt.buffer.len));
+ uart_monitor.uart->write_locked(pkt.buffer.data, pkt.buffer.len, TUNNEL_LOCK_KEY);
+ } else {
+ uart_monitor.uart->write(pkt.buffer.data, pkt.buffer.len);
+ }
+ } else {
+ debug("locked keepalive");
+ }
+}
+
+/*
+ send tunnelled serial data
+ */
+void AP_Periph_FW::send_serial_monitor_data()
+{
+ if (uart_monitor.uart == nullptr ||
+ uart_monitor.node_id == 0 ||
+ uart_monitor.buffer == nullptr) {
+ return;
+ }
+ const uint32_t last_req_ms = uart_monitor.last_request_ms;
+ const uint32_t now_ms = AP_HAL::millis();
+ if (now_ms - last_req_ms >= 3000) {
+ // stop sending and unlock, but don't release the buffer
+ if (uart_monitor.locked) {
+ debug("unlock");
+ uart_monitor.uart->lock_port(0, 0);
+ }
+ uart_monitor.uart = nullptr;
+ return;
+ }
+ if (uart_monitor.locked) {
+ /*
+ when the port is locked nobody is reading the uart so the
+ monitor doesn't fill. We read here to ensure it fills
+ */
+ uint8_t buf[120];
+ for (uint8_t i=0; i<8; i++) {
+ if (uart_monitor.uart->read_locked(buf, sizeof(buf), TUNNEL_LOCK_KEY) <= 0) {
+ break;
+ }
+ }
+ }
+ uint8_t sends = 8;
+ while (uart_monitor.buffer->available() > 0 && sends-- > 0) {
+ uint32_t n;
+ const uint8_t *buf = uart_monitor.buffer->readptr(n);
+ if (n == 0) {
+ return;
+ }
+ // broadcast data as tunnel packets, can be used for uCenter debug and device fw update
+ uavcan_tunnel_Targetted pkt {};
+ n = MIN(n, sizeof(pkt.buffer.data));
+ pkt.target_node = uart_monitor.node_id;
+ pkt.protocol.protocol = uart_monitor.protocol;
+ pkt.buffer.len = n;
+ pkt.baudrate = uart_monitor.baudrate;
+ memcpy(pkt.buffer.data, buf, n);
+
+ uint8_t buffer[UAVCAN_TUNNEL_TARGETTED_MAX_SIZE] {};
+ const uint16_t total_size = uavcan_tunnel_Targetted_encode(&pkt, buffer, !canfdout());
+
+ debug("read %u", unsigned(n));
+
+ if (!canard_broadcast(UAVCAN_TUNNEL_TARGETTED_SIGNATURE,
+ UAVCAN_TUNNEL_TARGETTED_ID,
+ CANARD_TRANSFER_PRIORITY_MEDIUM,
+ &buffer[0],
+ total_size)) {
+ break;
+ }
+ uart_monitor.buffer->advance(n);
+ }
+}
+#endif // AP_UART_MONITOR_ENABLED
diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py
index 7b8a302679477..98005453a6061 100644
--- a/Tools/ardupilotwaf/boards.py
+++ b/Tools/ardupilotwaf/boards.py
@@ -803,6 +803,7 @@ def configure_env(self, cfg, env):
CAN_APP_NODE_NAME = '"org.ardupilot.ap_periph_gps"',
AP_AIRSPEED_ENABLED = 0,
HAL_PERIPH_ENABLE_GPS = 1,
+ AP_UART_MONITOR_ENABLED = 1,
HAL_CAN_DEFAULT_NODE_ID = 0,
HAL_RAM_RESERVE_START = 0,
APJ_BOARD_ID = 100,