HAL_ChibiOS: support mavcan message forwarding

libraries/AP_HAL_ChibiOS/CANFDIface.cpp

@@ -378,7 +378,8 @@ int16_t CANIface::send(const AP_HAL::CANFrame& frame, uint64_t tx_deadline,
     pending_tx_[index].aborted        = false;
     pending_tx_[index].setup          = true;
     pending_tx_[index].pushed         = false;
-    return 1;
+
+    return AP_HAL::CANIface::send(frame, tx_deadline, flags);
 }
 
 int16_t CANIface::receive(AP_HAL::CANFrame& out_frame, uint64_t& out_timestamp_us, CanIOFlags& out_flags)
@@ -391,7 +392,8 @@ int16_t CANIface::receive(AP_HAL::CANFrame& out_frame, uint64_t& out_timestamp_u
     out_frame    = rx_item.frame;
     out_timestamp_us = rx_item.timestamp_us;
     out_flags    = rx_item.flags;
-    return 1;
+
+    return AP_HAL::CANIface::receive(out_frame, out_timestamp_us, out_flags);
 }
 
 bool CANIface::configureFilters(const CanFilterConfig* filter_configs,
@@ -761,7 +763,7 @@ void CANIface::handleTxCompleteInterrupt(const uint64_t timestamp_us)
                 rx_item.frame = pending_tx_[i].frame;
                 rx_item.timestamp_us = timestamp_us;
                 rx_item.flags = AP_HAL::CANIface::Loopback;
-                rx_queue_.push(rx_item);
+                add_to_rx_queue(rx_item);
             }
             if (event_handle_ != nullptr) {
                 stats.num_events++;
@@ -855,7 +857,7 @@ bool CANIface::readRxFIFO(uint8_t fifo_index)
     rx_item.frame = frame;
     rx_item.timestamp_us = timestamp_us;
     rx_item.flags = 0;
-    if (rx_queue_.push(rx_item)) {
+    if (add_to_rx_queue(rx_item)) {
         stats.rx_received++;
     } else {
         stats.rx_overflow++;

libraries/AP_HAL_ChibiOS/CANFDIface.h

@@ -240,6 +240,15 @@ public:
     // CAN Peripheral register structure, pointing at base
     // register. Indexed by locical interface number
     static constexpr CanType* const Can[HAL_NUM_CAN_IFACES] = { HAL_CAN_BASE_LIST };
+
+protected:
+    bool add_to_rx_queue(const CanRxItem &rx_item) override {
+        return rx_queue_.push(rx_item);
+    }
+
+    int8_t get_iface_num(void) const override {
+        return self_index_;
+    }
 };
 
 

libraries/AP_HAL_ChibiOS/CANIface.h

@@ -238,6 +238,15 @@ public:
 
     // CAN Peripheral register structure
     static constexpr bxcan::CanType* const Can[HAL_NUM_CAN_IFACES] = { HAL_CAN_BASE_LIST };
+
+protected:
+    bool add_to_rx_queue(const CanRxItem &rx_item) override {
+        return rx_queue_.push(rx_item);
+    }
+
+    int8_t get_iface_num(void) const override {
+        return self_index_;
+    }
 };
 #endif //HAL_NUM_CAN_IFACES
 #endif //# if defined(STM32H7XX) || defined(STM32G4)

libraries/AP_HAL_ChibiOS/CanIface.cpp

@@ -362,7 +362,8 @@ int16_t CANIface::send(const AP_HAL::CANFrame& frame, uint64_t tx_deadline,
     txi.abort_on_error = (flags & AbortOnError) != 0;
     // setup frame initial state
     txi.pushed         = false;
-    return 1;
+
+    return AP_HAL::CANIface::send(frame, tx_deadline, flags);
 }
 
 int16_t CANIface::receive(AP_HAL::CANFrame& out_frame, uint64_t& out_timestamp_us, CanIOFlags& out_flags)
@@ -375,7 +376,8 @@ int16_t CANIface::receive(AP_HAL::CANFrame& out_frame, uint64_t& out_timestamp_u
     out_frame    = rx_item.frame;
     out_timestamp_us = rx_item.timestamp_us;
     out_flags    = rx_item.flags;
-    return 1;
+
+    return AP_HAL::CANIface::receive(out_frame, out_timestamp_us, out_flags);
 }
 
 #if !defined(HAL_BOOTLOADER_BUILD)
@@ -481,7 +483,7 @@ void CANIface::handleTxMailboxInterrupt(uint8_t mailbox_index, bool txok, const
         rx_item.timestamp_us = timestamp_us;
         rx_item.flags = AP_HAL::CANIface::Loopback;
         PERF_STATS(stats.tx_loopback);
-        rx_queue_.push(rx_item);
+        add_to_rx_queue(rx_item);
     }
 
     if (txok && !txi.pushed) {
@@ -569,7 +571,7 @@ void CANIface::handleRxInterrupt(uint8_t fifo_index, uint64_t timestamp_us)
     rx_item.frame = frame;
     rx_item.timestamp_us = timestamp_us;
     rx_item.flags = 0;
-    if (rx_queue_.push(rx_item)) {
+    if (add_to_rx_queue(rx_item)) {
         PERF_STATS(stats.rx_received);
     } else {
         PERF_STATS(stats.rx_overflow);