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

AP_Periph: support CAN redundancy 

send and recv on multiple CAN interfaces
This commit is contained in:
Andrew Tridgell 2021-03-14 17:15:28 +11:00
parent 2fc7ea1a8b
commit ef16eb56bb
1 changed files with 33 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
Tools/AP_Periph/can.cpp
View File

@ -96,9 +96,9 @@ static uint8_t transfer_id;
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
static ChibiOS::CANIface can_iface(0);
static ChibiOS::CANIface can_iface[HAL_NUM_CAN_IFACES];
#elif CONFIG_HAL_BOARD == HAL_BOARD_SITL
static HALSITL::CANIface can_iface(0);
static HALSITL::CANIface can_iface[HAL_NUM_CAN_IFACES];
#endif
/*
* Variables used for dynamic node ID allocation.
@ -978,8 +978,13 @@ static void processTx(void)
txmsg.dlc = txf->data_len;
memcpy(txmsg.data, txf->data, 8);
txmsg.id = (txf->id | AP_HAL::CANFrame::FlagEFF);
// push message with 1s timeout
if (can_iface.send(txmsg, AP_HAL::native_micros64() + 1000000, 0) > 0) {
// push message with 1s timeout
bool sent_ok = false;
const uint64_t deadline = AP_HAL::native_micros64() + 1000000;
for (uint8_t i=0; i<HAL_NUM_CAN_IFACES; i++) {
sent_ok |= (can_iface[i].send(txmsg, deadline, 0) > 0);
}
if (sent_ok) {
canardPopTxQueue(&canard);
fail_count = 0;
} else {
@ -999,22 +1004,29 @@ static void processRx(void)
{
AP_HAL::CANFrame rxmsg;
while (true) {
bool read_select = true;
bool write_select = false;
can_iface.select(read_select, write_select, nullptr, 0);
if (!read_select) {
bool got_pkt = false;
for (uint8_t i=0; i<HAL_NUM_CAN_IFACES; i++) {
bool read_select = true;
bool write_select = false;
can_iface[i].select(read_select, write_select, nullptr, 0);
if (!read_select) {
continue;
}
CanardCANFrame rx_frame {};
//palToggleLine(HAL_GPIO_PIN_LED);
uint64_t timestamp;
AP_HAL::CANIface::CanIOFlags flags;
can_iface[i].receive(rxmsg, timestamp, flags);
memcpy(rx_frame.data, rxmsg.data, 8);
rx_frame.data_len = rxmsg.dlc;
rx_frame.id = rxmsg.id;
canardHandleRxFrame(&canard, &rx_frame, timestamp);
got_pkt = true;
}
if (!got_pkt) {
break;
}
CanardCANFrame rx_frame {};
//palToggleLine(HAL_GPIO_PIN_LED);
uint64_t timestamp;
AP_HAL::CANIface::CanIOFlags flags;
can_iface.receive(rxmsg, timestamp, flags);
memcpy(rx_frame.data, rxmsg.data, 8);
rx_frame.data_len = rxmsg.dlc;
rx_frame.id = rxmsg.id;
canardHandleRxFrame(&canard, &rx_frame, timestamp);
}
}
@ -1242,7 +1254,9 @@ void AP_Periph_FW::can_start()
periph.g.flash_bootloader.set_and_save_ifchanged(0);
#endif
can_iface.init(1000000, AP_HAL::CANIface::NormalMode);
for (int8_t i=0; i<HAL_NUM_CAN_IFACES; i++) {
can_iface[i].init(1000000, AP_HAL::CANIface::NormalMode);
}
canardInit(&canard, (uint8_t *)canard_memory_pool, sizeof(canard_memory_pool),
onTransferReceived, shouldAcceptTransfer, NULL);