Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-21 16:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Bootloader: support dual CAN

Browse Source
This commit is contained in:
Andrew Tridgell 2021-03-15 11:52:23 +11:00
parent ef16eb56bb
commit 3590fe36e8
1 changed files with 33 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

53
Tools/AP_Bootloader/can.cpp
View File

@ -54,7 +54,7 @@ static CANConfig cancfg = {
0 // filled in below
};
#else
static ChibiOS::CANIface can_iface(0);
static ChibiOS::CANIface can_iface[HAL_NUM_CAN_IFACES];
#endif
#ifndef CAN_APP_VERSION_MAJOR
@ -483,7 +483,11 @@ static void processTx(void)
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::micros64() + 1000000, 0) > 0) {
bool send_ok = false;
for (uint8_t i=0; i<HAL_NUM_CAN_IFACES; i++) {
send_ok |= (can_iface[i].send(txmsg, AP_HAL::micros64() + 1000000, 0) > 0);
}
if (send_ok) {
canardPopTxQueue(&canard);
fail_count = 0;
} else {
@ -503,25 +507,32 @@ 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;
}
#ifdef HAL_GPIO_PIN_LED_BOOTLOADER
palToggleLine(HAL_GPIO_PIN_LED_BOOTLOADER);
#endif
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;
}
#ifdef HAL_GPIO_PIN_LED_BOOTLOADER
palToggleLine(HAL_GPIO_PIN_LED_BOOTLOADER);
#endif
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);
}
}
#endif //#if HAL_USE_CAN
@ -695,7 +706,9 @@ void can_start()
CAN_BTR_BRP(timings.bit_rate_prescaler-1);
canStart(&CAND1, &cancfg);
#else
can_iface.init(baudrate, AP_HAL::CANIface::NormalMode);
for (uint8_t i=0; i<HAL_NUM_CAN_IFACES; i++) {
can_iface[i].init(baudrate, AP_HAL::CANIface::NormalMode);
}
#endif
canardInit(&canard, (uint8_t *)canard_memory_pool, sizeof(canard_memory_pool),
onTransferReceived, shouldAcceptTransfer, NULL);