AP_HAL: add option to disable rx callback for CAN iface

ArduPilot · Dec 23, 2024 · 710b9ea · 710b9ea
1 parent 169d6f6
commit 710b9ea
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 7 deletions.
diff --git a/libraries/AP_HAL/CANIface.cpp b/libraries/AP_HAL/CANIface.cpp
@@ -64,9 +64,9 @@ int16_t AP_HAL::CANIface::receive(CANFrame& out_frame, uint64_t& out_ts_monotoni
 #ifndef HAL_BOOTLOADER_BUILD
     WITH_SEMAPHORE(callbacks.sem);
 #endif
-    for (auto &cb : callbacks.cb) {
-        if (cb != nullptr) {
-            cb(get_iface_num(), out_frame);
+    for (uint8_t i=0; i<ARRAY_SIZE(callbacks.cb); i++) {
+        if (callbacks.cb[i] != nullptr && !callbacks.rx_cb_disable[i]) {
+            callbacks.cb[i](get_iface_num(), out_frame);
         }
     }
     return 1;
@@ -129,6 +129,7 @@ void AP_HAL::CANIface::unregister_frame_callback(uint8_t callback_id)
     const uint8_t idx = callback_id - 1;
     if (idx < ARRAY_SIZE(callbacks.cb)) {
         callbacks.cb[idx] = nullptr;
+        callbacks.rx_cb_disable[idx] = false;
     }
 }
 

diff --git a/libraries/AP_HAL/CANIface.h b/libraries/AP_HAL/CANIface.h
@@ -262,19 +262,20 @@ class AP_HAL::CANIface
     FUNCTOR_TYPEDEF(FrameCb, void, uint8_t, const AP_HAL::CANFrame &);
 
     // register a frame callback function
-    virtual bool register_frame_callback(FrameCb cb, uint8_t &cb_id);
-    virtual void unregister_frame_callback(uint8_t cb_id);
+    bool register_frame_callback(FrameCb cb, uint8_t &cb_id);
+    void unregister_frame_callback(uint8_t cb_id);
+    virtual bool add_to_rx_queue(const CanRxItem &rx_item) = 0;
+    void set_rx_cb_disabled(uint8_t cb_id, bool rx_cb_disable) { callbacks.rx_cb_disable[cb_id-1] = rx_cb_disable; }
 
 protected:
     virtual int8_t get_iface_num() const = 0;
-    virtual bool add_to_rx_queue(const CanRxItem &rx_item) = 0;
-
     struct {
 #ifndef HAL_BOOTLOADER_BUILD
         HAL_Semaphore sem;
 #endif
         // allow up to 3 callbacks per interface
         FrameCb cb[3];
+        bool rx_cb_disable[3];
     } callbacks;
 
     uint32_t bitrate_;