fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #82 from ilia-sheremet/master

Browse Source 
Can_acceptance_filter_configurator and unit test are corrected
This commit is contained in:
Pavel Kirienko 2015-12-27 20:04:01 +03:00
commit 4c03b29401
4 changed files with 271 additions and 126 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

96
libuavcan/include/uavcan/transport/can_acceptance_filter_configurator.hpp
View File

@ -1,10 +1,10 @@
/*
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>,
* Copyright (C) 2015 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
#ifndef UAVCAN_TRANSPORT_CAN_ACCEPTANCE_FILTER_CONFIGURATOR_HPP_INCLUDED
#define UAVCAN_TRANSPORT_CAN_ACCEPTANCE_FILTER_CONFIGURATOR_HPP_INCLUDED
#include <cassert>
#include <uavcan/data_type.hpp>
@ -21,10 +21,14 @@ 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.
*
* Note that if the application adds additional server or subscriber objects after the filters have been configured,
* the configuration procedure will have to be performed again.
*
* 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 +61,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;
@ -67,12 +71,12 @@ private:
typedef uavcan::Multiset<CanFilterConfig> MultisetConfigContainer;
static CanFilterConfig mergeFilters(CanFilterConfig &a_, CanFilterConfig &b_);
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()
* Fills the multiset_configs_ to proceed it with mergeConfigurations()
*/
int16_t loadInputConfiguration(AnonymousMessages load_mode);
@ -80,35 +84,56 @@ 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)
/**
* @param node Libuavcan node whose subscribers/servers/etc will be used to configure the filters.
*
* @param filters_number Allows to override the maximum number of hardware filters to use.
* If set to zero (which is default), the class will obtain the number of available
* filters from the CAN driver via @ref ICanIface::getNumFilters().
*/
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
* that optimal acceptance filter configuration will be computed and loaded through CanDriver::configureFilters()
* This method invokes loadInputConfiguration() and mergeConfigurations() consequently
* in order to comute optimal filter configurations for the current hardware.
*
* It can only be invoked when all of the subscriber and server objects are initialized.
* If new subscriber or server objects are added later, the filters will have to be reconfigured again.
*
* @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 an additional filter configuration.
* This method must not be invoked after @ref computeConfiguration().
* @return 0 = success, negative for error.
*/
int 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().
* @return 0 = success, negative for error.
*/
int applyConfiguration();
/**
* Returns the configuration computed with mergeConfigurations() or added by addFilterConfig().
* If mergeConfigurations() or addFilterConfig() have not been called yet, an empty configuration will be returned.
*/
const MultisetConfigContainer& getConfiguration() const
{
@ -116,6 +141,31 @@ public:
}
};
/**
* This function is a shortcut for @ref CanAcceptanceFilterConfigurator.
* It allows to compute filter configuration and then apply it in just one step.
* It implements only the most common use case; if you have special requirements,
* use @ref CanAcceptanceFilterConfigurator directly.
*
* @param node Refer to @ref CanAcceptanceFilterConfigurator constructor for explanation.
* @param mode Refer to @ref CanAcceptanceFilterConfigurator::computeConfiguration() for explanation.
* @return non-negative on success, negative error code on error.
*/
static inline int configureCanAcceptanceFilters(INode& node,
CanAcceptanceFilterConfigurator::AnonymousMessages mode
= CanAcceptanceFilterConfigurator::AcceptAnonymousMessages)
{
CanAcceptanceFilterConfigurator cfger(node);
const int compute_res = cfger.computeConfiguration(mode);
if (compute_res < 0)
{
return compute_res;
}
return cfger.applyConfiguration();
}
#endif // UAVCAN_BUILD_CONFIG_HPP_INCLUDED
}
#endif

129
libuavcan/src/transport/uc_can_acceptance_filter_configurator.cpp
View File

@ -1,5 +1,5 @@
/*
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>,
* Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>,
* Ilia Sheremet <illia.sheremet@gmail.com>
*/
@ -21,19 +21,19 @@ int16_t CanAcceptanceFilterConfigurator::loadInputConfiguration(AnonymousMessage
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<uint32_t>(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<uint32_t>(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 +42,8 @@ int16_t CanAcceptanceFilterConfigurator::loadInputConfiguration(AnonymousMessage
while (p != NULL)
{
CanFilterConfig cfg;
cfg.id = static_cast<uint32_t>(p->getDataTypeDescriptor().getID().get()) << 8;
cfg.mask = DefaultFilterMsgMask;
cfg.id = (static_cast<uint32_t>(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 +56,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)
@ -112,14 +102,29 @@ int16_t CanAcceptanceFilterConfigurator::computeConfiguration()
return 0;
}
int16_t CanAcceptanceFilterConfigurator::applyConfiguration(void)
int 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()");
mergeConfigurations();
filter_array_size = multiset_configs_.getSize();
}
if (filter_array_size > MaxCanAcceptanceFilters)
{
UAVCAN_ASSERT(0);
UAVCAN_TRACE("CanAcceptanceFilter", "Failed to apply HW filter configuration");
return -ErrLogic;
}
@ -130,18 +135,30 @@ 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] = %u", i,
multiset_configs_.getByIndex(i)->id);
UAVCAN_TRACE("CanAcceptanceFilterConfigurator::applyConfiguration()", "cfg.MK [%u] = %u", 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++)
{
ICanIface* iface = can_driver.getIface(i);
if (iface == NULL)
{
UAVCAN_TRACE("CanAcceptanceFilter", "Failed to apply HW filter configuration");
return -ErrDriver;
}
int16_t num = iface->configureFilters(reinterpret_cast<CanFilterConfig*>(&filter_conf_array),
static_cast<uint16_t>(filter_array_size));
if (num < 0)
{
UAVCAN_TRACE("CanAcceptanceFilter", "Failed to apply HW filter configuration");
return -ErrDriver;
}
}
@ -149,7 +166,7 @@ int16_t CanAcceptanceFilterConfigurator::applyConfiguration(void)
return 0;
}
int CanAcceptanceFilterConfigurator::configureFilters(AnonymousMessages mode)
int CanAcceptanceFilterConfigurator::computeConfiguration(AnonymousMessages mode)
{
if (getNumFilters() == 0)
{
@ -164,17 +181,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,28 +193,45 @@ int CanAcceptanceFilterConfigurator::configureFilters(AnonymousMessages mode)
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++)
if (filters_number_ == 0)
{
const ICanIface* iface = can_driver.getIface(i);
if (iface == NULL)
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++)
{
UAVCAN_ASSERT(0);
out = 0;
break;
}
const uint16_t num = iface->getNumFilters();
out = min(out, num);
if (out > MaxCanAcceptanceFilters)
{
out = MaxCanAcceptanceFilters;
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;
}
else
{
return filters_number_;
}
}
int CanAcceptanceFilterConfigurator::addFilterConfig(const CanFilterConfig& config)
{
if (multiset_configs_.emplace<const CanFilterConfig&>(config) == NULL)
{
return -ErrMemory;
}
return (out == InvalidOut) ? 0 : out;
return 0;
}
CanFilterConfig CanAcceptanceFilterConfigurator::mergeFilters(CanFilterConfig& a_, CanFilterConfig& b_)
@ -225,5 +253,4 @@ uint8_t CanAcceptanceFilterConfigurator::countBits(uint32_t n_)
return c_;
}
}

101
libuavcan/test/transport/can_acceptance_filter_configurator.cpp
View File

@ -123,21 +123,41 @@ TEST(CanAcceptanceFilter, Basic_test)
sub_6.start(listener_6.bindExtended());
sub_6_1.start(listener_6.bindExtended());
server.start(writeServiceServerCallback);
std::cout << "Subscribers are initialized ..." << std::endl;
std::cout << "Subscribers are initialized." << std::endl;
uavcan::CanAcceptanceFilterConfigurator anon_test_configuration(node, 10);
uavcan::CanAcceptanceFilterConfigurator anon_test_configuration(node);
int configure_filters_assert = anon_test_configuration.configureFilters();
if (configure_filters_assert == 0)
{
std::cout << "Filters are configured with anonymous configuration..." << std::endl;
}
int configure_filters_assert = anon_test_configuration.computeConfiguration();
ASSERT_EQ(configure_filters_assert, 0);
std::cout << "Filters are calculated with anonymous configuration." << std::endl;
const auto& configure_array = anon_test_configuration.getConfiguration();
uint32_t configure_array_size = configure_array.getSize();
std::cout << "Number of configurations after first time computeConfiguration() invoked: "
<< configure_array_size << std::endl;
ASSERT_EQ(configure_array_size, 9);
std::cout << "Adding two additional configurations ... " << std::endl;
uavcan::CanFilterConfig aux_config_1, aux_config_2;
aux_config_1.id = 911;
aux_config_1.mask = 1488;
aux_config_2.id = 999999;
aux_config_2.mask = 849128412;
configure_filters_assert = anon_test_configuration.addFilterConfig(aux_config_1);
ASSERT_EQ(configure_filters_assert, 0);
ASSERT_EQ(configure_array_size, 4);
configure_filters_assert = anon_test_configuration.addFilterConfig(aux_config_2);
ASSERT_EQ(configure_filters_assert, 0);
configure_array_size = configure_array.getSize();
std::cout << "New configuration anon_container size: " << configure_array_size << std::endl;
ASSERT_EQ(configure_array_size, 11);
std::cout << "Applying configuration ... " << std::endl;
configure_filters_assert = anon_test_configuration.applyConfiguration();
ASSERT_EQ(configure_filters_assert, 0);
std::cout << "Filters are configured." << std::endl;
configure_array_size = configure_array.getSize();
std::cout << "Final configuration anon_container size: " << configure_array_size << std::endl;
ASSERT_EQ(configure_array_size, 10);
for (uint16_t i = 0; i<configure_array_size; i++)
{
@ -145,42 +165,55 @@ TEST(CanAcceptanceFilter, Basic_test)
std::cout << "config.MK [" << i << "]= " << configure_array.getByIndex(i)->mask << std::endl;
}
ASSERT_EQ(configure_array.getByIndex(0)->id, 256000);
ASSERT_EQ(configure_array.getByIndex(0)->mask, 16771968);
ASSERT_EQ(configure_array.getByIndex(1)->id, 0);
ASSERT_EQ(configure_array.getByIndex(1)->mask, 255);
ASSERT_EQ(configure_array.getByIndex(2)->id, 6272);
ASSERT_EQ(configure_array.getByIndex(2)->mask, 32640);
ASSERT_EQ(configure_array.getByIndex(3)->id, 262144);
ASSERT_EQ(configure_array.getByIndex(3)->mask, 16771200);
ASSERT_EQ(configure_array.getByIndex(0)->id, 911);
ASSERT_EQ(configure_array.getByIndex(0)->mask, 1488);
ASSERT_EQ(configure_array.getByIndex(1)->id, 999999);
ASSERT_EQ(configure_array.getByIndex(1)->mask, 849128412);
ASSERT_EQ(configure_array.getByIndex(2)->id, 2147746048);
ASSERT_EQ(configure_array.getByIndex(2)->mask, 3774873472);
ASSERT_EQ(configure_array.getByIndex(3)->id, 2147744768);
ASSERT_EQ(configure_array.getByIndex(3)->mask, 3774873472);
ASSERT_EQ(configure_array.getByIndex(4)->id, 2147739648);
ASSERT_EQ(configure_array.getByIndex(4)->mask, 3774873472);
ASSERT_EQ(configure_array.getByIndex(5)->id, 2147746816);
ASSERT_EQ(configure_array.getByIndex(5)->mask, 3774873472);
ASSERT_EQ(configure_array.getByIndex(6)->id, 2147746304);
ASSERT_EQ(configure_array.getByIndex(6)->mask, 3774873472);
ASSERT_EQ(configure_array.getByIndex(7)->id, 2147483648);
ASSERT_EQ(configure_array.getByIndex(7)->mask, 3758096639);
ASSERT_EQ(configure_array.getByIndex(8)->id, 2147489920);
ASSERT_EQ(configure_array.getByIndex(8)->mask, 3758129024);
ASSERT_EQ(configure_array.getByIndex(9)->id, 2147749888);
ASSERT_EQ(configure_array.getByIndex(9)->mask, 3774873472);
uavcan::CanAcceptanceFilterConfigurator no_anon_test_confiruration(node);
configure_filters_assert = no_anon_test_confiruration.configureFilters
uavcan::CanAcceptanceFilterConfigurator no_anon_test_confiruration(node, 4);
configure_filters_assert = no_anon_test_confiruration.computeConfiguration
(uavcan::CanAcceptanceFilterConfigurator::IgnoreAnonymousMessages);
if (configure_filters_assert == 0)
{
std::cout << "Filters are configured without anonymous configuration..." << std::endl;
}
ASSERT_EQ(configure_filters_assert, 0);
std::cout << "Filters are configured without anonymous configuration." << std::endl;
configure_filters_assert = no_anon_test_confiruration.applyConfiguration();
ASSERT_EQ(configure_filters_assert, 0);
std::cout << "Filters are configured without anonymous configuration." << std::endl;
const auto& configure_array_2 = no_anon_test_confiruration.getConfiguration();
configure_array_size = configure_array_2.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_2.getByIndex(i)->id << std::endl;
std::cout << "config.MK [" << i << "]= " << configure_array_2.getByIndex(i)->mask << std::endl;
std::cout << "config.ID [" << i << "] = " << configure_array_2.getByIndex(i)->id << std::endl;
std::cout << "config.MK [" << i << "] = " << configure_array_2.getByIndex(i)->mask << std::endl;
}
ASSERT_EQ(configure_array_2.getByIndex(0)->id, 256000);
ASSERT_EQ(configure_array_2.getByIndex(0)->mask, 16771968);
ASSERT_EQ(configure_array_2.getByIndex(1)->id, 262144);
ASSERT_EQ(configure_array_2.getByIndex(1)->mask, 16776320);
ASSERT_EQ(configure_array_2.getByIndex(2)->id, 6272);
ASSERT_EQ(configure_array_2.getByIndex(2)->mask, 32640);
ASSERT_EQ(configure_array_2.getByIndex(3)->id, 262144);
ASSERT_EQ(configure_array_2.getByIndex(3)->mask, 16771968);
ASSERT_EQ(configure_array_2.getByIndex(0)->id, 2147739648);
ASSERT_EQ(configure_array_2.getByIndex(0)->mask, 3774868352);
ASSERT_EQ(configure_array_2.getByIndex(1)->id, 2147745792);
ASSERT_EQ(configure_array_2.getByIndex(1)->mask, 3774872704);
ASSERT_EQ(configure_array_2.getByIndex(2)->id, 2147489920);
ASSERT_EQ(configure_array_2.getByIndex(2)->mask, 3758129024);
ASSERT_EQ(configure_array_2.getByIndex(3)->id, 2147745792);
ASSERT_EQ(configure_array_2.getByIndex(3)->mask, 3774868352);
}
#endif

71
libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp
View File

@ -1,5 +1,6 @@
/*
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
* Copyright (C) 2014-2015 Pavel Kirienko <pavel.kirienko@gmail.com>
* Ilia Sheremet <illia.sheremet@gmail.com>
*/
#pragma once
@ -58,7 +59,7 @@ class SocketCanIface : public uavcan::ICanIface
{
static inline ::can_frame makeSocketCanFrame(const uavcan::CanFrame& uavcan_frame)
{
::can_frame sockcan_frame { uavcan_frame.id & uavcan::CanFrame::MaskExtID, uavcan_frame.dlc, { } };
::can_frame sockcan_frame { uavcan_frame.id& uavcan::CanFrame::MaskExtID, uavcan_frame.dlc, { } };
(void)std::copy(uavcan_frame.data, uavcan_frame.data + uavcan_frame.dlc, sockcan_frame.data);
if (uavcan_frame.isExtended())
{
@ -148,6 +149,8 @@ class SocketCanIface : public uavcan::ICanIface
std::queue<RxItem> rx_queue_; // TODO: Use pool allocator
std::unordered_multiset<std::uint32_t> pending_loopback_ids_; // TODO: Use pool allocator
std::vector<::can_filter> hw_filters_container_;
void registerError(SocketCanError e) { errors_[e]++; }
void incrementNumFramesInSocketTxQueue()
@ -225,6 +228,16 @@ class SocketCanIface : public uavcan::ICanIface
{
return (res < 0 && errno == EWOULDBLOCK) ? 0 : res;
}
/*
* Flags
*/
loopback = (msg.msg_flags & static_cast<int>(MSG_CONFIRM)) != 0;
if (!loopback && !checkHWFilters(sockcan_frame))
{
return 0;
}
frame = makeUavcanFrame(sockcan_frame);
/*
* Timestamp
@ -243,10 +256,6 @@ class SocketCanIface : public uavcan::ICanIface
assert(0);
return -1;
}
/*
* Flags
*/
loopback = (msg.msg_flags & static_cast<int>(MSG_CONFIRM)) != 0;
return 1;
}
@ -315,6 +324,28 @@ class SocketCanIface : public uavcan::ICanIface
}
}
/**
* Returns true if a frame accepted by HW filters
*/
bool checkHWFilters(const ::can_frame& frame) const
{
if (!hw_filters_container_.empty())
{
for (auto& f : hw_filters_container_)
{
if (((frame.can_id & f.can_mask) ^ f.can_id) == 0)
{
return true;
}
}
return false;
}
else
{
return true;
}
}
public:
/**
* Takes ownership of socket's file descriptor.
@ -404,38 +435,42 @@ public:
assert(0);
return -1;
}
std::vector< ::can_filter> filts(num_configs);
hw_filters_container_.clear();
hw_filters_container_.resize(num_configs);
for (unsigned i = 0; i < num_configs; i++)
{
const uavcan::CanFilterConfig& fc = filter_configs[i];
filts[i].can_id = fc.id & uavcan::CanFrame::MaskExtID;
filts[i].can_mask = fc.mask & uavcan::CanFrame::MaskExtID;
hw_filters_container_[i].can_id = fc.id & uavcan::CanFrame::MaskExtID;
hw_filters_container_[i].can_mask = fc.mask & uavcan::CanFrame::MaskExtID;
if (fc.id & uavcan::CanFrame::FlagEFF)
{
filts[i].can_id |= CAN_EFF_FLAG;
hw_filters_container_[i].can_id |= CAN_EFF_FLAG;
}
if (fc.id & uavcan::CanFrame::FlagRTR)
{
filts[i].can_id |= CAN_RTR_FLAG;
hw_filters_container_[i].can_id |= CAN_RTR_FLAG;
}
if (fc.mask & uavcan::CanFrame::FlagEFF)
{
filts[i].can_mask |= CAN_EFF_FLAG;
hw_filters_container_[i].can_mask |= CAN_EFF_FLAG;
}
if (fc.mask & uavcan::CanFrame::FlagRTR)
{
filts[i].can_mask |= CAN_RTR_FLAG;
hw_filters_container_[i].can_mask |= CAN_RTR_FLAG;
}
}
int ret = setsockopt(fd_, SOL_CAN_RAW, CAN_RAW_FILTER, filts.data(), sizeof(::can_filter) * num_configs);
return (ret < 0) ? -1 : 0;
return 0;
}
/**
* SocketCAN emulates the CAN filters in software, so the number of filters is virtually unlimited.
* This method returns a constant value.
*/
std::uint16_t getNumFilters() const override { return 255; }
static constexpr unsigned NumFilters = 8;
std::uint16_t getNumFilters() const override { return NumFilters; }
/**
* Returns total number of errors of each kind detected since the object was created.
@ -450,7 +485,7 @@ public:
/**
* Returns number of errors of each kind in a map.
*/
const decltype(errors_)& getErrors() const { return errors_; }
const decltype(errors_) & getErrors() const { return errors_; }
int getFileDescriptor() const { return fd_; }
@ -501,7 +536,7 @@ public:
goto fail;
}
// Non-blocking
if (::fcntl(s, F_SETFL , O_NONBLOCK) < 0)
if (::fcntl(s, F_SETFL, O_NONBLOCK) < 0)
{
goto fail;
}