Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_DroneCAN: support CAN serial ports 

this allows any serial protocol to be mapped to a remote DroneCAN node
This commit is contained in:
Andrew Tridgell 2023-11-19 07:36:20 +11:00
parent e06d65bd75
commit a725d8b90c
4 changed files with 424 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

113
libraries/AP_DroneCAN/AP_DroneCAN.cpp
View File

@ -51,6 +51,10 @@
#include <AP_Mount/AP_Mount_Xacti.h>
#include <string.h>
#if AP_DRONECAN_SERIAL_ENABLED
#include "AP_DroneCAN_serial.h"
#endif
extern const AP_HAL::HAL& hal;
// setup default pool size
@ -148,7 +152,106 @@ 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: DroneCAN 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 remote node number
// @Description: CAN remote 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: DroneCAN Serial1 index
// @Description: Serial port number on remote CAN node
// @Range: 0 100
// @Values: -1:Disabled,0:Serial0,1:Serial1,2:Serial2,3:Serial3,4:Serial4,5:Serial5,6:Serial6
// @RebootRequired: True
// @User: Advanced
AP_GROUPINFO("S1_IDX", 12, AP_DroneCAN, serial.ports[0].idx, -1),
// @Param: S1_BD
// @DisplayName: DroneCAN Serial default baud rate
// @Description: Serial baud rate on remote CAN node
// @CopyFieldsFrom: SERIAL1_BAUD
// @RebootRequired: True
// @User: Advanced
AP_GROUPINFO("S1_BD", 13, AP_DroneCAN, serial.ports[0].state.baud, 57600),
// @Param: S1_PRO
// @DisplayName: Serial protocol of DroneCAN serial port
// @Description: Serial protocol of DroneCAN serial port
// @CopyFieldsFrom: SERIAL1_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: Serial CAN remote node number
// @Description: CAN remote 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: Serial port number on remote CAN node
// @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, -1),
// @Param: S2_BD
// @DisplayName: DroneCAN Serial default baud rate
// @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: Serial protocol of DroneCAN serial port
// @Description: Serial protocol of DroneCAN serial port
// @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: Serial CAN remote node number
// @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: Serial port number on remote CAN node
// @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: Serial baud rate on remote CAN node
// @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: Serial protocol of DroneCAN serial port
// @Description: Serial protocol of DroneCAN serial port
// @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 +472,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 +528,10 @@ void AP_DroneCAN::loop(void)
}
}
}
#if AP_DRONECAN_SERIAL_ENABLED
serial.update();
#endif
}
}

13
libraries/AP_DroneCAN/AP_DroneCAN.h
View File

@ -34,6 +34,7 @@
#include "AP_DroneCAN_DNA_Server.h"
#include <canard.h>
#include <dronecan_msgs.h>
#include <AP_SerialManager/AP_SerialManager_config.h>
#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 @@ private:
CanardInterface canard_iface;
#if AP_DRONECAN_SERIAL_ENABLED
AP_DroneCAN_Serial serial;
#endif
Canard::Publisher<uavcan_protocol_NodeStatus> node_status{canard_iface};
Canard::Publisher<dronecan_protocol_CanStats> can_stats{canard_iface};
Canard::Publisher<dronecan_protocol_Stats> protocol_stats{canard_iface};

224
libraries/AP_DroneCAN/AP_DroneCAN_serial.cpp Normal file
View File

@ -0,0 +1,224 @@
/*
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 <AP_Math/AP_Math.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
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;
for (uint8_t i=0; i<ARRAY_SIZE(ports); i++) {
auto &p = ports[i];
p.state.idx = base_port + i;
if (p.node > 0 && p.idx >= 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<uavcan_tunnel_Targetted>(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 || p.node <= 0 || p.idx < 0) {
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

75
libraries/AP_DroneCAN/AP_DroneCAN_serial.h Normal file
View File

@ -0,0 +1,75 @@
#pragma once
#include <AP_SerialManager/AP_SerialManager.h>
#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<uavcan_tunnel_Targetted> *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];
};