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

Frame parse()/compile(), some renamings

Browse Source
This commit is contained in:
Pavel Kirienko
2014-02-02 22:54:27 +04:00
parent 4bf2b2e81a
commit 8794c7eab9
9 changed files with 226 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libuavcan/include/uavcan/internal/transport/can_io.hpp
+5
View File
@@ -20,6 +20,11 @@ struct CanRxFrame
CanFrame frame;
uint64_t timestamp;
uint8_t iface_index;
CanRxFrame()
: timestamp(0)
, iface_index(0)
{ }
};
libuavcan/include/uavcan/internal/transport/transfer.hpp
+33 -11
View File
@@ -22,7 +22,7 @@ enum TransferType
class TransferID
{
uint_fast8_t value_;
uint8_t value_;
public:
enum { BITLEN = 4 };
@@ -32,7 +32,7 @@ public:
: value_(0)
{ }
TransferID(uint_fast8_t value) // implicit
TransferID(uint8_t value) // implicit
: value_(value)
{
value_ &= MAX;
@@ -47,7 +47,7 @@ public:
value_ = (value_ + 1) & MAX;
}
uint_fast8_t get() const
uint8_t get() const
{
assert(value_ <= MAX);
return value_;
@@ -62,6 +62,10 @@ public:
struct Frame
{
enum { DATA_TYPE_ID_MAX = 1023 };
enum { NODE_ID_MAX = 127 };
enum { FRAME_INDEX_MAX = 31 };
uint8_t payload[8];
TransferType transfer_type;
uint_fast16_t data_type_id;
@@ -71,6 +75,18 @@ struct Frame
TransferID transfer_id;
bool last_frame;
Frame()
: transfer_type(TransferType(0))
, data_type_id(0)
, payload_len(0)
, source_node_id(0)
, frame_index(0)
, transfer_id(0)
, last_frame(false)
{
std::fill(payload, payload + sizeof(payload), 0);
}
Frame(const uint8_t* payload, uint_fast8_t payload_len, uint_fast16_t data_type_id, TransferType transfer_type,
uint_fast8_t source_node_id, uint_fast8_t frame_index, TransferID transfer_id, bool last_frame)
: transfer_type(transfer_type)
@@ -81,11 +97,14 @@ struct Frame
, transfer_id(transfer_id)
, last_frame(last_frame)
{
assert(payload && payload_len <= 8);
assert(data_type_id <= DATA_TYPE_ID_MAX);
assert(source_node_id <= NODE_ID_MAX);
assert(frame_index <= FRAME_INDEX_MAX);
assert(payload && payload_len <= sizeof(payload));
std::copy(payload, payload + payload_len, this->payload);
}
static Frame parse(const CanFrame& can_frame);
bool parse(const CanFrame& can_frame);
CanFrame compile() const;
@@ -111,15 +130,18 @@ struct RxFrame
Frame frame;
uint_fast8_t iface_index;
RxFrame(const Frame& frame, uint_fast64_t timestamp, uint_fast8_t iface_index)
: timestamp(timestamp)
, frame(frame)
, iface_index(iface_index)
RxFrame()
: timestamp(0)
, iface_index(0)
{ }
static RxFrame parse(const CanRxFrame& can_frame)
bool parse(const CanRxFrame& can_frame)
{
return RxFrame(Frame::parse(can_frame.frame), can_frame.timestamp, can_frame.iface_index);
if (!frame.parse(can_frame.frame))
return false;
timestamp = can_frame.timestamp;
iface_index = can_frame.iface_index;
return true;
}
};
libuavcan/src/can_driver.cpp
+6
View File
@@ -10,6 +10,12 @@
namespace uavcan
{
const uint32_t CanFrame::MASK_STDID;
const uint32_t CanFrame::MASK_EXTID;
const uint32_t CanFrame::FLAG_EFF;
const uint32_t CanFrame::FLAG_RTR;
std::string CanFrame::toString(StringRepresentation mode) const
{
using std::snprintf;
libuavcan/src/transport/transfer.cpp
+56
View File
@@ -18,4 +18,60 @@ int TransferID::forwardDistance(TransferID rhs) const
return d;
}
template <int OFFSET, int WIDTH>
inline static uint32_t bitunpack(uint32_t val)
{
return (val >> OFFSET) & ((1UL << WIDTH) - 1);
}
bool Frame::parse(const CanFrame& can_frame)
{
if ((can_frame.id & CanFrame::FLAG_RTR) || !(can_frame.id & CanFrame::FLAG_EFF))
return false;
if (can_frame.dlc > 8)
{
assert(0);
return false;
}
const uint32_t id = can_frame.id & CanFrame::MASK_EXTID;
transfer_id = bitunpack<0, 4>(id);
last_frame = bitunpack<4, 1>(id);
frame_index = bitunpack<5, 5>(id);
source_node_id = bitunpack<10, 7>(id);
transfer_type = TransferType(bitunpack<17, 2>(id));
data_type_id = bitunpack<19, 10>(id);
payload_len = can_frame.dlc;
std::copy(can_frame.data, can_frame.data + can_frame.dlc, payload);
return true;
}
template <int OFFSET, int WIDTH>
inline static uint32_t bitpack(uint32_t field)
{
return (field & ((1UL << WIDTH) - 1)) << OFFSET;
}
CanFrame Frame::compile() const
{
CanFrame frame;
frame.id = CanFrame::FLAG_EFF |
bitpack<0, 4>(transfer_id.get()) |
bitpack<4, 1>(last_frame) |
bitpack<5, 5>(frame_index) |
bitpack<10, 7>(source_node_id) |
bitpack<17, 2>(transfer_type) |
bitpack<19, 10>(data_type_id);
assert(payload_len <= sizeof(payload));
frame.dlc = payload_len;
std::copy(payload, payload + payload_len, frame.data);
return frame;
}
}
libuavcan/test/can_driver.cpp
+11 -11
View File
@@ -8,35 +8,35 @@
TEST(CanFrame, FrameProperties)
{
EXPECT_TRUE(makeFrame(0, "", EXT).isExtended());
EXPECT_FALSE(makeFrame(0, "", STD).isExtended());
EXPECT_FALSE(makeFrame(0, "", EXT).isRemoteTransmissionRequest());
EXPECT_FALSE(makeFrame(0, "", STD).isRemoteTransmissionRequest());
EXPECT_TRUE(makeCanFrame(0, "", EXT).isExtended());
EXPECT_FALSE(makeCanFrame(0, "", STD).isExtended());
EXPECT_FALSE(makeCanFrame(0, "", EXT).isRemoteTransmissionRequest());
EXPECT_FALSE(makeCanFrame(0, "", STD).isRemoteTransmissionRequest());
uavcan::CanFrame frame = makeFrame(123, "", STD);
uavcan::CanFrame frame = makeCanFrame(123, "", STD);
frame.id |= uavcan::CanFrame::FLAG_RTR;
EXPECT_TRUE(frame.isRemoteTransmissionRequest());
}
TEST(CanFrame, ToString)
{
uavcan::CanFrame frame = makeFrame(123, "\x01\x02\x03\x04""1234", EXT);
uavcan::CanFrame frame = makeCanFrame(123, "\x01\x02\x03\x04""1234", EXT);
EXPECT_EQ("0x0000007b 01 02 03 04 31 32 33 34 '....1234'", frame.toString());
EXPECT_TRUE(frame.toString() == frame.toString(uavcan::CanFrame::STR_ALIGNED));
frame = makeFrame(123, "z", EXT);
frame = makeCanFrame(123, "z", EXT);
EXPECT_EQ("0x0000007b 7a 'z'", frame.toString(uavcan::CanFrame::STR_ALIGNED));
EXPECT_EQ("0x0000007b 7a 'z'", frame.toString());
EXPECT_EQ(" 0x141 61 62 63 64 aa bb cc dd 'abcd....'",
makeFrame(321, "abcd""\xaa\xbb\xcc\xdd", STD).toString(uavcan::CanFrame::STR_ALIGNED));
makeCanFrame(321, "abcd""\xaa\xbb\xcc\xdd", STD).toString(uavcan::CanFrame::STR_ALIGNED));
EXPECT_EQ(" 0x100 ''",
makeFrame(256, "", STD).toString(uavcan::CanFrame::STR_ALIGNED));
makeCanFrame(256, "", STD).toString(uavcan::CanFrame::STR_ALIGNED));
EXPECT_EQ("0x100 ''",
makeFrame(256, "", STD).toString());
makeCanFrame(256, "", STD).toString());
EXPECT_EQ("0x141 61 62 63 64 aa bb cc dd 'abcd....'",
makeFrame(321, "abcd""\xaa\xbb\xcc\xdd", STD).toString());
makeCanFrame(321, "abcd""\xaa\xbb\xcc\xdd", STD).toString());
}
libuavcan/test/common.hpp
+1 -1
View File
@@ -44,7 +44,7 @@ public:
};
enum FrameType { STD, EXT };
static uavcan::CanFrame makeFrame(uint32_t id, const std::string& str_data, FrameType type)
static uavcan::CanFrame makeCanFrame(uint32_t id, const std::string& str_data, FrameType type)
{
id |= (type == EXT) ? uavcan::CanFrame::FLAG_EFF : 0;
return uavcan::CanFrame(id, reinterpret_cast<const uint8_t*>(str_data.c_str()), str_data.length());
libuavcan/test/transport/can/io.cpp
+5 -5
View File
@@ -170,7 +170,7 @@ TEST(CanIOManager, CanDriverMock)
EXPECT_EQ(100, clockmock.utc);
// No WR, #1 RD
const CanFrame fr1 = makeFrame(123, "foo", EXT);
const CanFrame fr1 = makeCanFrame(123, "foo", EXT);
driver.ifaces.at(1).pushRx(fr1);
mask_wr = 7;
mask_rd = 6;
@@ -232,8 +232,8 @@ TEST(CanIOManager, Reception)
* Non empty from multiple ifaces
*/
const uavcan::CanFrame frames[2][3] = {
{ makeFrame(1, "a0", EXT), makeFrame(99, "a1", EXT), makeFrame(803, "a2", STD) },
{ makeFrame(6341, "b0", EXT), makeFrame(196, "b1", STD), makeFrame(73, "b2", EXT) },
{ makeCanFrame(1, "a0", EXT), makeCanFrame(99, "a1", EXT), makeCanFrame(803, "a2", STD) },
{ makeCanFrame(6341, "b0", EXT), makeCanFrame(196, "b1", STD), makeCanFrame(73, "b2", EXT) },
};
clockmock.advance(10);
@@ -307,7 +307,7 @@ TEST(CanIOManager, Transmission)
const int ALL_IFACES_MASK = 3;
const uavcan::CanFrame frames[] = {
makeFrame(1, "a0", EXT), makeFrame(99, "a1", EXT), makeFrame(803, "a2", STD)
makeCanFrame(1, "a0", EXT), makeCanFrame(99, "a1", EXT), makeCanFrame(803, "a2", STD)
};
/*
@@ -396,7 +396,7 @@ TEST(CanIOManager, Transmission)
// Preparing the driver mock for receive() call
driver.ifaces.at(0).writeable = true;
driver.ifaces.at(1).writeable = true;
const uavcan::CanFrame rx_frames[] = { makeFrame(123, "rx0", STD), makeFrame(321, "rx1", EXT) };
const uavcan::CanFrame rx_frames[] = { makeCanFrame(123, "rx0", STD), makeCanFrame(321, "rx1", EXT) };
driver.ifaces.at(0).pushRx(rx_frames[0]);
driver.ifaces.at(1).pushRx(rx_frames[1]);
libuavcan/test/transport/can/tx_queue.cpp
+10 -10
View File
@@ -33,8 +33,8 @@ static bool isInQueue(uavcan::CanTxQueue& queue, const uavcan::CanFrame& frame)
TEST(CanTxQueue, Qos)
{
uavcan::CanTxQueue::Entry e1(makeFrame(100, "", EXT), 1000, uavcan::CanTxQueue::VOLATILE);
uavcan::CanTxQueue::Entry e2(makeFrame(100, "", EXT), 1000, uavcan::CanTxQueue::VOLATILE);
uavcan::CanTxQueue::Entry e1(makeCanFrame(100, "", EXT), 1000, uavcan::CanTxQueue::VOLATILE);
uavcan::CanTxQueue::Entry e2(makeCanFrame(100, "", EXT), 1000, uavcan::CanTxQueue::VOLATILE);
EXPECT_FALSE(e1.qosHigherThan(e2));
EXPECT_FALSE(e2.qosHigherThan(e1));
@@ -74,14 +74,14 @@ TEST(CanTxQueue, TxQueue)
EXPECT_TRUE(queue.isEmpty());
// Descending priority
const CanFrame f0 = makeFrame(0, "f0", EXT);
const CanFrame f1 = makeFrame(10, "f1", EXT);
const CanFrame f2 = makeFrame(20, "f2", EXT);
const CanFrame f3 = makeFrame(100, "f3", EXT);
const CanFrame f4 = makeFrame(10000, "f4", EXT);
const CanFrame f5 = makeFrame(99999, "f5", EXT);
const CanFrame f5a = makeFrame(99999, "f5a", EXT);
const CanFrame f6 = makeFrame(999999, "f6", EXT);
const CanFrame f0 = makeCanFrame(0, "f0", EXT);
const CanFrame f1 = makeCanFrame(10, "f1", EXT);
const CanFrame f2 = makeCanFrame(20, "f2", EXT);
const CanFrame f3 = makeCanFrame(100, "f3", EXT);
const CanFrame f4 = makeCanFrame(10000, "f4", EXT);
const CanFrame f5 = makeCanFrame(99999, "f5", EXT);
const CanFrame f5a = makeCanFrame(99999, "f5a", EXT);
const CanFrame f6 = makeCanFrame(999999, "f6", EXT);
/*
* Priority insertion
libuavcan/test/transport/transfer.cpp
+99
View File
@@ -5,6 +5,7 @@
#include <string>
#include <gtest/gtest.h>
#include <uavcan/internal/transport/transfer.hpp>
#include "../common.hpp"
TEST(Transfer, TransferID)
@@ -58,3 +59,101 @@ TEST(Transfer, TransferID)
ASSERT_EQ(0, tid.forwardDistance(tid));
}
}
TEST(Transfer, FrameParseCompile)
{
using uavcan::Frame;
using uavcan::CanFrame;
using uavcan::TransferID;
using uavcan::TransferType;
Frame frame;
const uint32_t can_id =
(2 << 0) | // Transfer ID
(1 << 4) | // Last Frame
(29 << 5) | // Frame Index
(42 << 10) | // Source Node ID
(3 << 17) | // Transfer Type
(456 << 19); // Data Type ID
const std::string payload_string = "hello";
/*
* Parse
*/
// Invalid CAN frames
ASSERT_FALSE(frame.parse(CanFrame(can_id | CanFrame::FLAG_RTR, (const uint8_t*)"", 0)));
ASSERT_FALSE(frame.parse(makeCanFrame(can_id, payload_string, STD)));
// Valid
ASSERT_TRUE(frame.parse(makeCanFrame(can_id, payload_string, EXT)));
EXPECT_EQ(TransferID(2), frame.transfer_id);
EXPECT_TRUE(frame.last_frame);
EXPECT_EQ(29, frame.frame_index);
EXPECT_EQ(42, frame.source_node_id);
EXPECT_EQ(TransferType(3), frame.transfer_type);
EXPECT_EQ(456, frame.data_type_id);
EXPECT_EQ(payload_string.length(), frame.payload_len);
EXPECT_TRUE(std::equal(frame.payload, frame.payload + frame.payload_len, payload_string.begin()));
// Default
ASSERT_TRUE(frame.parse(CanFrame(CanFrame::FLAG_EFF, (const uint8_t*)"", 0)));
ASSERT_EQ(Frame(), frame);
/*
* Compile
*/
// Default
frame = Frame();
CanFrame can_frame = frame.compile();
ASSERT_EQ(can_frame.id, CanFrame::FLAG_EFF);
// Custom
ASSERT_TRUE(frame.parse(makeCanFrame(can_id, payload_string, EXT)));
can_frame = frame.compile();
ASSERT_EQ(can_frame, makeCanFrame(can_id, payload_string, EXT));
EXPECT_EQ(payload_string.length(), can_frame.dlc);
EXPECT_TRUE(std::equal(can_frame.data, can_frame.data + can_frame.dlc, payload_string.begin()));
/*
* Comparison
*/
ASSERT_FALSE(Frame() == frame);
ASSERT_TRUE(Frame() != frame);
frame = Frame();
ASSERT_TRUE(Frame() == frame);
ASSERT_FALSE(Frame() != frame);
}
TEST(Transfer, RxFrameParseCompile)
{
using uavcan::Frame;
using uavcan::RxFrame;
using uavcan::CanFrame;
using uavcan::CanRxFrame;
CanRxFrame can_rx_frame;
RxFrame rx_frame;
// Failure
ASSERT_FALSE(rx_frame.parse(can_rx_frame));
// Default
can_rx_frame.frame.id = CanFrame::FLAG_EFF;
ASSERT_TRUE(rx_frame.parse(can_rx_frame));
ASSERT_EQ(0, rx_frame.timestamp);
ASSERT_EQ(0, rx_frame.iface_index);
// Custom
can_rx_frame.timestamp = 123;
can_rx_frame.iface_index = 2;
ASSERT_TRUE(rx_frame.parse(can_rx_frame));
ASSERT_EQ(123, rx_frame.timestamp);
ASSERT_EQ(2, rx_frame.iface_index);
}