AP_Scripting: CANSensor: Add filtering of incoming frames

Co-authored-by: Andras Schaffer <eosbandi@gmail.com>

libraries/AP_Scripting/AP_Scripting_CANSensor.cpp

@@ -16,6 +16,7 @@
   Scripting CANSensor class, for easy scripting CAN support
  */
 #include "AP_Scripting_CANSensor.h"
+#include <AP_Math/AP_Math.h>
 
 #if AP_SCRIPTING_CAN_SENSOR_ENABLED
 
@@ -62,8 +63,25 @@ bool ScriptingCANBuffer::read_frame(AP_HAL::CANFrame &frame)
 // recursively add frame to buffer
 void ScriptingCANBuffer::handle_frame(AP_HAL::CANFrame &frame)
 {
+    // accept everything if no filters are setup
+    bool accept = num_filters == 0;
+
+    // Check if frame matches any filters
+    for (uint8_t i = 0; i < MIN(num_filters, ARRAY_SIZE(filter)); i++) {
+        if ((frame.id & filter[i].mask) == filter[i].value) {
+            accept = true;
+            break;
+        }
+    }
+
     WITH_SEMAPHORE(sem);
-    buffer.push(frame);
+
+    // Add to buffer for scripting to read
+    if (accept) {
+        buffer.push(frame);
+    }
+
+    // filtering is not applied to other buffers
     if (next != nullptr) {
         next->handle_frame(frame);
     }
@@ -79,4 +97,19 @@ void ScriptingCANBuffer::add_buffer(ScriptingCANBuffer* new_buff) {
     next->add_buffer(new_buff);
 }
 
+// Add a filter, will pass ID's that match value given the mask
+bool ScriptingCANBuffer::add_filter(uint32_t mask, uint32_t value) {
+
+    // Run out of filters
+    if (num_filters >= ARRAY_SIZE(filter)) {
+        return false;
+    }
+
+    // Add to list and increment
+    filter[num_filters].mask = mask;
+    filter[num_filters].value = value & mask;
+    num_filters++;
+    return true;
+}
+
 #endif // AP_SCRIPTING_CAN_SENSOR_ENABLED

libraries/AP_Scripting/AP_Scripting_CANSensor.h

@@ -74,6 +74,9 @@ public:
     // recursively add new buffer
     void add_buffer(ScriptingCANBuffer* new_buff);
 
+    // Add a filter to this buffer
+    bool add_filter(uint32_t mask, uint32_t value);
+
 private:
 
     ObjectBuffer<AP_HAL::CANFrame> buffer;
@@ -84,6 +87,12 @@ private:
 
     HAL_Semaphore sem;
 
+    struct {
+        uint32_t mask;
+        uint32_t value;
+    } filter[8];
+    uint8_t num_filters;
+
 };
 
 #endif // AP_SCRIPTING_CAN_SENSOR_ENABLED

libraries/AP_Scripting/docs/docs.lua

@@ -1057,6 +1057,14 @@ local ScriptingCANBuffer_ud = {}
 ---@return CANFrame_ud|nil
 function ScriptingCANBuffer_ud:read_frame() end
 
+-- Add a filter to the CAN buffer, mask is bitwise ANDed with the frame id and compared to value if not match frame is not buffered
+-- By default no filters are added and all frames are buffered, write is not affected by filters
+-- Maximum number of filters is 8
+---@param mask uint32_t_ud
+---@param value uint32_t_ud
+---@return boolean -- returns true if the filler was added successfully
+function ScriptingCANBuffer_ud:add_filter(mask, value) end
+
 -- desc
 ---@param frame CANFrame_ud
 ---@param timeout_us uint32_t_ud

libraries/AP_Scripting/examples/CAN_read.lua

@@ -10,6 +10,11 @@ if not driver1 and not driver2 then
    return
 end
 
+-- Only accept DroneCAN node status msg on second driver
+-- node status is message ID 341
+-- Message ID is 16 bits left shifted by 8 in the CAN frame ID.
+driver2:add_filter(uint32_t(0xFFFF) << 8, uint32_t(341) << 8)
+
 function show_frame(dnum, frame)
     gcs:send_text(0,string.format("CAN[%u] msg from " .. tostring(frame:id()) .. ": %i, %i, %i, %i, %i, %i, %i, %i", dnum, frame:data(0), frame:data(1), frame:data(2), frame:data(3), frame:data(4), frame:data(5), frame:data(6), frame:data(7)))
 end

libraries/AP_Scripting/generator/description/bindings.desc

@@ -620,7 +620,7 @@ userdata AP_HAL::CANFrame method isErrorFrame boolean
 ap_object ScriptingCANBuffer depends AP_SCRIPTING_CAN_SENSOR_ENABLED
 ap_object ScriptingCANBuffer method write_frame boolean AP_HAL::CANFrame uint32_t'skip_check
 ap_object ScriptingCANBuffer method read_frame boolean AP_HAL::CANFrame'Null
-
+ap_object ScriptingCANBuffer method add_filter boolean uint32_t'skip_check uint32_t'skip_check
 
 include ../Tools/AP_Periph/AP_Periph.h depends defined(HAL_BUILD_AP_PERIPH)
 singleton AP_Periph_FW depends defined(HAL_BUILD_AP_PERIPH)