mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-21 21:33:34 +08:00
CAN HW filters are added using map container
This commit is contained in:
ilia-sheremet
@@ -132,7 +132,7 @@ MonotonicTime Scheduler::computeDispatcherSpinDeadline(MonotonicTime spin_deadli
|
||||
const MonotonicTime ts = getMonotonicTime();
|
||||
if (earliest > ts)
|
||||
{
|
||||
if (ts - earliest > deadline_resolution_)
|
||||
if (earliest - ts > deadline_resolution_)
|
||||
{
|
||||
return ts + deadline_resolution_;
|
||||
}
|
||||
|
||||
@@ -0,0 +1,228 @@
|
||||
/*
|
||||
* 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();
|
||||
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_;
|
||||
}
|
||||
|
||||
}
|
||||
@@ -11,21 +11,13 @@
|
||||
namespace uavcan
|
||||
{
|
||||
|
||||
const uint32_t TransferReceiver::MinTransferIntervalUSec;
|
||||
const uint32_t TransferReceiver::MaxTransferIntervalUSec;
|
||||
const uint32_t TransferReceiver::DefaultTransferIntervalUSec;
|
||||
const uint8_t TransferReceiver::IfaceIndexNotSet;
|
||||
const uint16_t TransferReceiver::MinTransferIntervalMSec;
|
||||
const uint16_t TransferReceiver::MaxTransferIntervalMSec;
|
||||
const uint16_t TransferReceiver::DefaultTransferIntervalMSec;
|
||||
|
||||
void TransferReceiver::registerError() const
|
||||
{
|
||||
if (error_cnt_ < 0xFF)
|
||||
{
|
||||
error_cnt_ = static_cast<uint8_t>(error_cnt_ + 1);
|
||||
}
|
||||
else
|
||||
{
|
||||
UAVCAN_ASSERT(0);
|
||||
}
|
||||
error_cnt_ = static_cast<uint8_t>(error_cnt_ + 1) & ErrorCntMask;
|
||||
}
|
||||
|
||||
TransferReceiver::TidRelation TransferReceiver::getTidRelation(const RxFrame& frame) const
|
||||
@@ -51,10 +43,10 @@ void TransferReceiver::updateTransferTimings()
|
||||
|
||||
if ((!prev_prev_ts.isZero()) && (!prev_transfer_ts_.isZero()) && (prev_transfer_ts_ >= prev_prev_ts))
|
||||
{
|
||||
uint64_t interval_usec = uint64_t((prev_transfer_ts_ - prev_prev_ts).toUSec());
|
||||
interval_usec = min(interval_usec, uint64_t(MaxTransferIntervalUSec));
|
||||
interval_usec = max(interval_usec, uint64_t(MinTransferIntervalUSec));
|
||||
transfer_interval_usec_ = static_cast<uint32_t>((uint64_t(transfer_interval_usec_) * 7 + interval_usec) / 8);
|
||||
uint64_t interval_msec = uint64_t((prev_transfer_ts_ - prev_prev_ts).toMSec());
|
||||
interval_msec = min(interval_msec, uint64_t(MaxTransferIntervalMSec));
|
||||
interval_msec = max(interval_msec, uint64_t(MinTransferIntervalMSec));
|
||||
transfer_interval_msec_ = static_cast<uint16_t>((uint64_t(transfer_interval_msec_) * 7U + interval_msec) / 8U);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -118,7 +110,7 @@ bool TransferReceiver::writePayload(const RxFrame& frame, ITransferBuffer& buf)
|
||||
const bool success = res == static_cast<int>(effective_payload_len);
|
||||
if (success)
|
||||
{
|
||||
buffer_write_pos_ = static_cast<uint16_t>(buffer_write_pos_ + effective_payload_len);
|
||||
buffer_write_pos_ = static_cast<uint16_t>(buffer_write_pos_ + effective_payload_len) & BufferWritePosMask;
|
||||
}
|
||||
return success;
|
||||
}
|
||||
@@ -128,7 +120,7 @@ bool TransferReceiver::writePayload(const RxFrame& frame, ITransferBuffer& buf)
|
||||
const bool success = res == static_cast<int>(payload_len);
|
||||
if (success)
|
||||
{
|
||||
buffer_write_pos_ = static_cast<uint16_t>(buffer_write_pos_ + payload_len);
|
||||
buffer_write_pos_ = static_cast<uint16_t>(buffer_write_pos_ + payload_len) & BufferWritePosMask;
|
||||
}
|
||||
return success;
|
||||
}
|
||||
@@ -186,12 +178,12 @@ TransferReceiver::ResultCode TransferReceiver::receive(const RxFrame& frame, Tra
|
||||
|
||||
bool TransferReceiver::isTimedOut(MonotonicTime current_ts) const
|
||||
{
|
||||
static const int64_t INTERVAL_MULT = (1 << TransferID::BitLen) / 2 + 1;
|
||||
static const int64_t IntervalMult = (1 << TransferID::BitLen) / 2 + 1;
|
||||
if (current_ts <= this_transfer_ts_)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
return (current_ts - this_transfer_ts_).toUSec() > (int64_t(transfer_interval_usec_) * INTERVAL_MULT);
|
||||
return (current_ts - this_transfer_ts_).toUSec() > (int64_t(transfer_interval_msec_) * 1000 * IntervalMult);
|
||||
}
|
||||
|
||||
TransferReceiver::ResultCode TransferReceiver::addFrame(const RxFrame& frame, TransferBufferAccessor& tba)
|
||||
@@ -209,7 +201,7 @@ TransferReceiver::ResultCode TransferReceiver::addFrame(const RxFrame& frame, Tr
|
||||
const bool first_fame = frame.isFirst();
|
||||
const TidRelation tid_rel = getTidRelation(frame);
|
||||
const bool iface_timed_out =
|
||||
(frame.getMonotonicTimestamp() - this_transfer_ts_).toUSec() > (int64_t(transfer_interval_usec_) * 2);
|
||||
(frame.getMonotonicTimestamp() - this_transfer_ts_).toUSec() > (int64_t(transfer_interval_msec_) * 1000 * 2);
|
||||
|
||||
// FSM, the hard way
|
||||
const bool need_restart =
|
||||
@@ -230,7 +222,7 @@ TransferReceiver::ResultCode TransferReceiver::addFrame(const RxFrame& frame, Tr
|
||||
int(not_initialized), int(iface_timed_out), int(receiver_timed_out), int(same_iface),
|
||||
int(first_fame), int(tid_rel), frame.toString().c_str());
|
||||
tba.remove();
|
||||
iface_index_ = frame.getIfaceIndex();
|
||||
iface_index_ = frame.getIfaceIndex() & IfaceIndexMask;
|
||||
tid_ = frame.getTransferID();
|
||||
next_frame_index_ = 0;
|
||||
buffer_write_pos_ = 0;
|
||||
|
||||
@@ -33,17 +33,7 @@ void LimitedPoolAllocator::deallocate(const void* ptr)
|
||||
}
|
||||
}
|
||||
|
||||
bool LimitedPoolAllocator::isInPool(const void* ptr) const
|
||||
{
|
||||
return allocator_.isInPool(ptr);
|
||||
}
|
||||
|
||||
std::size_t LimitedPoolAllocator::getBlockSize() const
|
||||
{
|
||||
return allocator_.getBlockSize();
|
||||
}
|
||||
|
||||
std::size_t LimitedPoolAllocator::getNumBlocks() const
|
||||
uint16_t LimitedPoolAllocator::getNumBlocks() const
|
||||
{
|
||||
return min(max_blocks_, allocator_.getNumBlocks());
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user