fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-26 23:33:38 +08:00
Code Issues Packages Projects Releases Wiki Activity

Acceptance filter update for new transport layer

Browse Source
This commit is contained in:
ilia-sheremet
2015-07-24 21:04:56 +01:00
parent ad12760d7f
commit 5fa5a4f365
3 changed files with 90 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libuavcan/src/transport/uc_can_acceptance_filter_configurator.cpp
+22 -25
View File
@@ -9,18 +9,30 @@
namespace uavcan
{
const unsigned CanAcceptanceFilterConfigurator::DefaultFilterMsgMask;
const unsigned CanAcceptanceFilterConfigurator::DefaultFilterServiceRequestID;
const unsigned CanAcceptanceFilterConfigurator::DefaultFilterServiceRequestMask;
const unsigned CanAcceptanceFilterConfigurator::ServiceRespFrameID;
const unsigned CanAcceptanceFilterConfigurator::ServiceRespFrameMask;
const unsigned CanAcceptanceFilterConfigurator::DefaultFilterServiceID;
const unsigned CanAcceptanceFilterConfigurator::DefaultFilterServiceMask;
const unsigned CanAcceptanceFilterConfigurator::DefaultAnonMsgMask;
const unsigned CanAcceptanceFilterConfigurator::DefaultAnonMsgID;
int16_t CanAcceptanceFilterConfigurator::loadInputConfiguration()
int16_t CanAcceptanceFilterConfigurator::loadInputConfiguration(AnonMassagePresence load_mode)
{
multiset_configs_.clear();
if (load_mode == WithAnonMsg)
{
CanFilterConfig anon_frame_cfg;
anon_frame_cfg.id = DefaultAnonMsgID;
anon_frame_cfg.mask = DefaultAnonMsgMask;
if (multiset_configs_.emplace(anon_frame_cfg) == NULL)
{
return -ErrMemory;
}
}
CanFilterConfig service_resp_cfg;
service_resp_cfg.id = ServiceRespFrameID;
service_resp_cfg.mask = ServiceRespFrameMask;
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)
{
return -ErrMemory;
@@ -30,8 +42,7 @@ int16_t CanAcceptanceFilterConfigurator::loadInputConfiguration()
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.id = static_cast<uint32_t>(p->getDataTypeDescriptor().getID().get()) << 8;
cfg.mask = DefaultFilterMsgMask;
if (multiset_configs_.emplace(cfg) == NULL)
{
@@ -40,20 +51,6 @@ int16_t CanAcceptanceFilterConfigurator::loadInputConfiguration()
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;
@@ -152,7 +149,7 @@ int16_t CanAcceptanceFilterConfigurator::applyConfiguration(void)
return 0;
}
int CanAcceptanceFilterConfigurator::configureFilters()
int CanAcceptanceFilterConfigurator::configureFilters(AnonMassagePresence mode)
{
if (getNumFilters() == 0)
{
@@ -160,7 +157,7 @@ int CanAcceptanceFilterConfigurator::configureFilters()
return -ErrDriver;
}
int16_t fill_array_error = loadInputConfiguration();
int16_t fill_array_error = loadInputConfiguration(mode);
if (fill_array_error != 0)
{
UAVCAN_TRACE("CanAcceptanceFilter::loadInputConfiguration", "Failed to execute loadInputConfiguration()");