fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-25 16:40:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Finished Linux driver

Browse Source
This commit is contained in:
Pavel Kirienko
2014-03-31 17:13:33 +04:00
parent 75f475fac2
commit 39933ba41d
2 changed files with 328 additions and 112 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libuavcan_drivers/linux/test/test_socket.cpp
+179 -85
View File
@@ -3,10 +3,15 @@
*/
#include <iostream>
#include <vector>
#include <cerrno>
#include <uavcan_linux/uavcan_linux.hpp>
#define ASSERT(x) if (!(x)) { throw std::runtime_error(#x); }
#ifndef STRINGIZE
# define STRINGIZE2(x) #x
# define STRINGIZE(x) STRINGIZE2(x)
#endif
#define ENFORCE(x) if (!(x)) { throw std::runtime_error(__FILE__ ":" STRINGIZE(__LINE__) ": " #x); }
static uavcan::CanFrame makeFrame(std::uint32_t id, const std::string& data)
{
@@ -21,16 +26,16 @@ static uavcan::MonotonicTime tsMonoOffsetMs(std::int64_t ms)
static void testNonexistentIface()
{
const int sock1 = uavcan_linux::SocketCanIface::openSocket("noif9");
ASSERT(sock1 < 0);
ENFORCE(sock1 < 0);
const int sock2 = uavcan_linux::SocketCanIface::openSocket("verylongifacenameverylongifacenameverylongifacename");
ASSERT(sock2 < 0);
ENFORCE(sock2 < 0);
}
static void testSocketRxTx(const std::string& iface_name)
{
const int sock1 = uavcan_linux::SocketCanIface::openSocket(iface_name);
const int sock2 = uavcan_linux::SocketCanIface::openSocket(iface_name);
ASSERT(sock1 >= 0 && sock2 >= 0);
ENFORCE(sock1 >= 0 && sock2 >= 0);
uavcan_linux::SocketCanIface if1(sock1);
uavcan_linux::SocketCanIface if2(sock2);
@@ -38,27 +43,25 @@ static void testSocketRxTx(const std::string& iface_name)
/*
* Sending two frames, one of which must be returned back
*/
ASSERT(1 == if1.send(makeFrame(123, "if1-1"), tsMonoOffsetMs(100), 0));
ASSERT(1 == if1.send(makeFrame(456, "if1-2"), tsMonoOffsetMs(100), uavcan::CanIOFlagLoopback));
ASSERT(if1.hasPendingTx());
if1.poll(true, true); // Reads confirmation for the first, writes the second
if1.poll(true, true); // Reads confirmation for the second and stores it in RX queue, writes nothing
ASSERT(0 == if1.getErrorCount());
ASSERT(!if1.hasPendingTx());
ASSERT(if1.hasReadyRx()); // Second loopback
ENFORCE(1 == if1.send(makeFrame(123, "if1-1"), tsMonoOffsetMs(100), 0));
ENFORCE(1 == if1.send(makeFrame(456, "if1-2"), tsMonoOffsetMs(100), uavcan::CanIOFlagLoopback));
if1.poll(true, true);
if1.poll(true, true);
ENFORCE(0 == if1.getErrorCount());
ENFORCE(!if1.hasPendingTx());
ENFORCE(if1.hasReadyRx()); // Second loopback
/*
* Second iface, same thing
*/
ASSERT(1 == if2.send(makeFrame(321, "if2-1"), tsMonoOffsetMs(100), 0));
ASSERT(1 == if2.send(makeFrame(654, "if2-2"), tsMonoOffsetMs(100), uavcan::CanIOFlagLoopback));
ASSERT(1 == if2.send(makeFrame(1, "discard"), tsMonoOffsetMs(0), uavcan::CanIOFlagLoopback)); // Will timeout
ASSERT(if2.hasPendingTx());
if2.poll(true, true); // Reads confirmation for the first, writes the second
if2.poll(true, true); // Reads confirmation for the second and stores it in RX queue, writes nothing
ASSERT(1 == if2.getErrorCount()); // One timed out
ASSERT(!if2.hasPendingTx());
ASSERT(if2.hasReadyRx());
ENFORCE(1 == if2.send(makeFrame(321, "if2-1"), tsMonoOffsetMs(100), 0));
ENFORCE(1 == if2.send(makeFrame(654, "if2-2"), tsMonoOffsetMs(100), uavcan::CanIOFlagLoopback));
ENFORCE(1 == if2.send(makeFrame(1, "discard"), tsMonoOffsetMs(0), uavcan::CanIOFlagLoopback)); // Will timeout
if2.poll(true, true);
if2.poll(true, true);
ENFORCE(1 == if2.getErrorCount()); // One timed out
ENFORCE(!if2.hasPendingTx());
ENFORCE(if2.hasReadyRx());
/*
* No-op
@@ -76,54 +79,54 @@ static void testSocketRxTx(const std::string& iface_name)
/*
* Read first
*/
ASSERT(1 == if1.receive(frame, ts_mono, ts_utc, flags));
ASSERT(frame == makeFrame(321, "if2-1"));
ASSERT(flags == 0);
ASSERT(!ts_mono.isZero());
ASSERT(!ts_utc.isZero());
ASSERT((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10);
ASSERT((clock.getUtc() - ts_utc).getAbs().toMSec() < 10);
ENFORCE(1 == if1.receive(frame, ts_mono, ts_utc, flags));
ENFORCE(frame == makeFrame(321, "if2-1"));
ENFORCE(flags == 0);
ENFORCE(!ts_mono.isZero());
ENFORCE(!ts_utc.isZero());
ENFORCE((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10);
ENFORCE((clock.getUtc() - ts_utc).getAbs().toMSec() < 10);
ASSERT(1 == if1.receive(frame, ts_mono, ts_utc, flags));
ASSERT(frame == makeFrame(456, "if1-2"));
ASSERT(flags == uavcan::CanIOFlagLoopback);
ASSERT((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10);
ASSERT((clock.getUtc() - ts_utc).getAbs().toMSec() < 10);
ENFORCE(1 == if1.receive(frame, ts_mono, ts_utc, flags));
ENFORCE(frame == makeFrame(456, "if1-2"));
ENFORCE(flags == uavcan::CanIOFlagLoopback);
ENFORCE((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10);
ENFORCE((clock.getUtc() - ts_utc).getAbs().toMSec() < 10);
ASSERT(1 == if1.receive(frame, ts_mono, ts_utc, flags));
ASSERT(frame == makeFrame(654, "if2-2"));
ASSERT(flags == 0);
ASSERT((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10);
ASSERT((clock.getUtc() - ts_utc).getAbs().toMSec() < 10);
ENFORCE(1 == if1.receive(frame, ts_mono, ts_utc, flags));
ENFORCE(frame == makeFrame(654, "if2-2"));
ENFORCE(flags == 0);
ENFORCE((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10);
ENFORCE((clock.getUtc() - ts_utc).getAbs().toMSec() < 10);
ASSERT(0 == if1.receive(frame, ts_mono, ts_utc, flags));
ASSERT(!if1.hasPendingTx());
ASSERT(!if1.hasReadyRx());
ENFORCE(0 == if1.receive(frame, ts_mono, ts_utc, flags));
ENFORCE(!if1.hasPendingTx());
ENFORCE(!if1.hasReadyRx());
/*
* Read second
*/
ASSERT(1 == if2.receive(frame, ts_mono, ts_utc, flags));
ASSERT(frame == makeFrame(123, "if1-1"));
ASSERT(flags == 0);
ASSERT((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10);
ASSERT((clock.getUtc() - ts_utc).getAbs().toMSec() < 10);
ENFORCE(1 == if2.receive(frame, ts_mono, ts_utc, flags));
ENFORCE(frame == makeFrame(123, "if1-1"));
ENFORCE(flags == 0);
ENFORCE((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10);
ENFORCE((clock.getUtc() - ts_utc).getAbs().toMSec() < 10);
ASSERT(1 == if2.receive(frame, ts_mono, ts_utc, flags));
ASSERT(frame == makeFrame(456, "if1-2"));
ASSERT(flags == 0);
ASSERT((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10);
ASSERT((clock.getUtc() - ts_utc).getAbs().toMSec() < 10);
ENFORCE(1 == if2.receive(frame, ts_mono, ts_utc, flags));
ENFORCE(frame == makeFrame(456, "if1-2"));
ENFORCE(flags == 0);
ENFORCE((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10);
ENFORCE((clock.getUtc() - ts_utc).getAbs().toMSec() < 10);
ASSERT(1 == if2.receive(frame, ts_mono, ts_utc, flags));
ASSERT(frame == makeFrame(654, "if2-2"));
ASSERT(flags == uavcan::CanIOFlagLoopback);
ASSERT((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10);
ASSERT((clock.getUtc() - ts_utc).getAbs().toMSec() < 10);
ENFORCE(1 == if2.receive(frame, ts_mono, ts_utc, flags));
ENFORCE(frame == makeFrame(654, "if2-2"));
ENFORCE(flags == uavcan::CanIOFlagLoopback);
ENFORCE((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10);
ENFORCE((clock.getUtc() - ts_utc).getAbs().toMSec() < 10);
ASSERT(0 == if2.receive(frame, ts_mono, ts_utc, flags));
ASSERT(!if2.hasPendingTx());
ASSERT(!if2.hasReadyRx());
ENFORCE(0 == if2.receive(frame, ts_mono, ts_utc, flags));
ENFORCE(!if2.hasPendingTx());
ENFORCE(!if2.hasReadyRx());
}
static void testSocketFilters(const std::string& iface_name)
@@ -132,7 +135,7 @@ static void testSocketFilters(const std::string& iface_name)
const int sock1 = uavcan_linux::SocketCanIface::openSocket(iface_name);
const int sock2 = uavcan_linux::SocketCanIface::openSocket(iface_name);
ASSERT(sock1 >= 0 && sock2 >= 0);
ENFORCE(sock1 >= 0 && sock2 >= 0);
uavcan_linux::SocketCanIface if1(sock1);
uavcan_linux::SocketCanIface if2(sock2);
@@ -151,29 +154,29 @@ static void testSocketFilters(const std::string& iface_name)
fcs[2].id = 0;
fcs[2].mask = CanFrame::MaskExtID | CanFrame::FlagEFF;
ASSERT(0 == if2.configureFilters(fcs, 3));
ENFORCE(0 == if2.configureFilters(fcs, 3));
/*
* Sending data from 1 to 2, making sure only filtered data will be accepted
*/
const auto EFF = CanFrame::FlagEFF;
ASSERT(1 == if1.send(makeFrame(123, "1"), tsMonoOffsetMs(100), 0)); // Accept 0
ASSERT(1 == if1.send(makeFrame(123 | EFF, "2"), tsMonoOffsetMs(100), 0)); // Accept 0
ASSERT(1 == if1.send(makeFrame(456, "3"), tsMonoOffsetMs(100), 0)); // Drop
ASSERT(1 == if1.send(makeFrame(456789, "4"), tsMonoOffsetMs(100), 0)); // Drop
ASSERT(1 == if1.send(makeFrame(456789 | EFF, "5"), tsMonoOffsetMs(100), 0)); // Accept 1
ASSERT(1 == if1.send(makeFrame(0, "6"), tsMonoOffsetMs(100), 0)); // Accept 2
ASSERT(1 == if1.send(makeFrame(EFF, "7"), tsMonoOffsetMs(100), 0)); // Drop
ENFORCE(1 == if1.send(makeFrame(123, "1"), tsMonoOffsetMs(100), 0)); // Accept 0
ENFORCE(1 == if1.send(makeFrame(123 | EFF, "2"), tsMonoOffsetMs(100), 0)); // Accept 0
ENFORCE(1 == if1.send(makeFrame(456, "3"), tsMonoOffsetMs(100), 0)); // Drop
ENFORCE(1 == if1.send(makeFrame(456789, "4"), tsMonoOffsetMs(100), 0)); // Drop
ENFORCE(1 == if1.send(makeFrame(456789 | EFF, "5"), tsMonoOffsetMs(100), 0)); // Accept 1
ENFORCE(1 == if1.send(makeFrame(0, "6"), tsMonoOffsetMs(100), 0)); // Accept 2
ENFORCE(1 == if1.send(makeFrame(EFF, "7"), tsMonoOffsetMs(100), 0)); // Drop
for (int i = 0; i < 7; i++)
{
if1.poll(true, true);
if2.poll(true, false);
}
ASSERT(!if1.hasPendingTx());
ASSERT(!if1.hasReadyRx());
ASSERT(0 == if1.getErrorCount());
ASSERT(if2.hasReadyRx());
ENFORCE(!if1.hasPendingTx());
ENFORCE(!if1.hasReadyRx());
ENFORCE(0 == if1.getErrorCount());
ENFORCE(if2.hasReadyRx());
/*
* Checking RX on 2
@@ -184,33 +187,124 @@ static void testSocketFilters(const std::string& iface_name)
uavcan::UtcTime ts_utc;
uavcan::CanIOFlags flags = 0;
ASSERT(1 == if2.receive(frame, ts_mono, ts_utc, flags));
ASSERT(frame == makeFrame(0, "6"));
ASSERT(flags == 0);
ENFORCE(1 == if2.receive(frame, ts_mono, ts_utc, flags));
ENFORCE(frame == makeFrame(0, "6"));
ENFORCE(flags == 0);
ASSERT(1 == if2.receive(frame, ts_mono, ts_utc, flags));
ASSERT(frame == makeFrame(123 | EFF, "2"));
ENFORCE(1 == if2.receive(frame, ts_mono, ts_utc, flags));
ENFORCE(frame == makeFrame(123 | EFF, "2"));
ASSERT(1 == if2.receive(frame, ts_mono, ts_utc, flags));
ASSERT(frame == makeFrame(456789 | EFF, "5"));
ENFORCE(1 == if2.receive(frame, ts_mono, ts_utc, flags));
ENFORCE(frame == makeFrame(456789 | EFF, "5"));
ASSERT(1 == if2.receive(frame, ts_mono, ts_utc, flags));
ASSERT(frame == makeFrame(123, "1"));
ENFORCE(1 == if2.receive(frame, ts_mono, ts_utc, flags));
ENFORCE(frame == makeFrame(123, "1"));
ASSERT(!if2.hasReadyRx());
ENFORCE(!if2.hasReadyRx());
}
static void testDriver(const std::vector<std::string>& iface_names)
{
uavcan_linux::SocketCanDriver driver;
for (auto ifn : iface_names)
{
std::cout << "Adding iface " << ifn << std::endl;
ENFORCE(0 == driver.addIface(ifn));
}
ENFORCE(-1 == driver.addIface("noif9"));
ENFORCE(-1 == driver.addIface("noif9"));
ENFORCE(-1 == driver.addIface("noif9"));
ENFORCE(driver.getNumIfaces() == iface_names.size());
ENFORCE(nullptr == driver.getIface(255));
ENFORCE(nullptr == driver.getIface(driver.getNumIfaces()));
const unsigned AllIfacesMask = (1 << driver.getNumIfaces()) - 1;
/*
* Send, no loopback
*/
std::cout << "select() 1" << std::endl;
uavcan::CanSelectMasks masks; // Driver provides masks for all available events
ENFORCE(driver.getNumIfaces() == driver.select(masks, tsMonoOffsetMs(1000)));
ENFORCE(masks.read == 0);
ENFORCE(masks.write == AllIfacesMask);
for (int i = 0; i < driver.getNumIfaces(); i++)
{
ENFORCE(1 == driver.getIface(i)->send(makeFrame(123, std::to_string(i)), tsMonoOffsetMs(10), 0));
}
std::cout << "select() 2" << std::endl;
ENFORCE(driver.getNumIfaces() == driver.select(masks, tsMonoOffsetMs(1000)));
ENFORCE(masks.read == 0);
ENFORCE(masks.write == AllIfacesMask);
/*
* Send with loopback
*/
for (int i = 0; i < driver.getNumIfaces(); i++)
{
ENFORCE(1 == driver.getIface(i)->send(makeFrame(456, std::to_string(i)), tsMonoOffsetMs(10),
uavcan::CanIOFlagLoopback));
ENFORCE(1 == driver.getIface(i)->send(makeFrame(789, std::to_string(i)), tsMonoOffsetMs(-1), // Will timeout
uavcan::CanIOFlagLoopback));
}
std::cout << "select() 3" << std::endl;
ENFORCE(driver.getNumIfaces() == driver.select(masks, tsMonoOffsetMs(1000)));
ENFORCE(masks.read == AllIfacesMask);
ENFORCE(masks.write == AllIfacesMask);
/*
* Receive loopback
*/
const uavcan_linux::SystemClock clock(uavcan_linux::ClockAdjustmentMode::PerDriverPrivate);
for (int i = 0; i < driver.getNumIfaces(); i++)
{
uavcan::CanFrame frame;
uavcan::MonotonicTime ts_mono;
uavcan::UtcTime ts_utc;
uavcan::CanIOFlags flags = 0;
ENFORCE(1 == driver.getIface(i)->receive(frame, ts_mono, ts_utc, flags));
ENFORCE(frame == makeFrame(456, std::to_string(i)));
ENFORCE(flags == uavcan::CanIOFlagLoopback);
ENFORCE((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10);
ENFORCE((clock.getUtc() - ts_utc).getAbs().toMSec() < 10);
ENFORCE(!driver.getIface(i)->hasPendingTx());
ENFORCE(!driver.getIface(i)->hasReadyRx());
}
std::cout << "select() 4" << std::endl;
masks.write = 0;
ENFORCE(driver.getNumIfaces() == driver.select(masks, tsMonoOffsetMs(1000)));
ENFORCE(masks.read == 0);
ENFORCE(masks.write == AllIfacesMask);
std::cout << "exit" << std::endl;
}
int main(int argc, const char** argv)
{
if (argc < 2)
{
std::cout << "Usage:\n\t" << argv[0] << " <can-iface-name>" << std::endl;
std::cout << "Usage:\n\t" << argv[0] << " <can-iface-name-1> [can-iface-name-N...]" << std::endl;
return 1;
}
std::vector<std::string> iface_names;
for (int i = 1; i < argc; i++)
{
iface_names.emplace_back(argv[i]);
}
testNonexistentIface();
testSocketRxTx(argv[1]);
testSocketFilters(argv[1]);
testSocketRxTx(iface_names[0]);
testSocketFilters(iface_names[0]);
testDriver(iface_names);
return 0;
}