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

Proper priority comparison for CAN frames of different types

Browse Source
This commit is contained in:
Pavel Kirienko 2014-03-31 00:32:52 +04:00
parent fc4d54ed86
commit d2b3832860
3 changed files with 103 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libuavcan/include/uavcan/driver/can.hpp
View File

@ -61,16 +61,13 @@ struct CanFrame
enum StringRepresentation { StrTight, StrAligned };
std::string toString(StringRepresentation mode = StrTight) const;
// TODO: priority comparison for EXT vs STD frames
bool priorityHigherThan(const CanFrame& rhs) const
{
return (id & CanFrame::MaskExtID) < (rhs.id & CanFrame::MaskExtID);
}
bool priorityLowerThan(const CanFrame& rhs) const
{
return (id & CanFrame::MaskExtID) > (rhs.id & CanFrame::MaskExtID);
}
/**
* CAN frames arbitration rules, particularly STD vs EXT:
* Marco Di Natale - "Understanding and using the Controller Area Network"
* http://www6.in.tum.de/pub/Main/TeachingWs2013MSE/CANbus.pdf
*/
bool priorityHigherThan(const CanFrame& rhs) const;
bool priorityLowerThan(const CanFrame& rhs) const { return rhs.priorityHigherThan(*this); }
};
UAVCAN_PACKED_END

39
libuavcan/src/driver/can.cpp
View File

@ -17,6 +17,45 @@ const uint32_t CanFrame::FlagRTR;
const uint32_t CanFrame::FlagERR;
const uint8_t CanFrame::MaxDataLen;
bool CanFrame::priorityHigherThan(const CanFrame& rhs) const
{
const uint32_t clean_id = id & MaskExtID;
const uint32_t rhs_clean_id = rhs.id & MaskExtID;
/*
* STD vs EXT - if 11 most significant bits are the same, EXT loses.
*/
const bool ext = id & FlagEFF;
const bool rhs_ext = rhs.id & FlagEFF;
if (ext != rhs_ext)
{
const uint32_t arb11 = ext ? (clean_id >> 18) : clean_id;
const uint32_t rhs_arb11 = rhs_ext ? (rhs_clean_id >> 18) : rhs_clean_id;
if (arb11 != rhs_arb11)
{
return arb11 < rhs_arb11;
}
else
{
return rhs_ext;
}
}
/*
* RTR vs Data frame - if frame identifiers and frame types are the same, RTR loses.
*/
const bool rtr = id & FlagRTR;
const bool rhs_rtr = rhs.id & FlagRTR;
if (clean_id == rhs_clean_id && rtr != rhs_rtr)
{
return rhs_rtr;
}
/*
* Plain ID arbitration - greater value loses.
*/
return clean_id < rhs_clean_id;
}
std::string CanFrame::toString(StringRepresentation mode) const
{

57
libuavcan/test/transport/can/can_driver.cpp
View File

@ -21,6 +21,63 @@ TEST(CanFrame, FrameProperties)
EXPECT_TRUE(frame.isErrorFrame());
}
TEST(CanFrame, Arbitration)
{
using uavcan::CanFrame;
CanFrame a;
CanFrame b;
/*
* Simple
*/
a.id = 123;
b.id = 122;
ASSERT_TRUE(a.priorityLowerThan(b));
ASSERT_TRUE(b.priorityHigherThan(a));
a.id = 123 | CanFrame::FlagEFF;
b.id = 122 | CanFrame::FlagEFF;
ASSERT_TRUE(a.priorityLowerThan(b));
ASSERT_TRUE(b.priorityHigherThan(a));
a.id = 8;
b.id = 8;
ASSERT_FALSE(a.priorityLowerThan(b));
ASSERT_FALSE(b.priorityHigherThan(a));
/*
* EXT vs STD
*/
a.id = 1000; // 1000
b.id = 2000 | CanFrame::FlagEFF; // 2000 >> 18, wins
ASSERT_TRUE(a.priorityLowerThan(b));
ASSERT_TRUE(b.priorityHigherThan(a));
a.id = 0x400;
b.id = 0x10000000 | CanFrame::FlagEFF; // (0x400 << 18), 11 most significant bits are the same --> EFF loses
ASSERT_TRUE(a.priorityHigherThan(b));
ASSERT_TRUE(b.priorityLowerThan(a));
/*
* RTR vs Data
*/
a.id = 123 | CanFrame::FlagRTR; // On the same ID, RTR loses
b.id = 123;
ASSERT_TRUE(a.priorityLowerThan(b));
ASSERT_TRUE(b.priorityHigherThan(a));
a.id = CanFrame::MaskStdID | CanFrame::FlagRTR; // RTR is STD, so it wins
b.id = CanFrame::MaskExtID | CanFrame::FlagEFF; // Lowest possible priority for data frame
ASSERT_TRUE(a.priorityHigherThan(b));
ASSERT_TRUE(b.priorityLowerThan(a));
a.id = 123 | CanFrame::FlagRTR; // Both RTR arbitrate as usually
b.id = 122 | CanFrame::FlagRTR;
ASSERT_TRUE(a.priorityLowerThan(b));
ASSERT_TRUE(b.priorityHigherThan(a));
}
TEST(CanFrame, ToString)
{
uavcan::CanFrame frame = makeCanFrame(123, "\x01\x02\x03\x04" "1234", EXT);