diff --git a/libuavcan/test/node/test_node.hpp b/libuavcan/test/node/test_node.hpp index 418159aa33..d36fcc8e2e 100644 --- a/libuavcan/test/node/test_node.hpp +++ b/libuavcan/test/node/test_node.hpp @@ -6,6 +6,8 @@ #include #include +#include +#include #include "../transport/can/can.hpp" @@ -43,21 +45,21 @@ struct TestNode : public uavcan::INode struct PairableCanDriver : public uavcan::ICanDriver, public uavcan::ICanIface { uavcan::ISystemClock& clock; - PairableCanDriver* other; + std::set others; std::queue read_queue; std::queue loopback_queue; uint64_t error_count; PairableCanDriver(uavcan::ISystemClock& clock) : clock(clock) - , other(NULL) , error_count(0) { } void linkTogether(PairableCanDriver* with) { - this->other = with; - with->other = this; + this->others.insert(with); + with->others.insert(this); + others.erase(this); } virtual uavcan::ICanIface* getIface(uavcan::uint8_t iface_index) @@ -73,7 +75,6 @@ struct PairableCanDriver : public uavcan::ICanDriver, public uavcan::ICanIface virtual uavcan::int16_t select(uavcan::CanSelectMasks& inout_masks, uavcan::MonotonicTime blocking_deadline) { - assert(other); if (inout_masks.read == 1) { inout_masks.read = (!read_queue.empty() || !loopback_queue.empty()) ? 1 : 0; @@ -91,8 +92,11 @@ struct PairableCanDriver : public uavcan::ICanDriver, public uavcan::ICanIface virtual uavcan::int16_t send(const uavcan::CanFrame& frame, uavcan::MonotonicTime, uavcan::CanIOFlags flags) { - assert(other); - other->read_queue.push(frame); + assert(!others.empty()); + for (std::set::iterator it = others.begin(); it != others.end(); ++it) + { + (*it)->read_queue.push(frame); + } if (flags & uavcan::CanIOFlagLoopback) { loopback_queue.push(frame); @@ -103,7 +107,6 @@ struct PairableCanDriver : public uavcan::ICanDriver, public uavcan::ICanIface virtual uavcan::int16_t receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic, uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags) { - assert(other); out_flags = 0; if (loopback_queue.empty()) { @@ -186,3 +189,62 @@ struct InterlinkedTestNodes typedef InterlinkedTestNodes InterlinkedTestNodesWithSysClock; typedef InterlinkedTestNodes InterlinkedTestNodesWithClockMock; + + +template +struct TestNetwork +{ + struct NodeEnvironment + { + SystemClockDriver clock; + PairableCanDriver can_driver; + TestNode node; + + NodeEnvironment(uavcan::NodeID node_id) + : can_driver(clock) + , node(can_driver, clock, node_id) + { } + }; + + std::auto_ptr nodes[NumNodes]; + + TestNetwork(uavcan::uint8_t first_node_id = 1) + { + for (uavcan::uint8_t i = 0; i < NumNodes; i++) + { + nodes[i].reset(new NodeEnvironment(first_node_id + i)); + } + + for (uavcan::uint8_t i = 0; i < NumNodes; i++) + { + for (uavcan::uint8_t k = 0; k < NumNodes; k++) + { + nodes[i]->linkTogether(nodes[k].get()); + } + } + + for (uavcan::uint8_t i = 0; i < NumNodes; i++) + { + assert(nodes[i]->others.size() == (NumNodes - 1)); + } + } + + int spinAll(uavcan::MonotonicDuration duration) + { + assert(!duration.isNegative()); + unsigned nspins = unsigned(duration.toMSec() / NumNodes); + nspins = nspins ? nspins : 1; + while (nspins --> 0) + { + for (uavcan::uint8_t i = 0; i < NumNodes; i++) + { + int ret = nodes[i]->spin(uavcan::MonotonicDuration::fromMSec(1)); + if (ret < 0) + { + return ret; + } + } + } + return 0; + } +}; diff --git a/libuavcan/test/node/test_node_test.cpp b/libuavcan/test/node/test_node_test.cpp new file mode 100644 index 0000000000..c5731640eb --- /dev/null +++ b/libuavcan/test/node/test_node_test.cpp @@ -0,0 +1,32 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include "test_node.hpp" + + +TEST(TestNode, TestNetwork) +{ + TestNetwork<4> nwk; + + uavcan::CanFrame frame; + for (int i = 0; i < 8; i++) + { + frame.data[i] = i; + } + frame.id = 1234U; + + ASSERT_EQ(1, nwk.nodes[0]->can_driver.send(frame, uavcan::MonotonicTime(), uavcan::CanIOFlags())); + + for (int i = 1; i < 4; i++) + { + uavcan::CanFrame rx; + uavcan::MonotonicTime ts_mono; + uavcan::UtcTime ts_utc; + uavcan::CanIOFlags flags = 0; + ASSERT_EQ(1, nwk.nodes[i]->can_driver.receive(rx, ts_mono, ts_utc, flags)); + + ASSERT_TRUE(rx == frame); + } +} diff --git a/libuavcan/test/protocol/global_time_sync_master.cpp b/libuavcan/test/protocol/global_time_sync_master.cpp index 404d649027..bed4d908dc 100644 --- a/libuavcan/test/protocol/global_time_sync_master.cpp +++ b/libuavcan/test/protocol/global_time_sync_master.cpp @@ -30,9 +30,9 @@ struct GlobalTimeSyncTestNetwork , master_low(120) , master_high(8) { - slave.can.other = &master_low.can; - master_low.can.other = &slave.can; - master_high.can.other = &slave.can; + slave.can.others.insert(&master_low.can); + master_low.can.others.insert(&slave.can); + master_high.can.others.insert(&slave.can); } void spinAll(uavcan::MonotonicDuration duration = uavcan::MonotonicDuration::fromMSec(9)) diff --git a/libuavcan/test/protocol/global_time_sync_slave.cpp b/libuavcan/test/protocol/global_time_sync_slave.cpp index 5bb2e91c0d..25c4e50975 100644 --- a/libuavcan/test/protocol/global_time_sync_slave.cpp +++ b/libuavcan/test/protocol/global_time_sync_slave.cpp @@ -109,7 +109,7 @@ TEST(GlobalTimeSyncSlave, Basic) master2_clock.monotonic_auto_advance = 1000; master2_clock.preserve_utc = true; PairableCanDriver master2_can(master2_clock); - master2_can.other = &nodes.can_a; + master2_can.others.insert(&nodes.can_a); TestNode master2_node(master2_can, master2_clock, 8); uavcan::Publisher gts_pub2(master2_node);