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

Reorganized directory structure - directory 'internal' pulled up one level. No logical changes.

Browse Source
This commit is contained in:
Pavel Kirienko
2014-03-14 15:01:20 +04:00
parent ab34c94ba2
commit 15cbf96378
90 changed files with 165 additions and 166 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libuavcan/test/node/global_data_type_registry.cpp
+260
View File
@@ -0,0 +1,260 @@
/*
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include <gtest/gtest.h>
#include <uavcan/node/global_data_type_registry.hpp>
namespace
{
struct DataTypeAMessage
{
enum { DefaultDataTypeID = 0 };
enum { DataTypeKind = uavcan::DataTypeKindMessage };
static uavcan::DataTypeSignature getDataTypeSignature() { return uavcan::DataTypeSignature(123); }
static const char* getDataTypeFullName() { return "my_namespace.DataTypeA"; }
};
struct DataTypeAService
{
enum { DefaultDataTypeID = 0 };
enum { DataTypeKind = uavcan::DataTypeKindService };
static uavcan::DataTypeSignature getDataTypeSignature() { return uavcan::DataTypeSignature(789); }
static const char* getDataTypeFullName() { return "my_namespace.DataTypeA"; }
};
struct DataTypeB
{
enum { DefaultDataTypeID = 42 };
enum { DataTypeKind = uavcan::DataTypeKindMessage };
static uavcan::DataTypeSignature getDataTypeSignature() { return uavcan::DataTypeSignature(456); }
static const char* getDataTypeFullName() { return "my_namespace.DataTypeB"; }
};
struct DataTypeC
{
enum { DefaultDataTypeID = 1023 };
enum { DataTypeKind = uavcan::DataTypeKindMessage };
static uavcan::DataTypeSignature getDataTypeSignature() { return uavcan::DataTypeSignature(654); }
static const char* getDataTypeFullName() { return "foo.DataTypeC"; }
};
struct DataTypeD
{
enum { DefaultDataTypeID = 43 };
enum { DataTypeKind = uavcan::DataTypeKindService };
static uavcan::DataTypeSignature getDataTypeSignature() { return uavcan::DataTypeSignature(987); }
static const char* getDataTypeFullName() { return "foo.DataTypeD"; }
};
template <typename Type>
uavcan::DataTypeDescriptor extractDescriptor(uint16_t dtid = Type::DefaultDataTypeID)
{
return uavcan::DataTypeDescriptor(uavcan::DataTypeKind(Type::DataTypeKind), dtid,
Type::getDataTypeSignature(), Type::getDataTypeFullName());
}
}
TEST(GlobalDataTypeRegistry, Basic)
{
using uavcan::GlobalDataTypeRegistry;
using uavcan::DataTypeIDMask;
using uavcan::DataTypeSignature;
GlobalDataTypeRegistry::instance().reset();
ASSERT_FALSE(GlobalDataTypeRegistry::instance().isFrozen());
ASSERT_EQ(0, GlobalDataTypeRegistry::instance().getNumMessageTypes());
ASSERT_EQ(0, GlobalDataTypeRegistry::instance().getNumServiceTypes());
DataTypeIDMask dtmask;
dtmask.set();
GlobalDataTypeRegistry::instance().getDataTypeIDMask(uavcan::DataTypeKindMessage, dtmask);
ASSERT_FALSE(dtmask.any());
dtmask.set();
GlobalDataTypeRegistry::instance().getDataTypeIDMask(uavcan::DataTypeKindService, dtmask);
ASSERT_FALSE(dtmask.any());
/*
* Static registrations
*/
uavcan::DefaultDataTypeRegistrator<DataTypeAMessage> reg_DataTypeAMessage;
uavcan::DefaultDataTypeRegistrator<DataTypeB> reg_DataTypeB;
ASSERT_EQ(2, GlobalDataTypeRegistry::instance().getNumMessageTypes());
ASSERT_EQ(0, GlobalDataTypeRegistry::instance().getNumServiceTypes());
GlobalDataTypeRegistry::instance().getDataTypeIDMask(uavcan::DataTypeKindMessage, dtmask);
ASSERT_TRUE(dtmask[0]);
ASSERT_TRUE(dtmask[42]);
dtmask[0] = dtmask[42] = false;
ASSERT_FALSE(dtmask.any());
/*
* Runtime registrations
*/
ASSERT_EQ(GlobalDataTypeRegistry::RegistResultOk,
GlobalDataTypeRegistry::instance().regist<DataTypeAService>(DataTypeAService::DefaultDataTypeID));
ASSERT_EQ(2, GlobalDataTypeRegistry::instance().getNumMessageTypes());
ASSERT_EQ(1, GlobalDataTypeRegistry::instance().getNumServiceTypes());
GlobalDataTypeRegistry::instance().getDataTypeIDMask(uavcan::DataTypeKindService, dtmask);
ASSERT_TRUE(dtmask[0]);
dtmask[0] = false;
ASSERT_FALSE(dtmask.any());
/*
* Runtime re-registration
*/
ASSERT_EQ(GlobalDataTypeRegistry::RegistResultOk, GlobalDataTypeRegistry::instance().regist<DataTypeAService>(147));
ASSERT_EQ(GlobalDataTypeRegistry::RegistResultOk, GlobalDataTypeRegistry::instance().regist<DataTypeB>(741));
ASSERT_EQ(2, GlobalDataTypeRegistry::instance().getNumMessageTypes());
ASSERT_EQ(1, GlobalDataTypeRegistry::instance().getNumServiceTypes());
GlobalDataTypeRegistry::instance().getDataTypeIDMask(uavcan::DataTypeKindMessage, dtmask);
ASSERT_TRUE(dtmask[0]);
ASSERT_TRUE(dtmask[741]);
dtmask[0] = dtmask[741] = false;
ASSERT_FALSE(dtmask.any());
GlobalDataTypeRegistry::instance().getDataTypeIDMask(uavcan::DataTypeKindService, dtmask);
ASSERT_TRUE(dtmask[147]);
dtmask[147] = false;
ASSERT_FALSE(dtmask.any());
/*
* These types will be necessary for the aggregate signature test
*/
ASSERT_EQ(GlobalDataTypeRegistry::RegistResultCollision,
GlobalDataTypeRegistry::instance().regist<DataTypeC>(741)); // ID COLLISION
ASSERT_EQ(GlobalDataTypeRegistry::RegistResultOk,
GlobalDataTypeRegistry::instance().regist<DataTypeC>(DataTypeC::DefaultDataTypeID));
uavcan::DefaultDataTypeRegistrator<DataTypeD> reg_DataTypeD;
GlobalDataTypeRegistry::instance().getDataTypeIDMask(uavcan::DataTypeKindMessage, dtmask);
ASSERT_TRUE(dtmask[0]);
ASSERT_TRUE(dtmask[741]);
ASSERT_TRUE(dtmask[1023]);
dtmask[0] = dtmask[1023] = dtmask[741] = false;
ASSERT_FALSE(dtmask.any());
GlobalDataTypeRegistry::instance().getDataTypeIDMask(uavcan::DataTypeKindService, dtmask);
ASSERT_TRUE(dtmask[147]);
ASSERT_TRUE(dtmask[43]);
dtmask[43] = dtmask[147] = false;
ASSERT_FALSE(dtmask.any());
/*
* Frozen state
*/
GlobalDataTypeRegistry::instance().freeze();
ASSERT_EQ(GlobalDataTypeRegistry::RegistResultFrozen,
GlobalDataTypeRegistry::instance().regist<DataTypeAService>(555)); // Rejected
ASSERT_EQ(GlobalDataTypeRegistry::RegistResultFrozen,
GlobalDataTypeRegistry::instance().regist<DataTypeAMessage>(999)); // Rejected
ASSERT_EQ(GlobalDataTypeRegistry::RegistResultFrozen,
GlobalDataTypeRegistry::instance().regist<DataTypeB>(888)); // Rejected
/*
* Searching
*/
const uavcan::DataTypeDescriptor* pdtd = NULL;
ASSERT_FALSE(GlobalDataTypeRegistry::instance().find(uavcan::DataTypeKindMessage, "Nonexistent"));
// Asking for service, but this is a message:
ASSERT_FALSE(GlobalDataTypeRegistry::instance().find(uavcan::DataTypeKindService, "my_namespace.DataTypeB"));
ASSERT_TRUE((pdtd = GlobalDataTypeRegistry::instance().find(uavcan::DataTypeKindMessage, "my_namespace.DataTypeB")));
ASSERT_EQ(extractDescriptor<DataTypeB>(741), *pdtd);
ASSERT_TRUE((pdtd = GlobalDataTypeRegistry::instance().find(uavcan::DataTypeKindMessage, "my_namespace.DataTypeA")));
ASSERT_EQ(extractDescriptor<DataTypeAMessage>(), *pdtd);
ASSERT_TRUE((pdtd = GlobalDataTypeRegistry::instance().find(uavcan::DataTypeKindService, "my_namespace.DataTypeA")));
ASSERT_EQ(extractDescriptor<DataTypeAService>(147), *pdtd);
}
TEST(GlobalDataTypeRegistry, AggregateSignature)
{
using uavcan::GlobalDataTypeRegistry;
using uavcan::DataTypeIDMask;
using uavcan::DataTypeSignature;
ASSERT_TRUE(GlobalDataTypeRegistry::instance().isFrozen());
DataTypeIDMask mask;
DataTypeSignature sign;
// Zero - empty mask
sign = GlobalDataTypeRegistry::instance().computeAggregateSignature(uavcan::DataTypeKindMessage, mask);
ASSERT_EQ(DataTypeSignature(), sign);
ASSERT_FALSE(mask.any());
// All set
mask.set();
sign = GlobalDataTypeRegistry::instance().computeAggregateSignature(uavcan::DataTypeKindMessage, mask);
ASSERT_TRUE(mask[0]); // DataTypeAMessage
ASSERT_TRUE(mask[741]); // DataTypeB
ASSERT_TRUE(mask[1023]); // DataTypeC
mask[0] = mask[741] = mask[1023] = false;
ASSERT_FALSE(mask.any());
{
DataTypeSignature check_signature(DataTypeAMessage::getDataTypeSignature()); // Order matters - low --> high
check_signature.extend(DataTypeB::getDataTypeSignature());
check_signature.extend(DataTypeC::getDataTypeSignature());
ASSERT_EQ(check_signature, sign);
}
mask.set();
sign = GlobalDataTypeRegistry::instance().computeAggregateSignature(uavcan::DataTypeKindService, mask);
ASSERT_TRUE(mask[43]); // DataTypeD
ASSERT_TRUE(mask[147]); // DataTypeAService
mask[43] = mask[147] = false;
ASSERT_FALSE(mask.any());
{
DataTypeSignature check_signature(DataTypeD::getDataTypeSignature());
check_signature.extend(DataTypeAService::getDataTypeSignature());
ASSERT_EQ(check_signature, sign);
}
// Random
mask[0] = mask[99] = mask[147] = mask[741] = mask[999] = mask[1022] = true;
sign = GlobalDataTypeRegistry::instance().computeAggregateSignature(uavcan::DataTypeKindMessage, mask);
ASSERT_TRUE(mask[0]); // DataTypeAMessage
ASSERT_TRUE(mask[741]); // DataTypeB
mask[0] = mask[741] = false;
ASSERT_FALSE(mask.any());
{
DataTypeSignature check_signature(DataTypeAMessage::getDataTypeSignature()); // Order matters - low --> high
check_signature.extend(DataTypeB::getDataTypeSignature());
ASSERT_EQ(check_signature, sign);
}
// One
mask[1] = mask[43] = true;
sign = GlobalDataTypeRegistry::instance().computeAggregateSignature(uavcan::DataTypeKindService, mask);
ASSERT_TRUE(mask[43]); // DataTypeD
mask[43] = false;
ASSERT_FALSE(mask.any());
ASSERT_EQ(DataTypeD::getDataTypeSignature(), sign);
}
TEST(GlobalDataTypeRegistry, Reset)
{
using uavcan::GlobalDataTypeRegistry;
/*
* Since we're dealing with singleton, we need to reset it for other tests to use
*/
ASSERT_TRUE(GlobalDataTypeRegistry::instance().isFrozen());
GlobalDataTypeRegistry::instance().reset();
ASSERT_FALSE(GlobalDataTypeRegistry::instance().isFrozen());
}
libuavcan/test/node/publisher.cpp
+116
View File
@@ -0,0 +1,116 @@
/*
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include <gtest/gtest.h>
#include <uavcan/node/publisher.hpp>
#include <uavcan/mavlink/Message.hpp>
#include "../clock.hpp"
#include "../transport/can/can.hpp"
TEST(Publisher, Basic)
{
uavcan::PoolAllocator<uavcan::MemPoolBlockSize * 8, uavcan::MemPoolBlockSize> pool;
uavcan::PoolManager<1> poolmgr;
poolmgr.addPool(&pool);
SystemClockMock clock_mock(100);
CanDriverMock can_driver(2, clock_mock);
uavcan::OutgoingTransferRegistry<8> out_trans_reg(poolmgr);
uavcan::Scheduler sch(can_driver, poolmgr, clock_mock, out_trans_reg, uavcan::NodeID(1));
uavcan::MarshalBufferProvider<> buffer_provider;
uavcan::Publisher<uavcan::mavlink::Message> publisher(sch, buffer_provider);
std::cout <<
"sizeof(uavcan::Publisher<uavcan::mavlink::Message>): " <<
sizeof(uavcan::Publisher<uavcan::mavlink::Message>) << std::endl;
// Manual type registration - we can't rely on the GDTR state
uavcan::GlobalDataTypeRegistry::instance().reset();
uavcan::DefaultDataTypeRegistrator<uavcan::mavlink::Message> _registrator;
/*
* Message layout:
* uint8 seq
* uint8 sysid
* uint8 compid
* uint8 msgid
* uint8[<256] payload
*/
uavcan::mavlink::Message msg;
msg.seq = 0x42;
msg.sysid = 0x72;
msg.compid = 0x08;
msg.msgid = 0xa5;
msg.payload = "Msg";
const uint8_t expected_transfer_payload[] = {0x42, 0x72, 0x08, 0xa5, 'M', 's', 'g'};
const uint64_t tx_timeout_usec = publisher.getDefaultTxTimeout().toUSec();
/*
* Broadcast
*/
{
ASSERT_LT(0, publisher.broadcast(msg));
// uint_fast16_t data_type_id, TransferType transfer_type, NodeID src_node_id, NodeID dst_node_id,
// uint_fast8_t frame_index, TransferID transfer_id, bool last_frame = false
uavcan::Frame expected_frame(uavcan::mavlink::Message::DefaultDataTypeID, uavcan::TransferTypeMessageBroadcast,
sch.getDispatcher().getSelfNodeID(), uavcan::NodeID::Broadcast, 0, 0, true);
expected_frame.setPayload(expected_transfer_payload, 7);
uavcan::CanFrame expected_can_frame;
ASSERT_TRUE(expected_frame.compile(expected_can_frame));
ASSERT_TRUE(can_driver.ifaces[0].matchAndPopTx(expected_can_frame, tx_timeout_usec + 100));
ASSERT_TRUE(can_driver.ifaces[1].matchAndPopTx(expected_can_frame, tx_timeout_usec + 100));
ASSERT_TRUE(can_driver.ifaces[0].tx.empty());
ASSERT_TRUE(can_driver.ifaces[1].tx.empty());
// Second shot - checking the transfer ID
ASSERT_LT(0, publisher.broadcast(msg));
expected_frame = uavcan::Frame(uavcan::mavlink::Message::DefaultDataTypeID, uavcan::TransferTypeMessageBroadcast,
sch.getDispatcher().getSelfNodeID(), uavcan::NodeID::Broadcast, 0, 1, true);
expected_frame.setPayload(expected_transfer_payload, 7);
ASSERT_TRUE(expected_frame.compile(expected_can_frame));
ASSERT_TRUE(can_driver.ifaces[0].matchAndPopTx(expected_can_frame, tx_timeout_usec + 100));
ASSERT_TRUE(can_driver.ifaces[1].matchAndPopTx(expected_can_frame, tx_timeout_usec + 100));
ASSERT_TRUE(can_driver.ifaces[0].tx.empty());
ASSERT_TRUE(can_driver.ifaces[1].tx.empty());
}
clock_mock.advance(1000);
/*
* Unicast
*/
{
ASSERT_LT(0, publisher.unicast(msg, 0x44));
// uint_fast16_t data_type_id, TransferType transfer_type, NodeID src_node_id, NodeID dst_node_id,
// uint_fast8_t frame_index, TransferID transfer_id, bool last_frame = false
uavcan::Frame expected_frame(uavcan::mavlink::Message::DefaultDataTypeID, uavcan::TransferTypeMessageUnicast,
sch.getDispatcher().getSelfNodeID(), uavcan::NodeID(0x44), 0, 0, true);
expected_frame.setPayload(expected_transfer_payload, 7);
uavcan::CanFrame expected_can_frame;
ASSERT_TRUE(expected_frame.compile(expected_can_frame));
ASSERT_TRUE(can_driver.ifaces[0].matchAndPopTx(expected_can_frame, tx_timeout_usec + 100 + 1000));
ASSERT_TRUE(can_driver.ifaces[1].matchAndPopTx(expected_can_frame, tx_timeout_usec + 100 + 1000));
ASSERT_TRUE(can_driver.ifaces[0].tx.empty());
ASSERT_TRUE(can_driver.ifaces[1].tx.empty());
}
/*
* Misc
*/
ASSERT_TRUE(uavcan::GlobalDataTypeRegistry::instance().isFrozen());
}
libuavcan/test/node/scheduler.cpp
+92
View File
@@ -0,0 +1,92 @@
/*
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include <gtest/gtest.h>
#include <uavcan/node/timer.hpp>
#include <uavcan/util/method_binder.hpp>
#include "../clock.hpp"
#include "../transport/can/can.hpp"
struct TimerCallCounter
{
std::vector<uavcan::TimerEvent> events_a;
std::vector<uavcan::TimerEvent> events_b;
void callA(const uavcan::TimerEvent& ev) { events_a.push_back(ev); }
void callB(const uavcan::TimerEvent& ev) { events_b.push_back(ev); }
typedef uavcan::MethodBinder<TimerCallCounter*, void(TimerCallCounter::*)(const uavcan::TimerEvent&)> Binder;
};
/*
* This test can fail on a non real time system. That's kinda sad but okay.
*/
TEST(Scheduler, Timers)
{
uavcan::PoolAllocator<uavcan::MemPoolBlockSize * 8, uavcan::MemPoolBlockSize> pool;
uavcan::PoolManager<1> poolmgr;
poolmgr.addPool(&pool);
SystemClockDriver clock_driver;
CanDriverMock can_driver(2, clock_driver);
uavcan::OutgoingTransferRegistry<8> out_trans_reg(poolmgr);
uavcan::Scheduler sch(can_driver, poolmgr, clock_driver, out_trans_reg, uavcan::NodeID(1));
/*
* Registration
*/
{
TimerCallCounter tcc;
uavcan::TimerEventForwarder<TimerCallCounter::Binder>
a(sch, TimerCallCounter::Binder(&tcc, &TimerCallCounter::callA));
uavcan::TimerEventForwarder<TimerCallCounter::Binder>
b(sch, TimerCallCounter::Binder(&tcc, &TimerCallCounter::callB));
ASSERT_EQ(0, sch.getDeadlineScheduler().getNumHandlers());
const uavcan::MonotonicTime start_ts = clock_driver.getMonotonic();
a.startOneShotWithDeadline(start_ts + durMono(100000));
b.startPeriodic(durMono(1000));
ASSERT_EQ(2, sch.getDeadlineScheduler().getNumHandlers());
/*
* Spinning
*/
ASSERT_EQ(0, sch.spin(start_ts + durMono(1000000)));
ASSERT_EQ(1, tcc.events_a.size());
ASSERT_TRUE(areTimestampsClose(tcc.events_a[0].scheduled_time, start_ts + durMono(100000)));
ASSERT_TRUE(areTimestampsClose(tcc.events_a[0].scheduled_time, tcc.events_a[0].real_time));
ASSERT_LT(900, tcc.events_b.size());
ASSERT_GT(1100, tcc.events_b.size());
{
uavcan::MonotonicTime next_expected_deadline = start_ts + durMono(1000);
for (unsigned int i = 0; i < tcc.events_b.size(); i++)
{
ASSERT_TRUE(areTimestampsClose(tcc.events_b[i].scheduled_time, next_expected_deadline));
ASSERT_TRUE(areTimestampsClose(tcc.events_b[i].scheduled_time, tcc.events_b[i].real_time));
next_expected_deadline += durMono(1000);
}
}
/*
* Deinitialization
*/
ASSERT_EQ(1, sch.getDeadlineScheduler().getNumHandlers());
ASSERT_FALSE(a.isRunning());
ASSERT_EQ(uavcan::MonotonicDuration::getInfinite(), a.getPeriod());
ASSERT_TRUE(b.isRunning());
ASSERT_EQ(1000, b.getPeriod().toUSec());
}
ASSERT_EQ(0, sch.getDeadlineScheduler().getNumHandlers()); // Both timers were destroyed now
ASSERT_EQ(0, sch.spin(clock_driver.getMonotonic() + durMono(1000))); // Spin some more without timers
}
libuavcan/test/node/service_client.cpp
+248
View File
@@ -0,0 +1,248 @@
/*
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include <gtest/gtest.h>
#include <uavcan/node/service_client.hpp>
#include <uavcan/node/service_server.hpp>
#include <uavcan/util/method_binder.hpp>
#include <uavcan/protocol/ComputeAggregateTypeSignature.hpp>
#include <uavcan/protocol/GetDataTypeInfo.hpp>
#include <root_ns_a/StringService.hpp>
#include <queue>
#include "../clock.hpp"
#include "../transport/can/can.hpp"
template <typename DataType>
struct ServiceCallResultHandler
{
typedef typename uavcan::ServiceCallResult<DataType>::Status StatusType;
StatusType last_status;
uavcan::NodeID last_server_node_id;
typename DataType::Response last_response;
void handleResponse(const uavcan::ServiceCallResult<DataType>& result)
{
std::cout << result << std::endl;
last_status = result.status;
last_response = result.response;
last_server_node_id = result.server_node_id;
}
bool match(StatusType status, uavcan::NodeID server_node_id, const typename DataType::Response& response) const
{
return
status == last_status &&
server_node_id == last_server_node_id &&
response == last_response;
}
typedef uavcan::MethodBinder<ServiceCallResultHandler*,
void (ServiceCallResultHandler::*)(const uavcan::ServiceCallResult<DataType>&)> Binder;
Binder bind() { return Binder(this, &ServiceCallResultHandler::handleResponse); }
};
static void stringServiceServerCallback(const uavcan::ReceivedDataStructure<root_ns_a::StringService::Request>& req,
root_ns_a::StringService::Response& rsp)
{
rsp.string_response = "Request string: ";
rsp.string_response += req.string_request;
}
struct MakeshiftNode : uavcan::Noncopyable
{
uavcan::PoolAllocator<uavcan::MemPoolBlockSize * 8, uavcan::MemPoolBlockSize> pool;
uavcan::PoolManager<1> poolmgr;
SystemClockDriver clock_driver;
uavcan::MarshalBufferProvider<> buffer_provider;
uavcan::OutgoingTransferRegistry<8> otr;
uavcan::Scheduler scheduler;
MakeshiftNode(uavcan::ICanDriver& can_driver, uavcan::NodeID self_node_id)
: otr(poolmgr)
, scheduler(can_driver, poolmgr, clock_driver, otr, self_node_id)
{
poolmgr.addPool(&pool);
}
void spin(uavcan::MonotonicDuration duration)
{
scheduler.spin(clock_driver.getMonotonic() + duration);
}
};
struct PairableCanDriver : public uavcan::ICanDriver, public uavcan::ICanIface
{
uavcan::ISystemClock& clock;
PairableCanDriver* other;
std::queue<uavcan::CanFrame> read_queue;
PairableCanDriver(uavcan::ISystemClock& clock)
: clock(clock)
, other(NULL)
{ }
void linkTogether(PairableCanDriver* with)
{
this->other = with;
with->other = this;
}
uavcan::ICanIface* getIface(int iface_index)
{
if (iface_index == 0)
return this;
return NULL;
}
int getNumIfaces() const { return 1; }
int select(int& inout_write_iface_mask, int& inout_read_iface_mask, uavcan::MonotonicTime blocking_deadline)
{
assert(other);
if (inout_read_iface_mask == 1)
inout_read_iface_mask = read_queue.size() ? 1 : 0;
if (inout_read_iface_mask || inout_write_iface_mask)
return 1;
while (clock.getMonotonic() < blocking_deadline)
usleep(1000);
return 0;
}
int send(const uavcan::CanFrame& frame, uavcan::MonotonicTime)
{
assert(other);
other->read_queue.push(frame);
return 1;
}
int receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic, uavcan::UtcTime& out_ts_utc)
{
assert(other);
assert(read_queue.size());
out_frame = read_queue.front();
read_queue.pop();
out_ts_monotonic = clock.getMonotonic();
out_ts_utc = clock.getUtc();
return 1;
}
int configureFilters(const uavcan::CanFilterConfig*, int) { return -1; }
int getNumFilters() const { return 0; }
uint64_t getNumErrors() const { return 0; }
};
TEST(ServiceClient, Basic)
{
SystemClockDriver clock;
PairableCanDriver can_a(clock), can_b(clock);
can_a.linkTogether(&can_b);
MakeshiftNode node_a(can_a, 1), node_b(can_b, 2);
// Type registration
uavcan::GlobalDataTypeRegistry::instance().reset();
uavcan::DefaultDataTypeRegistrator<root_ns_a::StringService> _registrator;
// Server
uavcan::ServiceServer<root_ns_a::StringService> server(node_a.scheduler, node_a.poolmgr, node_a.buffer_provider);
ASSERT_EQ(1, server.start(stringServiceServerCallback));
{
// Caller
typedef uavcan::ServiceCallResult<root_ns_a::StringService> ResultType;
typedef uavcan::ServiceClient<root_ns_a::StringService,
typename ServiceCallResultHandler<root_ns_a::StringService>::Binder > ClientType;
ServiceCallResultHandler<root_ns_a::StringService> handler;
ClientType client1(node_b.scheduler, node_b.poolmgr, node_b.buffer_provider);
ClientType client2(node_b.scheduler, node_b.poolmgr, node_b.buffer_provider);
ClientType client3(node_b.scheduler, node_b.poolmgr, node_b.buffer_provider);
client1.setCallback(handler.bind());
client2.setCallback(client1.getCallback());
client3.setCallback(client1.getCallback());
client3.setRequestTimeout(uavcan::MonotonicDuration::fromMSec(100));
ASSERT_EQ(1, node_a.scheduler.getDispatcher().getNumServiceRequestListeners());
ASSERT_EQ(0, node_b.scheduler.getDispatcher().getNumServiceResponseListeners()); // NOT listening!
root_ns_a::StringService::Request request;
request.string_request = "Hello world";
ASSERT_LT(0, client1.call(1, request)); // OK
ASSERT_LT(0, client2.call(1, request)); // OK
ASSERT_LT(0, client3.call(99, request)); // Will timeout!
ASSERT_EQ(3, node_b.scheduler.getDispatcher().getNumServiceResponseListeners()); // Listening now!
ASSERT_TRUE(client1.isPending());
ASSERT_TRUE(client2.isPending());
ASSERT_TRUE(client3.isPending());
node_a.spin(uavcan::MonotonicDuration::fromMSec(10));
node_b.spin(uavcan::MonotonicDuration::fromMSec(10));
ASSERT_EQ(1, node_b.scheduler.getDispatcher().getNumServiceResponseListeners()); // Third is still listening!
ASSERT_FALSE(client1.isPending());
ASSERT_FALSE(client2.isPending());
ASSERT_TRUE(client3.isPending());
// Validating
root_ns_a::StringService::Response expected_response;
expected_response.string_response = "Request string: Hello world";
ASSERT_TRUE(handler.match(ResultType::Success, 1, expected_response));
node_a.spin(uavcan::MonotonicDuration::fromMSec(100));
node_b.spin(uavcan::MonotonicDuration::fromMSec(100));
ASSERT_FALSE(client1.isPending());
ASSERT_FALSE(client2.isPending());
ASSERT_FALSE(client3.isPending());
ASSERT_EQ(0, node_b.scheduler.getDispatcher().getNumServiceResponseListeners()); // Third has timed out :(
// Validating
ASSERT_TRUE(handler.match(ResultType::ErrorTimeout, 99, root_ns_a::StringService::Response()));
// Stray request
ASSERT_LT(0, client3.call(99, request)); // Will timeout!
ASSERT_TRUE(client3.isPending());
ASSERT_EQ(1, node_b.scheduler.getDispatcher().getNumServiceResponseListeners());
}
// All destroyed - nobody listening
ASSERT_EQ(0, node_b.scheduler.getDispatcher().getNumServiceResponseListeners());
}
TEST(ServiceClient, Sizes)
{
using namespace uavcan;
std::cout << "ComputeAggregateTypeSignature server: " <<
sizeof(ServiceServer<protocol::ComputeAggregateTypeSignature>) << std::endl;
std::cout << "ComputeAggregateTypeSignature client: " <<
sizeof(ServiceClient<protocol::ComputeAggregateTypeSignature>) << std::endl;
std::cout << "ComputeAggregateTypeSignature request data struct: " <<
sizeof(protocol::ComputeAggregateTypeSignature::Request) << std::endl;
std::cout << "GetDataTypeInfo server: " <<
sizeof(ServiceServer<protocol::GetDataTypeInfo>) << std::endl;
std::cout << "GetDataTypeInfo client: " <<
sizeof(ServiceClient<protocol::GetDataTypeInfo>) << std::endl;
}
libuavcan/test/node/service_server.cpp
+119
View File
@@ -0,0 +1,119 @@
/*
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include <gtest/gtest.h>
#include <uavcan/node/service_server.hpp>
#include <uavcan/util/method_binder.hpp>
#include <root_ns_a/StringService.hpp>
#include "../clock.hpp"
#include "../transport/can/can.hpp"
struct ServerImpl
{
const char* string_response;
ServerImpl(const char* string_response) : string_response(string_response) { }
void handleRequest(const uavcan::ReceivedDataStructure<root_ns_a::StringService::Request>& request,
root_ns_a::StringService::Response& response)
{
std::cout << request << std::endl;
response.string_response = request.string_request;
response.string_response += " --> ";
response.string_response += string_response;
std::cout << response << std::endl;
}
typedef uavcan::MethodBinder<ServerImpl*,
void (ServerImpl::*)(const uavcan::ReceivedDataStructure<root_ns_a::StringService::Request>&,
root_ns_a::StringService::Response&)> Binder;
Binder bind() { return Binder(this, &ServerImpl::handleRequest); }
};
TEST(ServiceServer, Basic)
{
uavcan::PoolAllocator<uavcan::MemPoolBlockSize * 8, uavcan::MemPoolBlockSize> pool;
uavcan::PoolManager<1> poolmgr;
poolmgr.addPool(&pool);
// Manual type registration - we can't rely on the GDTR state
uavcan::GlobalDataTypeRegistry::instance().reset();
uavcan::DefaultDataTypeRegistrator<root_ns_a::StringService> _registrator;
SystemClockDriver clock_driver;
CanDriverMock can_driver(1, clock_driver);
uavcan::MarshalBufferProvider<> buffer_provider;
uavcan::OutgoingTransferRegistry<8> out_trans_reg(poolmgr);
uavcan::Scheduler sch(can_driver, poolmgr, clock_driver, out_trans_reg, uavcan::NodeID(1));
ServerImpl impl("456");
{
uavcan::ServiceServer<root_ns_a::StringService, ServerImpl::Binder> server(sch, poolmgr, buffer_provider);
ASSERT_EQ(0, sch.getDispatcher().getNumServiceRequestListeners());
server.start(impl.bind());
ASSERT_EQ(1, sch.getDispatcher().getNumServiceRequestListeners());
/*
* Request frames
*/
for (int i = 0; i < 2; i++)
{
// uint_fast16_t data_type_id, TransferType transfer_type, NodeID src_node_id, NodeID dst_node_id,
// uint_fast8_t frame_index, TransferID transfer_id, bool last_frame
uavcan::Frame frame(root_ns_a::StringService::DefaultDataTypeID, uavcan::TransferTypeServiceRequest,
uavcan::NodeID(i + 0x10), 1, 0, i, true);
const uint8_t req[] = {'r', 'e', 'q', uint8_t(i + '0')};
frame.setPayload(req, sizeof(req));
uavcan::RxFrame rx_frame(frame, clock_driver.getMonotonic(), clock_driver.getUtc(), 0);
can_driver.ifaces[0].pushRx(rx_frame);
}
sch.spin(clock_driver.getMonotonic() + uavcan::MonotonicDuration::fromUSec(10000));
/*
* Responses (MFT)
*/
ASSERT_EQ(4, can_driver.ifaces[0].tx.size());
for (int i = 0; i < 2; i++)
{
char payloads[2][8];
std::snprintf(payloads[0], 8, "req%i ", i);
std::snprintf(payloads[1], 8, "--> 456");
// First frame
uavcan::Frame fr;
ASSERT_TRUE(fr.parse(can_driver.ifaces[0].popTxFrame()));
std::cout << fr.toString() << std::endl;
ASSERT_STREQ(payloads[0], reinterpret_cast<const char*>(fr.getPayloadPtr() + 2)); // Skipping CRC
ASSERT_EQ(i, fr.getTransferID().get());
ASSERT_EQ(uavcan::TransferTypeServiceResponse, fr.getTransferType());
ASSERT_EQ(i + 0x10, fr.getDstNodeID().get());
// Second frame
ASSERT_TRUE(fr.parse(can_driver.ifaces[0].popTxFrame()));
std::cout << fr.toString() << std::endl;
ASSERT_STREQ(payloads[1], reinterpret_cast<const char*>(fr.getPayloadPtr()));
ASSERT_EQ(i, fr.getTransferID().get());
ASSERT_EQ(uavcan::TransferTypeServiceResponse, fr.getTransferType());
ASSERT_EQ(i + 0x10, fr.getDstNodeID().get());
}
ASSERT_EQ(0, server.getRequestFailureCount());
ASSERT_EQ(0, server.getResponseFailureCount());
ASSERT_EQ(1, sch.getDispatcher().getNumServiceRequestListeners());
}
ASSERT_EQ(0, sch.getDispatcher().getNumServiceRequestListeners());
}
libuavcan/test/node/subscriber.cpp
+292
View File
@@ -0,0 +1,292 @@
/*
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include <gtest/gtest.h>
#include <uavcan/node/subscriber.hpp>
#include <uavcan/util/method_binder.hpp>
#include <uavcan/mavlink/Message.hpp>
#include <root_ns_a/EmptyMessage.hpp>
#include "../clock.hpp"
#include "../transport/can/can.hpp"
template <typename DataType>
struct SubscriptionListener
{
typedef uavcan::ReceivedDataStructure<DataType> ReceivedDataStructure;
struct ReceivedDataStructureCopy
{
uavcan::MonotonicTime ts_monotonic;
uavcan::UtcTime ts_utc;
uavcan::TransferType transfer_type;
uavcan::TransferID transfer_id;
uavcan::NodeID src_node_id;
DataType msg;
ReceivedDataStructureCopy(const ReceivedDataStructure& s)
: ts_monotonic(s.getMonotonicTimestamp())
, ts_utc(s.getUtcTimestamp())
, transfer_type(s.getTransferType())
, transfer_id(s.getTransferID())
, src_node_id(s.getSrcNodeID())
, msg(s)
{ }
};
std::vector<DataType> simple;
std::vector<ReceivedDataStructureCopy> extended;
void receiveExtended(ReceivedDataStructure& msg)
{
extended.push_back(msg);
}
void receiveSimple(DataType& msg)
{
simple.push_back(msg);
}
typedef SubscriptionListener<DataType> SelfType;
typedef uavcan::MethodBinder<SelfType*, void (SelfType::*)(ReceivedDataStructure&)> ExtendedBinder;
typedef uavcan::MethodBinder<SelfType*, void (SelfType::*)(DataType&)> SimpleBinder;
ExtendedBinder bindExtended() { return ExtendedBinder(this, &SelfType::receiveExtended); }
SimpleBinder bindSimple() { return SimpleBinder(this, &SelfType::receiveSimple); }
};
TEST(Subscriber, Basic)
{
uavcan::PoolAllocator<uavcan::MemPoolBlockSize * 8, uavcan::MemPoolBlockSize> pool;
uavcan::PoolManager<1> poolmgr;
poolmgr.addPool(&pool);
// Manual type registration - we can't rely on the GDTR state
uavcan::GlobalDataTypeRegistry::instance().reset();
uavcan::DefaultDataTypeRegistrator<uavcan::mavlink::Message> _registrator;
SystemClockDriver clock_driver;
CanDriverMock can_driver(2, clock_driver);
uavcan::OutgoingTransferRegistry<8> out_trans_reg(poolmgr);
uavcan::Scheduler sch(can_driver, poolmgr, clock_driver, out_trans_reg, uavcan::NodeID(1));
typedef SubscriptionListener<uavcan::mavlink::Message> Listener;
uavcan::Subscriber<uavcan::mavlink::Message, Listener::ExtendedBinder> sub_extended(sch, poolmgr);
uavcan::Subscriber<uavcan::mavlink::Message, Listener::ExtendedBinder> sub_extended2(sch, poolmgr); // Not used
uavcan::Subscriber<uavcan::mavlink::Message, Listener::SimpleBinder> sub_simple(sch, poolmgr);
uavcan::Subscriber<uavcan::mavlink::Message, Listener::SimpleBinder> sub_simple2(sch, poolmgr); // Not used
std::cout <<
"sizeof(uavcan::Subscriber<uavcan::mavlink::Message, Listener::ExtendedBinder>): " <<
sizeof(uavcan::Subscriber<uavcan::mavlink::Message, Listener::ExtendedBinder>) << std::endl;
// Null binder - will fail
ASSERT_EQ(-1, sub_extended.start(Listener::ExtendedBinder(NULL, NULL)));
Listener listener;
/*
* Message layout:
* uint8 seq
* uint8 sysid
* uint8 compid
* uint8 msgid
* uint8[<256] payload
*/
uavcan::mavlink::Message expected_msg;
expected_msg.seq = 0x42;
expected_msg.sysid = 0x72;
expected_msg.compid = 0x08;
expected_msg.msgid = 0xa5;
expected_msg.payload = "Msg";
const uint8_t transfer_payload[] = {0x42, 0x72, 0x08, 0xa5, 'M', 's', 'g'};
/*
* RxFrame generation
*/
std::vector<uavcan::RxFrame> rx_frames;
for (int i = 0; i < 4; i++)
{
uavcan::TransferType tt = (i & 1) ? uavcan::TransferTypeMessageUnicast : uavcan::TransferTypeMessageBroadcast;
uavcan::NodeID dni = (tt == uavcan::TransferTypeMessageBroadcast) ?
uavcan::NodeID::Broadcast : sch.getDispatcher().getSelfNodeID();
// uint_fast16_t data_type_id, TransferType transfer_type, NodeID src_node_id, NodeID dst_node_id,
// uint_fast8_t frame_index, TransferID transfer_id, bool last_frame
uavcan::Frame frame(uavcan::mavlink::Message::DefaultDataTypeID, tt, uavcan::NodeID(i + 100), dni, 0, i, true);
frame.setPayload(transfer_payload, 7);
uavcan::RxFrame rx_frame(frame, clock_driver.getMonotonic(), clock_driver.getUtc(), 0);
rx_frames.push_back(rx_frame);
}
/*
* Reception
*/
ASSERT_EQ(0, sch.getDispatcher().getNumMessageListeners());
ASSERT_EQ(1, sub_extended.start(listener.bindExtended()));
ASSERT_EQ(1, sub_extended2.start(listener.bindExtended()));
ASSERT_EQ(1, sub_simple.start(listener.bindSimple()));
ASSERT_EQ(1, sub_simple2.start(listener.bindSimple()));
ASSERT_EQ(4, sch.getDispatcher().getNumMessageListeners());
sub_extended2.stop(); // These are not used - making sure they aren't receiving anything
sub_simple2.stop();
ASSERT_EQ(2, sch.getDispatcher().getNumMessageListeners());
for (unsigned int i = 0; i < rx_frames.size(); i++)
{
can_driver.ifaces[0].pushRx(rx_frames[i]);
can_driver.ifaces[1].pushRx(rx_frames[i]);
}
ASSERT_LE(0, sch.spin(clock_driver.getMonotonic() + durMono(10000)));
/*
* Validation
*/
ASSERT_EQ(listener.extended.size(), rx_frames.size());
for (unsigned int i = 0; i < rx_frames.size(); i++)
{
const Listener::ReceivedDataStructureCopy s = listener.extended.at(i);
ASSERT_TRUE(s.msg == expected_msg);
ASSERT_EQ(rx_frames[i].getSrcNodeID(), s.src_node_id);
ASSERT_EQ(rx_frames[i].getTransferID(), s.transfer_id);
ASSERT_EQ(rx_frames[i].getTransferType(), s.transfer_type);
ASSERT_EQ(rx_frames[i].getMonotonicTimestamp(), s.ts_monotonic);
}
ASSERT_EQ(listener.simple.size(), rx_frames.size());
for (unsigned int i = 0; i < rx_frames.size(); i++)
{
ASSERT_TRUE(listener.simple.at(i) == expected_msg);
}
ASSERT_EQ(0, sub_extended.getFailureCount());
ASSERT_EQ(0, sub_simple.getFailureCount());
/*
* Unregistration
*/
ASSERT_EQ(2, sch.getDispatcher().getNumMessageListeners());
sub_extended.stop();
sub_extended2.stop();
sub_simple.stop();
sub_simple2.stop();
ASSERT_EQ(0, sch.getDispatcher().getNumMessageListeners());
}
static void panickingSink(const uavcan::ReceivedDataStructure<uavcan::mavlink::Message>&)
{
FAIL() << "I just went mad";
}
TEST(Subscriber, FailureCount)
{
uavcan::PoolAllocator<uavcan::MemPoolBlockSize * 8, uavcan::MemPoolBlockSize> pool;
uavcan::PoolManager<1> poolmgr;
poolmgr.addPool(&pool);
// Manual type registration - we can't rely on the GDTR state
uavcan::GlobalDataTypeRegistry::instance().reset();
uavcan::DefaultDataTypeRegistrator<uavcan::mavlink::Message> _registrator;
SystemClockDriver clock_driver;
CanDriverMock can_driver(2, clock_driver);
uavcan::OutgoingTransferRegistry<8> out_trans_reg(poolmgr);
uavcan::Scheduler sch(can_driver, poolmgr, clock_driver, out_trans_reg, uavcan::NodeID(1));
{
uavcan::Subscriber<uavcan::mavlink::Message> sub(sch, poolmgr);
ASSERT_EQ(0, sch.getDispatcher().getNumMessageListeners());
sub.start(panickingSink);
ASSERT_EQ(1, sch.getDispatcher().getNumMessageListeners());
ASSERT_EQ(0, sub.getFailureCount());
for (int i = 0; i < 4; i++)
{
// uint_fast16_t data_type_id, TransferType transfer_type, NodeID src_node_id, NodeID dst_node_id,
// uint_fast8_t frame_index, TransferID transfer_id, bool last_frame
uavcan::Frame frame(uavcan::mavlink::Message::DefaultDataTypeID, uavcan::TransferTypeMessageBroadcast,
uavcan::NodeID(i + 100), uavcan::NodeID::Broadcast, 0, i, true);
// No payload - broken transfer
uavcan::RxFrame rx_frame(frame, clock_driver.getMonotonic(), clock_driver.getUtc(), 0);
can_driver.ifaces[0].pushRx(rx_frame);
can_driver.ifaces[1].pushRx(rx_frame);
}
ASSERT_LE(0, sch.spin(clock_driver.getMonotonic() + durMono(10000)));
ASSERT_EQ(4, sub.getFailureCount());
ASSERT_EQ(1, sch.getDispatcher().getNumMessageListeners()); // Still there
}
ASSERT_EQ(0, sch.getDispatcher().getNumMessageListeners()); // Removed
}
TEST(Subscriber, SingleFrameTransfer)
{
uavcan::PoolAllocator<uavcan::MemPoolBlockSize * 8, uavcan::MemPoolBlockSize> pool;
uavcan::PoolManager<1> poolmgr;
poolmgr.addPool(&pool);
// Manual type registration - we can't rely on the GDTR state
uavcan::GlobalDataTypeRegistry::instance().reset();
uavcan::DefaultDataTypeRegistrator<root_ns_a::EmptyMessage> _registrator;
SystemClockDriver clock_driver;
CanDriverMock can_driver(2, clock_driver);
uavcan::OutgoingTransferRegistry<8> out_trans_reg(poolmgr);
uavcan::Scheduler sch(can_driver, poolmgr, clock_driver, out_trans_reg, uavcan::NodeID(1));
typedef SubscriptionListener<root_ns_a::EmptyMessage> Listener;
uavcan::Subscriber<root_ns_a::EmptyMessage, Listener::SimpleBinder> sub(sch, poolmgr);
std::cout <<
"sizeof(uavcan::Subscriber<root_ns_a::EmptyMessage, Listener::SimpleBinder>): " <<
sizeof(uavcan::Subscriber<root_ns_a::EmptyMessage, Listener::SimpleBinder>) << std::endl;
Listener listener;
sub.start(listener.bindSimple());
for (int i = 0; i < 4; i++)
{
// uint_fast16_t data_type_id, TransferType transfer_type, NodeID src_node_id, NodeID dst_node_id,
// uint_fast8_t frame_index, TransferID transfer_id, bool last_frame
uavcan::Frame frame(root_ns_a::EmptyMessage::DefaultDataTypeID, uavcan::TransferTypeMessageBroadcast,
uavcan::NodeID(i + 100), uavcan::NodeID::Broadcast, 0, i, true);
// No payload - message is empty
uavcan::RxFrame rx_frame(frame, clock_driver.getMonotonic(), clock_driver.getUtc(), 0);
can_driver.ifaces[0].pushRx(rx_frame);
can_driver.ifaces[1].pushRx(rx_frame);
}
ASSERT_LE(0, sch.spin(clock_driver.getMonotonic() + durMono(10000)));
ASSERT_EQ(0, sub.getFailureCount());
ASSERT_EQ(4, listener.simple.size());
for (unsigned int i = 0; i < 4; i++)
{
ASSERT_TRUE(listener.simple.at(i) == root_ns_a::EmptyMessage());
}
}