From 782605e7ea60c3ff15e15b62cb94056049599bf1 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Tue, 5 Dec 2023 13:32:45 +0000 Subject: [PATCH] AP_Scripting: CANSensor: Add filtering of incoming frames Co-authored-by: Andras Schaffer --- .../AP_Scripting/AP_Scripting_CANSensor.cpp | 35 ++++++++++++++++++- .../AP_Scripting/AP_Scripting_CANSensor.h | 9 +++++ libraries/AP_Scripting/docs/docs.lua | 8 +++++ libraries/AP_Scripting/examples/CAN_read.lua | 5 +++ .../generator/description/bindings.desc | 2 +- 5 files changed, 57 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Scripting/AP_Scripting_CANSensor.cpp b/libraries/AP_Scripting/AP_Scripting_CANSensor.cpp index d3b0ab2375..39f1bf9f87 100644 --- a/libraries/AP_Scripting/AP_Scripting_CANSensor.cpp +++ b/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 #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 diff --git a/libraries/AP_Scripting/AP_Scripting_CANSensor.h b/libraries/AP_Scripting/AP_Scripting_CANSensor.h index b6f19d28d8..962aef8fbf 100644 --- a/libraries/AP_Scripting/AP_Scripting_CANSensor.h +++ b/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 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 diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index 1d57957c7b..7e3be1f487 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/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 diff --git a/libraries/AP_Scripting/examples/CAN_read.lua b/libraries/AP_Scripting/examples/CAN_read.lua index ca310faf3e..789f9e3200 100644 --- a/libraries/AP_Scripting/examples/CAN_read.lua +++ b/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 diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index 74cc6c109d..7ddc378d84 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/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)