From 5563dbacff677c88f74f44a805a206b88dc44efe Mon Sep 17 00:00:00 2001 From: Ilia Date: Wed, 16 Dec 2015 00:33:00 +0000 Subject: [PATCH] addFilterConfig() added. computeConfiguration() separated from applyConfiguration. Other small corrections --- .../can_acceptance_filter_configurator.hpp | 44 +++++---- .../uc_can_acceptance_filter_configurator.cpp | 95 ++++++++++++------- 2 files changed, 88 insertions(+), 51 deletions(-) diff --git a/libuavcan/include/uavcan/transport/can_acceptance_filter_configurator.hpp b/libuavcan/include/uavcan/transport/can_acceptance_filter_configurator.hpp index e7c7d6e7a1..d73adda99e 100644 --- a/libuavcan/include/uavcan/transport/can_acceptance_filter_configurator.hpp +++ b/libuavcan/include/uavcan/transport/can_acceptance_filter_configurator.hpp @@ -21,10 +21,11 @@ namespace uavcan * 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. + * By means of computeConfiguration() method the class determines the number of available HW filters and the number + * of listeners. In case if custom configuration required, it is possible to add it through addFilterConfig(). + * Subsequently obtained configurations are then loaded into the CAN driver by calling the applyConfiguration() method. + * If the cumulative number of configurations obtained by computeConfiguration() and addFilterConfig() is higher than + * the number of available HW filters, configurations will be merged automatically in the most efficient way. * * 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 @@ -57,7 +58,7 @@ private: * HW filters. * DefaultAnonMsgMask = 00000 00000000 00000000 11111111 * DefaultAnonMsgID = 00000 00000000 00000000 00000000, by default the config is added to accept all anonymous - * frames. In case there are no anonymous messages, invoke configureFilters(IgnoreAnonymousMessages). + * frames. In case there are no anonymous messages, invoke computeConfiguration(IgnoreAnonymousMessages). */ static const unsigned DefaultFilterMsgMask = 0xFFFF80; static const unsigned DefaultFilterServiceMask = 0x7F80; @@ -72,7 +73,7 @@ private: uint16_t getNumFilters() const; /** - * Fills the multiset_configs_ to proceed it with computeConfiguration() + * Fills the multiset_configs_ to proceed it with mergeConfigurations() */ int16_t loadInputConfiguration(AnonymousMessages load_mode); @@ -80,35 +81,44 @@ private: * 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(); + int16_t mergeConfigurations(); INode& node_; //< Node reference is needed for access to ICanDriver and Dispatcher MultisetConfigContainer multiset_configs_; + uint16_t filters_number_; public: - explicit CanAcceptanceFilterConfigurator(INode& node) + explicit CanAcceptanceFilterConfigurator(INode& node, uint16_t filters_number = 0) : node_(node) , multiset_configs_(node.getAllocator()) + , filters_number_(filters_number) { } /** - * This method invokes loadInputConfiguration(), computeConfiguration() and applyConfiguration() consequently, so + * This method invokes loadInputConfiguration() and mergeConfigurations() consequently, so * that optimal acceptance filter configuration will be computed and loaded through CanDriver::configureFilters() * * @param mode Either: AcceptAnonymousMessages - the filters will accept all anonymous messages (this is default) * IgnoreAnonymousMessages - anonymous messages will be ignored * @return 0 = success, negative for error. */ - int configureFilters(AnonymousMessages mode = AcceptAnonymousMessages); + int computeConfiguration(AnonymousMessages mode = AcceptAnonymousMessages); /** - * Returns the configuration computed with computeConfiguration(). - * If computeConfiguration() has not been called yet, an empty configuration will be returned. + * Add the additional filter configuration to multiset_configs_. This method should be invoked only before + * computeConfiguration() member. + */ + int16_t addFilterConfig(const CanFilterConfig& config); + + /** + * This method loads the configuration computed with mergeConfigurations() or explicitly added by addFilterConfig() + * to the CAN driver. Must be called after computeConfiguration() and addFilterConfig(). + */ + int16_t applyConfiguration(); + + /** + * Returns the configuration computed with mergeConfigurations() or added by addFilterConfig(). + * If mergeConfigurations() or addFilterConfig() has not been called yet, an empty configuration will be returned. */ const MultisetConfigContainer& getConfiguration() const { diff --git a/libuavcan/src/transport/uc_can_acceptance_filter_configurator.cpp b/libuavcan/src/transport/uc_can_acceptance_filter_configurator.cpp index 820c0b22d2..448ebe8405 100644 --- a/libuavcan/src/transport/uc_can_acceptance_filter_configurator.cpp +++ b/libuavcan/src/transport/uc_can_acceptance_filter_configurator.cpp @@ -16,24 +16,22 @@ const unsigned CanAcceptanceFilterConfigurator::DefaultAnonMsgID; int16_t CanAcceptanceFilterConfigurator::loadInputConfiguration(AnonymousMessages load_mode) { - multiset_configs_.clear(); - if (load_mode == AcceptAnonymousMessages) { CanFilterConfig anon_frame_cfg; - anon_frame_cfg.id = DefaultAnonMsgID; - anon_frame_cfg.mask = DefaultAnonMsgMask; + anon_frame_cfg.id = DefaultAnonMsgID | CanFrame::FlagEFF; + anon_frame_cfg.mask = DefaultAnonMsgMask | CanFrame::FlagEFF | CanFrame::FlagRTR | CanFrame::FlagERR; if (multiset_configs_.emplace(anon_frame_cfg) == NULL) { return -ErrMemory; } } - CanFilterConfig service_resp_cfg; - service_resp_cfg.id = DefaultFilterServiceID; - service_resp_cfg.id |= static_cast(node_.getNodeID().get()) << 8; - service_resp_cfg.mask = DefaultFilterServiceMask; - if (multiset_configs_.emplace(service_resp_cfg) == NULL) + CanFilterConfig service_cfg; + service_cfg.id = DefaultFilterServiceID; + service_cfg.id |= (static_cast(node_.getNodeID().get()) << 8) | CanFrame::FlagEFF; + service_cfg.mask = DefaultFilterServiceMask | CanFrame::FlagEFF | CanFrame::FlagRTR | CanFrame::FlagERR; + if (multiset_configs_.emplace(service_cfg) == NULL) { return -ErrMemory; } @@ -42,8 +40,8 @@ int16_t CanAcceptanceFilterConfigurator::loadInputConfiguration(AnonymousMessage while (p != NULL) { CanFilterConfig cfg; - cfg.id = static_cast(p->getDataTypeDescriptor().getID().get()) << 8; - cfg.mask = DefaultFilterMsgMask; + cfg.id = (static_cast(p->getDataTypeDescriptor().getID().get()) << 8) | CanFrame::FlagEFF; + cfg.mask = DefaultFilterMsgMask | CanFrame::FlagEFF | CanFrame::FlagRTR | CanFrame::FlagERR; if (multiset_configs_.emplace(cfg) == NULL) { return -ErrMemory; @@ -56,20 +54,10 @@ int16_t CanAcceptanceFilterConfigurator::loadInputConfiguration(AnonymousMessage 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() +int16_t CanAcceptanceFilterConfigurator::mergeConfigurations() { const uint16_t acceptance_filters_number = getNumFilters(); if (acceptance_filters_number == 0) @@ -115,12 +103,27 @@ int16_t CanAcceptanceFilterConfigurator::computeConfiguration() int16_t CanAcceptanceFilterConfigurator::applyConfiguration(void) { CanFilterConfig filter_conf_array[MaxCanAcceptanceFilters]; - const unsigned int filter_array_size = multiset_configs_.getSize(); + unsigned int filter_array_size = multiset_configs_.getSize(); + + const uint16_t acceptance_filters_number = getNumFilters(); + if (acceptance_filters_number == 0) + { + UAVCAN_TRACE("CanAcceptanceFilter", "No HW filters available"); + return -ErrDriver; + } + + if (filter_array_size > acceptance_filters_number) + { + UAVCAN_TRACE("CanAcceptanceFilter", "Too many filter configurations. Executing computeConfiguration()"); + computeConfiguration(IgnoreAnonymousMessages); + filter_array_size = multiset_configs_.getSize(); + } if (filter_array_size > MaxCanAcceptanceFilters) { UAVCAN_ASSERT(0); return -ErrLogic; + UAVCAN_TRACE("CanAcceptanceFilter", "Failed to apply HW filter configuration"); } for (uint16_t i = 0; i < filter_array_size; i++) @@ -130,6 +133,16 @@ int16_t CanAcceptanceFilterConfigurator::applyConfiguration(void) filter_conf_array[i] = temp_filter_config; } +#if UAVCAN_DEBUG + for (uint16_t i = 0; i < multiset_configs_.getSize(); i++) + { + UAVCAN_TRACE("CanAcceptanceFilterConfigurator::applyConfiguration()", "cfg.ID [%u] = %d", i, + multiset_configs_.getByIndex(i)->id); + UAVCAN_TRACE("CanAcceptanceFilterConfigurator::applyConfiguration()", "cfg.MK [%u] = %d", i, + multiset_configs_.getByIndex(i)->mask); + } +#endif + ICanDriver& can_driver = node_.getDispatcher().getCanIOManager().getCanDriver(); for (uint8_t i = 0; i < node_.getDispatcher().getCanIOManager().getNumIfaces(); i++) { @@ -137,19 +150,21 @@ int16_t CanAcceptanceFilterConfigurator::applyConfiguration(void) if (iface == NULL) { return -ErrDriver; + UAVCAN_TRACE("CanAcceptanceFilter", "Failed to apply HW filter configuration"); } int16_t num = iface->configureFilters(reinterpret_cast(&filter_conf_array), static_cast(filter_array_size)); if (num < 0) { return -ErrDriver; + UAVCAN_TRACE("CanAcceptanceFilter", "Failed to apply HW filter configuration"); } } return 0; } -int CanAcceptanceFilterConfigurator::configureFilters(AnonymousMessages mode) +int CanAcceptanceFilterConfigurator::computeConfiguration(AnonymousMessages mode) { if (getNumFilters() == 0) { @@ -164,17 +179,11 @@ int CanAcceptanceFilterConfigurator::configureFilters(AnonymousMessages mode) return fill_array_error; } - int16_t compute_configuration_error = computeConfiguration(); - if (compute_configuration_error != 0) + int16_t merge_configurations_error = mergeConfigurations(); + if (merge_configurations_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 merge_configurations_error; } return 0; @@ -182,8 +191,10 @@ int CanAcceptanceFilterConfigurator::configureFilters(AnonymousMessages mode) uint16_t CanAcceptanceFilterConfigurator::getNumFilters() const { + if (filters_number_ == 0) + { static const uint16_t InvalidOut = 0xFFFF; - uint16_t out = InvalidOut; + uint16_t out = InvalidOut; ICanDriver& can_driver = node_.getDispatcher().getCanIOManager().getCanDriver(); for (uint8_t i = 0; i < node_.getDispatcher().getCanIOManager().getNumIfaces(); i++) @@ -204,6 +215,22 @@ uint16_t CanAcceptanceFilterConfigurator::getNumFilters() const } return (out == InvalidOut) ? 0 : out; + } + else + { + return filters_number_; + } +} + +int16_t CanAcceptanceFilterConfigurator::addFilterConfig(const CanFilterConfig& config) +{ + CanFilterConfig supl_config = config; + if (multiset_configs_.emplace(supl_config) == NULL) + { + return -ErrMemory; + } + + return 0; } CanFilterConfig CanAcceptanceFilterConfigurator::mergeFilters(CanFilterConfig& a_, CanFilterConfig& b_)