Merge pull request #45 from ilia-sheremet/master

Can HW acceptance filter feature + getIface() corrections + dsdl hash fixed

libuavcan/include/uavcan/build_config.hpp

@@ -213,6 +213,17 @@ static const unsigned FloatComparisonEpsilonMult = UAVCAN_FLOAT_COMPARISON_EPSIL
 static const unsigned FloatComparisonEpsilonMult = 10;
 #endif
 
+/**
+ * Maximum number of CAN acceptance filters available on the platform
+ */
+#ifdef UAVCAN_MAX_CAN_ACCEPTANCE_FILTERS
+/// Explicitly specified by the user.
+static const unsigned MaxCanAcceptanceFilters = UAVCAN_MAX_CAN_ACCEPTANCE_FILTERS;
+#else
+/// Default that should be OK for any platform.
+static const unsigned MaxCanAcceptanceFilters = 32;
+#endif
+
 /**
  * This constant defines how many receiver objects will be statically pre-allocated by classes that listen to messages
  * of type uavcan.protocol.NodeStatus from other nodes. If the number of publishers exceeds this value, extra

libuavcan/include/uavcan/driver/can.hpp

@@ -30,16 +30,16 @@ struct UAVCAN_EXPORT CanFrame
     uint8_t data[MaxDataLen];
     uint8_t dlc;                ///< Data Length Code
 
-    CanFrame()
-        : id(0)
-        , dlc(0)
+    CanFrame() :
+        id(0),
+        dlc(0)
     {
         fill(data, data + MaxDataLen, uint8_t(0));
     }
 
-    CanFrame(uint32_t can_id, const uint8_t* can_data, uint8_t data_len)
-        : id(can_id)
-        , dlc((data_len > MaxDataLen) ? MaxDataLen : data_len)
+    CanFrame(uint32_t can_id, const uint8_t* can_data, uint8_t data_len) :
+        id(can_id),
+        dlc((data_len > MaxDataLen) ? MaxDataLen : data_len)
     {
         UAVCAN_ASSERT(can_data != NULL);
         UAVCAN_ASSERT(data_len == dlc);
@@ -62,7 +62,9 @@ struct UAVCAN_EXPORT CanFrame
         StrTight,   ///< Minimum string length (default)
         StrAligned  ///< Fixed formatting for any frame
     };
+
     std::string toString(StringRepresentation mode = StrTight) const;
+
 #endif
 
     /**
@@ -83,6 +85,16 @@ struct UAVCAN_EXPORT CanFilterConfig
 {
     uint32_t id;
     uint32_t mask;
+
+    bool operator==(const CanFilterConfig& rhs) const
+    {
+        return rhs.id == id && rhs.mask == mask;
+    }
+
+    CanFilterConfig() :
+        id(0),
+        mask(0)
+    { }
 };
 
 /**
@@ -94,9 +106,9 @@ struct UAVCAN_EXPORT CanSelectMasks
     uint8_t read;
     uint8_t write;
 
-    CanSelectMasks()
-        : read(0)
-        , write(0)
+    CanSelectMasks() :
+        read(0),
+        write(0)
     { }
 };
 
@@ -166,6 +178,11 @@ public:
      */
     virtual ICanIface* getIface(uint8_t iface_index) = 0;
 
+    virtual const ICanIface* getIface(uint8_t iface_index) const
+    {
+        return const_cast<ICanDriver*>(this)->getIface(iface_index);
+    }
+
     /**
      * Total number of available CAN interfaces.
      * This value shall not change after initialization.

libuavcan/include/uavcan/transport/can_acceptance_filter_configurator.hpp

@@ -0,0 +1,105 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>,
+ *                    Ilia  Sheremet <illia.sheremet@gmail.com>
+ */
+
+#ifndef UAVCAN_ACCEPTANCE_FILTER_CONFIGURATOR_HPP_INCLUDED
+#define UAVCAN_ACCEPTANCE_FILTER_CONFIGURATOR_HPP_INCLUDED
+
+#include <cassert>
+#include <uavcan/data_type.hpp>
+#include <uavcan/error.hpp>
+#include <uavcan/transport/dispatcher.hpp>
+#include <uavcan/node/abstract_node.hpp>
+#include <uavcan/build_config.hpp>
+#include <uavcan/util/multiset.hpp>
+
+namespace uavcan
+{
+
+/**
+ * This class configures hardware acceptance filters (if this feature is present on the particular CAN driver) to
+ * preclude reception of irrelevant CAN frames on the hardware level.
+ *
+ * Configuration starts by creating an object of class @ref CanAcceptanceFilterConfigurator on the stack.
+ * Through the method configureFilters() it determines the number of available HW filters and the number
+ * of listeners. In case the number of listeners is higher than the number of available HW filters, the function
+ * automatically merges configs in the most efficient way until their number is reduced to the number of
+ * available HW filters. Subsequently obtained configurations are then loaded into the CAN driver.
+ *
+ * The maximum number of CAN acceptance filters is predefined in uavcan/build_config.hpp through a constant
+ * @ref MaxCanAcceptanceFilters. The algorithm doesn't allow to have higher number of HW filters configurations than
+ * defined by MaxCanAcceptanceFilters. You can change this value according to the number specified in your CAN driver
+ * datasheet.
+ */
+class CanAcceptanceFilterConfigurator
+{
+    /**
+     * Below constants based on UAVCAN transport layer specification. Masks and ID's depends on message Priority,
+     * TypeID, TransferID (RequestNotResponse - for service types, BroadcastNotUnicast - for message types).
+     * For more details refer to uavcan.org/CAN_bus_transport_layer_specification.
+     * For clarity let's represent "i" as Data Type ID
+     * DefaultFilterMsgMask = 00111111111110000000000000000
+     * DefaultFilterMsgID   = 00iiiiiiiiiii0000000000000000, no need to explicitly define, since MsgID initialized as 0.
+     * DefaultFilterServiceRequestMask = 11111111111100000000000000000
+     * DefaultFilterServiceRequestID   = 101iiiiiiiii00000000000000000
+     * ServiceRespFrameMask = 11100000000000000000000000000
+     * ServiceRespFrameID   = 10000000000000000000000000000, all Service Response Frames are accepted by HW filters.
+     */
+    static const unsigned DefaultFilterMsgMask = 0x7FF0000;
+    static const unsigned DefaultFilterServiceRequestID = 0x14000000;
+    static const unsigned DefaultFilterServiceRequestMask = 0x1FFE0000;
+    static const unsigned ServiceRespFrameID = 0x10000000;
+    static const unsigned ServiceRespFrameMask = 0x1C000000;
+
+    typedef uavcan::Multiset<CanFilterConfig, 1> MultisetConfigContainer;
+
+    static CanFilterConfig mergeFilters(CanFilterConfig &a_, CanFilterConfig &b_);
+    static uint8_t countBits(uint32_t n_);
+    uint16_t getNumFilters() const;
+
+    /**
+     * Fills the multiset_configs_ to proceed it with computeConfiguration()
+     */
+    int16_t loadInputConfiguration();
+
+    /**
+     * This method merges several listeners's filter configurations by predetermined algorithm
+     * if number of available hardware acceptance filters less than number of listeners
+     */
+    int16_t computeConfiguration();
+
+    /**
+     * This method loads the configuration computed with computeConfiguration() to the CAN driver.
+     */
+    int16_t applyConfiguration();
+
+    INode& node_;               //< Node reference is needed for access to ICanDriver and Dispatcher
+    MultisetConfigContainer multiset_configs_;
+
+public:
+    explicit CanAcceptanceFilterConfigurator(INode& node)
+        : node_(node)
+        , multiset_configs_(node.getAllocator())
+    { }
+
+    /**
+     * This method invokes loadInputConfiguration(), computeConfiguration() and applyConfiguration() consequently, so that
+     * optimal acceptance filter configuration will be computed and loaded through CanDriver::configureFilters()
+     * @return 0 = success, negative for error.
+     */
+    int configureFilters();
+
+    /**
+     * Returns the configuration computed with computeConfiguration().
+     * If computeConfiguration() has not been called yet, an empty configuration will be returned.
+     */
+    const MultisetConfigContainer& getConfiguration() const
+    {
+        return multiset_configs_;
+    }
+};
+
+}
+
+#endif // UAVCAN_BUILD_CONFIG_HPP_INCLUDED

libuavcan/src/transport/uc_can_acceptance_filter_configurator.cpp

@@ -0,0 +1,232 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>,
+ *                    Ilia  Sheremet <illia.sheremet@gmail.com>
+ */
+
+#include <uavcan/transport/can_acceptance_filter_configurator.hpp>
+#include <cassert>
+
+namespace uavcan
+{
+const unsigned CanAcceptanceFilterConfigurator::DefaultFilterMsgMask;
+const unsigned CanAcceptanceFilterConfigurator::DefaultFilterServiceRequestID;
+const unsigned CanAcceptanceFilterConfigurator::DefaultFilterServiceRequestMask;
+const unsigned CanAcceptanceFilterConfigurator::ServiceRespFrameID;
+const unsigned CanAcceptanceFilterConfigurator::ServiceRespFrameMask;
+
+int16_t CanAcceptanceFilterConfigurator::loadInputConfiguration()
+{
+    multiset_configs_.clear();
+
+    CanFilterConfig service_resp_cfg;
+    service_resp_cfg.id = ServiceRespFrameID;
+    service_resp_cfg.mask = ServiceRespFrameMask;
+    if (multiset_configs_.emplace(service_resp_cfg) == NULL)
+    {
+        return -ErrMemory;
+    }
+
+    const TransferListenerBase* p = node_.getDispatcher().getListOfMessageListeners().get();
+    while (p)
+    {
+        CanFilterConfig cfg;
+        cfg.id = static_cast<uint32_t>(p->getDataTypeDescriptor().getID().get()) << 16;
+        cfg.id |= static_cast<uint32_t>(p->getDataTypeDescriptor().getKind()) << 8;
+        cfg.mask = DefaultFilterMsgMask;
+        if (multiset_configs_.emplace(cfg) == NULL)
+        {
+            return -ErrMemory;
+        }
+        p = p->getNextListNode();
+    }
+
+    const TransferListenerBase* p1 = node_.getDispatcher().getListOfServiceRequestListeners().get();
+    while (p1)
+    {
+        CanFilterConfig cfg;
+        cfg.id = DefaultFilterServiceRequestID;
+        cfg.id |= static_cast<uint32_t>(p1->getDataTypeDescriptor().getID().get()) << 17;
+        cfg.mask = DefaultFilterServiceRequestMask;
+        if (multiset_configs_.emplace(cfg) == NULL)
+        {
+            return -ErrMemory;
+        }
+        p1 = p1->getNextListNode();
+    }
+
+    if (multiset_configs_.getSize() == 0)
+    {
+        return -ErrLogic;
+    }
+
+#if UAVCAN_DEBUG
+    for (uint16_t i = 0; i < multiset_configs_.getSize(); i++)
+    {
+        UAVCAN_TRACE("CanAcceptanceFilterConfigurator::loadInputConfiguration()", "cfg.ID [%u] = %d", i,
+                     multiset_configs_.getByIndex(i)->id);
+        UAVCAN_TRACE("CanAcceptanceFilterConfigurator::loadInputConfiguration()", "cfg.MK [%u] = %d", i,
+                     multiset_configs_.getByIndex(i)->mask);
+    }
+#endif
+
+    return 0;
+}
+
+int16_t CanAcceptanceFilterConfigurator::computeConfiguration()
+{
+    const uint16_t acceptance_filters_number = getNumFilters();
+    if (acceptance_filters_number == 0)
+    {
+        UAVCAN_TRACE("CanAcceptanceFilter", "No HW filters available");
+        return -ErrDriver;
+    }
+    UAVCAN_ASSERT(multiset_configs_.getSize() != 0);
+
+    while (acceptance_filters_number < multiset_configs_.getSize())
+    {
+        uint16_t i_rank = 0, j_rank = 0;
+        uint8_t best_rank = 0;
+
+        const uint16_t multiset_array_size = static_cast<uint16_t>(multiset_configs_.getSize());
+
+        for (uint16_t i_ind = 0; i_ind < multiset_array_size - 1; i_ind++)
+        {
+            for (uint16_t j_ind = static_cast<uint8_t>(i_ind + 1); j_ind < multiset_array_size; j_ind++)
+            {
+                CanFilterConfig temp_config = mergeFilters(*multiset_configs_.getByIndex(i_ind),
+                                                           *multiset_configs_.getByIndex(j_ind));
+                uint8_t rank = countBits(temp_config.mask);
+                if (rank > best_rank)
+                {
+                    best_rank = rank;
+                    i_rank = i_ind;
+                    j_rank = j_ind;
+                }
+            }
+        }
+
+        *multiset_configs_.getByIndex(j_rank) = mergeFilters(*multiset_configs_.getByIndex(i_rank),
+                                                             *multiset_configs_.getByIndex(j_rank));
+        multiset_configs_.removeFirst(*multiset_configs_.getByIndex(i_rank));
+    }
+
+    UAVCAN_ASSERT(acceptance_filters_number >= multiset_configs_.getSize());
+
+    return 0;
+}
+
+int16_t CanAcceptanceFilterConfigurator::applyConfiguration(void)
+{
+    CanFilterConfig filter_conf_array[MaxCanAcceptanceFilters];
+    const unsigned int filter_array_size = multiset_configs_.getSize();
+
+    if (filter_array_size > MaxCanAcceptanceFilters)
+    {
+        UAVCAN_ASSERT(0);
+        return -ErrLogic;
+    }
+
+    for (uint16_t i = 0; i < filter_array_size; i++)
+    {
+        CanFilterConfig temp_filter_config = *multiset_configs_.getByIndex(i);
+
+        filter_conf_array[i] = temp_filter_config;
+    }
+
+    ICanDriver& can_driver = node_.getDispatcher().getCanIOManager().getCanDriver();
+    for (uint8_t i = 0; i < node_.getDispatcher().getCanIOManager().getNumIfaces(); i++)
+    {
+        ICanIface* iface = can_driver.getIface(i);
+        if (iface == NULL)
+        {
+            return -ErrDriver;
+        }
+        int16_t num = iface->configureFilters(reinterpret_cast<CanFilterConfig*>(&filter_conf_array),
+                                              static_cast<uint16_t>(filter_array_size));
+        if (num < 0)
+        {
+            return -ErrDriver;
+        }
+    }
+
+    return 0;
+}
+
+int CanAcceptanceFilterConfigurator::configureFilters()
+{
+    if (getNumFilters() == 0)
+    {
+        UAVCAN_TRACE("CanAcceptanceFilter", "No HW filters available");
+        return -ErrDriver;
+    }
+
+    int16_t fill_array_error = loadInputConfiguration();
+    if (fill_array_error != 0)
+    {
+        UAVCAN_TRACE("CanAcceptanceFilter::loadInputConfiguration", "Failed to execute loadInputConfiguration()");
+        return fill_array_error;
+    }
+
+    int16_t compute_configuration_error = computeConfiguration();
+    if (compute_configuration_error != 0)
+    {
+        UAVCAN_TRACE("CanAcceptanceFilter", "Failed to compute optimal acceptance fliter's configuration");
+        return compute_configuration_error;
+    }
+
+    if (applyConfiguration() != 0)
+    {
+        UAVCAN_TRACE("CanAcceptanceFilter", "Failed to apply HW filter configuration");
+        return -ErrDriver;
+    }
+
+    return 0;
+}
+
+uint16_t CanAcceptanceFilterConfigurator::getNumFilters() const
+{
+    static const uint16_t InvalidOut = 0xFFFF;
+    uint16_t out = InvalidOut;
+    ICanDriver& can_driver = node_.getDispatcher().getCanIOManager().getCanDriver();
+
+    for (uint8_t i = 0; i < node_.getDispatcher().getCanIOManager().getNumIfaces(); i++)
+    {
+        const ICanIface* iface = can_driver.getIface(i);
+        if (iface == NULL)
+        {
+            UAVCAN_ASSERT(0);
+            out = 0;
+            break;
+        }
+        const uint16_t num = iface->getNumFilters();
+        out = min(out, num);
+        if (out > MaxCanAcceptanceFilters)
+        {
+            out = MaxCanAcceptanceFilters;
+        }
+    }
+
+    return (out == InvalidOut) ? 0 : out;
+}
+
+CanFilterConfig CanAcceptanceFilterConfigurator::mergeFilters(CanFilterConfig& a_, CanFilterConfig& b_)
+{
+    CanFilterConfig temp_arr;
+    temp_arr.mask = a_.mask & b_.mask & ~(a_.id ^ b_.id);
+    temp_arr.id = a_.id & temp_arr.mask;
+
+    return temp_arr;
+}
+
+uint8_t CanAcceptanceFilterConfigurator::countBits(uint32_t n_)
+{
+    uint8_t c_; // c accumulates the total bits set in v
+    for (c_ = 0; n_; c_++)
+    {
+        n_ &= n_ - 1; // clear the least significant bit set
+    }
+
+    return c_;
+}
+
+}

libuavcan/test/node/test_node.hpp

@@ -14,7 +14,8 @@
 #include <set>
 #include <queue>
 #include "../transport/can/can.hpp"
-
+#include <uavcan/util/method_binder.hpp>
+#include <uavcan/node/subscriber.hpp>
 
 struct TestNode : public uavcan::INode
 {

libuavcan/test/transport/can/can.hpp

@@ -150,9 +150,9 @@ public:
 
     // cppcheck-suppress unusedFunction
     // cppcheck-suppress functionConst
-    virtual uavcan::int16_t configureFilters(const uavcan::CanFilterConfig*, uavcan::uint16_t) { return -1; }
+    virtual uavcan::int16_t configureFilters(const uavcan::CanFilterConfig*, uavcan::uint16_t) { return 0; }
     // cppcheck-suppress unusedFunction
-    virtual uavcan::uint16_t getNumFilters() const { return 0; }
+    virtual uavcan::uint16_t getNumFilters() const { return 4; } // decrease number of HW_filters from 9 to 4
     virtual uavcan::uint64_t getErrorCount() const { return num_errors; }
 };
 

libuavcan/test/transport/can_acceptance_filter_configurator.cpp

@@ -0,0 +1,157 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>,
+ *                    Ilia  Sheremet <illia.sheremet@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <cassert>
+
+#include <uavcan/transport/can_acceptance_filter_configurator.hpp>
+#include "../node/test_node.hpp"
+#include "uavcan/node/subscriber.hpp"
+#include <uavcan/equipment/camera_gimbal/AngularCommand.hpp>
+#include <uavcan/equipment/air_data/Sideslip.hpp>
+#include <uavcan/equipment/air_data/TrueAirspeed.hpp>
+#include <uavcan/equipment/air_data/AngleOfAttack.hpp>
+#include <uavcan/equipment/ahrs/AHRS.hpp>
+#include <uavcan/equipment/air_data/StaticPressure.hpp>
+#include <uavcan/protocol/file/BeginFirmwareUpdate.hpp>
+#include <uavcan/node/service_client.hpp>
+#include <uavcan/node/service_server.hpp>
+#include <iostream>
+#include <bitset>
+
+#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
+
+template <typename DataType>
+struct SubscriptionListener
+{
+    typedef uavcan::ReceivedDataStructure<DataType> ReceivedDataStructure;
+
+    struct ReceivedDataStructureCopy
+    {
+        uavcan::MonotonicTime ts_monotonic;
+        uavcan::UtcTime ts_utc;
+        uavcan::TransferType transfer_type;
+        uavcan::TransferID transfer_id;
+        uavcan::NodeID src_node_id;
+        uavcan::uint8_t iface_index;
+        DataType msg;
+
+        ReceivedDataStructureCopy(const ReceivedDataStructure& s) :
+            ts_monotonic(s.getMonotonicTimestamp()),
+            ts_utc(s.getUtcTimestamp()),
+            transfer_type(s.getTransferType()),
+            transfer_id(s.getTransferID()),
+            src_node_id(s.getSrcNodeID()),
+            iface_index(s.getIfaceIndex()),
+            msg(s)
+        { }
+    };
+
+    std::vector<DataType> simple;
+    std::vector<ReceivedDataStructureCopy> extended;
+
+    void receiveExtended(ReceivedDataStructure& msg)
+    {
+        extended.push_back(msg);
+    }
+
+    void receiveSimple(DataType& msg)
+    {
+        simple.push_back(msg);
+    }
+
+    typedef SubscriptionListener<DataType> SelfType;
+    typedef uavcan::MethodBinder<SelfType*, void(SelfType::*) (ReceivedDataStructure&)> ExtendedBinder;
+    typedef uavcan::MethodBinder<SelfType*, void(SelfType::*) (DataType&)> SimpleBinder;
+
+    ExtendedBinder bindExtended() { return ExtendedBinder(this, &SelfType::receiveExtended); }
+    SimpleBinder bindSimple() { return SimpleBinder(this, &SelfType::receiveSimple); }
+};
+
+static void writeServiceServerCallback(
+    const uavcan::ReceivedDataStructure<uavcan::protocol::file::BeginFirmwareUpdate::Request>& req,
+    uavcan::protocol::file::BeginFirmwareUpdate::Response& rsp)
+{
+    std::cout << req << std::endl;
+    rsp.error = rsp.ERROR_UNKNOWN;
+}
+
+TEST(CanAcceptanceFilter, Basic_test)
+{
+    uavcan::GlobalDataTypeRegistry::instance().reset();
+    uavcan::DefaultDataTypeRegistrator<uavcan::equipment::camera_gimbal::AngularCommand> _reg1;
+    uavcan::DefaultDataTypeRegistrator<uavcan::equipment::air_data::Sideslip> _reg2;
+    uavcan::DefaultDataTypeRegistrator<uavcan::equipment::air_data::TrueAirspeed> _reg3;
+    uavcan::DefaultDataTypeRegistrator<uavcan::equipment::air_data::AngleOfAttack> _reg4;
+    uavcan::DefaultDataTypeRegistrator<uavcan::equipment::ahrs::AHRS> _reg5;
+    uavcan::DefaultDataTypeRegistrator<uavcan::equipment::air_data::StaticPressure> _reg6;
+    uavcan::DefaultDataTypeRegistrator<uavcan::protocol::file::BeginFirmwareUpdate> _reg7;
+
+    SystemClockDriver clock_driver;
+    CanDriverMock can_driver(1, clock_driver);
+    TestNode node(can_driver, clock_driver, 24);
+
+    uavcan::Subscriber<uavcan::equipment::camera_gimbal::AngularCommand,
+                    SubscriptionListener<uavcan::equipment::camera_gimbal::AngularCommand>::ExtendedBinder> sub_1(node);
+    uavcan::Subscriber<uavcan::equipment::air_data::Sideslip,
+                       SubscriptionListener<uavcan::equipment::air_data::Sideslip>::ExtendedBinder> sub_2(node);
+    uavcan::Subscriber<uavcan::equipment::air_data::TrueAirspeed,
+                       SubscriptionListener<uavcan::equipment::air_data::TrueAirspeed>::ExtendedBinder> sub_3(node);
+    uavcan::Subscriber<uavcan::equipment::air_data::AngleOfAttack,
+                       SubscriptionListener<uavcan::equipment::air_data::AngleOfAttack>::ExtendedBinder> sub_4(node);
+    uavcan::Subscriber<uavcan::equipment::ahrs::AHRS,
+                       SubscriptionListener<uavcan::equipment::ahrs::AHRS>::ExtendedBinder> sub_5(node);
+    uavcan::Subscriber<uavcan::equipment::air_data::StaticPressure,
+                       SubscriptionListener<uavcan::equipment::air_data::StaticPressure>::ExtendedBinder> sub_6(node);
+    uavcan::Subscriber<uavcan::equipment::air_data::StaticPressure,
+                       SubscriptionListener<uavcan::equipment::air_data::StaticPressure>::ExtendedBinder> sub_6_1(node);
+    uavcan::ServiceServer<uavcan::protocol::file::BeginFirmwareUpdate> server(node);
+
+    SubscriptionListener<uavcan::equipment::camera_gimbal::AngularCommand> listener_1;
+    SubscriptionListener<uavcan::equipment::air_data::Sideslip> listener_2;
+    SubscriptionListener<uavcan::equipment::air_data::TrueAirspeed> listener_3;
+    SubscriptionListener<uavcan::equipment::air_data::AngleOfAttack> listener_4;
+    SubscriptionListener<uavcan::equipment::ahrs::AHRS> listener_5;
+    SubscriptionListener<uavcan::equipment::air_data::StaticPressure> listener_6;
+
+    sub_1.start(listener_1.bindExtended());
+    sub_2.start(listener_2.bindExtended());
+    sub_3.start(listener_3.bindExtended());
+    sub_4.start(listener_4.bindExtended());
+    sub_5.start(listener_5.bindExtended());
+    sub_6.start(listener_6.bindExtended());
+    sub_6_1.start(listener_6.bindExtended());
+    server.start(writeServiceServerCallback);
+    std::cout << "Subscribers are initialized ..." << std::endl;
+
+    uavcan::CanAcceptanceFilterConfigurator test_configurator(node);
+    int configure_filters_assert = test_configurator.configureFilters();
+    if (configure_filters_assert == 0)
+    {
+        std::cout << "Filters are configured ..." << std::endl;
+    }
+
+    const auto& configure_array = test_configurator.getConfiguration();
+    uint32_t configure_array_size = configure_array.getSize();
+
+    ASSERT_EQ(configure_filters_assert, 0);
+    ASSERT_EQ(configure_array_size, 4);
+
+    for (uint16_t i = 0; i<configure_array_size; i++)
+    {
+        std::cout << "config.ID [" << i << "]= " << configure_array.getByIndex(i)->id << std::endl;
+        std::cout << "config.MK [" << i << "]= " << configure_array.getByIndex(i)->mask << std::endl;
+    }
+
+    ASSERT_EQ(configure_array.getByIndex(0)->id, 268435456);
+    ASSERT_EQ(configure_array.getByIndex(0)->mask, 469762048);
+    ASSERT_EQ(configure_array.getByIndex(1)->id, 363069440);
+    ASSERT_EQ(configure_array.getByIndex(1)->mask, 536739840);
+    ASSERT_EQ(configure_array.getByIndex(2)->id, 16777216);
+    ASSERT_EQ(configure_array.getByIndex(2)->mask, 124452864);
+    ASSERT_EQ(configure_array.getByIndex(3)->id, 18874368);
+    ASSERT_EQ(configure_array.getByIndex(3)->mask, 133169152);
+}
+#endif

libuavcan_drivers/lpc11c24/driver/include/uavcan_lpc11c24/can.hpp

@@ -48,7 +48,7 @@ public:
                                  uavcan::CanIOFlags flags);
 
     virtual uavcan::int16_t receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic,
-                            uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags);
+                                    uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags);
 
     virtual uavcan::int16_t select(uavcan::CanSelectMasks& inout_masks, uavcan::MonotonicTime blocking_deadline);
 

libuavcan_drivers/lpc11c24/driver/src/can.cpp

@@ -348,6 +348,11 @@ uavcan::ICanIface* CanDriver::getIface(uavcan::uint8_t iface_index)
     return (iface_index == 0) ? this : NULL;
 }
 
+const uavcan::ICanIface* CanDriver::getIface(uavcan::uint8_t iface_index) const
+{
+    return (iface_index == 0) ? this : NULL;
+}
+
 uavcan::uint8_t CanDriver::getNumIfaces() const
 {
     return 1;