AP_Bootloader: support dual CAN

Tools/AP_Bootloader/can.cpp

@@ -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) {
-            break;
-        }
+        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);
+            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);
+            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;
+        }
     }
 }
 #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);