diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.cpp b/libraries/AP_DroneCAN/AP_DroneCAN.cpp index 7853319cc8e7a..83f4bfa15720e 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN.cpp +++ b/libraries/AP_DroneCAN/AP_DroneCAN.cpp @@ -51,6 +51,10 @@ #include #include +#if AP_DRONECAN_SERIAL_ENABLED +#include "AP_DroneCAN_serial.h" +#endif + extern const AP_HAL::HAL& hal; // setup default pool size @@ -148,7 +152,105 @@ const AP_Param::GroupInfo AP_DroneCAN::var_info[] = { // @Bitmask: 0: ESC 1, 1: ESC 2, 2: ESC 3, 3: ESC 4, 4: ESC 5, 5: ESC 6, 6: ESC 7, 7: ESC 8, 8: ESC 9, 9: ESC 10, 10: ESC 11, 11: ESC 12, 12: ESC 13, 13: ESC 14, 14: ESC 15, 15: ESC 16, 16: ESC 17, 17: ESC 18, 18: ESC 19, 19: ESC 20, 20: ESC 21, 21: ESC 22, 22: ESC 23, 23: ESC 24, 24: ESC 25, 25: ESC 26, 26: ESC 27, 27: ESC 28, 28: ESC 29, 29: ESC 30, 30: ESC 31, 31: ESC 32 // @User: Advanced AP_GROUPINFO("ESC_RV", 9, AP_DroneCAN, _esc_rv, 0), - + +#if AP_DRONECAN_SERIAL_ENABLED + /* + due to the parameter tree depth limitation we can't use a sub-table for the serial parameters + */ + + // @Param: SER_EN + // @DisplayName: Serial enable + // @Description: Enable DroneCAN virtual serial ports + // @Values: 0:Disabled, 1:Enabled + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO_FLAGS("SER_EN", 10, AP_DroneCAN, serial.enable, 0, AP_PARAM_FLAG_ENABLE), + + // @Param: S1_NOD + // @DisplayName: Serial CAN node number + // @Description: CAN node number for serial port + // @Range: 0 127 + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO("S1_NOD", 11, AP_DroneCAN, serial.ports[0].node, 0), + + // @Param: S1_IDX + // @DisplayName: Serial1 index + // @Description: Serial port number on remote CAN node + // @Range: 0 100 + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO("S1_IDX", 12, AP_DroneCAN, serial.ports[0].idx, 0), + + // @Param: S1_BD + // @DisplayName: Serial default baud rate + // @Description: Serial baud rate on remote CAN node + // @CopyFieldsFrom: SERIAL0_BAUD + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO("S1_BD", 13, AP_DroneCAN, serial.ports[0].state.baud, 57600), + + // @Param: S1_PRO + // @DisplayName: Serial protocol + // @Description: Serial protocol + // @CopyFieldsFrom: SERIAL0_PROTOCOL + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO("S1_PRO", 14, AP_DroneCAN, serial.ports[0].state.protocol, -1), + +#if AP_DRONECAN_SERIAL_NUM_PORTS > 1 + // @Param: S2_NOD + // @DisplayName: S2_NOD + // @Description: CAN node number for serial port + // @CopyFieldsFrom: CAN_D1_UC_S1_NOD + AP_GROUPINFO("S2_NOD", 15, AP_DroneCAN, serial.ports[1].node, 0), + + // @Param: S2_IDX + // @DisplayName: S2_IDX + // @Description: Serial port number on remote CAN node + // @CopyFieldsFrom: CAN_D1_UC_S1_IDX + AP_GROUPINFO("S2_IDX", 16, AP_DroneCAN, serial.ports[1].idx, 0), + + // @Param: S2_BD + // @DisplayName: S2_BD + // @Description: Serial baud rate on remote CAN node + // @CopyFieldsFrom: CAN_D1_UC_S1_BD + AP_GROUPINFO("S2_BD", 17, AP_DroneCAN, serial.ports[1].state.baud, 57600), + + // @Param: S2_PRO + // @DisplayName: S2_PRO + // @Description: Serial protocol + // @CopyFieldsFrom: CAN_D1_UC_S1_PRO + AP_GROUPINFO("S2_PRO", 18, AP_DroneCAN, serial.ports[1].state.protocol, -1), +#endif + +#if AP_DRONECAN_SERIAL_NUM_PORTS > 2 + // @Param: S3_NOD + // @DisplayName: S3_NOD + // @Description: CAN node number for serial port + // @CopyFieldsFrom: CAN_D1_UC_S1_NOD + AP_GROUPINFO("S3_NOD", 19, AP_DroneCAN, serial.ports[2].node, 0), + + // @Param: S3_IDX + // @DisplayName: S3_IDX + // @Description: Serial port number on remote CAN node + // @CopyFieldsFrom: CAN_D1_UC_S1_IDX + AP_GROUPINFO("S3_IDX", 20, AP_DroneCAN, serial.ports[2].idx, 0), + + // @Param: S3_BD + // @DisplayName: S3_BD + // @Description: Serial baud rate on remote CAN node + // @CopyFieldsFrom: CAN_D1_UC_S1_BD + AP_GROUPINFO("S3_BD", 21, AP_DroneCAN, serial.ports[2].state.baud, 57600), + + // @Param: S3_PRO + // @DisplayName: S3_PRO + // @Description: Serial protocol + // @CopyFieldsFrom: CAN_D1_UC_S1_PRO + AP_GROUPINFO("S3_PRO", 22, AP_DroneCAN, serial.ports[2].state.protocol, -1), +#endif +#endif // AP_DRONECAN_SERIAL_ENABLED + AP_GROUPEND }; @@ -369,6 +471,10 @@ void AP_DroneCAN::init(uint8_t driver_index, bool enable_filters) return; } +#if AP_DRONECAN_SERIAL_ENABLED + serial.init(this); +#endif + _initialized = true; debug_dronecan(AP_CANManager::LOG_INFO, "DroneCAN: init done\n\r"); } @@ -421,6 +527,10 @@ void AP_DroneCAN::loop(void) } } } + +#if AP_DRONECAN_SERIAL_ENABLED + serial.update(); +#endif } } diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.h b/libraries/AP_DroneCAN/AP_DroneCAN.h index 81abfbdda4430..f502e05a39f27 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN.h +++ b/libraries/AP_DroneCAN/AP_DroneCAN.h @@ -34,6 +34,7 @@ #include "AP_DroneCAN_DNA_Server.h" #include #include +#include #ifndef DRONECAN_SRV_NUMBER #define DRONECAN_SRV_NUMBER NUM_SERVO_CHANNELS @@ -58,6 +59,14 @@ #define AP_DRONECAN_HIMARK_SERVO_SUPPORT (BOARD_FLASH_SIZE>1024) #endif +#ifndef AP_DRONECAN_SERIAL_ENABLED +#define AP_DRONECAN_SERIAL_ENABLED AP_SERIALMANAGER_REGISTER_ENABLED && (BOARD_FLASH_SIZE>1024) +#endif + +#if AP_DRONECAN_SERIAL_ENABLED +#include "AP_DroneCAN_serial.h" +#endif + // fwd-declare callback classes class AP_DroneCAN_DNA_Server; @@ -258,6 +267,10 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend { CanardInterface canard_iface; +#if AP_DRONECAN_SERIAL_ENABLED + AP_DroneCAN_Serial serial; +#endif + Canard::Publisher node_status{canard_iface}; Canard::Publisher can_stats{canard_iface}; Canard::Publisher protocol_stats{canard_iface}; diff --git a/libraries/AP_DroneCAN/AP_DroneCAN_serial.cpp b/libraries/AP_DroneCAN/AP_DroneCAN_serial.cpp new file mode 100644 index 0000000000000..c55ea330c2727 --- /dev/null +++ b/libraries/AP_DroneCAN/AP_DroneCAN_serial.cpp @@ -0,0 +1,226 @@ +/* + map local serial ports to remote DroneCAN serial ports using the + TUNNEL_TARGETTED message + */ + +#include "AP_DroneCAN.h" + +#if HAL_ENABLE_DRONECAN_DRIVERS && AP_DRONECAN_SERIAL_ENABLED + +#include +#include + +AP_DroneCAN_Serial *AP_DroneCAN_Serial::serial[HAL_MAX_CAN_PROTOCOL_DRIVERS]; + +#ifndef AP_DRONECAN_SERIAL_MIN_TXSIZE +#define AP_DRONECAN_SERIAL_MIN_TXSIZE 2048 +#endif + +#ifndef AP_DRONECAN_SERIAL_MIN_RXSIZE +#define AP_DRONECAN_SERIAL_MIN_RXSIZE 2048 +#endif + +/* + initialise DroneCAN serial aports +*/ +void AP_DroneCAN_Serial::init(AP_DroneCAN *_dronecan) +{ + if (enable == 0) { + return; + } + const uint8_t driver_index = _dronecan->get_driver_index(); + if (driver_index >= ARRAY_SIZE(serial)) { + return; + } + serial[driver_index] = this; + dronecan = _dronecan; + + const uint8_t base_port = driver_index == 0? AP_SERIALMANAGER_CAN_D1_PORT_1 : AP_SERIALMANAGER_CAN_D2_PORT_1; + bool need_subscriber = false; + + // init in reverse order to keep the linked list in + // AP_SerialManager in the right order + for (int8_t i=ARRAY_SIZE(ports)-1; i>= 0; i--) { + auto &p = ports[i]; + p.state.idx = base_port + i; + if (p.node > 0) { + p.init(); + AP::serialmanager().register_port(&p); + need_subscriber = true; + } + } + + if (need_subscriber) { + if (Canard::allocate_sub_arg_callback(dronecan, &handle_tunnel_targetted, dronecan->get_driver_index()) == nullptr) { + AP_BoardConfig::allocation_error("serial_tunnel_sub"); + } + targetted = new Canard::Publisher(dronecan->get_canard_iface()); + if (targetted == nullptr) { + AP_BoardConfig::allocation_error("serial_tunnel_pub"); + } + targetted->set_timeout_ms(20); + targetted->set_priority(CANARD_TRANSFER_PRIORITY_MEDIUM); + } +} + +/* + update DroneCAN serial ports +*/ +void AP_DroneCAN_Serial::update(void) +{ + const uint32_t now_ms = AP_HAL::millis(); + for (auto &p : ports) { + if (p.baudrate == 0) { + continue; + } + if (p.writebuffer == nullptr) { + continue; + } + WITH_SEMAPHORE(p.sem); + uint32_t avail; + const bool send_keepalive = now_ms - p.last_send_ms > 500; + const auto *ptr = p.writebuffer->readptr(avail); + if (!send_keepalive && (ptr == nullptr || avail <= 0)) { + continue; + } + uavcan_tunnel_Targetted pkt {}; + auto n = MIN(avail, sizeof(pkt.buffer.data)); + pkt.target_node = p.node; + pkt.protocol.protocol = UAVCAN_TUNNEL_PROTOCOL_UNDEFINED; + pkt.buffer.len = n; + pkt.baudrate = p.baudrate; + pkt.serial_id = p.idx; + pkt.options = UAVCAN_TUNNEL_TARGETTED_OPTION_LOCK_PORT; + if (ptr != nullptr) { + memcpy(pkt.buffer.data, ptr, n); + } + + if (targetted->broadcast(pkt)) { + p.writebuffer->advance(n); + p.last_send_ms = now_ms; + } + } +} + +/* + handle incoming tunnel serial packet + */ +void AP_DroneCAN_Serial::handle_tunnel_targetted(AP_DroneCAN *dronecan, + const CanardRxTransfer& transfer, + const uavcan_tunnel_Targetted &msg) +{ + uint8_t driver_index = dronecan->get_driver_index(); + if (driver_index >= ARRAY_SIZE(serial) || serial[driver_index] == nullptr) { + return; + } + auto &s = *serial[driver_index]; + for (auto &p : s.ports) { + if (p.idx == msg.serial_id && transfer.source_node_id == p.node) { + WITH_SEMAPHORE(p.sem); + if (p.readbuffer != nullptr) { + p.readbuffer->write(msg.buffer.data, msg.buffer.len); + p.last_recv_us = AP_HAL::micros64(); + } + break; + } + } +} + +/* + initialise port + */ +void AP_DroneCAN_Serial::Port::init(void) +{ + baudrate = state.baud; + begin(baudrate, 0, 0); +} + +/* + available space in outgoing buffer + */ +uint32_t AP_DroneCAN_Serial::Port::txspace(void) +{ + WITH_SEMAPHORE(sem); + return writebuffer != nullptr ? writebuffer->space() : 0; +} + +void AP_DroneCAN_Serial::Port::_begin(uint32_t b, uint16_t rxS, uint16_t txS) +{ + rxS = MAX(rxS, AP_DRONECAN_SERIAL_MIN_RXSIZE); + txS = MAX(txS, AP_DRONECAN_SERIAL_MIN_TXSIZE); + init_buffers(rxS, txS); + if (b != 0) { + baudrate = b; + } +} + +size_t AP_DroneCAN_Serial::Port::_write(const uint8_t *buffer, size_t size) +{ + WITH_SEMAPHORE(sem); + return writebuffer != nullptr ? writebuffer->write(buffer, size) : 0; +} + +ssize_t AP_DroneCAN_Serial::Port::_read(uint8_t *buffer, uint16_t count) +{ + WITH_SEMAPHORE(sem); + return readbuffer != nullptr ? readbuffer->read(buffer, count) : -1; +} + +uint32_t AP_DroneCAN_Serial::Port::_available() +{ + WITH_SEMAPHORE(sem); + return readbuffer != nullptr ? readbuffer->available() : 0; +} + + +bool AP_DroneCAN_Serial::Port::_discard_input() +{ + WITH_SEMAPHORE(sem); + if (readbuffer != nullptr) { + readbuffer->clear(); + } + return true; +} + +/* + initialise read/write buffers + */ +bool AP_DroneCAN_Serial::Port::init_buffers(const uint32_t size_rx, const uint32_t size_tx) +{ + if (size_tx == last_size_tx && + size_rx == last_size_rx) { + return true; + } + WITH_SEMAPHORE(sem); + if (readbuffer == nullptr) { + readbuffer = new ByteBuffer(size_rx); + } else { + readbuffer->set_size_best(size_rx); + } + if (writebuffer == nullptr) { + writebuffer = new ByteBuffer(size_tx); + } else { + writebuffer->set_size_best(size_tx); + } + last_size_rx = size_rx; + last_size_tx = size_tx; + return readbuffer != nullptr && writebuffer != nullptr; +} + +/* + return timestamp estimate in microseconds for when the start of + a nbytes packet arrived on the uart. +*/ +uint64_t AP_DroneCAN_Serial::Port::receive_time_constraint_us(uint16_t nbytes) +{ + WITH_SEMAPHORE(sem); + uint64_t last_receive_us = last_recv_us; + if (baudrate > 0) { + // assume 10 bits per byte. + uint32_t transport_time_us = (1000000UL * 10UL / baudrate) * (nbytes+available()); + last_receive_us -= transport_time_us; + } + return last_receive_us; +} + +#endif // HAL_ENABLE_DRONECAN_DRIVERS && AP_DRONECAN_SERIAL_ENABLED diff --git a/libraries/AP_DroneCAN/AP_DroneCAN_serial.h b/libraries/AP_DroneCAN/AP_DroneCAN_serial.h new file mode 100644 index 0000000000000..9329ffdc917c6 --- /dev/null +++ b/libraries/AP_DroneCAN/AP_DroneCAN_serial.h @@ -0,0 +1,75 @@ +#pragma once + +#include + +#ifndef AP_DRONECAN_SERIAL_NUM_PORTS +#define AP_DRONECAN_SERIAL_NUM_PORTS 3 +#endif + +class AP_DroneCAN; + +class AP_DroneCAN_Serial +{ +public: + /* Do not allow copies */ + CLASS_NO_COPY(AP_DroneCAN_Serial); + + AP_DroneCAN_Serial() {} + + AP_Int8 enable; + + void init(AP_DroneCAN *dronecan); + void update(void); + +public: + class Port : public AP_SerialManager::RegisteredPort { + public: + friend class AP_DroneCAN_Serial; + void init(void); + + AP_Int8 node; + AP_Int8 idx; + + private: + bool is_initialized() override { + return true; + } + bool tx_pending() override { + return false; + } + + bool init_buffers(const uint32_t size_rx, const uint32_t size_tx); + + uint32_t txspace() override; + void _begin(uint32_t b, uint16_t rxS, uint16_t txS) override; + size_t _write(const uint8_t *buffer, size_t size) override; + ssize_t _read(uint8_t *buffer, uint16_t count) override; + uint32_t _available() override; + void _end() override {} + void _flush() override {} + bool _discard_input() override; + uint64_t receive_time_constraint_us(uint16_t nbytes) override; + + ByteBuffer *readbuffer; + ByteBuffer *writebuffer; + uint32_t baudrate; + uint32_t last_send_ms; + uint32_t last_size_tx; + uint32_t last_size_rx; + uint64_t last_recv_us; + + HAL_Semaphore sem; + }; + + Port ports[AP_DRONECAN_SERIAL_NUM_PORTS]; + +private: + AP_DroneCAN *dronecan; + + Canard::Publisher *targetted; + static void handle_tunnel_targetted(AP_DroneCAN *dronecan, + const CanardRxTransfer& transfer, + const uavcan_tunnel_Targetted &msg); + + static AP_DroneCAN_Serial *serial[HAL_MAX_CAN_PROTOCOL_DRIVERS]; +};