mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-11-22 06:53:19 +08:00
pull request corrections
This commit is contained in:
@@ -16,6 +16,8 @@ const unsigned CanAcceptanceFilterConfigurator::DefaultAnonMsgID;
|
||||
|
||||
int16_t CanAcceptanceFilterConfigurator::loadInputConfiguration(AnonymousMessages load_mode)
|
||||
{
|
||||
multiset_configs_.clear();
|
||||
|
||||
if (load_mode == AcceptAnonymousMessages)
|
||||
{
|
||||
CanFilterConfig anon_frame_cfg;
|
||||
@@ -115,15 +117,15 @@ int16_t CanAcceptanceFilterConfigurator::applyConfiguration(void)
|
||||
if (filter_array_size > acceptance_filters_number)
|
||||
{
|
||||
UAVCAN_TRACE("CanAcceptanceFilter", "Too many filter configurations. Executing computeConfiguration()");
|
||||
computeConfiguration(IgnoreAnonymousMessages);
|
||||
mergeConfigurations();
|
||||
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");
|
||||
return -ErrLogic;
|
||||
}
|
||||
|
||||
for (uint16_t i = 0; i < filter_array_size; i++)
|
||||
@@ -149,15 +151,15 @@ int16_t CanAcceptanceFilterConfigurator::applyConfiguration(void)
|
||||
ICanIface* iface = can_driver.getIface(i);
|
||||
if (iface == NULL)
|
||||
{
|
||||
return -ErrDriver;
|
||||
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)
|
||||
{
|
||||
return -ErrDriver;
|
||||
UAVCAN_TRACE("CanAcceptanceFilter", "Failed to apply HW filter configuration");
|
||||
return -ErrDriver;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -194,7 +196,7 @@ 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++)
|
||||
@@ -224,8 +226,7 @@ uint16_t CanAcceptanceFilterConfigurator::getNumFilters() const
|
||||
|
||||
int16_t CanAcceptanceFilterConfigurator::addFilterConfig(const CanFilterConfig& config)
|
||||
{
|
||||
CanFilterConfig supl_config = config;
|
||||
if (multiset_configs_.emplace(supl_config) == NULL)
|
||||
if (multiset_configs_.emplace<const CanFilterConfig&>(config) == NULL)
|
||||
{
|
||||
return -ErrMemory;
|
||||
}
|
||||
|
||||
@@ -127,28 +127,20 @@ TEST(CanAcceptanceFilter, Basic_test)
|
||||
|
||||
uavcan::CanAcceptanceFilterConfigurator anon_test_configuration(node, 10);
|
||||
|
||||
uavcan::CanFilterConfig aux_config_1, aux_config_2;
|
||||
aux_config_1.id = 0;
|
||||
aux_config_1.mask = 1488;
|
||||
aux_config_2.id = 24;
|
||||
aux_config_2.mask = 72;
|
||||
int configure_filters_assert;
|
||||
configure_filters_assert = anon_test_configuration.addFilterConfig(aux_config_1);
|
||||
ASSERT_EQ(configure_filters_assert, 0);
|
||||
configure_filters_assert = anon_test_configuration.addFilterConfig(aux_config_2);
|
||||
ASSERT_EQ(configure_filters_assert, 0);
|
||||
|
||||
configure_filters_assert = anon_test_configuration.computeConfiguration();
|
||||
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 configs after first time computeConfiguration() invoked: "
|
||||
std::cout << "Number of configurations after first time computeConfiguration() invoked: "
|
||||
<< configure_array_size << std::endl;
|
||||
ASSERT_EQ(configure_array_size, 10);
|
||||
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);
|
||||
@@ -157,7 +149,7 @@ TEST(CanAcceptanceFilter, Basic_test)
|
||||
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, 12);
|
||||
ASSERT_EQ(configure_array_size, 11);
|
||||
|
||||
std::cout << "Applying configuration ... " << std::endl;
|
||||
configure_filters_assert = anon_test_configuration.applyConfiguration();
|
||||
@@ -173,26 +165,26 @@ TEST(CanAcceptanceFilter, Basic_test)
|
||||
std::cout << "config.MK [" << i << "]= " << configure_array.getByIndex(i)->mask << std::endl;
|
||||
}
|
||||
|
||||
ASSERT_EQ(configure_array.getByIndex(0)->id, 2147489920);
|
||||
ASSERT_EQ(configure_array.getByIndex(0)->mask, 3758129024);
|
||||
ASSERT_EQ(configure_array.getByIndex(1)->id, 2147739648);
|
||||
ASSERT_EQ(configure_array.getByIndex(1)->mask, 3774868352);
|
||||
ASSERT_EQ(configure_array.getByIndex(2)->id, 0);
|
||||
ASSERT_EQ(configure_array.getByIndex(2)->mask, 1488);
|
||||
ASSERT_EQ(configure_array.getByIndex(3)->id, 999999);
|
||||
ASSERT_EQ(configure_array.getByIndex(3)->mask, 849128412);
|
||||
ASSERT_EQ(configure_array.getByIndex(4)->id, 2147745792);
|
||||
ASSERT_EQ(configure_array.getByIndex(4)->mask, 3774872704);
|
||||
ASSERT_EQ(configure_array.getByIndex(5)->id, 2147489920);
|
||||
ASSERT_EQ(configure_array.getByIndex(5)->mask, 3758129024);
|
||||
ASSERT_EQ(configure_array.getByIndex(6)->id, 2147745792);
|
||||
ASSERT_EQ(configure_array.getByIndex(6)->mask, 3774868352);
|
||||
ASSERT_EQ(configure_array.getByIndex(7)->id, 0);
|
||||
ASSERT_EQ(configure_array.getByIndex(7)->mask, 1488);
|
||||
ASSERT_EQ(configure_array.getByIndex(8)->id, 24);
|
||||
ASSERT_EQ(configure_array.getByIndex(8)->mask, 72);
|
||||
ASSERT_EQ(configure_array.getByIndex(9)->id, 2147483648);
|
||||
ASSERT_EQ(configure_array.getByIndex(9)->mask, 3758096639);
|
||||
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, 4);
|
||||
configure_filters_assert = no_anon_test_confiruration.computeConfiguration
|
||||
|
||||
@@ -148,8 +148,7 @@ 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<uavcan::CanFilterConfig> hw_filters_container_;
|
||||
bool filters_configured_ = false;
|
||||
std::vector<::can_filter> hw_filters_container_;
|
||||
|
||||
void registerError(SocketCanError e) { errors_[e]++; }
|
||||
|
||||
@@ -228,6 +227,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 (!checkHWFilters(sockcan_frame) && !loopback)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
frame = makeUavcanFrame(sockcan_frame);
|
||||
/*
|
||||
* Timestamp
|
||||
@@ -246,10 +255,6 @@ class SocketCanIface : public uavcan::ICanIface
|
||||
assert(0);
|
||||
return -1;
|
||||
}
|
||||
/*
|
||||
* Flags
|
||||
*/
|
||||
loopback = (msg.msg_flags & static_cast<int>(MSG_CONFIRM)) != 0;
|
||||
return 1;
|
||||
}
|
||||
|
||||
@@ -304,17 +309,8 @@ class SocketCanIface : public uavcan::ICanIface
|
||||
if (accept)
|
||||
{
|
||||
rx.ts_utc += clock_.getPrivateAdjustment();
|
||||
if (filters_configured_)
|
||||
{
|
||||
if (checkHWFilters(rx.frame))
|
||||
{
|
||||
rx_queue_.push(rx);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
rx_queue_.push(rx);
|
||||
}
|
||||
rx_queue_.push(rx);
|
||||
|
||||
}
|
||||
}
|
||||
else if (res == 0)
|
||||
@@ -331,17 +327,24 @@ class SocketCanIface : public uavcan::ICanIface
|
||||
/**
|
||||
* Returns true if a frame accepted by HW filters
|
||||
*/
|
||||
bool checkHWFilters(const uavcan::CanFrame frame)
|
||||
bool checkHWFilters (const ::can_frame frame) const
|
||||
{
|
||||
uint16_t container_size = hw_filters_container_.size();
|
||||
for (uint16_t i = 0; i < container_size; i++)
|
||||
if (! hw_filters_container_.empty())
|
||||
{
|
||||
if (((frame.id & hw_filters_container_[i].mask) ^ hw_filters_container_[i].id) == 0)
|
||||
uint16_t container_size = hw_filters_container_.size();
|
||||
for (uint16_t i = 0; i < container_size; i++)
|
||||
{
|
||||
return 1;
|
||||
if (((frame.can_id & hw_filters_container_[i].can_mask) ^ hw_filters_container_[i].can_id) == 0)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
else
|
||||
{
|
||||
return true;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
public:
|
||||
@@ -439,40 +442,36 @@ public:
|
||||
for (unsigned i = 0; i < num_configs; i++)
|
||||
{
|
||||
const uavcan::CanFilterConfig& fc = filter_configs[i];
|
||||
hw_filters_container_[i].id = fc.id & uavcan::CanFrame::MaskExtID;
|
||||
hw_filters_container_[i].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)
|
||||
{
|
||||
hw_filters_container_[i].id |= CAN_EFF_FLAG;
|
||||
hw_filters_container_[i].can_id |= CAN_EFF_FLAG;
|
||||
}
|
||||
if (fc.id & uavcan::CanFrame::FlagRTR)
|
||||
{
|
||||
hw_filters_container_[i].id |= CAN_RTR_FLAG;
|
||||
hw_filters_container_[i].can_id |= CAN_RTR_FLAG;
|
||||
}
|
||||
if (fc.mask & uavcan::CanFrame::FlagEFF)
|
||||
{
|
||||
hw_filters_container_[i].mask |= CAN_EFF_FLAG;
|
||||
hw_filters_container_[i].can_mask |= CAN_EFF_FLAG;
|
||||
}
|
||||
if (fc.mask & uavcan::CanFrame::FlagRTR)
|
||||
{
|
||||
hw_filters_container_[i].mask |= CAN_RTR_FLAG;
|
||||
hw_filters_container_[i].can_mask |= CAN_RTR_FLAG;
|
||||
}
|
||||
}
|
||||
|
||||
if (! hw_filters_container_.empty())
|
||||
{
|
||||
filters_configured_ = true;
|
||||
return 0;
|
||||
}
|
||||
|
||||
return -1;
|
||||
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 8; }
|
||||
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.
|
||||
|
||||
Reference in New Issue
Block a user