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

Merge branch 'dynamic_node_id_raft'

Browse Source
This commit is contained in:
Pavel Kirienko
2015-06-03 02:38:00 +03:00
parent 489c178681 3d5f6f5d40
commit 06f5489c1f
139 changed files with 13398 additions and 1917 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+1
View File
@@ -33,5 +33,6 @@ add_subdirectory(libuavcan)
#
if (${CMAKE_SYSTEM_NAME} MATCHES "Linux")
message(STATUS "Adding Linux support library")
add_subdirectory(libuavcan_drivers/posix)
add_subdirectory(libuavcan_drivers/linux)
endif ()
dsdl/uavcan/protocol/dynamic_node_id/1010.Allocation.uavcan
+5
View File
@@ -39,6 +39,11 @@
#
uint16 DEFAULT_REQUEST_PERIOD_MS = 1000
#
# Server will reset its state if there was no follow-up request in this amount of time.
#
uint16 FOLLOWUP_TIMEOUT_MS = 500
#
# Any request message can accommodate no more than this number of bytes of unique ID.
# This limitation is needed to ensure that all request transfers are single-frame.
dsdl/uavcan/protocol/dynamic_node_id/server/1011.Discovery.uavcan
+20
View File
@@ -0,0 +1,20 @@
#
# This message is used by allocation servers to find each other's node ID.
# Please refer to the specification for details.
#
#
# This message should be broadcasted by the server at this interval until all other servers are discovered.
#
uint16 BROADCASTING_INTERVAL_MS = 1000
#
# Number of servers in the cluster as configured on the sender.
#
uint8 configured_cluster_size
#
# Node ID of servers that are known to the publishing server, including the publishing server itself.
# Capacity of this array defines maximum size of the server cluster.
#
uint8[<=5] known_nodes
dsdl/uavcan/protocol/dynamic_node_id/server/220.AppendEntries.uavcan
+33
View File
@@ -0,0 +1,33 @@
#
# This type is a part of the Raft consensus algorithm.
# Please refer to the specification for details.
#
#
# Given min election timeout and cluster size, the maximum recommended request interval can be derived as follows:
# max recommended request interval = (min election timeout) / 2 requests / (cluster size - 1)
# The equation assumes that the Leader requests one Follower at a time, so that there's at most one pending call
# at any moment. Such behavior is optimal as it creates uniform bus load, but it is actually implementation-specific.
# Obviously, request interval can be lower than that if needed, but higher values are not recommended as they may
# cause Followers to initiate premature elections in case of intensive frame losses or delays.
#
# Real timeout is randomized in the range (MIN, MAX], according to the Raft paper.
#
uint16 DEFAULT_MIN_ELECTION_TIMEOUT_MS = 4000
uint16 DEFAULT_MAX_ELECTION_TIMEOUT_MS = 6000
uint32 term
uint32 prev_log_term
uint8 prev_log_index
uint8 leader_commit
#
# Worst-case replication time per Follower can be computed as:
# worst replication time = (127 log entries) * (2 trips of next_index) * (request interval per Follower)
#
Entry[<=1] entries
---
uint32 term
bool success
dsdl/uavcan/protocol/dynamic_node_id/server/221.RequestVote.uavcan
+13
View File
@@ -0,0 +1,13 @@
#
# This type is a part of the Raft consensus algorithm.
# Please refer to the specification for details.
#
uint32 term
uint32 last_log_term
uint8 last_log_index
---
uint32 term
bool vote_granted
dsdl/uavcan/protocol/dynamic_node_id/server/Entry.uavcan
+10
View File
@@ -0,0 +1,10 @@
#
# One dynamic node ID allocation entry.
# This type is a part of the Raft consensus algorithm.
# Please refer to the specification for details.
#
uint32 term
uint8[16] unique_id
uint7 node_id
bool alignment
dsdl/uavcan/protocol/file/215.GetInfo.uavcan
+1 -2
View File
@@ -11,7 +11,6 @@ Path path
---
uint64 crc64 # Undefined for directories
uint32 size # Undefined for directories
uint40 size # Undefined for directories
Error error
EntryType entry_type
dsdl/uavcan/protocol/file/218.Read.uavcan
+1 -1
View File
@@ -12,7 +12,7 @@
# will be returned, and data array will be empty.
#
uint32 offset
uint40 offset
Path path
---
dsdl/uavcan/protocol/file/219.Write.uavcan
+2 -2
View File
@@ -15,9 +15,9 @@
# example by means of creating a staging area for uncompleted writes (like FTP servers do).
#
uint32 offset
uint40 offset
Path path
uint8[<=200] data
uint8[<=192] data
---
dsdl/uavcan/protocol/file/Error.uavcan
+10 -9
View File
@@ -3,16 +3,17 @@
# File operation result code.
#
int16 OK = 0
int16 UNKNOWN_ERROR = 32767
int16 OK = 0
int16 UNKNOWN_ERROR = 32767
# These error codes match some standard UNIX errno values
int16 NOT_FOUND = 2
int16 IO_ERROR = 5
int16 ACCESS_DENIED = 13
int16 IS_DIRECTORY = 21 # I.e. attempt to read/write on a path that points to a directory
int16 INVALID_VALUE = 22 # E.g. file name is not valid for the target file system
int16 FILE_TOO_LARGE = 27
int16 OUT_OF_SPACE = 28
int16 NOT_FOUND = 2
int16 IO_ERROR = 5
int16 ACCESS_DENIED = 13
int16 IS_DIRECTORY = 21 # I.e. attempt to read/write on a path that points to a directory
int16 INVALID_VALUE = 22 # E.g. file name is not valid for the target file system
int16 FILE_TOO_LARGE = 27
int16 OUT_OF_SPACE = 28
int16 NOT_IMPLEMENTED = 38
int16 value
libuavcan/include/uavcan/error.hpp
+1
View File
@@ -34,6 +34,7 @@ const int16_t ErrRecursiveCall = 9;
const int16_t ErrLogic = 10;
const int16_t ErrPassiveMode = 11; ///< Operation not permitted in passive mode
const int16_t ErrTransferTooLong = 12; ///< Transfer of this length cannot be sent with given transfer type
const int16_t ErrInvalidConfiguration = 13;
/**
* @}
*/
libuavcan/include/uavcan/marshal/array.hpp
+27
View File
@@ -667,6 +667,20 @@ public:
static_cast<unsigned>(RawValueType::MaxBitLen) * static_cast<unsigned>(MaxSize)
};
/**
* Default constructor zero-initializes the storage even if it consists of primitive types.
*/
Array() { }
/**
* String constructor - only for string-like arrays.
* Refer to @ref operator+=(const char*) for details.
*/
Array(const char* str) // Implicit
{
operator+=(str);
}
static int encode(const SelfType& array, ScalarCodec& codec, const TailArrayOptimizationMode tao_mode)
{
return array.encodeImpl(codec, tao_mode, BooleanType<IsDynamic>());
@@ -1073,6 +1087,19 @@ inline bool operator!=(const R& rhs, const Array<T, ArrayMode, MaxSize>& lhs)
return lhs.operator!=(rhs);
}
/**
* Shortcut for string-like array type instantiation.
* The proper way of doing this is actually "template<> using ... = ...", but this feature is not available in
* older C++ revisions which the library has to support.
*/
template <unsigned MaxSize>
class MakeString
{
MakeString(); // This class is not instantiatable.
public:
typedef Array<IntegerSpec<8, SignednessUnsigned, CastModeSaturate>, ArrayModeDynamic, MaxSize> Type;
};
/**
* YAML streamer specification for any Array<>
*/
libuavcan/include/uavcan/node/abstract_node.hpp
+12 -2
View File
@@ -7,7 +7,6 @@
#include <uavcan/build_config.hpp>
#include <uavcan/node/scheduler.hpp>
#include <uavcan/node/marshal_buffer.hpp>
namespace uavcan
{
@@ -23,7 +22,6 @@ public:
virtual IPoolAllocator& getAllocator() = 0;
virtual Scheduler& getScheduler() = 0;
virtual const Scheduler& getScheduler() const = 0;
virtual IMarshalBufferProvider& getMarshalBufferProvider() = 0;
virtual void registerInternalFailure(const char* msg) = 0;
Dispatcher& getDispatcher() { return getScheduler().getDispatcher(); }
@@ -76,6 +74,18 @@ public:
{
return getScheduler().spin(getMonotonicTime() + duration);
}
/**
* This method is designed for non-blocking applications.
* Instead of blocking, it returns immediately once all available CAN frames and timer events are processed.
* Note that this is unlike plain @ref spin(), which will strictly return when the deadline is reached,
* even if there still are unprocessed events.
* This method returns 0 if no errors occurred, or a negative error code if something failed (see error.hpp).
*/
int spinOnce()
{
return getScheduler().spinOnce();
}
};
}
libuavcan/include/uavcan/node/generic_publisher.hpp
+25 -25
View File
@@ -9,8 +9,8 @@
#include <uavcan/node/abstract_node.hpp>
#include <uavcan/data_type.hpp>
#include <uavcan/node/global_data_type_registry.hpp>
#include <uavcan/util/lazy_constructor.hpp>
#include <uavcan/debug.hpp>
#include <uavcan/transport/transfer_buffer.hpp>
#include <uavcan/transport/transfer_sender.hpp>
#include <uavcan/marshal/scalar_codec.hpp>
#include <uavcan/marshal/types.hpp>
@@ -20,15 +20,14 @@ namespace uavcan
class GenericPublisherBase : Noncopyable
{
const MonotonicDuration max_transfer_interval_; // TODO: memory usage can be reduced
TransferSender sender_;
MonotonicDuration tx_timeout_;
INode& node_;
LazyConstructor<TransferSender> sender_;
protected:
GenericPublisherBase(INode& node, MonotonicDuration tx_timeout,
MonotonicDuration max_transfer_interval)
: max_transfer_interval_(max_transfer_interval)
: sender_(node.getDispatcher(), max_transfer_interval)
, tx_timeout_(tx_timeout)
, node_(node)
{
@@ -46,12 +45,11 @@ protected:
MonotonicTime getTxDeadline() const;
IMarshalBuffer* getBuffer(unsigned byte_len);
int genericPublish(const StaticTransferBufferImpl& buffer, TransferType transfer_type,
NodeID dst_node_id, TransferID* tid, MonotonicTime blocking_deadline);
int genericPublish(const IMarshalBuffer& buffer, TransferType transfer_type, NodeID dst_node_id,
TransferID* tid, MonotonicTime blocking_deadline);
TransferSender* getTransferSender();
TransferSender& getTransferSender() { return sender_; }
const TransferSender& getTransferSender() const { return sender_; }
public:
static MonotonicDuration getMinTxTimeout() { return MonotonicDuration::fromUSec(200); }
@@ -66,7 +64,7 @@ public:
*/
void allowAnonymousTransfers()
{
sender_->allowAnonymousTransfers();
sender_.allowAnonymousTransfers();
}
INode& getNode() const { return node_; }
@@ -80,6 +78,15 @@ public:
template <typename DataSpec, typename DataStruct>
class UAVCAN_EXPORT GenericPublisher : public GenericPublisherBase
{
struct ZeroTransferBuffer : public StaticTransferBufferImpl
{
ZeroTransferBuffer() : StaticTransferBufferImpl(NULL, 0) { }
};
typedef typename Select<DataStruct::MaxBitLen == 0,
ZeroTransferBuffer,
StaticTransferBuffer<BitLenToByteLen<DataStruct::MaxBitLen>::Result> >::Result Buffer;
enum
{
Qos = (DataTypeKind(DataSpec::DataTypeKind) == DataTypeKindMessage) ?
@@ -88,7 +95,7 @@ class UAVCAN_EXPORT GenericPublisher : public GenericPublisherBase
int checkInit();
int doEncode(const DataStruct& message, IMarshalBuffer& buffer) const;
int doEncode(const DataStruct& message, ITransferBuffer& buffer) const;
int genericPublish(const DataStruct& message, TransferType transfer_type, NodeID dst_node_id,
TransferID* tid, MonotonicTime blocking_deadline);
@@ -124,12 +131,6 @@ public:
{
return genericPublish(message, transfer_type, dst_node_id, &tid, blocking_deadline);
}
TransferSender* getTransferSender()
{
(void)checkInit();
return GenericPublisherBase::getTransferSender();
}
};
// ----------------------------------------------------------------------------
@@ -145,7 +146,7 @@ int GenericPublisher<DataSpec, DataStruct>::checkInit()
}
template <typename DataSpec, typename DataStruct>
int GenericPublisher<DataSpec, DataStruct>::doEncode(const DataStruct& message, IMarshalBuffer& buffer) const
int GenericPublisher<DataSpec, DataStruct>::doEncode(const DataStruct& message, ITransferBuffer& buffer) const
{
BitStream bitstream(buffer);
ScalarCodec codec(bitstream);
@@ -168,17 +169,16 @@ int GenericPublisher<DataSpec, DataStruct>::genericPublish(const DataStruct& mes
{
return res;
}
IMarshalBuffer* const buf = getBuffer(BitLenToByteLen<DataStruct::MaxBitLen>::Result);
if (!buf)
{
return -ErrMemory;
}
const int encode_res = doEncode(message, *buf);
Buffer buffer;
const int encode_res = doEncode(message, buffer);
if (encode_res < 0)
{
return encode_res;
}
return GenericPublisherBase::genericPublish(*buf, transfer_type, dst_node_id, tid, blocking_deadline);
return GenericPublisherBase::genericPublish(buffer, transfer_type, dst_node_id, tid, blocking_deadline);
}
}
libuavcan/include/uavcan/node/generic_subscriber.hpp
+42 -37
View File
@@ -33,26 +33,27 @@ namespace uavcan
template <typename DataType_>
class UAVCAN_EXPORT ReceivedDataStructure : public DataType_
{
const IncomingTransfer* _transfer_; ///< Such weird name is necessary to avoid clashing with DataType fields
const IncomingTransfer* const _transfer_; ///< Such weird name is necessary to avoid clashing with DataType fields
template <typename Ret, Ret(IncomingTransfer::*Fun) () const>
Ret safeget() const
{
if (_transfer_ == NULL)
{
UAVCAN_ASSERT(0);
return Ret();
}
return (_transfer_->*Fun)();
}
protected:
ReceivedDataStructure() : _transfer_(NULL) { }
ReceivedDataStructure()
: _transfer_(NULL)
{ }
void setTransfer(const IncomingTransfer* arg_transfer)
ReceivedDataStructure(const IncomingTransfer* arg_transfer)
: _transfer_(arg_transfer)
{
UAVCAN_ASSERT(arg_transfer != NULL);
_transfer_ = arg_transfer;
}
public:
@@ -139,7 +140,10 @@ public:
typedef TransferListenerTemplate<BufferSize, NumStaticBufs, NumStaticReceivers> Type;
};
/**
* Please note that the reference passed to the RX callback points to a stack-allocated object, which means
* that it gets invalidated shortly after the callback returns.
*/
template <typename DataSpec, typename DataStruct, typename TransferListenerType>
class UAVCAN_EXPORT GenericSubscriber : public GenericSubscriberBase
{
@@ -162,23 +166,24 @@ class UAVCAN_EXPORT GenericSubscriber : public GenericSubscriberBase
{ }
};
struct ReceivedDataStructureSpec : public ReceivedDataStructure<DataStruct>
{
using ReceivedDataStructure<DataStruct>::setTransfer;
};
LazyConstructor<TransferForwarder> forwarder_;
ReceivedDataStructureSpec message_;
int checkInit();
bool decodeTransfer(IncomingTransfer& transfer);
void handleIncomingTransfer(IncomingTransfer& transfer);
int genericStart(bool (Dispatcher::*registration_method)(TransferListenerBase*));
protected:
struct ReceivedDataStructureSpec : public ReceivedDataStructure<DataStruct>
{
ReceivedDataStructureSpec() { }
ReceivedDataStructureSpec(const IncomingTransfer* arg_transfer)
: ReceivedDataStructure<DataStruct>(arg_transfer)
{ }
};
explicit GenericSubscriber(INode& node)
: GenericSubscriberBase(node)
{ }
@@ -189,16 +194,21 @@ protected:
int startAsMessageListener()
{
UAVCAN_TRACE("GenericSubscriber", "Start as message listener; dtname=%s", DataSpec::getDataTypeFullName());
return genericStart(&Dispatcher::registerMessageListener);
}
int startAsServiceRequestListener()
{
UAVCAN_TRACE("GenericSubscriber", "Start as service request listener; dtname=%s",
DataSpec::getDataTypeFullName());
return genericStart(&Dispatcher::registerServiceRequestListener);
}
int startAsServiceResponseListener()
{
UAVCAN_TRACE("GenericSubscriber", "Start as service response listener; dtname=%s",
DataSpec::getDataTypeFullName());
return genericStart(&Dispatcher::registerServiceResponseListener);
}
@@ -217,19 +227,11 @@ protected:
*/
void stop()
{
UAVCAN_TRACE("GenericSubscriber", "Stop; dtname=%s", DataSpec::getDataTypeFullName());
GenericSubscriberBase::stop(forwarder_);
}
TransferListenerType* getTransferListener() { return forwarder_; }
/**
* Returns a reference to the temporary storage for decoded received messages.
* Reference to this storage is used as a parameter for subscription callbacks.
* This storage is guaranteed to stay intact after the last message was decoded, i.e.
* the application can use it to access the last received message object.
*/
ReceivedDataStructure<DataStruct>& getReceivedStructStorage() { return message_; }
const ReceivedDataStructure<DataStruct>& getReceivedStructStorage() const { return message_; }
};
// ----------------------------------------------------------------------------
@@ -244,48 +246,51 @@ int GenericSubscriber<DataSpec, DataStruct, TransferListenerType>::checkInit()
{
return 0;
}
GlobalDataTypeRegistry::instance().freeze();
const DataTypeDescriptor* const descr =
GlobalDataTypeRegistry::instance().find(DataTypeKind(DataSpec::DataTypeKind), DataSpec::getDataTypeFullName());
if (!descr)
if (descr == NULL)
{
UAVCAN_TRACE("GenericSubscriber", "Type [%s] is not registered", DataSpec::getDataTypeFullName());
return -ErrUnknownDataType;
}
forwarder_.template construct<SelfType&, const DataTypeDescriptor&, IPoolAllocator&>
(*this, *descr, node_.getAllocator());
return 0;
}
template <typename DataSpec, typename DataStruct, typename TransferListenerType>
bool GenericSubscriber<DataSpec, DataStruct, TransferListenerType>::decodeTransfer(IncomingTransfer& transfer)
void GenericSubscriber<DataSpec, DataStruct, TransferListenerType>::handleIncomingTransfer(IncomingTransfer& transfer)
{
ReceivedDataStructureSpec rx_struct(&transfer);
/*
* Decoding into the temporary storage
*/
BitStream bitstream(transfer);
ScalarCodec codec(bitstream);
message_.setTransfer(&transfer);
const int decode_res = DataStruct::decode(rx_struct, codec);
const int decode_res = DataStruct::decode(message_, codec);
// We don't need the data anymore, the memory can be reused from the callback:
transfer.release();
if (decode_res <= 0)
{
UAVCAN_TRACE("GenericSubscriber", "Unable to decode the message [%i] [%s]",
decode_res, DataSpec::getDataTypeFullName());
failure_count_++;
node_.getDispatcher().getTransferPerfCounter().addError();
return false;
return;
}
return true;
}
template <typename DataSpec, typename DataStruct, typename TransferListenerType>
void GenericSubscriber<DataSpec, DataStruct, TransferListenerType>::handleIncomingTransfer(IncomingTransfer& transfer)
{
if (decodeTransfer(transfer))
{
handleReceivedDataStruct(message_);
}
/*
* Invoking the callback
*/
handleReceivedDataStruct(rx_struct);
}
template <typename DataSpec, typename DataStruct, typename TransferListenerType>
libuavcan/include/uavcan/node/marshal_buffer.hpp
-92
View File
@@ -1,92 +0,0 @@
/*
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#ifndef UAVCAN_NODE_MARSHAL_BUFFER_HPP_INCLUDED
#define UAVCAN_NODE_MARSHAL_BUFFER_HPP_INCLUDED
#include <uavcan/build_config.hpp>
#include <uavcan/transport/transfer.hpp>
#include <uavcan/transport/transfer_buffer.hpp>
namespace uavcan
{
/**
* Abstract temporary buffer for data marshalling.
*/
class UAVCAN_EXPORT IMarshalBuffer : public ITransferBuffer
{
public:
virtual const uint8_t* getDataPtr() const = 0;
virtual unsigned getDataLength() const = 0;
};
/**
* Abstract provider of abstract buffer for data marshalling.
*/
class UAVCAN_EXPORT IMarshalBufferProvider
{
public:
virtual ~IMarshalBufferProvider() { }
/**
* Returns pointer to abstract buffer for data marshalling,
* but does not transfer ownership.
* If the requested buffer size is larger than available,
* null pointer will be returned.
* @param size Maximum buffer size needed for marshaling.
* @return Pointer to the buffer, or null pointer if
* the requested size is too large.
*/
virtual IMarshalBuffer* getBuffer(unsigned size) = 0;
};
/**
* Default implementation of marshal buffer provider.
* This implementation provides the buffer large enough to
* serialize any UAVCAN data structure.
*/
template <unsigned MaxSize_ = MaxPossibleTransferPayloadLen>
class UAVCAN_EXPORT MarshalBufferProvider : public IMarshalBufferProvider
{
class Buffer : public IMarshalBuffer
{
StaticTransferBuffer<MaxSize_> buf_;
virtual int read(unsigned offset, uint8_t* data, unsigned len) const
{
return buf_.read(offset, data, len);
}
virtual int write(unsigned offset, const uint8_t* data, unsigned len)
{
return buf_.write(offset, data, len);
}
virtual const uint8_t* getDataPtr() const { return buf_.getRawPtr(); }
virtual unsigned getDataLength() const { return buf_.getMaxWritePos(); }
public:
void reset() { buf_.reset(); }
};
Buffer buffer_;
public:
enum { MaxSize = MaxSize_ };
virtual IMarshalBuffer* getBuffer(unsigned size)
{
if (size > MaxSize)
{
return NULL;
}
buffer_.reset();
return &buffer_;
}
};
}
#endif // UAVCAN_NODE_MARSHAL_BUFFER_HPP_INCLUDED
libuavcan/include/uavcan/node/node.hpp
+19 -29
View File
@@ -10,13 +10,12 @@
#include <uavcan/build_config.hpp>
#include <uavcan/dynamic_memory.hpp>
#include <uavcan/node/abstract_node.hpp>
#include <uavcan/node/marshal_buffer.hpp>
// High-level functionality available by default
#include <uavcan/protocol/data_type_info_provider.hpp>
#include <uavcan/protocol/node_status_provider.hpp>
#if !UAVCAN_TINY
# include <uavcan/protocol/data_type_info_provider.hpp>
# include <uavcan/protocol/logger.hpp>
# include <uavcan/protocol/restart_request_server.hpp>
# include <uavcan/protocol/transport_stats_provider.hpp>
@@ -46,21 +45,12 @@ namespace uavcan
* Additional objects for Transfer ID tracking will
* be allocated in the memory pool if needed.
* Default value is acceptable for any use case.
*
* @tparam OutgoingTransferMaxPayloadLen Maximum outgoing transfer payload length.
* It's pointless to make this value larger than
* @ref MaxTransferPayloadLen, which is default.
* Note that in tiny mode the default value is actually
* smaller than @ref MaxTransferPayloadLen (may cause
* run-time failures).
*/
template <std::size_t MemPoolSize_,
#if UAVCAN_TINY
unsigned OutgoingTransferRegistryStaticEntries = 0,
unsigned OutgoingTransferMaxPayloadLen = 264
unsigned OutgoingTransferRegistryStaticEntries = 0
#else
unsigned OutgoingTransferRegistryStaticEntries = 10,
unsigned OutgoingTransferMaxPayloadLen = MaxPossibleTransferPayloadLen
unsigned OutgoingTransferRegistryStaticEntries = 10
#endif
>
class UAVCAN_EXPORT Node : public INode
@@ -73,23 +63,24 @@ class UAVCAN_EXPORT Node : public INode
typedef PoolAllocator<MemPoolSize, MemPoolBlockSize> Allocator;
Allocator pool_allocator_;
MarshalBufferProvider<OutgoingTransferMaxPayloadLen> marsh_buf_;
OutgoingTransferRegistry<OutgoingTransferRegistryStaticEntries> outgoing_trans_reg_;
Scheduler scheduler_;
DataTypeInfoProvider proto_dtp_;
NodeStatusProvider proto_nsp_;
#if !UAVCAN_TINY
DataTypeInfoProvider proto_dtp_;
Logger proto_logger_;
RestartRequestServer proto_rrs_;
TransportStatsProvider proto_tsp_;
#endif
uint64_t internal_failure_cnt_;
bool started_;
protected:
virtual void registerInternalFailure(const char* msg)
{
internal_failure_cnt_++;
UAVCAN_TRACE("Node", "Internal failure: %s", msg);
#if UAVCAN_TINY
(void)msg;
@@ -98,19 +89,18 @@ protected:
#endif
}
virtual IMarshalBufferProvider& getMarshalBufferProvider() { return marsh_buf_; }
public:
Node(ICanDriver& can_driver, ISystemClock& system_clock)
: outgoing_trans_reg_(pool_allocator_)
, scheduler_(can_driver, pool_allocator_, system_clock, outgoing_trans_reg_)
, proto_dtp_(*this)
, proto_nsp_(*this)
#if !UAVCAN_TINY
, proto_dtp_(*this)
, proto_logger_(*this)
, proto_rrs_(*this)
, proto_tsp_(*this)
#endif
, internal_failure_cnt_(0)
, started_(false)
{ }
@@ -139,6 +129,8 @@ public:
bool isStarted() const { return started_; }
uint64_t getInternalFailureCount() const { return internal_failure_cnt_; }
/**
* Starts the node and publishes uavcan.protocol.NodeStatus immediately.
* Does not so anything if the node is already started.
@@ -260,9 +252,8 @@ public:
// ----------------------------------------------------------------------------
template <std::size_t MemPoolSize_, unsigned OutgoingTransferRegistryStaticEntries,
unsigned OutgoingTransferMaxPayloadLen>
int Node<MemPoolSize_, OutgoingTransferRegistryStaticEntries, OutgoingTransferMaxPayloadLen>::start(
template <std::size_t MemPoolSize_, unsigned OutgoingTransferRegistryStaticEntries>
int Node<MemPoolSize_, OutgoingTransferRegistryStaticEntries>::start(
const TransferPriority priority)
{
if (started_)
@@ -272,17 +263,17 @@ int Node<MemPoolSize_, OutgoingTransferRegistryStaticEntries, OutgoingTransferMa
GlobalDataTypeRegistry::instance().freeze();
int res = 0;
res = proto_dtp_.start();
if (res < 0)
{
goto fail;
}
res = proto_nsp_.startAndPublish(priority);
if (res < 0)
{
goto fail;
}
#if !UAVCAN_TINY
res = proto_dtp_.start();
if (res < 0)
{
goto fail;
}
res = proto_logger_.init();
if (res < 0)
{
@@ -308,9 +299,8 @@ fail:
#if !UAVCAN_TINY
template <std::size_t MemPoolSize_, unsigned OutgoingTransferRegistryStaticEntries,
unsigned OutgoingTransferMaxPayloadLen>
int Node<MemPoolSize_, OutgoingTransferRegistryStaticEntries, OutgoingTransferMaxPayloadLen>::
template <std::size_t MemPoolSize_, unsigned OutgoingTransferRegistryStaticEntries>
int Node<MemPoolSize_, OutgoingTransferRegistryStaticEntries>::
checkNetworkCompatibility(NetworkCompatibilityCheckResult& result)
{
if (!started_)
libuavcan/include/uavcan/node/publisher.hpp
+5 -31
View File
@@ -81,46 +81,20 @@ public:
/**
* Returns priority of outgoing transfers.
* TODO: Make const.
*/
TransferPriority getPriority()
TransferPriority getPriority() const
{
// TODO probably TransferSender must be transformed into regular field?
TransferSender* const ts = getTransferSender();
if (ts != NULL)
{
return ts->getPriority();
}
else
{
return TransferPriorityNormal; // This is default
}
return BaseType::getTransferSender().getPriority();
}
/**
* Allows to change the priority of outgoing transfers.
* Note that only High, Normal and Low priorities can be used; Service priority is not available for messages.
* Returns negative error code if priority cannot be set, non-negative on success.
* If the priority value is invalid, an assertion failure will be generated, and the value will not be updated.
*/
int setPriority(const TransferPriority prio)
void setPriority(const TransferPriority prio)
{
if (prio < NumTransferPriorities && prio != TransferPriorityService)
{
TransferSender* const ts = getTransferSender(); // TODO: Static TransferSender?
if (ts != NULL)
{
ts->setPriority(prio);
return 0;
}
else
{
return -ErrLogic;
}
}
else
{
return -ErrInvalidParam;
}
BaseType::getTransferSender().setPriority(prio);
}
static MonotonicDuration getDefaultTxTimeout() { return MonotonicDuration::fromMSec(10); }
libuavcan/include/uavcan/node/scheduler.hpp
+20
View File
@@ -32,6 +32,7 @@ public:
void startWithDeadline(MonotonicTime deadline);
void startWithDelay(MonotonicDuration delay);
void generateDeadlineImmediately() { startWithDeadline(MonotonicTime::fromUSec(1)); }
void stop();
@@ -76,6 +77,17 @@ class UAVCAN_EXPORT Scheduler : Noncopyable
MonotonicDuration cleanup_period_;
bool inside_spin_;
struct InsideSpinSetter
{
Scheduler& owner;
InsideSpinSetter(Scheduler& o)
: owner(o)
{
owner.inside_spin_ = true;
}
~InsideSpinSetter() { owner.inside_spin_ = false; }
};
MonotonicTime computeDispatcherSpinDeadline(MonotonicTime spin_deadline) const;
void pollCleanup(MonotonicTime mono_ts, uint32_t num_frames_processed_with_last_spin);
@@ -90,10 +102,18 @@ public:
/**
* Spin until the deadline, or until some error occurs.
* This function will return strictly when the deadline is reached, even if there are unprocessed frames.
* Returns negative error code.
*/
int spin(MonotonicTime deadline);
/**
* Non-blocking version of @ref spin() - spins until all pending frames and events are processed,
* or until some error occurs. If there's nothing to do, returns immediately.
* Returns negative error code.
*/
int spinOnce();
DeadlineScheduler& getDeadlineScheduler() { return deadline_scheduler_; }
Dispatcher& getDispatcher() { return dispatcher_; }
libuavcan/include/uavcan/node/service_client.hpp
+316 -106
View File
@@ -6,6 +6,7 @@
#define UAVCAN_NODE_SERVICE_CLIENT_HPP_INCLUDED
#include <uavcan/build_config.hpp>
#include <uavcan/util/multiset.hpp>
#include <uavcan/node/generic_publisher.hpp>
#include <uavcan/node/generic_subscriber.hpp>
@@ -20,42 +21,83 @@
namespace uavcan
{
template <typename ServiceDataType>
template <typename ServiceDataType, unsigned NumStaticReceiversAndBuffers>
class UAVCAN_EXPORT ServiceResponseTransferListenerInstantiationHelper
{
public: // so much templating it hurts
typedef typename TransferListenerInstantiationHelper<typename ServiceDataType::Response,
1, 1, ServiceResponseTransferListener>::Type Type;
NumStaticReceiversAndBuffers,
NumStaticReceiversAndBuffers,
TransferListenerWithFilter>::Type Type;
};
/**
* This struct describes a pending service call.
* Refer to @ref ServiceClient to learn more about service calls.
*/
struct ServiceCallID
{
NodeID server_node_id;
TransferID transfer_id;
ServiceCallID() { }
ServiceCallID(NodeID arg_server_node_id, TransferID arg_transfer_id)
: server_node_id(arg_server_node_id)
, transfer_id(arg_transfer_id)
{ }
bool operator==(const ServiceCallID rhs) const
{
return (rhs.server_node_id == server_node_id) &&
(rhs.transfer_id == transfer_id);
}
bool isValid() const { return server_node_id.isUnicast(); }
};
/**
* Object of this type will be returned to the application as a result of service call.
* Note that application ALWAYS gets this result, even when it times out or fails because of some other reason.
* The class is made noncopyable because it keeps a reference to a stack-allocated object.
*/
template <typename DataType>
struct UAVCAN_EXPORT ServiceCallResult
class UAVCAN_EXPORT ServiceCallResult : Noncopyable
{
public:
typedef ReceivedDataStructure<typename DataType::Response> ResponseFieldType;
enum Status { Success, ErrorTimeout };
const Status status; ///< Whether successful or not. Failure to decode the response causes timeout.
NodeID server_node_id; ///< Node ID of the server this call was addressed to.
ResponseFieldType& response; ///< Returned data structure. Undefined if the service call has failed.
private:
const Status status_; ///< Whether successful or not. Failure to decode the response causes timeout.
ServiceCallID call_id_; ///< Identifies the call
ResponseFieldType& response_; ///< Returned data structure. Value undefined if the service call has failed.
ServiceCallResult(Status arg_status, NodeID arg_server_node_id, ResponseFieldType& arg_response)
: status(arg_status)
, server_node_id(arg_server_node_id)
, response(arg_response)
public:
ServiceCallResult(Status arg_status, ServiceCallID arg_call_id, ResponseFieldType& arg_response)
: status_(arg_status)
, call_id_(arg_call_id)
, response_(arg_response)
{
UAVCAN_ASSERT(server_node_id.isUnicast());
UAVCAN_ASSERT((status == Success) || (status == ErrorTimeout));
UAVCAN_ASSERT(call_id_.isValid());
UAVCAN_ASSERT((status_ == Success) || (status_ == ErrorTimeout));
}
/**
* Shortcut to quickly check whether the call was successful.
*/
bool isSuccessful() const { return status == Success; }
bool isSuccessful() const { return status_ == Success; }
Status getStatus() const { return status_; }
ServiceCallID getCallID() const { return call_id_; }
/**
* Returned reference points to a stack-allocated object.
*/
const ResponseFieldType& getResponse() const { return response_; }
ResponseFieldType& getResponse() { return response_; }
};
/**
@@ -67,10 +109,11 @@ static Stream& operator<<(Stream& s, const ServiceCallResult<DataType>& scr)
{
s << "# Service call result [" << DataType::getDataTypeFullName() << "] "
<< (scr.isSuccessful() ? "OK" : "FAILURE")
<< " server_node_id=" << int(scr.server_node_id.get()) << "\n";
<< " server_node_id=" << int(scr.getCallID().server_node_id.get())
<< " tid=" << int(scr.getCallID().transfer_id.get()) << "\n";
if (scr.isSuccessful())
{
s << scr.response;
s << scr.getResponse();
}
else
{
@@ -82,64 +125,113 @@ static Stream& operator<<(Stream& s, const ServiceCallResult<DataType>& scr)
/**
* Do not use directly.
*/
class ServiceClientBase : protected DeadlineHandler
class ServiceClientBase : protected ITransferAcceptanceFilter
, protected DeadlineHandler
{
const DataTypeDescriptor* data_type_descriptor_; ///< This will be initialized at the time of first call
protected:
MonotonicDuration request_timeout_;
bool pending_;
class CallState : DeadlineHandler
{
ServiceClientBase& owner_;
const ServiceCallID id_;
bool timed_out_;
explicit ServiceClientBase(INode& node)
virtual void handleDeadline(MonotonicTime);
public:
CallState(INode& node, ServiceClientBase& owner, ServiceCallID call_id)
: DeadlineHandler(node.getScheduler())
, owner_(owner)
, id_(call_id)
, timed_out_(false)
{
UAVCAN_ASSERT(id_.isValid());
DeadlineHandler::startWithDelay(owner_.request_timeout_);
}
ServiceCallID getCallID() const { return id_; }
bool hasTimedOut() const { return timed_out_; }
static bool hasTimedOutPredicate(const CallState& cs) { return cs.hasTimedOut(); }
bool operator==(const CallState& rhs) const
{
return (&owner_ == &rhs.owner_) && (id_ == rhs.id_);
}
};
struct CallStateMatchingPredicate
{
const ServiceCallID id;
CallStateMatchingPredicate(ServiceCallID reference) : id(reference) { }
bool operator()(const CallState& state) const { return (state.getCallID() == id) && !state.hasTimedOut(); }
};
struct ServerSearchPredicate
{
const NodeID server_node_id;
ServerSearchPredicate(NodeID nid) : server_node_id(nid) { }
bool operator()(const CallState& state) const { return state.getCallID().server_node_id == server_node_id; }
};
MonotonicDuration request_timeout_;
ServiceClientBase(INode& node)
: DeadlineHandler(node.getScheduler())
, data_type_descriptor_(NULL)
, request_timeout_(getDefaultRequestTimeout())
, pending_(false)
{ }
virtual ~ServiceClientBase() { }
int prepareToCall(INode& node, const char* dtname, NodeID server_node_id, TransferID& out_transfer_id);
int prepareToCall(INode& node, const char* dtname, NodeID server_node_id, ServiceCallID& out_call_id);
public:
/**
* Returns true if the service call is currently in progress.
*/
bool isPending() const { return pending_; }
/**
* It's not recommended to override default timeouts.
* Change of this value will not affect pending calls.
*/
static MonotonicDuration getDefaultRequestTimeout() { return MonotonicDuration::fromMSec(1000); }
static MonotonicDuration getDefaultRequestTimeout() { return MonotonicDuration::fromMSec(500); }
static MonotonicDuration getMinRequestTimeout() { return MonotonicDuration::fromMSec(10); }
static MonotonicDuration getMaxRequestTimeout() { return MonotonicDuration::fromMSec(60000); }
/**
* Returns the service response waiting deadline, if pending.
*/
using DeadlineHandler::getDeadline;
};
/**
* Use this class to invoke services on remote nodes.
*
* This class can manage multiple concurrent calls to the same or different remote servers. Number of concurrent
* calls is limited only by amount of available pool memory.
*
* Note that the reference passed to the callback points to a stack-allocated object, which means that the
* reference invalidates once the callback returns. If you want to use this object after the callback execution,
* you need to copy it somewhere.
*
* @tparam DataType_ Service data type.
*
* @tparam Callback_ Service response will be delivered through the callback of this type.
* In C++11 mode this type defaults to std::function<>.
* In C++03 mode this type defaults to a plain function pointer; use binder to
* call member functions as callbacks.
*
* @tparam NumStaticCalls_ Number of concurrent calls that the class will be able to handle without using the
* memory pool. Note that this is NOT the maximum possible number of concurrent calls,
* there's no such limit. Defaults to one.
*/
template <typename DataType_,
#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
typename Callback_ = std::function<void (const ServiceCallResult<DataType_>&)>
typename Callback_ = std::function<void (const ServiceCallResult<DataType_>&)>,
#else
typename Callback_ = void (*)(const ServiceCallResult<DataType_>&)
typename Callback_ = void (*)(const ServiceCallResult<DataType_>&),
#endif
unsigned NumStaticCalls_ = 1
>
class UAVCAN_EXPORT ServiceClient
: public GenericSubscriber<DataType_, typename DataType_::Response,
typename ServiceResponseTransferListenerInstantiationHelper<DataType_>::Type >
: public GenericSubscriber<DataType_,
typename DataType_::Response,
typename ServiceResponseTransferListenerInstantiationHelper<DataType_,
NumStaticCalls_>::Type>
, public ServiceClientBase
{
public:
@@ -149,16 +241,47 @@ public:
typedef ServiceCallResult<DataType> ServiceCallResultType;
typedef Callback_ Callback;
enum { NumStaticCalls = NumStaticCalls_ };
private:
typedef ServiceClient<DataType, Callback> SelfType;
typedef GenericPublisher<DataType, RequestType> PublisherType;
typedef typename ServiceResponseTransferListenerInstantiationHelper<DataType>::Type TransferListenerType;
typedef typename ServiceResponseTransferListenerInstantiationHelper<DataType, NumStaticCalls>::Type
TransferListenerType;
typedef GenericSubscriber<DataType, ResponseType, TransferListenerType> SubscriberType;
typedef Multiset<CallState, NumStaticCalls> CallRegistry;
struct TimeoutCallbackCaller
{
ServiceClient& owner;
TimeoutCallbackCaller(ServiceClient& arg_owner) : owner(arg_owner) { }
void operator()(const CallState& state)
{
if (state.hasTimedOut())
{
UAVCAN_TRACE("ServiceClient::TimeoutCallbackCaller", "Timeout from nid=%d, tid=%d, dtname=%s",
int(state.getCallID().server_node_id.get()), int(state.getCallID().transfer_id.get()),
DataType::getDataTypeFullName());
typename SubscriberType::ReceivedDataStructureSpec rx_struct; // Default-initialized
ServiceCallResultType result(ServiceCallResultType::ErrorTimeout, state.getCallID(),
rx_struct); // Mutable!
owner.invokeCallback(result);
}
}
};
CallRegistry call_registry_;
PublisherType publisher_;
Callback callback_;
bool isCallbackValid() const { return try_implicit_cast<bool>(callback_, true); }
virtual bool shouldAcceptFrame(const RxFrame& frame) const; // Called from the transfer listener
void invokeCallback(ServiceCallResultType& result);
@@ -166,6 +289,8 @@ private:
virtual void handleDeadline(MonotonicTime);
int addCallState(ServiceCallID call_id);
public:
/**
* @param node Node instance this client will be registered with.
@@ -174,6 +299,7 @@ public:
explicit ServiceClient(INode& node, const Callback& callback = Callback())
: SubscriberType(node)
, ServiceClientBase(node)
, call_registry_(node.getAllocator())
, publisher_(node, getDefaultRequestTimeout())
, callback_(callback)
{
@@ -183,7 +309,7 @@ public:
#endif
}
virtual ~ServiceClient() { cancel(); }
virtual ~ServiceClient() { cancelAllCalls(); }
/**
* Shall be called before first use.
@@ -199,21 +325,39 @@ public:
* This method transmits the service request and returns immediately.
*
* Service response will be delivered into the application via the callback.
* Note that the callback will ALWAYS be called even if the service call has timed out; the
* Note that the callback will ALWAYS be called even if the service call times out; the
* actual result of the call (success/failure) will be passed to the callback as well.
*
* If this client instance is already pending service response, it will be cancelled and the new
* call will be performed.
*
* Returns negative error code.
*/
int call(NodeID server_node_id, const RequestType& request);
/**
* Cancel the pending service call.
* Does nothing if it is not pending.
* Same as plain @ref call() above, but this overload also returns the call ID of the new call.
* The call ID structure can be used to cancel this request later if needed.
*/
void cancel();
int call(NodeID server_node_id, const RequestType& request, ServiceCallID& out_call_id);
/**
* Cancels certain call referred via call ID structure.
*/
void cancelCall(ServiceCallID call_id);
/**
* Cancels all pending calls.
*/
void cancelAllCalls();
/**
* Checks whether there's currently a pending call addressed to the specified node ID.
*/
bool hasPendingCallToServer(NodeID server_node_id) const;
/**
* This method allows to traverse pending calls. If the index is out of range, an invalid call ID will be returned.
* Warning: average complexity is O(index); worst case complexity is O(size).
*/
ServiceCallID getCallIDByIndex(unsigned index) const;
/**
* Service response callback must be set prior service call.
@@ -221,6 +365,18 @@ public:
const Callback& getCallback() const { return callback_; }
void setCallback(const Callback& cb) { callback_ = cb; }
/**
* Complexity is O(N) of number of pending calls.
* Note that the number of pending calls will not be updated until the callback is executed.
*/
unsigned getNumPendingCalls() const { return call_registry_.getSize(); }
/**
* Complexity is O(1).
* Note that the number of pending calls will not be updated until the callback is executed.
*/
bool hasPendingCalls() const { return !call_registry_.isEmpty(); }
/**
* Returns the number of failed attempts to decode received response. Generally, a failed attempt means either:
* - Transient failure in the transport layer.
@@ -229,7 +385,7 @@ public:
uint32_t getResponseFailureCount() const { return SubscriberType::getFailureCount(); }
/**
* Request timeouts.
* Request timeouts. Note that changing the request timeout will not affect calls that are already pending.
* There is no such config as TX timeout - TX timeouts are configured automagically according to request timeouts.
* Not recommended to change.
*/
@@ -246,10 +402,10 @@ public:
// ----------------------------------------------------------------------------
template <typename DataType_, typename Callback_>
void ServiceClient<DataType_, Callback_>::invokeCallback(ServiceCallResultType& result)
template <typename DataType_, typename Callback_, unsigned NumStaticCalls_>
void ServiceClient<DataType_, Callback_, NumStaticCalls_>::invokeCallback(ServiceCallResultType& result)
{
if (isCallbackValid())
if (try_implicit_cast<bool>(callback_, true))
{
callback_(result);
}
@@ -259,118 +415,172 @@ void ServiceClient<DataType_, Callback_>::invokeCallback(ServiceCallResultType&
}
}
template <typename DataType_, typename Callback_>
void ServiceClient<DataType_, Callback_>::handleReceivedDataStruct(ReceivedDataStructure<ResponseType>& response)
template <typename DataType_, typename Callback_, unsigned NumStaticCalls_>
bool ServiceClient<DataType_, Callback_, NumStaticCalls_>::shouldAcceptFrame(const RxFrame& frame) const
{
UAVCAN_ASSERT(frame.getTransferType() == TransferTypeServiceResponse); // Other types filtered out by dispatcher
return NULL != call_registry_.find(CallStateMatchingPredicate(ServiceCallID(frame.getSrcNodeID(),
frame.getTransferID())));
}
template <typename DataType_, typename Callback_, unsigned NumStaticCalls_>
void ServiceClient<DataType_, Callback_, NumStaticCalls_>::
handleReceivedDataStruct(ReceivedDataStructure<ResponseType>& response)
{
UAVCAN_ASSERT(response.getTransferType() == TransferTypeServiceResponse);
const TransferListenerType* const listener = SubscriberType::getTransferListener();
if (listener)
ServiceCallID call_id(response.getSrcNodeID(), response.getTransferID());
cancelCall(call_id);
ServiceCallResultType result(ServiceCallResultType::Success, call_id, response); // Mutable!
invokeCallback(result);
}
template <typename DataType_, typename Callback_, unsigned NumStaticCalls_>
void ServiceClient<DataType_, Callback_, NumStaticCalls_>::handleDeadline(MonotonicTime)
{
UAVCAN_TRACE("ServiceClient", "Shared deadline event received");
/*
* Invoking callbacks for timed out call state objects.
*/
TimeoutCallbackCaller callback_caller(*this);
call_registry_.template forEach<TimeoutCallbackCaller&>(callback_caller);
/*
* Removing timed out objects.
* This operation cannot be merged with the previous one because that will not work with recursive calls.
*/
call_registry_.removeAllWhere(&CallState::hasTimedOutPredicate);
/*
* Subscriber does not need to be registered if we don't have any pending calls.
* Removing it makes processing of incoming frames a bit faster.
*/
if (call_registry_.isEmpty())
{
const typename TransferListenerType::ExpectedResponseParams erp = listener->getExpectedResponseParams();
ServiceCallResultType result(ServiceCallResultType::Success, erp.src_node_id, response);
cancel();
invokeCallback(result);
}
else
{
UAVCAN_ASSERT(0);
cancel();
SubscriberType::stop();
}
}
template <typename DataType_, typename Callback_>
void ServiceClient<DataType_, Callback_>::handleDeadline(MonotonicTime)
template <typename DataType_, typename Callback_, unsigned NumStaticCalls_>
int ServiceClient<DataType_, Callback_, NumStaticCalls_>::addCallState(ServiceCallID call_id)
{
const TransferListenerType* const listener = SubscriberType::getTransferListener();
if (listener)
if (call_registry_.isEmpty())
{
const typename TransferListenerType::ExpectedResponseParams erp = listener->getExpectedResponseParams();
ReceivedDataStructure<ResponseType>& ref = SubscriberType::getReceivedStructStorage();
ServiceCallResultType result(ServiceCallResultType::ErrorTimeout, erp.src_node_id, ref);
const int subscriber_res = SubscriberType::startAsServiceResponseListener();
if (subscriber_res < 0)
{
UAVCAN_TRACE("ServiceClient", "Failed to start the subscriber, error: %i", subscriber_res);
return subscriber_res;
}
}
UAVCAN_TRACE("ServiceClient", "Timeout from nid=%i, dtname=%s",
erp.src_node_id.get(), DataType::getDataTypeFullName());
cancel();
invokeCallback(result);
}
else
if (NULL == call_registry_.template emplace<INode&, ServiceClientBase&, ServiceCallID>(SubscriberType::getNode(),
*this, call_id))
{
UAVCAN_ASSERT(0);
cancel();
SubscriberType::stop();
return -ErrMemory;
}
return 0;
}
template <typename DataType_, typename Callback_>
int ServiceClient<DataType_, Callback_>::call(NodeID server_node_id, const RequestType& request)
template <typename DataType_, typename Callback_, unsigned NumStaticCalls_>
int ServiceClient<DataType_, Callback_, NumStaticCalls_>::call(NodeID server_node_id, const RequestType& request)
{
cancel();
if (!isCallbackValid())
ServiceCallID dummy;
return call(server_node_id, request, dummy);
}
template <typename DataType_, typename Callback_, unsigned NumStaticCalls_>
int ServiceClient<DataType_, Callback_, NumStaticCalls_>::call(NodeID server_node_id, const RequestType& request,
ServiceCallID& out_call_id)
{
if (!try_implicit_cast<bool>(callback_, true))
{
UAVCAN_TRACE("ServiceClient", "Invalid callback");
return -ErrInvalidParam;
return -ErrInvalidConfiguration;
}
/*
* Common procedures that don't depend on the struct data type
*/
TransferID transfer_id;
const int prep_res =
prepareToCall(SubscriberType::getNode(), DataType::getDataTypeFullName(), server_node_id, transfer_id);
prepareToCall(SubscriberType::getNode(), DataType::getDataTypeFullName(), server_node_id, out_call_id);
if (prep_res < 0)
{
UAVCAN_TRACE("ServiceClient", "Failed to prepare the call, error: %i", prep_res);
cancel();
return prep_res;
}
/*
* Starting the subscriber
* Initializing the call state - this will start the subscriber ad-hoc
*/
const int subscriber_res = SubscriberType::startAsServiceResponseListener();
if (subscriber_res < 0)
const int call_state_res = addCallState(out_call_id);
if (call_state_res < 0)
{
UAVCAN_TRACE("ServiceClient", "Failed to start the subscriber, error: %i", subscriber_res);
cancel();
return subscriber_res;
UAVCAN_TRACE("ServiceClient", "Failed to add call state, error: %i", call_state_res);
return call_state_res;
}
/*
* Configuring the listener so it will accept only the matching response
* Configuring the listener so it will accept only the matching responses
* TODO move to init(), but this requires to somewhat refactor GenericSubscriber<> (remove TransferForwarder)
*/
TransferListenerType* const tl = SubscriberType::getTransferListener();
if (tl == NULL)
{
UAVCAN_ASSERT(0); // Must have been created
cancel();
cancelCall(out_call_id);
return -ErrLogic;
}
const typename TransferListenerType::ExpectedResponseParams erp(server_node_id, transfer_id);
tl->setExpectedResponseParams(erp);
tl->installAcceptanceFilter(this);
/*
* Publishing the request
*/
const int publisher_res = publisher_.publish(request, TransferTypeServiceRequest, server_node_id, transfer_id);
const int publisher_res = publisher_.publish(request, TransferTypeServiceRequest, server_node_id,
out_call_id.transfer_id);
if (publisher_res < 0)
{
cancel();
cancelCall(out_call_id);
return publisher_res;
}
UAVCAN_ASSERT(server_node_id == out_call_id.server_node_id);
return publisher_res;
}
template <typename DataType_, typename Callback_>
void ServiceClient<DataType_, Callback_>::cancel()
template <typename DataType_, typename Callback_, unsigned NumStaticCalls_>
void ServiceClient<DataType_, Callback_, NumStaticCalls_>::cancelCall(ServiceCallID call_id)
{
pending_ = false;
SubscriberType::stop();
DeadlineHandler::stop();
TransferListenerType* const tl = SubscriberType::getTransferListener();
if (tl)
call_registry_.removeFirstWhere(CallStateMatchingPredicate(call_id));
if (call_registry_.isEmpty())
{
tl->stopAcceptingAnything();
SubscriberType::stop();
}
}
template <typename DataType_, typename Callback_, unsigned NumStaticCalls_>
void ServiceClient<DataType_, Callback_, NumStaticCalls_>::cancelAllCalls()
{
call_registry_.clear();
SubscriberType::stop();
}
template <typename DataType_, typename Callback_, unsigned NumStaticCalls_>
bool ServiceClient<DataType_, Callback_, NumStaticCalls_>::hasPendingCallToServer(NodeID server_node_id) const
{
return NULL != call_registry_.find(ServerSearchPredicate(server_node_id));
}
template <typename DataType_, typename Callback_, unsigned NumStaticCalls_>
ServiceCallID ServiceClient<DataType_, Callback_, NumStaticCalls_>::getCallIDByIndex(unsigned index) const
{
const CallState* const id = call_registry_.getByIndex(index);
return (id == NULL) ? ServiceCallID() : id->getCallID();
}
}
#endif // UAVCAN_NODE_SERVICE_CLIENT_HPP_INCLUDED
libuavcan/include/uavcan/node/service_server.hpp
+66 -12
View File
@@ -19,16 +19,61 @@
namespace uavcan
{
/**
* This type can be used in place of the response type in a service server callback to get more advanced control
* of service request processing.
*
* PLEASE NOTE that since this class inherits the response type, service server callbacks can accept either
* object of this class or the response type directly if the extra options are not needed.
*
* For example, both of these callbacks can be used with the same service type 'Foo':
*
* void first(const ReceivedDataStructure<Foo::Request>& request,
* ServiceResponseDataStructure<Foo::Response>& response);
*
* void second(const Foo::Request& request,
* Foo::Response& response);
*
* In the latter case, an implicit cast will happen before the callback is invoked.
*/
template <typename ResponseDataType_>
class ServiceResponseDataStructure : public ResponseDataType_
{
// Fields are weirdly named to avoid name clashing with the inherited data type
bool _enabled_;
public:
typedef ResponseDataType_ ResponseDataType;
ServiceResponseDataStructure()
: _enabled_(true)
{ }
/**
* When disabled, the server will not transmit the response transfer.
* By default it is enabled, i.e. response will be sent.
*/
void setResponseEnabled(bool x) { _enabled_ = x; }
/**
* Whether the response will be sent. By default it will.
*/
bool isResponseEnabled() const { return _enabled_; }
};
/**
* Use this class to implement UAVCAN service servers.
*
* Note that the references passed to the callback may point to stack-allocated objects, which means that the
* references get invalidated once the callback returns.
*
* @tparam DataType_ Service data type.
*
* @tparam Callback_ Service calls will be delivered through the callback of this type, and service
* response will be returned via the output parameter of the callback. Note that
* the reference to service response data struct passed to the callback always points
* to a default initialized response object.
* Please also refer to @ref ReceivedDataStructure<>.
* Please also refer to @ref ReceivedDataStructure<> and @ref ServiceResponseDataStructure<>.
* In C++11 mode this type defaults to std::function<>.
* In C++03 mode this type defaults to a plain function pointer; use binder to
* call member functions as callbacks.
@@ -42,10 +87,10 @@ namespace uavcan
template <typename DataType_,
#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
typename Callback_ = std::function<void (const ReceivedDataStructure<typename DataType_::Request>&,
typename DataType_::Response&)>,
ServiceResponseDataStructure<typename DataType_::Response>&)>,
#else
typename Callback_ = void (*)(const ReceivedDataStructure<typename DataType_::Request>&,
typename DataType_::Response&),
ServiceResponseDataStructure<typename DataType_::Response>&),
#endif
#if UAVCAN_TINY
unsigned NumStaticReceivers = 0,
@@ -75,28 +120,37 @@ private:
PublisherType publisher_;
Callback callback_;
uint32_t response_failure_count_;
ResponseType response_;
virtual void handleReceivedDataStruct(ReceivedDataStructure<RequestType>& request)
{
UAVCAN_ASSERT(request.getTransferType() == TransferTypeServiceRequest);
ServiceResponseDataStructure<ResponseType> response;
if (try_implicit_cast<bool>(callback_, true))
{
response_ = ResponseType(); // The application needs newly initialized structure
callback_(request, response_);
UAVCAN_ASSERT(response.isResponseEnabled()); // Enabled by default
callback_(request, response);
}
else
{
handleFatalError("Srv serv clbk");
}
const int res = publisher_.publish(response_, TransferTypeServiceResponse, request.getSrcNodeID(),
request.getTransferID());
if (res < 0)
if (response.isResponseEnabled())
{
UAVCAN_TRACE("ServiceServer", "Response publication failure: %i", res);
publisher_.getNode().getDispatcher().getTransferPerfCounter().addError();
response_failure_count_++;
const int res = publisher_.publish(response, TransferTypeServiceResponse, request.getSrcNodeID(),
request.getTransferID());
if (res < 0)
{
UAVCAN_TRACE("ServiceServer", "Response publication failure: %i", res);
publisher_.getNode().getDispatcher().getTransferPerfCounter().addError();
response_failure_count_++;
}
}
else
{
UAVCAN_TRACE("ServiceServer", "Response was suppressed by the application");
}
}
libuavcan/include/uavcan/protocol/README.md
+13
View File
@@ -0,0 +1,13 @@
High-level protocol logic
=========================
Classes defined in this directory implement some high-level functions of UAVCAN.
Since most applications won't use all of them at the same time, their definitions are provided right in the header
files rather than in source files, in order to not pollute the build outputs with unused code (which is also a
requirement for most safety-critical software).
However, some of the high-level functions that are either always used by the library itself or those that are extremely
likely to be used by the application are defined in .cpp files.
When adding a new class here, please think twice before putting its definition into a .cpp file.
libuavcan/include/uavcan/protocol/data_type_info_provider.hpp
+124 -4
View File
@@ -10,6 +10,7 @@
#include <uavcan/build_config.hpp>
#include <uavcan/protocol/ComputeAggregateTypeSignature.hpp>
#include <uavcan/protocol/GetDataTypeInfo.hpp>
#include <uavcan/debug.hpp>
namespace uavcan
{
@@ -34,13 +35,107 @@ class UAVCAN_EXPORT DataTypeInfoProvider : Noncopyable
INode& getNode() { return cats_srv_.getNode(); }
static bool isValidDataTypeKind(DataTypeKind kind);
static bool isValidDataTypeKind(DataTypeKind kind)
{
return (kind == DataTypeKindMessage) || (kind == DataTypeKindService);
}
void handleComputeAggregateTypeSignatureRequest(const protocol::ComputeAggregateTypeSignature::Request& request,
protocol::ComputeAggregateTypeSignature::Response& response);
protocol::ComputeAggregateTypeSignature::Response& response)
{
const DataTypeKind kind = DataTypeKind(request.kind.value); // No mapping needed
if (!isValidDataTypeKind(kind))
{
UAVCAN_TRACE("DataTypeInfoProvider", "ComputeAggregateTypeSignature request with invalid DataTypeKind %d",
kind);
return;
}
UAVCAN_TRACE("DataTypeInfoProvider", "ComputeAggregateTypeSignature request for dtk=%d, len(known_ids)=%d",
int(request.kind.value), int(request.known_ids.size()));
// Correcting the mask length according to the data type kind
response.mutually_known_ids = request.known_ids;
response.mutually_known_ids.resize(
static_cast<uint16_t>(DataTypeID::getMaxValueForDataTypeKind(kind).get() + 1U));
response.aggregate_signature =
GlobalDataTypeRegistry::instance().computeAggregateSignature(kind, response.mutually_known_ids).get();
}
void handleGetDataTypeInfoRequest(const protocol::GetDataTypeInfo::Request& request,
protocol::GetDataTypeInfo::Response& response);
protocol::GetDataTypeInfo::Response& response)
{
/*
* Asking the Global Data Type Registry for the matching type descriptor, either by name or by ID
*/
const DataTypeDescriptor* desc = NULL;
if (request.name.empty())
{
response.id = request.id; // Pre-setting the fields so they have meaningful values even in
response.kind = request.kind; // ...case of failure.
if (!isValidDataTypeKind(DataTypeKind(request.kind.value)))
{
UAVCAN_TRACE("DataTypeInfoProvider", "GetDataTypeInfo request with invalid DataTypeKind %i",
static_cast<int>(request.kind.value));
return;
}
desc = GlobalDataTypeRegistry::instance().find(DataTypeKind(request.kind.value), request.id);
}
else
{
response.name = request.name;
desc = GlobalDataTypeRegistry::instance().find(request.name.c_str());
}
if (desc == NULL)
{
UAVCAN_TRACE("DataTypeInfoProvider",
"Cannot process GetDataTypeInfo for nonexistent type: dtid=%i dtk=%i name='%s'",
static_cast<int>(request.id), static_cast<int>(request.kind.value), request.name.c_str());
return;
}
UAVCAN_TRACE("DataTypeInfoProvider", "GetDataTypeInfo request for %s", desc->toString().c_str());
/*
* Filling the response struct
*/
response.signature = desc->getSignature().get();
response.id = desc->getID().get();
response.kind.value = desc->getKind();
response.mask = protocol::GetDataTypeInfo::Response::MASK_KNOWN;
response.name = desc->getFullName();
const Dispatcher& dispatcher = getNode().getDispatcher();
if (desc->getKind() == DataTypeKindService)
{
if (dispatcher.hasServer(desc->getID().get()))
{
response.mask |= protocol::GetDataTypeInfo::Response::MASK_SERVING;
}
}
else if (desc->getKind() == DataTypeKindMessage)
{
if (dispatcher.hasSubscriber(desc->getID().get()))
{
response.mask |= protocol::GetDataTypeInfo::Response::MASK_SUBSCRIBED;
}
if (dispatcher.hasPublisher(desc->getID().get()))
{
response.mask |= protocol::GetDataTypeInfo::Response::MASK_PUBLISHING;
}
}
else
{
UAVCAN_ASSERT(0); // That means that GDTR somehow found a type of an unknown kind. The horror.
}
}
public:
explicit DataTypeInfoProvider(INode& node)
@@ -48,7 +143,32 @@ public:
, gdti_srv_(node)
{ }
int start();
int start()
{
int res = 0;
res = cats_srv_.start(
ComputeAggregateTypeSignatureCallback(this, &DataTypeInfoProvider::handleComputeAggregateTypeSignatureRequest));
if (res < 0)
{
goto fail;
}
res = gdti_srv_.start(GetDataTypeInfoCallback(this, &DataTypeInfoProvider::handleGetDataTypeInfoRequest));
if (res < 0)
{
goto fail;
}
UAVCAN_ASSERT(res >= 0);
return res;
fail:
UAVCAN_ASSERT(res < 0);
cats_srv_.stop();
gdti_srv_.stop();
return res;
}
};
}
libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_client.hpp → libuavcan/include/uavcan/protocol/dynamic_node_id_client.hpp
+7 -7
View File
@@ -2,8 +2,8 @@
* Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_CLIENT_HPP_INCLUDED
#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_CLIENT_HPP_INCLUDED
#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_CLIENT_HPP_INCLUDED
#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_CLIENT_HPP_INCLUDED
#include <uavcan/node/subscriber.hpp>
#include <uavcan/node/publisher.hpp>
@@ -26,10 +26,10 @@ namespace uavcan
*
* Once dynamic allocation is complete (or not needed anymore), the object can be deleted.
*/
class DynamicNodeIDAllocationClient : private TimerBase
class UAVCAN_EXPORT DynamicNodeIDClient : private TimerBase
{
typedef MethodBinder<DynamicNodeIDAllocationClient*,
void (DynamicNodeIDAllocationClient::*)
typedef MethodBinder<DynamicNodeIDClient*,
void (DynamicNodeIDClient::*)
(const ReceivedDataStructure<protocol::dynamic_node_id::Allocation>&)>
AllocationCallback;
@@ -48,7 +48,7 @@ class DynamicNodeIDAllocationClient : private TimerBase
void handleAllocation(const ReceivedDataStructure<protocol::dynamic_node_id::Allocation>& msg);
public:
DynamicNodeIDAllocationClient(INode& node)
DynamicNodeIDClient(INode& node)
: TimerBase(node)
, dnida_pub_(node)
, dnida_sub_(node)
@@ -91,4 +91,4 @@ public:
}
#endif // UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_CLIENT_HPP_INCLUDED
#endif // UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_CLIENT_HPP_INCLUDED
libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp
+278
View File
@@ -0,0 +1,278 @@
/*
* Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_ALLOCATION_REQUEST_MANAGER_HPP_INCLUDED
#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_ALLOCATION_REQUEST_MANAGER_HPP_INCLUDED
#include <uavcan/build_config.hpp>
#include <uavcan/debug.hpp>
#include <uavcan/node/subscriber.hpp>
#include <uavcan/node/publisher.hpp>
#include <uavcan/util/method_binder.hpp>
#include <uavcan/protocol/dynamic_node_id_server/types.hpp>
#include <uavcan/protocol/dynamic_node_id_server/event.hpp>
// UAVCAN types
#include <uavcan/protocol/dynamic_node_id/Allocation.hpp>
namespace uavcan
{
namespace dynamic_node_id_server
{
/**
* The main allocator must implement this interface.
*/
class IAllocationRequestHandler
{
public:
/**
* Allocation request manager uses this method to detect if it is allowed to publish follow-up responses.
*/
virtual bool canPublishFollowupAllocationResponse() const = 0;
/**
* This method will be invoked when a new allocation request is received.
*/
virtual void handleAllocationRequest(const UniqueID& unique_id, NodeID preferred_node_id) = 0;
virtual ~IAllocationRequestHandler() { }
};
/**
* This class manages communication with allocation clients.
* Three-stage unique ID exchange is implemented here, as well as response publication.
*/
class AllocationRequestManager
{
typedef MethodBinder<AllocationRequestManager*,
void (AllocationRequestManager::*)(const ReceivedDataStructure<Allocation>&)>
AllocationCallback;
const MonotonicDuration stage_timeout_;
MonotonicTime last_message_timestamp_;
Allocation::FieldTypes::unique_id current_unique_id_;
IAllocationRequestHandler& handler_;
IEventTracer& tracer_;
Subscriber<Allocation, AllocationCallback> allocation_sub_;
Publisher<Allocation> allocation_pub_;
enum { InvalidStage = 0 };
void trace(TraceCode code, int64_t argument) { tracer_.onEvent(code, argument); }
static uint8_t detectRequestStage(const Allocation& msg)
{
const uint8_t max_bytes_per_request = Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST;
if ((msg.unique_id.size() != max_bytes_per_request) &&
(msg.unique_id.size() != (msg.unique_id.capacity() - max_bytes_per_request * 2U)) &&
(msg.unique_id.size() != msg.unique_id.capacity())) // Future proofness for CAN FD
{
return InvalidStage;
}
if (msg.first_part_of_unique_id)
{
return 1; // Note that CAN FD frames can deliver the unique ID in one stage!
}
if (msg.unique_id.size() == Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST)
{
return 2;
}
if (msg.unique_id.size() < Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST)
{
return 3;
}
return InvalidStage;
}
uint8_t getExpectedStage() const
{
if (current_unique_id_.empty())
{
return 1;
}
if (current_unique_id_.size() >= (Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST * 2))
{
return 3;
}
if (current_unique_id_.size() >= Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST)
{
return 2;
}
return InvalidStage;
}
void publishFollowupAllocationResponse()
{
Allocation msg;
msg.unique_id = current_unique_id_;
UAVCAN_ASSERT(msg.unique_id.size() < msg.unique_id.capacity());
UAVCAN_TRACE("AllocationRequestManager", "Intermediate response with %u bytes of unique ID",
unsigned(msg.unique_id.size()));
trace(TraceAllocationFollowupResponse, msg.unique_id.size());
const int res = allocation_pub_.broadcast(msg);
if (res < 0)
{
trace(TraceError, res);
allocation_pub_.getNode().registerInternalFailure("Dynamic allocation broadcast");
}
}
void handleAllocation(const ReceivedDataStructure<Allocation>& msg)
{
trace(TraceAllocationActivity, msg.getSrcNodeID().get());
if (!msg.isAnonymousTransfer())
{
return; // This is a response from another allocator, ignore
}
/*
* Reset the expected stage on timeout
*/
if (msg.getMonotonicTimestamp() > (last_message_timestamp_ + stage_timeout_))
{
UAVCAN_TRACE("AllocationRequestManager", "Stage timeout, reset");
current_unique_id_.clear();
trace(TraceAllocationFollowupTimeout, (msg.getMonotonicTimestamp() - last_message_timestamp_).toUSec());
}
/*
* Checking if request stage matches the expected stage
*/
const uint8_t request_stage = detectRequestStage(msg);
if (request_stage == InvalidStage)
{
trace(TraceAllocationBadRequest, msg.unique_id.size());
return; // Malformed request - ignore without resetting
}
const uint8_t expected_stage = getExpectedStage();
if (request_stage == InvalidStage)
{
UAVCAN_ASSERT(0);
return;
}
if (request_stage != expected_stage)
{
trace(TraceAllocationUnexpectedStage, request_stage);
return; // Ignore - stage mismatch
}
const uint8_t max_expected_bytes =
static_cast<uint8_t>(current_unique_id_.capacity() - current_unique_id_.size());
UAVCAN_ASSERT(max_expected_bytes > 0);
if (msg.unique_id.size() > max_expected_bytes)
{
trace(TraceAllocationBadRequest, msg.unique_id.size());
return; // Malformed request
}
/*
* Updating the local state
*/
for (uint8_t i = 0; i < msg.unique_id.size(); i++)
{
current_unique_id_.push_back(msg.unique_id[i]);
}
trace(TraceAllocationRequestAccepted, current_unique_id_.size());
/*
* Proceeding with allocation if possible
* Note that single-frame CAN FD allocation requests will be delivered to the server even if it's not leader.
*/
if (current_unique_id_.size() == current_unique_id_.capacity())
{
UAVCAN_TRACE("AllocationRequestManager", "Allocation request received; preferred node ID: %d",
int(msg.node_id));
UniqueID unique_id;
copy(current_unique_id_.begin(), current_unique_id_.end(), unique_id.begin());
current_unique_id_.clear();
{
uint64_t event_agrument = 0;
for (uint8_t i = 0; i < 8; i++)
{
event_agrument |= static_cast<uint64_t>(unique_id[i]) << (56U - i * 8U);
}
trace(TraceAllocationExchangeComplete, static_cast<int64_t>(event_agrument));
}
handler_.handleAllocationRequest(unique_id, msg.node_id);
}
else
{
if (handler_.canPublishFollowupAllocationResponse())
{
publishFollowupAllocationResponse();
}
else
{
trace(TraceAllocationFollowupDenied, 0);
current_unique_id_.clear();
}
}
/*
* It is super important to update timestamp only if the request has been processed successfully.
*/
last_message_timestamp_ = msg.getMonotonicTimestamp();
}
public:
AllocationRequestManager(INode& node, IEventTracer& tracer, IAllocationRequestHandler& handler)
: stage_timeout_(MonotonicDuration::fromMSec(Allocation::FOLLOWUP_TIMEOUT_MS))
, handler_(handler)
, tracer_(tracer)
, allocation_sub_(node)
, allocation_pub_(node)
{ }
int init()
{
int res = allocation_pub_.init();
if (res < 0)
{
return res;
}
(void)allocation_pub_.setPriority(TransferPriorityLow);
allocation_pub_.setTxTimeout(MonotonicDuration::fromMSec(Allocation::DEFAULT_REQUEST_PERIOD_MS));
res = allocation_sub_.start(AllocationCallback(this, &AllocationRequestManager::handleAllocation));
if (res < 0)
{
return res;
}
allocation_sub_.allowAnonymousTransfers();
return 0;
}
int broadcastAllocationResponse(const UniqueID& unique_id, NodeID allocated_node_id)
{
Allocation msg;
msg.unique_id.resize(msg.unique_id.capacity());
copy(unique_id.begin(), unique_id.end(), msg.unique_id.begin());
msg.node_id = allocated_node_id.get();
trace(TraceAllocationResponse, msg.node_id);
return allocation_pub_.broadcast(msg);
}
};
}
}
#endif // Include guard
libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized.hpp
+20
View File
@@ -0,0 +1,20 @@
/*
* Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_HPP_INCLUDED
#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_HPP_INCLUDED
#include <uavcan/protocol/dynamic_node_id_server/centralized/server.hpp>
namespace uavcan
{
namespace dynamic_node_id_server
{
typedef centralized::Server CentralizedServer;
}
}
#endif // UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_HPP_INCLUDED
libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/server.hpp
+208
View File
@@ -0,0 +1,208 @@
/*
* Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_SERVER_HPP_INCLUDED
#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_SERVER_HPP_INCLUDED
#include <uavcan/build_config.hpp>
#include <uavcan/debug.hpp>
#include <uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp>
#include <uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp>
#include <uavcan/protocol/dynamic_node_id_server/node_id_selector.hpp>
#include <uavcan/protocol/dynamic_node_id_server/storage_marshaller.hpp>
#include <uavcan/protocol/dynamic_node_id_server/centralized/storage.hpp>
namespace uavcan
{
namespace dynamic_node_id_server
{
namespace centralized
{
/**
* This server is an alternative to @ref DistributedServer with the following differences:
* - It is not distributed, so using it means introducing a single point of failure into the system.
* - It takes less code space and requires less RAM, which makes it suitable for resource-constrained applications.
*
* This version is suitable only for simple non-critical systems.
*/
class Server : IAllocationRequestHandler
, INodeDiscoveryHandler
{
UniqueID own_unique_id_;
INode& node_;
IEventTracer& tracer_;
AllocationRequestManager allocation_request_manager_;
NodeDiscoverer node_discoverer_;
Storage storage_;
/*
* Private methods
*/
bool isNodeIDTaken(const NodeID node_id) const
{
return storage_.findByNodeID(node_id) != NULL;
}
void tryPublishAllocationResult(const Storage::Entry& entry)
{
const int res = allocation_request_manager_.broadcastAllocationResponse(entry.unique_id, entry.node_id);
if (res < 0)
{
tracer_.onEvent(TraceError, res);
node_.registerInternalFailure("Dynamic allocation response");
}
}
/*
* Methods of IAllocationRequestHandler
*/
virtual bool canPublishFollowupAllocationResponse() const
{
return true; // Because there's only one Centralized server in the system
}
virtual void handleAllocationRequest(const UniqueID& unique_id, const NodeID preferred_node_id)
{
const Storage::Entry* const e = storage_.findByUniqueID(unique_id);
if (e != NULL)
{
tryPublishAllocationResult(*e);
}
else
{
const NodeID allocated_node_id =
NodeIDSelector<Server>(this, &Server::isNodeIDTaken).findFreeNodeID(preferred_node_id);
if (allocated_node_id.isUnicast())
{
const int res = storage_.add(allocated_node_id, unique_id);
if (res >= 0)
{
tryPublishAllocationResult(Storage::Entry(allocated_node_id, unique_id));
}
else
{
tracer_.onEvent(TraceError, res);
node_.registerInternalFailure("CentralizedServer storage add");
}
}
else
{
UAVCAN_TRACE("dynamic_node_id_server::distributed::Server", "Request ignored - no free node ID left");
}
}
}
/*
* Methods of INodeDiscoveryHandler
*/
virtual bool canDiscoverNewNodes() const
{
return true; // Because there's only one Centralized server in the system
}
virtual NodeAwareness checkNodeAwareness(NodeID node_id) const
{
return (storage_.findByNodeID(node_id) == NULL) ? NodeAwarenessUnknown : NodeAwarenessKnownAndCommitted;
}
virtual void handleNewNodeDiscovery(const UniqueID* unique_id_or_null, NodeID node_id)
{
if (storage_.findByNodeID(node_id) != NULL)
{
UAVCAN_ASSERT(0); // Such node is already known, the class that called this method should have known that
return;
}
const int res = storage_.add(node_id, (unique_id_or_null == NULL) ? UniqueID() : *unique_id_or_null);
if (res < 0)
{
tracer_.onEvent(TraceError, res);
node_.registerInternalFailure("CentralizedServer storage add");
}
}
public:
Server(INode& node,
IStorageBackend& storage,
IEventTracer& tracer)
: node_(node)
, tracer_(tracer)
, allocation_request_manager_(node, tracer, *this)
, node_discoverer_(node, tracer, *this)
, storage_(storage)
{ }
int init(const UniqueID& own_unique_id)
{
/*
* Initializing storage first, because the next step requires it to be loaded
*/
int res = storage_.init();
if (res < 0)
{
return res;
}
/*
* Making sure that the server is started with the same node ID
*/
own_unique_id_ = own_unique_id;
{
const Storage::Entry* e = storage_.findByNodeID(node_.getNodeID());
if (e != NULL)
{
if (e->unique_id != own_unique_id_)
{
return -ErrInvalidConfiguration;
}
}
e = storage_.findByUniqueID(own_unique_id_);
if (e != NULL)
{
if (e->node_id != node_.getNodeID())
{
return -ErrInvalidConfiguration;
}
}
if (e == NULL)
{
res = storage_.add(node_.getNodeID(), own_unique_id_);
if (res < 0)
{
return res;
}
}
}
/*
* Misc
*/
res = allocation_request_manager_.init();
if (res < 0)
{
return res;
}
res = node_discoverer_.init();
if (res < 0)
{
return res;
}
return 0;
}
Storage::Size getNumAllocations() const { return storage_.getSize(); }
};
}
}
}
#endif // UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_SERVER_HPP_INCLUDED
libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/storage.hpp
+253
View File
@@ -0,0 +1,253 @@
/*
* Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_STORAGE_HPP_INCLUDED
#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_STORAGE_HPP_INCLUDED
#include <uavcan/build_config.hpp>
#include <uavcan/debug.hpp>
#include <uavcan/protocol/dynamic_node_id_server/storage_marshaller.hpp>
#include <uavcan/protocol/dynamic_node_id_server/event.hpp>
namespace uavcan
{
namespace dynamic_node_id_server
{
namespace centralized
{
/**
* This class transparently replicates its state to the storage backend, keeping the most recent state in memory.
* Writes are slow, reads are instantaneous.
*/
class Storage
{
public:
typedef uint8_t Size;
enum { Capacity = NodeID::Max };
struct Entry
{
UniqueID unique_id;
NodeID node_id;
Entry() { }
Entry(const NodeID nid, const UniqueID& uid)
: unique_id(uid)
, node_id(nid)
{ }
bool operator==(const Entry& rhs) const
{
return unique_id == rhs.unique_id &&
node_id == rhs.node_id;
}
};
private:
IStorageBackend& storage_;
Entry entries_[Capacity];
Size size_;
static IStorageBackend::String getSizeKey() { return "size"; }
static IStorageBackend::String makeEntryKey(Size index, const char* postfix)
{
IStorageBackend::String str;
// "0_foobar"
str.appendFormatted("%d", int(index));
str += "_";
str += postfix;
return str;
}
int readEntryFromStorage(Size index, Entry& out_entry)
{
const StorageMarshaller io(storage_);
// Unique ID
if (io.get(makeEntryKey(index, "unique_id"), out_entry.unique_id) < 0)
{
return -ErrFailure;
}
// Node ID
uint32_t node_id = 0;
if (io.get(makeEntryKey(index, "node_id"), node_id) < 0)
{
return -ErrFailure;
}
if (node_id > NodeID::Max || node_id == 0)
{
return -ErrFailure;
}
out_entry.node_id = NodeID(static_cast<uint8_t>(node_id));
return 0;
}
int writeEntryToStorage(Size index, const Entry& entry)
{
Entry temp = entry;
StorageMarshaller io(storage_);
// Unique ID
if (io.setAndGetBack(makeEntryKey(index, "unique_id"), temp.unique_id) < 0)
{
return -ErrFailure;
}
// Node ID
uint32_t node_id = entry.node_id.get();
if (io.setAndGetBack(makeEntryKey(index, "node_id"), node_id) < 0)
{
return -ErrFailure;
}
temp.node_id = NodeID(static_cast<uint8_t>(node_id));
return (temp == entry) ? 0 : -ErrFailure;
}
public:
Storage(IStorageBackend& storage)
: storage_(storage)
, size_(0)
{ }
/**
* This method reads all entries from the storage.
*/
int init()
{
StorageMarshaller io(storage_);
// Reading size
size_ = 0;
{
uint32_t value = 0;
if (io.get(getSizeKey(), value) < 0)
{
if (storage_.get(getSizeKey()).empty())
{
int res = io.setAndGetBack(getSizeKey(), value);
if (res < 0)
{
return res;
}
return (value == 0) ? 0 : -ErrFailure;
}
else
{
// There's some data in the storage, but it cannot be parsed - reporting an error
return -ErrFailure;
}
}
if (value > Capacity)
{
return -ErrFailure;
}
size_ = Size(value);
}
// Restoring entries
for (Size index = 0; index < size_; index++)
{
const int result = readEntryFromStorage(index, entries_[index]);
if (result < 0)
{
return result;
}
}
return 0;
}
/**
* This method invokes storage IO.
* Returned value indicates whether the entry was successfully appended.
*/
int add(const NodeID node_id, const UniqueID& unique_id)
{
if (size_ == Capacity)
{
return -ErrLogic;
}
if (!node_id.isUnicast())
{
return -ErrInvalidParam;
}
Entry entry;
entry.node_id = node_id;
entry.unique_id = unique_id;
// If next operations fail, we'll get a dangling entry, but it's absolutely OK.
int res = writeEntryToStorage(size_, entry);
if (res < 0)
{
return res;
}
// Updating the size
StorageMarshaller io(storage_);
uint32_t new_size_index = size_ + 1U;
res = io.setAndGetBack(getSizeKey(), new_size_index);
if (res < 0)
{
return res;
}
if (new_size_index != size_ + 1U)
{
return -ErrFailure;
}
entries_[size_] = entry;
size_++;
return 0;
}
/**
* Returns nullptr if there's no such entry.
*/
const Entry* findByNodeID(const NodeID node_id) const
{
for (Size i = 0; i < size_; i++)
{
if (entries_[i].node_id == node_id)
{
return &entries_[i];
}
}
return NULL;
}
/**
* Returns nullptr if there's no such entry.
*/
const Entry* findByUniqueID(const UniqueID& unique_id) const
{
for (Size i = 0; i < size_; i++)
{
if (entries_[i].unique_id == unique_id)
{
return &entries_[i];
}
}
return NULL;
}
Size getSize() const { return size_; }
};
}
}
}
#endif // Include guard
libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed.hpp
+20
View File
@@ -0,0 +1,20 @@
/*
* Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_HPP_INCLUDED
#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_HPP_INCLUDED
#include <uavcan/protocol/dynamic_node_id_server/distributed/server.hpp>
namespace uavcan
{
namespace dynamic_node_id_server
{
typedef distributed::Server DistributedServer;
}
}
#endif // UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_HPP_INCLUDED
libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp
+434
View File
@@ -0,0 +1,434 @@
/*
* Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_CLUSTER_MANAGER_HPP_INCLUDED
#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_CLUSTER_MANAGER_HPP_INCLUDED
#include <uavcan/build_config.hpp>
#include <uavcan/debug.hpp>
#include <uavcan/util/method_binder.hpp>
#include <uavcan/node/timer.hpp>
#include <uavcan/node/subscriber.hpp>
#include <uavcan/node/publisher.hpp>
#include <uavcan/protocol/dynamic_node_id_server/distributed/log.hpp>
#include <uavcan/protocol/dynamic_node_id_server/distributed/types.hpp>
#include <uavcan/protocol/dynamic_node_id_server/storage_marshaller.hpp>
#include <uavcan/protocol/dynamic_node_id_server/event.hpp>
// UAVCAN types
#include <uavcan/protocol/dynamic_node_id/server/Discovery.hpp>
namespace uavcan
{
namespace dynamic_node_id_server
{
namespace distributed
{
/**
* This class maintains the cluster state.
*/
class ClusterManager : private TimerBase
{
public:
enum { MaxClusterSize = Discovery::FieldTypes::known_nodes::MaxSize };
private:
typedef MethodBinder<ClusterManager*,
void (ClusterManager::*)
(const ReceivedDataStructure<Discovery>&)>
DiscoveryCallback;
struct Server
{
NodeID node_id;
Log::Index next_index;
Log::Index match_index;
Server()
: next_index(0)
, match_index(0)
{ }
void resetIndices(const Log& log)
{
next_index = Log::Index(log.getLastIndex() + 1U);
match_index = 0;
}
};
IStorageBackend& storage_;
IEventTracer& tracer_;
const Log& log_;
Subscriber<Discovery, DiscoveryCallback> discovery_sub_;
mutable Publisher<Discovery> discovery_pub_;
Server servers_[MaxClusterSize - 1]; ///< Minus one because the local server is not listed there.
uint8_t cluster_size_;
uint8_t num_known_servers_;
static IStorageBackend::String getStorageKeyForClusterSize() { return "cluster_size"; }
INode& getNode() { return discovery_sub_.getNode(); }
const INode& getNode() const { return discovery_sub_.getNode(); }
const Server* findServer(NodeID node_id) const { return const_cast<ClusterManager*>(this)->findServer(node_id); }
Server* findServer(NodeID node_id)
{
for (uint8_t i = 0; i < num_known_servers_; i++)
{
UAVCAN_ASSERT(servers_[i].node_id.isUnicast());
if (servers_[i].node_id == node_id)
{
return &servers_[i];
}
}
return NULL;
}
virtual void handleTimerEvent(const TimerEvent&)
{
UAVCAN_ASSERT(num_known_servers_ < cluster_size_);
tracer_.onEvent(TraceRaftDiscoveryBroadcast, num_known_servers_);
/*
* Filling the message
*/
Discovery msg;
msg.configured_cluster_size = cluster_size_;
msg.known_nodes.push_back(getNode().getNodeID().get()); // Putting ourselves at index 0
for (uint8_t i = 0; i < num_known_servers_; i++)
{
UAVCAN_ASSERT(servers_[i].node_id.isUnicast());
msg.known_nodes.push_back(servers_[i].node_id.get());
}
UAVCAN_ASSERT(msg.known_nodes.size() == (num_known_servers_ + 1));
/*
* Broadcasting
*/
UAVCAN_TRACE("dynamic_node_id_server::distributed::ClusterManager",
"Broadcasting Discovery message; known nodes: %d of %d",
int(msg.known_nodes.size()), int(cluster_size_));
const int res = discovery_pub_.broadcast(msg);
if (res < 0)
{
UAVCAN_TRACE("dynamic_node_id_server::distributed::ClusterManager", "Discovery broadcst failed: %d", res);
getNode().registerInternalFailure("Raft discovery broadcast");
}
/*
* Termination condition
*/
if (isClusterDiscovered())
{
UAVCAN_TRACE("dynamic_node_id_server::distributed::ClusterManager",
"Discovery broadcasting timer stopped");
stop();
}
}
void handleDiscovery(const ReceivedDataStructure<Discovery>& msg)
{
tracer_.onEvent(TraceRaftDiscoveryReceived, msg.getSrcNodeID().get());
/*
* Validating cluster configuration
* If there's a case of misconfiguration, the message will be ignored.
*/
if (msg.configured_cluster_size != cluster_size_)
{
tracer_.onEvent(TraceRaftBadClusterSizeReceived, msg.configured_cluster_size);
getNode().registerInternalFailure("Bad Raft cluster size");
return;
}
/*
* Updating the set of known servers
*/
for (uint8_t i = 0; i < msg.known_nodes.size(); i++)
{
if (isClusterDiscovered())
{
break;
}
const NodeID node_id(msg.known_nodes[i]);
if (node_id.isUnicast() && !isKnownServer(node_id))
{
addServer(node_id);
}
}
/*
* Publishing a new Discovery request if the publishing server needs to learn about more servers.
*/
if (msg.configured_cluster_size > msg.known_nodes.size())
{
startDiscoveryPublishingTimerIfNotRunning();
}
}
void startDiscoveryPublishingTimerIfNotRunning()
{
if (!isRunning())
{
startPeriodic(MonotonicDuration::fromMSec(Discovery::BROADCASTING_INTERVAL_MS));
}
}
public:
enum { ClusterSizeUnknown = 0 };
/**
* @param node Needed to publish and subscribe to Discovery message
* @param storage Needed to read the cluster size parameter from the storage
* @param log Needed to initialize nextIndex[] values after elections
*/
ClusterManager(INode& node, IStorageBackend& storage, const Log& log, IEventTracer& tracer)
: TimerBase(node)
, storage_(storage)
, tracer_(tracer)
, log_(log)
, discovery_sub_(node)
, discovery_pub_(node)
, cluster_size_(0)
, num_known_servers_(0)
{ }
/**
* If cluster_size is set to ClusterSizeUnknown, the class will try to read this parameter from the
* storage backend using key 'cluster_size'.
* Returns negative error code.
*/
int init(uint8_t init_cluster_size = ClusterSizeUnknown)
{
/*
* Figuring out the cluster size
*/
if (init_cluster_size == ClusterSizeUnknown)
{
// Reading from the storage
StorageMarshaller io(storage_);
uint32_t value = 0;
int res = io.get(getStorageKeyForClusterSize(), value);
if (res < 0)
{
UAVCAN_TRACE("dynamic_node_id_server::distributed::ClusterManager",
"Cluster size is neither configured nor stored in the storage");
return res;
}
if ((value == 0) || (value > MaxClusterSize))
{
UAVCAN_TRACE("dynamic_node_id_server::distributed::ClusterManager", "Cluster size is invalid");
return -ErrInvalidConfiguration;
}
cluster_size_ = static_cast<uint8_t>(value);
}
else
{
if ((init_cluster_size == 0) || (init_cluster_size > MaxClusterSize))
{
return -ErrInvalidParam;
}
cluster_size_ = init_cluster_size;
// Writing the storage
StorageMarshaller io(storage_);
uint32_t value = init_cluster_size;
int res = io.setAndGetBack(getStorageKeyForClusterSize(), value);
if ((res < 0) || (value != init_cluster_size))
{
UAVCAN_TRACE("dynamic_node_id_server::distributed::ClusterManager", "Failed to store cluster size");
return -ErrFailure;
}
}
tracer_.onEvent(TraceRaftClusterSizeInited, cluster_size_);
UAVCAN_ASSERT(cluster_size_ > 0);
UAVCAN_ASSERT(cluster_size_ <= MaxClusterSize);
/*
* Initializing pub/sub and timer
*/
int res = discovery_pub_.init();
if (res < 0)
{
return res;
}
(void)discovery_pub_.setPriority(TransferPriorityLow);
res = discovery_sub_.start(DiscoveryCallback(this, &ClusterManager::handleDiscovery));
if (res < 0)
{
return res;
}
startDiscoveryPublishingTimerIfNotRunning();
/*
* Misc
*/
resetAllServerIndices();
return 0;
}
/**
* Adds once server regardless of the discovery logic.
*/
void addServer(NodeID node_id)
{
UAVCAN_ASSERT((num_known_servers_ + 1) < MaxClusterSize);
if (!isKnownServer(node_id) && node_id.isUnicast())
{
tracer_.onEvent(TraceRaftNewServerDiscovered, node_id.get());
servers_[num_known_servers_].node_id = node_id;
servers_[num_known_servers_].resetIndices(log_);
num_known_servers_ = static_cast<uint8_t>(num_known_servers_ + 1U);
}
else
{
UAVCAN_ASSERT(0);
}
}
/**
* Whether such server has been discovered.
*/
bool isKnownServer(NodeID node_id) const
{
if (node_id == getNode().getNodeID())
{
return true;
}
for (uint8_t i = 0; i < num_known_servers_; i++)
{
UAVCAN_ASSERT(servers_[i].node_id.isUnicast());
UAVCAN_ASSERT(servers_[i].node_id != getNode().getNodeID());
if (servers_[i].node_id == node_id)
{
return true;
}
}
return false;
}
/**
* An invalid node ID will be returned if there's no such server.
* The local server is not listed there.
*/
NodeID getRemoteServerNodeIDAtIndex(uint8_t index) const
{
if (index < num_known_servers_)
{
return servers_[index].node_id;
}
return NodeID();
}
/**
* See next_index[] in Raft paper.
*/
Log::Index getServerNextIndex(NodeID server_node_id) const
{
const Server* const s = findServer(server_node_id);
if (s != NULL)
{
return s->next_index;
}
UAVCAN_ASSERT(0);
return 0;
}
void incrementServerNextIndexBy(NodeID server_node_id, Log::Index increment)
{
Server* const s = findServer(server_node_id);
if (s != NULL)
{
s->next_index = Log::Index(s->next_index + increment);
}
else
{
UAVCAN_ASSERT(0);
}
}
void decrementServerNextIndex(NodeID server_node_id)
{
Server* const s = findServer(server_node_id);
if (s != NULL)
{
s->next_index--;
}
else
{
UAVCAN_ASSERT(0);
}
}
/**
* See match_index[] in Raft paper.
*/
Log::Index getServerMatchIndex(NodeID server_node_id) const
{
const Server* const s = findServer(server_node_id);
if (s != NULL)
{
return s->match_index;
}
UAVCAN_ASSERT(0);
return 0;
}
void setServerMatchIndex(NodeID server_node_id, Log::Index match_index)
{
Server* const s = findServer(server_node_id);
if (s != NULL)
{
s->match_index = match_index;
}
else
{
UAVCAN_ASSERT(0);
}
}
/**
* This method must be called when the current server becomes leader.
*/
void resetAllServerIndices()
{
for (uint8_t i = 0; i < num_known_servers_; i++)
{
UAVCAN_ASSERT(servers_[i].node_id.isUnicast());
servers_[i].resetIndices(log_);
}
}
/**
* Number of known servers can only grow, and it never exceeds the cluster size value.
* This number does not include the local server.
*/
uint8_t getNumKnownServers() const { return num_known_servers_; }
/**
* Cluster size and quorum size are constant.
*/
uint8_t getClusterSize() const { return cluster_size_; }
uint8_t getQuorumSize() const { return static_cast<uint8_t>(cluster_size_ / 2U + 1U); }
bool isClusterDiscovered() const { return num_known_servers_ == (cluster_size_ - 1); }
};
}
}
}
#endif // Include guard
libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/log.hpp
+310
View File
@@ -0,0 +1,310 @@
/*
* Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_LOG_HPP_INCLUDED
#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_LOG_HPP_INCLUDED
#include <uavcan/build_config.hpp>
#include <uavcan/debug.hpp>
#include <uavcan/protocol/dynamic_node_id_server/distributed/types.hpp>
#include <uavcan/protocol/dynamic_node_id_server/storage_marshaller.hpp>
#include <uavcan/protocol/dynamic_node_id_server/event.hpp>
namespace uavcan
{
namespace dynamic_node_id_server
{
namespace distributed
{
/**
* Raft log.
* This class transparently replicates its state to the storage backend, keeping the most recent state in memory.
* Writes are slow, reads are instantaneous.
*/
class Log
{
public:
typedef uint8_t Index;
enum { Capacity = NodeID::Max + 1 };
private:
IStorageBackend& storage_;
IEventTracer& tracer_;
Entry entries_[Capacity];
Index last_index_; // Index zero always contains an empty entry
static IStorageBackend::String getLastIndexKey() { return "log_last_index"; }
static IStorageBackend::String makeEntryKey(Index index, const char* postfix)
{
IStorageBackend::String str;
// "log0_foobar"
str += "log";
str.appendFormatted("%d", int(index));
str += "_";
str += postfix;
return str;
}
int readEntryFromStorage(Index index, Entry& out_entry)
{
const StorageMarshaller io(storage_);
// Term
if (io.get(makeEntryKey(index, "term"), out_entry.term) < 0)
{
return -ErrFailure;
}
// Unique ID
if (io.get(makeEntryKey(index, "unique_id"), out_entry.unique_id) < 0)
{
return -ErrFailure;
}
// Node ID
uint32_t node_id = 0;
if (io.get(makeEntryKey(index, "node_id"), node_id) < 0)
{
return -ErrFailure;
}
if (node_id > NodeID::Max)
{
return -ErrFailure;
}
out_entry.node_id = static_cast<uint8_t>(node_id);
return 0;
}
int writeEntryToStorage(Index index, const Entry& entry)
{
Entry temp = entry;
StorageMarshaller io(storage_);
// Term
if (io.setAndGetBack(makeEntryKey(index, "term"), temp.term) < 0)
{
return -ErrFailure;
}
// Unique ID
if (io.setAndGetBack(makeEntryKey(index, "unique_id"), temp.unique_id) < 0)
{
return -ErrFailure;
}
// Node ID
uint32_t node_id = entry.node_id;
if (io.setAndGetBack(makeEntryKey(index, "node_id"), node_id) < 0)
{
return -ErrFailure;
}
temp.node_id = static_cast<uint8_t>(node_id);
return (temp == entry) ? 0 : -ErrFailure;
}
int initEmptyLogStorage()
{
StorageMarshaller io(storage_);
/*
* Writing the zero entry - it must always be default-initialized
*/
entries_[0] = Entry();
int res = writeEntryToStorage(0, entries_[0]);
if (res < 0)
{
return res;
}
/*
* Initializing last index
* Last index must be written AFTER the zero entry, otherwise if the write fails here the storage will be
* left in an inconsistent state.
*/
last_index_ = 0;
uint32_t stored_index = 0;
res = io.setAndGetBack(getLastIndexKey(), stored_index);
if (res < 0)
{
return res;
}
if (stored_index != 0)
{
return -ErrFailure;
}
return 0;
}
public:
Log(IStorageBackend& storage, IEventTracer& tracer)
: storage_(storage)
, tracer_(tracer)
, last_index_(0)
{ }
int init()
{
StorageMarshaller io(storage_);
// Reading max index
{
uint32_t value = 0;
if (io.get(getLastIndexKey(), value) < 0)
{
if (storage_.get(getLastIndexKey()).empty())
{
UAVCAN_TRACE("dynamic_node_id_server::distributed::Log", "Initializing empty storage");
return initEmptyLogStorage();
}
else
{
// There's some data in the storage, but it cannot be parsed - reporting an error
UAVCAN_TRACE("dynamic_node_id_server::distributed::Log", "Failed to read last index");
return -ErrFailure;
}
}
if (value >= Capacity)
{
return -ErrFailure;
}
last_index_ = Index(value);
}
tracer_.onEvent(TraceRaftLogLastIndexRestored, last_index_);
// Restoring log entries - note that index 0 always exists
for (Index index = 0; index <= last_index_; index++)
{
const int result = readEntryFromStorage(index, entries_[index]);
if (result < 0)
{
UAVCAN_TRACE("dynamic_node_id_server::distributed::Log", "Failed to read entry at index %u: %d",
unsigned(index), result);
return result;
}
}
UAVCAN_TRACE("dynamic_node_id_server::distributed::Log", "Restored %u log entries", unsigned(last_index_));
return 0;
}
/**
* This method invokes storage IO.
* Returned value indicates whether the entry was successfully appended.
*/
int append(const Entry& entry)
{
if ((last_index_ + 1) >= Capacity)
{
return -ErrLogic;
}
tracer_.onEvent(TraceRaftLogAppend, last_index_ + 1U);
// If next operations fail, we'll get a dangling entry, but it's absolutely OK.
int res = writeEntryToStorage(Index(last_index_ + 1), entry);
if (res < 0)
{
return res;
}
// Updating the last index
StorageMarshaller io(storage_);
uint32_t new_last_index = last_index_ + 1U;
res = io.setAndGetBack(getLastIndexKey(), new_last_index);
if (res < 0)
{
return res;
}
if (new_last_index != last_index_ + 1U)
{
return -ErrFailure;
}
entries_[new_last_index] = entry;
last_index_ = Index(new_last_index);
UAVCAN_TRACE("dynamic_node_id_server::distributed::Log", "New entry, index %u, node ID %u, term %u",
unsigned(last_index_), unsigned(entry.node_id), unsigned(entry.term));
return 0;
}
/**
* This method invokes storage IO.
* Returned value indicates whether the requested operation has been carried out successfully.
*/
int removeEntriesWhereIndexGreaterOrEqual(Index index)
{
UAVCAN_ASSERT(last_index_ < Capacity);
if (((index) >= Capacity) || (index <= 0))
{
return -ErrLogic;
}
uint32_t new_last_index = index - 1U;
tracer_.onEvent(TraceRaftLogRemove, new_last_index);
if (new_last_index != last_index_)
{
StorageMarshaller io(storage_);
int res = io.setAndGetBack(getLastIndexKey(), new_last_index);
if (res < 0)
{
return res;
}
if (new_last_index != index - 1U)
{
return -ErrFailure;
}
UAVCAN_TRACE("dynamic_node_id_server::distributed::Log", "Entries removed, last index %u --> %u",
unsigned(last_index_), unsigned(new_last_index));
last_index_ = Index(new_last_index);
}
// Removal operation leaves dangling entries in storage, it's OK
return 0;
}
int removeEntriesWhereIndexGreater(Index index)
{
return removeEntriesWhereIndexGreaterOrEqual(Index(index + 1U));
}
/**
* Returns nullptr if there's no such index.
* This method does not use storage IO.
*/
const Entry* getEntryAtIndex(Index index) const
{
UAVCAN_ASSERT(last_index_ < Capacity);
return (index <= last_index_) ? &entries_[index] : NULL;
}
Index getLastIndex() const { return last_index_; }
bool isOtherLogUpToDate(Index other_last_index, Term other_last_term) const
{
UAVCAN_ASSERT(last_index_ < Capacity);
// Terms are different - the one with higher term is more up-to-date
if (other_last_term != entries_[last_index_].term)
{
return other_last_term > entries_[last_index_].term;
}
// Terms are equal - longer log wins
return other_last_index >= last_index_;
}
};
}
}
}
#endif // Include guard
libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/persistent_state.hpp
+237
View File
@@ -0,0 +1,237 @@
/*
* Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_PERSISTENT_STATE_HPP_INCLUDED
#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_PERSISTENT_STATE_HPP_INCLUDED
#include <uavcan/build_config.hpp>
#include <uavcan/debug.hpp>
#include <uavcan/protocol/dynamic_node_id_server/distributed/types.hpp>
#include <uavcan/protocol/dynamic_node_id_server/distributed/log.hpp>
#include <uavcan/protocol/dynamic_node_id_server/storage_marshaller.hpp>
#include <uavcan/protocol/dynamic_node_id_server/event.hpp>
namespace uavcan
{
namespace dynamic_node_id_server
{
namespace distributed
{
/**
* This class is a convenient container for persistent state variables defined by Raft.
* Writes are slow, reads are instantaneous.
*/
class PersistentState
{
IStorageBackend& storage_;
IEventTracer& tracer_;
Term current_term_;
NodeID voted_for_;
Log log_;
static IStorageBackend::String getCurrentTermKey() { return "current_term"; }
static IStorageBackend::String getVotedForKey() { return "voted_for"; }
public:
PersistentState(IStorageBackend& storage, IEventTracer& tracer)
: storage_(storage)
, tracer_(tracer)
, current_term_(0)
, log_(storage, tracer)
{ }
/**
* Initialization is performed as follows (every step may fail and return an error):
* 1. Log is restored or initialized.
* 2. Current term is restored. If there was no current term stored and the log is empty, it will be initialized
* with zero.
* 3. VotedFor value is restored. If there was no VotedFor value stored, the log is empty, and the current term is
* zero, the value will be initialized with zero.
*/
int init()
{
/*
* Reading log
*/
int res = log_.init();
if (res < 0)
{
UAVCAN_TRACE("dynamic_node_id_server::distributed::PersistentState", "Log init failed: %d", res);
return res;
}
const Entry* const last_entry = log_.getEntryAtIndex(log_.getLastIndex());
if (last_entry == NULL)
{
UAVCAN_ASSERT(0);
return -ErrLogic;
}
const bool log_is_empty = (log_.getLastIndex() == 0) && (last_entry->term == 0);
StorageMarshaller io(storage_);
/*
* Reading currentTerm
*/
if (storage_.get(getCurrentTermKey()).empty() && log_is_empty)
{
// First initialization
current_term_ = 0;
res = io.setAndGetBack(getCurrentTermKey(), current_term_);
if (res < 0)
{
UAVCAN_TRACE("dynamic_node_id_server::distributed::PersistentState",
"Failed to init current term: %d", res);
return res;
}
if (current_term_ != 0)
{
return -ErrFailure;
}
}
else
{
// Restoring
res = io.get(getCurrentTermKey(), current_term_);
if (res < 0)
{
UAVCAN_TRACE("dynamic_node_id_server::distributed::PersistentState",
"Failed to read current term: %d", res);
return res;
}
}
tracer_.onEvent(TraceRaftCurrentTermRestored, current_term_);
if (current_term_ < last_entry->term)
{
UAVCAN_TRACE("dynamic_node_id_server::distributed::PersistentState",
"Persistent storage is damaged: current term is less than term of the last log entry (%u < %u)",
unsigned(current_term_), unsigned(last_entry->term));
return -ErrLogic;
}
/*
* Reading votedFor
*/
if (storage_.get(getVotedForKey()).empty() && log_is_empty && (current_term_ == 0))
{
// First initialization
voted_for_ = NodeID(0);
uint32_t stored_voted_for = 0;
res = io.setAndGetBack(getVotedForKey(), stored_voted_for);
if (res < 0)
{
UAVCAN_TRACE("dynamic_node_id_server::distributed::PersistentState",
"Failed to init votedFor: %d", res);
return res;
}
if (stored_voted_for != 0)
{
return -ErrFailure;
}
}
else
{
// Restoring
uint32_t stored_voted_for = 0;
res = io.get(getVotedForKey(), stored_voted_for);
if (res < 0)
{
UAVCAN_TRACE("dynamic_node_id_server::distributed::PersistentState",
"Failed to read votedFor: %d", res);
return res;
}
if (stored_voted_for > NodeID::Max)
{
return -ErrInvalidConfiguration;
}
voted_for_ = NodeID(uint8_t(stored_voted_for));
}
tracer_.onEvent(TraceRaftVotedForRestored, voted_for_.get());
return 0;
}
Term getCurrentTerm() const { return current_term_; }
NodeID getVotedFor() const { return voted_for_; }
bool isVotedForSet() const { return voted_for_.isUnicast(); }
Log& getLog() { return log_; }
const Log& getLog() const { return log_; }
/**
* Invokes storage IO.
*/
int setCurrentTerm(Term term)
{
if (term < current_term_)
{
UAVCAN_ASSERT(0);
return -ErrInvalidParam;
}
tracer_.onEvent(TraceRaftCurrentTermUpdate, term);
StorageMarshaller io(storage_);
Term tmp = term;
int res = io.setAndGetBack(getCurrentTermKey(), tmp);
if (res < 0)
{
return res;
}
if (tmp != term)
{
return -ErrFailure;
}
current_term_ = term;
return 0;
}
/**
* Invokes storage IO.
*/
int setVotedFor(NodeID node_id)
{
if (!node_id.isValid())
{
UAVCAN_ASSERT(0);
return -ErrInvalidParam;
}
tracer_.onEvent(TraceRaftVotedForUpdate, node_id.get());
StorageMarshaller io(storage_);
uint32_t tmp = node_id.get();
int res = io.setAndGetBack(getVotedForKey(), tmp);
if (res < 0)
{
return res;
}
if (node_id.get() != tmp)
{
return -ErrFailure;
}
voted_for_ = node_id;
return 0;
}
int resetVotedFor() { return setVotedFor(NodeID(0)); }
};
}
}
}
#endif // Include guard
libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp
+916
View File
@@ -0,0 +1,916 @@
/*
* Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_RAFT_CORE_HPP_INCLUDED
#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_RAFT_CORE_HPP_INCLUDED
#include <cstdlib>
#include <uavcan/build_config.hpp>
#include <uavcan/debug.hpp>
#include <uavcan/util/method_binder.hpp>
#include <uavcan/node/timer.hpp>
#include <uavcan/node/service_server.hpp>
#include <uavcan/node/service_client.hpp>
#include <uavcan/protocol/dynamic_node_id_server/distributed/types.hpp>
#include <uavcan/protocol/dynamic_node_id_server/distributed/persistent_state.hpp>
#include <uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp>
#include <uavcan/protocol/dynamic_node_id_server/event.hpp>
// UAVCAN types
#include <uavcan/protocol/dynamic_node_id/server/AppendEntries.hpp>
#include <uavcan/protocol/dynamic_node_id/server/RequestVote.hpp>
namespace uavcan
{
namespace dynamic_node_id_server
{
namespace distributed
{
/**
* Allocator has to implement this interface so the RaftCore can inform it when a new entry gets committed to the log.
*/
class IRaftLeaderMonitor
{
public:
/**
* This method will be invoked when a new log entry is committed (only if the local server is the current Leader).
*/
virtual void handleLogCommitOnLeader(const Entry& committed_entry) = 0;
/**
* Invoked by the Raft core when the local node becomes a leader or ceases to be one.
* By default the local node is not leader.
* It is possible to commit to the log right from this method.
*/
virtual void handleLocalLeadershipChange(bool local_node_is_leader) = 0;
virtual ~IRaftLeaderMonitor() { }
};
/**
* This class implements log replication and voting.
* It does not implement client-server interaction at all; instead it just exposes a public method for adding
* allocation entries.
*
* Note that this class uses std::rand(), so the RNG must be properly seeded by the application.
*
* Activity registration:
* - persistent state update error
* - switch to candidate (this defines timeout between reelections)
* - newer term in response (also switch to follower)
* - append entries request with term >= currentTerm
* - vote granted
*/
class RaftCore : private TimerBase
{
public:
enum ServerState
{
ServerStateFollower,
ServerStateCandidate,
ServerStateLeader
};
private:
typedef MethodBinder<RaftCore*, void (RaftCore::*)(const ReceivedDataStructure<AppendEntries::Request>&,
ServiceResponseDataStructure<AppendEntries::Response>&)>
AppendEntriesCallback;
typedef MethodBinder<RaftCore*, void (RaftCore::*)(const ServiceCallResult<AppendEntries>&)>
AppendEntriesResponseCallback;
typedef MethodBinder<RaftCore*, void (RaftCore::*)(const ReceivedDataStructure<RequestVote::Request>&,
ServiceResponseDataStructure<RequestVote::Response>&)>
RequestVoteCallback;
typedef MethodBinder<RaftCore*, void (RaftCore::*)(const ServiceCallResult<RequestVote>&)>
RequestVoteResponseCallback;
struct PendingAppendEntriesFields
{
Log::Index prev_log_index;
Log::Index num_entries;
PendingAppendEntriesFields()
: prev_log_index(0)
, num_entries(0)
{ }
};
/*
* Constants
*/
enum { MaxNumFollowers = ClusterManager::MaxClusterSize - 1 };
IEventTracer& tracer_;
IRaftLeaderMonitor& leader_monitor_;
/*
* States
*/
PersistentState persistent_state_;
ClusterManager cluster_;
Log::Index commit_index_;
MonotonicTime last_activity_timestamp_;
MonotonicDuration randomized_activity_timeout_;
ServerState server_state_;
uint8_t next_server_index_; ///< Next server to query AE from
uint8_t num_votes_received_in_this_campaign_;
PendingAppendEntriesFields pending_append_entries_fields_;
/*
* Transport
*/
ServiceServer<AppendEntries, AppendEntriesCallback> append_entries_srv_;
ServiceClient<AppendEntries, AppendEntriesResponseCallback> append_entries_client_;
ServiceServer<RequestVote, RequestVoteCallback> request_vote_srv_;
ServiceClient<RequestVote, RequestVoteResponseCallback, MaxNumFollowers> request_vote_client_;
/*
* Methods
*/
void trace(TraceCode event, int64_t argument) { tracer_.onEvent(event, argument); }
INode& getNode() { return append_entries_srv_.getNode(); }
const INode& getNode() const { return append_entries_srv_.getNode(); }
void checkInvariants() const
{
// Commit index
UAVCAN_ASSERT(commit_index_ <= persistent_state_.getLog().getLastIndex());
// Term
UAVCAN_ASSERT(persistent_state_.getLog().getEntryAtIndex(persistent_state_.getLog().getLastIndex()) != NULL);
UAVCAN_ASSERT(persistent_state_.getLog().getEntryAtIndex(persistent_state_.getLog().getLastIndex())->term
<= persistent_state_.getCurrentTerm());
// Elections
UAVCAN_ASSERT(server_state_ != ServerStateCandidate || !request_vote_client_.hasPendingCalls() ||
persistent_state_.getVotedFor() == getNode().getNodeID());
UAVCAN_ASSERT(num_votes_received_in_this_campaign_ <= cluster_.getClusterSize());
// Transport
UAVCAN_ASSERT(append_entries_client_.getNumPendingCalls() <= 1);
UAVCAN_ASSERT(request_vote_client_.getNumPendingCalls() <= cluster_.getNumKnownServers());
UAVCAN_ASSERT(server_state_ != ServerStateCandidate || !append_entries_client_.hasPendingCalls());
UAVCAN_ASSERT(server_state_ != ServerStateLeader || !request_vote_client_.hasPendingCalls());
UAVCAN_ASSERT(server_state_ != ServerStateFollower ||
(!append_entries_client_.hasPendingCalls() && !request_vote_client_.hasPendingCalls()));
}
void registerActivity()
{
last_activity_timestamp_ = getNode().getMonotonicTime();
static const int32_t randomization_range_msec = AppendEntries::Request::DEFAULT_MAX_ELECTION_TIMEOUT_MS -
AppendEntries::Request::DEFAULT_MIN_ELECTION_TIMEOUT_MS;
const int32_t random_msec = (std::rand() % randomization_range_msec) + 1;
randomized_activity_timeout_ =
MonotonicDuration::fromMSec(AppendEntries::Request::DEFAULT_MIN_ELECTION_TIMEOUT_MS + random_msec);
UAVCAN_ASSERT(randomized_activity_timeout_.toMSec() > AppendEntries::Request::DEFAULT_MIN_ELECTION_TIMEOUT_MS);
UAVCAN_ASSERT(randomized_activity_timeout_.toMSec() <= AppendEntries::Request::DEFAULT_MAX_ELECTION_TIMEOUT_MS);
}
bool isActivityTimedOut() const
{
return getNode().getMonotonicTime() > (last_activity_timestamp_ + randomized_activity_timeout_);
}
void handlePersistentStateUpdateError(int error)
{
UAVCAN_ASSERT(error < 0);
trace(TraceRaftPersistStateUpdateError, error);
switchState(ServerStateFollower);
registerActivity(); // Deferring reelections
}
void updateFollower()
{
if (isActivityTimedOut())
{
switchState(ServerStateCandidate);
registerActivity();
}
}
void updateCandidate()
{
if (num_votes_received_in_this_campaign_ > 0)
{
trace(TraceRaftElectionComplete, num_votes_received_in_this_campaign_);
const bool won = num_votes_received_in_this_campaign_ >= cluster_.getQuorumSize();
UAVCAN_TRACE("dynamic_node_id_server::distributed::RaftCore", "Election complete, won: %d", int(won));
switchState(won ? ServerStateLeader : ServerStateFollower); // Start over or become leader
}
else
{
// Set votedFor, abort on failure
int res = persistent_state_.setVotedFor(getNode().getNodeID());
if (res < 0)
{
handlePersistentStateUpdateError(res);
return;
}
// Increment current term, abort on failure
res = persistent_state_.setCurrentTerm(persistent_state_.getCurrentTerm() + 1U);
if (res < 0)
{
handlePersistentStateUpdateError(res);
return;
}
num_votes_received_in_this_campaign_ = 1; // Voting for self
RequestVote::Request req;
req.last_log_index = persistent_state_.getLog().getLastIndex();
req.last_log_term = persistent_state_.getLog().getEntryAtIndex(req.last_log_index)->term;
req.term = persistent_state_.getCurrentTerm();
for (uint8_t i = 0; i < MaxNumFollowers; i++)
{
const NodeID node_id = cluster_.getRemoteServerNodeIDAtIndex(i);
if (!node_id.isUnicast())
{
break;
}
UAVCAN_TRACE("dynamic_node_id_server::distributed::RaftCore",
"Requesting vote from %d", int(node_id.get()));
trace(TraceRaftVoteRequestInitiation, node_id.get());
res = request_vote_client_.call(node_id, req);
if (res < 0)
{
trace(TraceError, res);
}
}
}
}
void updateLeader()
{
if (append_entries_client_.hasPendingCalls())
{
append_entries_client_.cancelAllCalls(); // Refer to the response callback to learn why
}
if (cluster_.getClusterSize() > 1)
{
const NodeID node_id = cluster_.getRemoteServerNodeIDAtIndex(next_server_index_);
UAVCAN_ASSERT(node_id.isUnicast());
next_server_index_++;
if (next_server_index_ >= cluster_.getNumKnownServers())
{
next_server_index_ = 0;
}
AppendEntries::Request req;
req.term = persistent_state_.getCurrentTerm();
req.leader_commit = commit_index_;
req.prev_log_index = Log::Index(cluster_.getServerNextIndex(node_id) - 1U);
const Entry* const entry = persistent_state_.getLog().getEntryAtIndex(req.prev_log_index);
if (entry == NULL)
{
UAVCAN_ASSERT(0);
handlePersistentStateUpdateError(-ErrLogic);
return;
}
req.prev_log_term = entry->term;
for (Log::Index index = cluster_.getServerNextIndex(node_id);
index <= persistent_state_.getLog().getLastIndex();
index++)
{
req.entries.push_back(*persistent_state_.getLog().getEntryAtIndex(index));
if (req.entries.size() == req.entries.capacity())
{
break;
}
}
pending_append_entries_fields_.num_entries = req.entries.size();
pending_append_entries_fields_.prev_log_index = req.prev_log_index;
const int res = append_entries_client_.call(node_id, req);
if (res < 0)
{
trace(TraceRaftAppendEntriesCallFailure, res);
}
}
propagateCommitIndex();
}
void switchState(ServerState new_state)
{
if (server_state_ == new_state)
{
return;
}
/*
* Logging
*/
UAVCAN_TRACE("dynamic_node_id_server::distributed::RaftCore", "State switch: %d --> %d",
int(server_state_), int(new_state));
trace(TraceRaftStateSwitch, new_state);
/*
* Updating the current state
*/
const ServerState old_state = server_state_;
server_state_ = new_state;
/*
* Resetting specific states
*/
cluster_.resetAllServerIndices();
next_server_index_ = 0;
num_votes_received_in_this_campaign_ = 0;
request_vote_client_.cancelAllCalls();
append_entries_client_.cancelAllCalls();
/*
* Calling the switch handler
* Note that the handler may commit to the log directly
*/
if ((old_state == ServerStateLeader) ||
(new_state == ServerStateLeader))
{
leader_monitor_.handleLocalLeadershipChange(new_state == ServerStateLeader);
}
}
void tryIncrementCurrentTermFromResponse(Term new_term)
{
trace(TraceRaftNewerTermInResponse, new_term);
const int res = persistent_state_.setCurrentTerm(new_term);
if (res < 0)
{
trace(TraceRaftPersistStateUpdateError, res);
}
registerActivity(); // Deferring future elections
switchState(ServerStateFollower);
}
void propagateCommitIndex()
{
// Objective is to estimate whether we can safely increment commit index value
UAVCAN_ASSERT(server_state_ == ServerStateLeader);
UAVCAN_ASSERT(commit_index_ <= persistent_state_.getLog().getLastIndex());
if (commit_index_ < persistent_state_.getLog().getLastIndex())
{
/*
* Not all local entries are committed.
* Deciding if it is safe to increment commit index.
*/
uint8_t num_nodes_with_next_log_entry_available = 1; // Local node
for (uint8_t i = 0; i < cluster_.getNumKnownServers(); i++)
{
const Log::Index match_index = cluster_.getServerMatchIndex(cluster_.getRemoteServerNodeIDAtIndex(i));
if (match_index > commit_index_)
{
num_nodes_with_next_log_entry_available++;
}
}
if (num_nodes_with_next_log_entry_available >= cluster_.getQuorumSize())
{
commit_index_++;
UAVCAN_ASSERT(commit_index_ > 0); // Index 0 is always committed
trace(TraceRaftNewEntryCommitted, commit_index_);
// AT THIS POINT ALLOCATION IS COMPLETE
leader_monitor_.handleLogCommitOnLeader(*persistent_state_.getLog().getEntryAtIndex(commit_index_));
}
}
}
void handleAppendEntriesRequest(const ReceivedDataStructure<AppendEntries::Request>& request,
ServiceResponseDataStructure<AppendEntries::Response>& response)
{
checkInvariants();
if (!cluster_.isKnownServer(request.getSrcNodeID()))
{
if (cluster_.isClusterDiscovered())
{
trace(TraceRaftRequestIgnored, request.getSrcNodeID().get());
response.setResponseEnabled(false);
return;
}
else
{
cluster_.addServer(request.getSrcNodeID());
}
}
UAVCAN_ASSERT(response.isResponseEnabled()); // This is default
/*
* Checking if our current state is up to date.
* The request will be ignored if persistent state cannot be updated.
*/
if (request.term > persistent_state_.getCurrentTerm())
{
int res = persistent_state_.setCurrentTerm(request.term);
if (res < 0)
{
handlePersistentStateUpdateError(res);
response.setResponseEnabled(false);
return;
}
res = persistent_state_.resetVotedFor();
if (res < 0)
{
handlePersistentStateUpdateError(res);
response.setResponseEnabled(false);
return;
}
}
/*
* Preparing the response
*/
response.term = persistent_state_.getCurrentTerm();
response.success = false;
/*
* Step 1 (see Raft paper)
* Reject the request if the leader has stale term number.
*/
if (request.term < persistent_state_.getCurrentTerm())
{
response.setResponseEnabled(true);
return;
}
registerActivity();
switchState(ServerStateFollower);
/*
* Step 2
* Reject the request if the assumed log index does not exist on the local node.
*/
const Entry* const prev_entry = persistent_state_.getLog().getEntryAtIndex(request.prev_log_index);
if (prev_entry == NULL)
{
response.setResponseEnabled(true);
return;
}
/*
* Step 3
* Drop log entries if term number does not match.
* Ignore the request if the persistent state cannot be updated.
*/
if (prev_entry->term != request.prev_log_term)
{
const int res = persistent_state_.getLog().removeEntriesWhereIndexGreaterOrEqual(request.prev_log_index);
response.setResponseEnabled(res >= 0);
if (res < 0)
{
trace(TraceRaftPersistStateUpdateError, res);
}
return;
}
/*
* Step 4
* Update the log with new entries - this will possibly require to rewrite existing entries.
* Ignore the request if the persistent state cannot be updated.
*/
if (request.prev_log_index != persistent_state_.getLog().getLastIndex())
{
const int res = persistent_state_.getLog().removeEntriesWhereIndexGreater(request.prev_log_index);
if (res < 0)
{
trace(TraceRaftPersistStateUpdateError, res);
response.setResponseEnabled(false);
return;
}
}
for (uint8_t i = 0; i < request.entries.size(); i++)
{
const int res = persistent_state_.getLog().append(request.entries[i]);
if (res < 0)
{
trace(TraceRaftPersistStateUpdateError, res);
response.setResponseEnabled(false);
return; // Response will not be sent, the server will assume that we're dead
}
}
/*
* Step 5
* Update the commit index.
*/
if (request.leader_commit > commit_index_)
{
commit_index_ = min(request.leader_commit, persistent_state_.getLog().getLastIndex());
trace(TraceRaftCommitIndexUpdate, commit_index_);
}
response.setResponseEnabled(true);
response.success = true;
}
void handleAppendEntriesResponse(const ServiceCallResult<AppendEntries>& result)
{
UAVCAN_ASSERT(server_state_ == ServerStateLeader); // When state switches, all requests must be cancelled
checkInvariants();
if (!result.isSuccessful())
{
return;
}
if (result.getResponse().term > persistent_state_.getCurrentTerm())
{
tryIncrementCurrentTermFromResponse(result.getResponse().term);
}
else
{
if (result.getResponse().success)
{
cluster_.incrementServerNextIndexBy(result.getCallID().server_node_id,
pending_append_entries_fields_.num_entries);
cluster_.setServerMatchIndex(result.getCallID().server_node_id,
Log::Index(pending_append_entries_fields_.prev_log_index +
pending_append_entries_fields_.num_entries));
}
else
{
cluster_.decrementServerNextIndex(result.getCallID().server_node_id);
trace(TraceRaftAppendEntriesRespUnsucfl, result.getCallID().server_node_id.get());
}
}
pending_append_entries_fields_ = PendingAppendEntriesFields();
// Rest of the logic is implemented in periodic update handlers.
}
void handleRequestVoteRequest(const ReceivedDataStructure<RequestVote::Request>& request,
ServiceResponseDataStructure<RequestVote::Response>& response)
{
checkInvariants();
trace(TraceRaftVoteRequestReceived, request.getSrcNodeID().get());
if (!cluster_.isKnownServer(request.getSrcNodeID()))
{
trace(TraceRaftRequestIgnored, request.getSrcNodeID().get());
response.setResponseEnabled(false);
return;
}
UAVCAN_ASSERT(response.isResponseEnabled()); // This is default
/*
* Checking if our current state is up to date.
* The request will be ignored if persistent state cannot be updated.
*/
if (request.term > persistent_state_.getCurrentTerm())
{
switchState(ServerStateFollower); // Our term is stale, so we can't serve as leader
int res = persistent_state_.setCurrentTerm(request.term);
if (res < 0)
{
handlePersistentStateUpdateError(res);
response.setResponseEnabled(false);
return;
}
res = persistent_state_.resetVotedFor();
if (res < 0)
{
handlePersistentStateUpdateError(res);
response.setResponseEnabled(false);
return;
}
}
/*
* Preparing the response
*/
response.term = persistent_state_.getCurrentTerm();
if (request.term < response.term)
{
response.vote_granted = false;
}
else
{
const bool can_vote = !persistent_state_.isVotedForSet() ||
(persistent_state_.getVotedFor() == request.getSrcNodeID());
const bool log_is_up_to_date =
persistent_state_.getLog().isOtherLogUpToDate(request.last_log_index, request.last_log_term);
response.vote_granted = can_vote && log_is_up_to_date;
if (response.vote_granted)
{
switchState(ServerStateFollower); // Avoiding race condition when Candidate
registerActivity(); // This is necessary to avoid excessive elections
const int res = persistent_state_.setVotedFor(request.getSrcNodeID());
if (res < 0)
{
trace(TraceRaftPersistStateUpdateError, res);
response.setResponseEnabled(false);
return;
}
}
}
}
void handleRequestVoteResponse(const ServiceCallResult<RequestVote>& result)
{
UAVCAN_ASSERT(server_state_ == ServerStateCandidate); // When state switches, all requests must be cancelled
checkInvariants();
if (!result.isSuccessful())
{
return;
}
trace(TraceRaftVoteRequestSucceeded, result.getCallID().server_node_id.get());
if (result.getResponse().term > persistent_state_.getCurrentTerm())
{
tryIncrementCurrentTermFromResponse(result.getResponse().term);
}
else
{
if (result.getResponse().vote_granted)
{
num_votes_received_in_this_campaign_++;
}
}
// Rest of the logic is implemented in periodic update handlers.
// I'm no fan of asynchronous programming. At all.
}
virtual void handleTimerEvent(const TimerEvent&)
{
checkInvariants();
switch (server_state_)
{
case ServerStateFollower:
{
updateFollower();
break;
}
case ServerStateCandidate:
{
updateCandidate();
break;
}
case ServerStateLeader:
{
updateLeader();
break;
}
default:
{
UAVCAN_ASSERT(0);
break;
}
}
}
public:
RaftCore(INode& node,
IStorageBackend& storage,
IEventTracer& tracer,
IRaftLeaderMonitor& leader_monitor)
: TimerBase(node)
, tracer_(tracer)
, leader_monitor_(leader_monitor)
, persistent_state_(storage, tracer)
, cluster_(node, storage, persistent_state_.getLog(), tracer)
, commit_index_(0) // Per Raft paper, commitIndex must be initialized to zero
, last_activity_timestamp_(node.getMonotonicTime())
, randomized_activity_timeout_(
MonotonicDuration::fromMSec(AppendEntries::Request::DEFAULT_MAX_ELECTION_TIMEOUT_MS))
, server_state_(ServerStateFollower)
, next_server_index_(0)
, num_votes_received_in_this_campaign_(0)
, append_entries_srv_(node)
, append_entries_client_(node)
, request_vote_srv_(node)
, request_vote_client_(node)
{ }
/**
* Once started, the logic runs in the background until destructor is called.
* @param cluster_size If set, this value will be used and stored in the persistent storage. If not set,
* value from the persistent storage will be used. If not set and there's no such key
* in the persistent storage, initialization will fail.
*/
int init(uint8_t cluster_size = ClusterManager::ClusterSizeUnknown)
{
/*
* Initializing state variables
*/
server_state_ = ServerStateFollower;
next_server_index_ = 0;
num_votes_received_in_this_campaign_ = 0;
commit_index_ = 0;
registerActivity();
/*
* Initializing internals
*/
int res = persistent_state_.init();
if (res < 0)
{
return res;
}
res = cluster_.init(cluster_size);
if (res < 0)
{
return res;
}
res = append_entries_srv_.start(AppendEntriesCallback(this, &RaftCore::handleAppendEntriesRequest));
if (res < 0)
{
return res;
}
res = request_vote_srv_.start(RequestVoteCallback(this, &RaftCore::handleRequestVoteRequest));
if (res < 0)
{
return res;
}
res = append_entries_client_.init();
if (res < 0)
{
return res;
}
append_entries_client_.setCallback(AppendEntriesResponseCallback(this,
&RaftCore::handleAppendEntriesResponse));
res = request_vote_client_.init();
if (res < 0)
{
return res;
}
request_vote_client_.setCallback(RequestVoteResponseCallback(this, &RaftCore::handleRequestVoteResponse));
/*
* Initializing timing constants
* Refer to the specification for the formula
*/
const uint8_t num_followers = static_cast<uint8_t>(cluster_.getClusterSize() - 1);
const MonotonicDuration update_interval =
MonotonicDuration::fromMSec(AppendEntries::Request::DEFAULT_MIN_ELECTION_TIMEOUT_MS /
2 / max(static_cast<uint8_t>(2), num_followers));
UAVCAN_TRACE("dynamic_node_id_server::distributed::RaftCore",
"Update interval: %ld msec", static_cast<long>(update_interval.toMSec()));
append_entries_client_.setRequestTimeout(min(append_entries_client_.getDefaultRequestTimeout(),
update_interval));
request_vote_client_.setRequestTimeout(min(request_vote_client_.getDefaultRequestTimeout(),
update_interval));
startPeriodic(update_interval);
trace(TraceRaftCoreInited, update_interval.toUSec());
UAVCAN_ASSERT(res >= 0);
return 0;
}
/**
* This function is mostly needed for testing.
*/
Log::Index getCommitIndex() const { return commit_index_; }
/**
* This essentially indicates whether the server could replicate log since last allocation.
*/
bool areAllLogEntriesCommitted() const { return commit_index_ == persistent_state_.getLog().getLastIndex(); }
/**
* Only the leader can call @ref appendLog().
*/
bool isLeader() const { return server_state_ == ServerStateLeader; }
/**
* Inserts one entry into log.
* This method will trigger an assertion failure and return error if the current node is not the leader.
* If operation fails, the node may give up its Leader status.
*/
void appendLog(const Entry::FieldTypes::unique_id& unique_id, NodeID node_id)
{
if (isLeader())
{
Entry entry;
entry.node_id = node_id.get();
entry.unique_id = unique_id;
entry.term = persistent_state_.getCurrentTerm();
trace(TraceRaftNewLogEntry, entry.node_id);
const int res = persistent_state_.getLog().append(entry);
if (res < 0)
{
handlePersistentStateUpdateError(res);
}
}
else
{
UAVCAN_ASSERT(0);
}
}
/**
* This class is used to perform log searches.
*/
struct LogEntryInfo
{
Entry entry;
bool committed;
LogEntryInfo(const Entry& arg_entry, bool arg_committed)
: entry(arg_entry)
, committed(arg_committed)
{ }
};
/**
* This method is used by the allocator to query existence of certain entries in the Raft log.
* Predicate is a callable of the following prototype:
* bool (const LogEntryInfo& entry)
* Once the predicate returns true, the loop will be terminated and the method will return an initialized lazy
* contructor with the last visited entry; otherwise the constructor will not be initialized.
* In this case, lazy constructor is used as boost::optional.
* The log is always traversed from HIGH to LOW index values, i.e. entry 0 will be traversed last.
*/
template <typename Predicate>
inline LazyConstructor<LogEntryInfo> traverseLogFromEndUntil(const Predicate& predicate) const
{
UAVCAN_ASSERT(try_implicit_cast<bool>(predicate, true));
for (int index = static_cast<int>(persistent_state_.getLog().getLastIndex()); index >= 0; index--)
{
const Entry* const entry = persistent_state_.getLog().getEntryAtIndex(Log::Index(index));
UAVCAN_ASSERT(entry != NULL);
const LogEntryInfo info(*entry, Log::Index(index) <= commit_index_);
if (predicate(info))
{
LazyConstructor<LogEntryInfo> ret;
ret.template construct<const LogEntryInfo&>(info);
return ret;
}
}
return LazyConstructor<LogEntryInfo>();
}
Log::Index getNumAllocations() const
{
// Remember that index zero contains a special-purpose entry that doesn't count as allocation
return persistent_state_.getLog().getLastIndex();
}
/**
* These accessors are needed for debugging, visualization and testing.
*/
const PersistentState& getPersistentState() const { return persistent_state_; }
const ClusterManager& getClusterManager() const { return cluster_; }
MonotonicTime getLastActivityTimestamp() const { return last_activity_timestamp_; }
ServerState getServerState() const { return server_state_; }
MonotonicDuration getUpdateInterval() const { return getPeriod(); }
MonotonicDuration getRandomizedTimeout() const { return randomized_activity_timeout_; }
};
}
}
}
#endif // Include guard
libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp
+375
View File
@@ -0,0 +1,375 @@
/*
* Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_SERVER_HPP_INCLUDED
#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_SERVER_HPP_INCLUDED
#include <uavcan/build_config.hpp>
#include <uavcan/debug.hpp>
#include <uavcan/protocol/dynamic_node_id_server/distributed/types.hpp>
#include <uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp>
#include <uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp>
#include <uavcan/protocol/dynamic_node_id_server/node_id_selector.hpp>
#include <uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp>
#include <uavcan/protocol/dynamic_node_id_server/event.hpp>
namespace uavcan
{
namespace dynamic_node_id_server
{
namespace distributed
{
/**
* This class implements the top-level allocation logic and server API.
*/
class UAVCAN_EXPORT Server : IAllocationRequestHandler
, INodeDiscoveryHandler
, IRaftLeaderMonitor
{
struct UniqueIDLogPredicate
{
const UniqueID unique_id;
UniqueIDLogPredicate(const UniqueID& uid)
: unique_id(uid)
{ }
bool operator()(const RaftCore::LogEntryInfo& info) const
{
return info.entry.unique_id == unique_id;
}
};
struct NodeIDLogPredicate
{
const NodeID node_id;
NodeIDLogPredicate(const NodeID& nid)
: node_id(nid)
{ }
bool operator()(const RaftCore::LogEntryInfo& info) const
{
return info.entry.node_id == node_id.get();
}
};
/*
* Constants
*/
UniqueID own_unique_id_;
/*
* States
*/
INode& node_;
IEventTracer& tracer_;
RaftCore raft_core_;
AllocationRequestManager allocation_request_manager_;
NodeDiscoverer node_discoverer_;
/*
* Methods of IAllocationRequestHandler
*/
virtual bool canPublishFollowupAllocationResponse() const
{
/*
* The server is allowed to publish follow-up allocation responses only if both conditions are met:
* - The server is leader.
* - The last allocation request has been completed successfully.
*
* Why second condition? Imagine a case when there's two Raft nodes that don't hear each other - A and B,
* both of them are leaders (but only A can commit to the log, B is in a minor partition); then there's a
* client X that can exchange with both leaders, and a client Y that can exchange only with A. Such a
* situation can occur in case of a very unlikely failure of redundant interfaces.
*
* Both clients X and Y initially send a first-stage Allocation request; A responds to Y with a first-stage
* response, whereas B responds to X. Both X and Y will issue a follow-up second-stage requests, which may
* cause A to mix second-stage Allocation requests from different nodes, leading to reception of an invalid
* unique ID. When both leaders receive full unique IDs (A will receive an invalid one, B will receive a valid
* unique ID of X), only A will be able to make a commit, because B is in a minority. Since both clients were
* unable to receive node ID values in this round, they will try again later.
*
* Now, in order to prevent B from disrupting client-server communication second time around, we introduce this
* second restriction: the server cannot exchange with clients as long as its log contains uncommitted entries.
*
* Note that this restriction does not apply to allocation requests sent via CAN FD frames, as in this case
* no follow-up responses are necessary. So only CAN FD can offer reliable Allocation exchange.
*/
return raft_core_.isLeader() && raft_core_.areAllLogEntriesCommitted();
}
virtual void handleAllocationRequest(const UniqueID& unique_id, const NodeID preferred_node_id)
{
/*
* Note that it is possible that the local node is not leader. We will still perform the log search
* and try to find the node that requested allocation. If the node is found, response will be sent;
* otherwise the request will be ignored because only leader can add new allocations.
*/
const LazyConstructor<RaftCore::LogEntryInfo> result =
raft_core_.traverseLogFromEndUntil(UniqueIDLogPredicate(unique_id));
if (result.isConstructed())
{
if (result->committed)
{
tryPublishAllocationResult(result->entry);
UAVCAN_TRACE("dynamic_node_id_server::distributed::Server",
"Allocation request served with existing allocation; node ID %d",
int(result->entry.node_id));
}
else
{
UAVCAN_TRACE("dynamic_node_id_server::distributed::Server",
"Allocation request ignored - allocation exists but not committed yet; node ID %d",
int(result->entry.node_id));
}
}
else
{
if (raft_core_.isLeader() && !node_discoverer_.hasUnknownNodes())
{
allocateNewNode(unique_id, preferred_node_id);
}
}
}
/*
* Methods of INodeDiscoveryHandler
*/
virtual bool canDiscoverNewNodes() const
{
return raft_core_.isLeader();
}
virtual NodeAwareness checkNodeAwareness(NodeID node_id) const
{
const LazyConstructor<RaftCore::LogEntryInfo> result =
raft_core_.traverseLogFromEndUntil(NodeIDLogPredicate(node_id));
if (result.isConstructed())
{
return result->committed ? NodeAwarenessKnownAndCommitted : NodeAwarenessKnownButNotCommitted;
}
else
{
return NodeAwarenessUnknown;
}
}
virtual void handleNewNodeDiscovery(const UniqueID* unique_id_or_null, NodeID node_id)
{
if (raft_core_.traverseLogFromEndUntil(NodeIDLogPredicate(node_id)).isConstructed())
{
UAVCAN_ASSERT(0); // Such node is already known, the class that called this method should have known that
return;
}
const UniqueID uid = (unique_id_or_null == NULL) ? UniqueID() : *unique_id_or_null;
if (raft_core_.isLeader())
{
raft_core_.appendLog(uid, node_id);
}
}
/*
* Methods of IRaftLeaderMonitor
*/
virtual void handleLogCommitOnLeader(const protocol::dynamic_node_id::server::Entry& entry)
{
/*
* Maybe this node did not request allocation at all, we don't care, we publish anyway.
*/
tryPublishAllocationResult(entry);
}
virtual void handleLocalLeadershipChange(bool local_node_is_leader)
{
if (!local_node_is_leader)
{
return;
}
const LazyConstructor<RaftCore::LogEntryInfo> result =
raft_core_.traverseLogFromEndUntil(NodeIDLogPredicate(node_.getNodeID()));
if (!result.isConstructed())
{
raft_core_.appendLog(own_unique_id_, node_.getNodeID());
}
}
/*
* Private methods
*/
bool isNodeIDTaken(const NodeID node_id) const
{
UAVCAN_TRACE("dynamic_node_id_server::distributed::Server",
"Testing if node ID %d is taken", int(node_id.get()));
return raft_core_.traverseLogFromEndUntil(NodeIDLogPredicate(node_id));
}
void allocateNewNode(const UniqueID& unique_id, const NodeID preferred_node_id)
{
const NodeID allocated_node_id =
NodeIDSelector<Server>(this, &Server::isNodeIDTaken).findFreeNodeID(preferred_node_id);
if (!allocated_node_id.isUnicast())
{
UAVCAN_TRACE("dynamic_node_id_server::distributed::Server", "Request ignored - no free node ID left");
return;
}
UAVCAN_TRACE("dynamic_node_id_server::distributed::Server", "New node ID allocated: %d",
int(allocated_node_id.get()));
raft_core_.appendLog(unique_id, allocated_node_id);
}
void tryPublishAllocationResult(const protocol::dynamic_node_id::server::Entry& entry)
{
const int res = allocation_request_manager_.broadcastAllocationResponse(entry.unique_id, entry.node_id);
if (res < 0)
{
tracer_.onEvent(TraceError, res);
node_.registerInternalFailure("Dynamic allocation response");
}
}
public:
Server(INode& node,
IStorageBackend& storage,
IEventTracer& tracer)
: node_(node)
, tracer_(tracer)
, raft_core_(node, storage, tracer, *this)
, allocation_request_manager_(node, tracer, *this)
, node_discoverer_(node, tracer, *this)
{ }
int init(const UniqueID& own_unique_id, const uint8_t cluster_size = ClusterManager::ClusterSizeUnknown)
{
/*
* Initializing Raft core first, because the next step requires Log to be loaded
*/
int res = raft_core_.init(cluster_size);
if (res < 0)
{
return res;
}
/*
* Making sure that the server is started with the same node ID
*/
own_unique_id_ = own_unique_id;
const LazyConstructor<RaftCore::LogEntryInfo> own_log_entry =
raft_core_.traverseLogFromEndUntil(NodeIDLogPredicate(node_.getNodeID()));
if (own_log_entry.isConstructed())
{
if (own_log_entry->entry.unique_id != own_unique_id_)
{
return -ErrInvalidConfiguration;
}
}
/*
* Misc
*/
res = allocation_request_manager_.init();
if (res < 0)
{
return res;
}
res = node_discoverer_.init();
if (res < 0)
{
return res;
}
return 0;
}
Log::Index getNumAllocations() const { return raft_core_.getNumAllocations(); }
/**
* These accessors are needed for debugging, visualization and testing.
*/
const RaftCore& getRaftCore() const { return raft_core_; }
const NodeDiscoverer& getNodeDiscoverer() const { return node_discoverer_; }
};
/**
* This structure represents immediate state of the server.
* It can be used for state visualization and debugging.
*/
struct StateReport
{
uint8_t cluster_size;
RaftCore::ServerState state;
Log::Index last_log_index;
Log::Index commit_index;
Term last_log_term;
Term current_term;
NodeID voted_for;
MonotonicTime last_activity_timestamp;
MonotonicDuration randomized_timeout;
uint8_t num_unknown_nodes;
struct FollowerState
{
NodeID node_id;
Log::Index next_index;
Log::Index match_index;
FollowerState()
: next_index(0)
, match_index(0)
{ }
} followers[ClusterManager::MaxClusterSize - 1];
StateReport(const Server& s)
: cluster_size (s.getRaftCore().getClusterManager().getClusterSize())
, state (s.getRaftCore().getServerState())
, last_log_index (s.getRaftCore().getPersistentState().getLog().getLastIndex())
, commit_index (s.getRaftCore().getCommitIndex())
, last_log_term (0) // See below
, current_term (s.getRaftCore().getPersistentState().getCurrentTerm())
, voted_for (s.getRaftCore().getPersistentState().getVotedFor())
, last_activity_timestamp(s.getRaftCore().getLastActivityTimestamp())
, randomized_timeout (s.getRaftCore().getRandomizedTimeout())
, num_unknown_nodes (s.getNodeDiscoverer().getNumUnknownNodes())
{
const Entry* const e = s.getRaftCore().getPersistentState().getLog().getEntryAtIndex(last_log_index);
UAVCAN_ASSERT(e != NULL);
if (e != NULL)
{
last_log_term = e->term;
}
for (uint8_t i = 0; i < (cluster_size - 1U); i++)
{
const ClusterManager& mgr = s.getRaftCore().getClusterManager();
const NodeID node_id = mgr.getRemoteServerNodeIDAtIndex(i);
if (node_id.isUnicast())
{
followers[i].node_id = node_id;
followers[i].next_index = mgr.getServerNextIndex(node_id);
followers[i].match_index = mgr.getServerMatchIndex(node_id);
}
}
}
};
}
}
}
#endif // Include guard
libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/types.hpp
+29
View File
@@ -0,0 +1,29 @@
/*
* Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_TYPES_HPP_INCLUDED
#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_TYPES_HPP_INCLUDED
#include <uavcan/build_config.hpp>
#include <uavcan/protocol/dynamic_node_id_server/types.hpp>
namespace uavcan
{
namespace dynamic_node_id_server
{
namespace distributed
{
using namespace ::uavcan::protocol::dynamic_node_id::server;
/**
* Raft term
*/
typedef StorageType<Entry::FieldTypes::term>::Type Term;
}
}
}
#endif // Include guard
libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp
+175
View File
@@ -0,0 +1,175 @@
/*
* Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_EVENT_HPP_INCLUDED
#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_EVENT_HPP_INCLUDED
#include <uavcan/build_config.hpp>
#include <uavcan/protocol/dynamic_node_id_server/types.hpp>
namespace uavcan
{
namespace dynamic_node_id_server
{
/**
* @ref IEventTracer.
* Event codes cannot be changed, only new ones can be added.
*/
enum UAVCAN_EXPORT TraceCode
{
// Event name Argument
// 0
TraceError, // error code (may be negated)
TraceRaftLogLastIndexRestored, // recovered last index value
TraceRaftLogAppend, // index of new entry
TraceRaftLogRemove, // new last index value
TraceRaftCurrentTermRestored, // current term
// 5
TraceRaftCurrentTermUpdate, // current term
TraceRaftVotedForRestored, // value of votedFor
TraceRaftVotedForUpdate, // value of votedFor
TraceRaftDiscoveryBroadcast, // number of known servers
TraceRaftNewServerDiscovered, // node ID of the new server
// 10
TraceRaftDiscoveryReceived, // node ID of the sender
TraceRaftClusterSizeInited, // cluster size
TraceRaftBadClusterSizeReceived, // received cluster size
TraceRaftCoreInited, // update interval in usec
TraceRaftStateSwitch, // 0 - Follower, 1 - Candidate, 2 - Leader
// 15
Trace0,
TraceRaftNewLogEntry, // node ID value
TraceRaftRequestIgnored, // node ID of the client
TraceRaftVoteRequestReceived, // node ID of the client
TraceRaftVoteRequestSucceeded, // node ID of the server
// 20
TraceRaftVoteRequestInitiation, // node ID of the server
TraceRaftPersistStateUpdateError, // negative error code
TraceRaftCommitIndexUpdate, // new commit index value
TraceRaftNewerTermInResponse, // new term value
TraceRaftNewEntryCommitted, // new commit index value
// 25
TraceRaftAppendEntriesCallFailure, // error code (may be negated)
TraceRaftElectionComplete, // number of votes collected
TraceRaftAppendEntriesRespUnsucfl, // node ID of the client
Trace2,
Trace3,
// 30
TraceAllocationFollowupResponse, // number of unique ID bytes in this response
TraceAllocationFollowupDenied, // reason code (see sources for details)
TraceAllocationFollowupTimeout, // timeout value in microseconds
TraceAllocationBadRequest, // number of unique ID bytes in this request
TraceAllocationUnexpectedStage, // stage number in the request - 1, 2, or 3
// 35
TraceAllocationRequestAccepted, // number of bytes of unique ID after request
TraceAllocationExchangeComplete, // first 8 bytes of unique ID interpreted as signed 64 bit big endian
TraceAllocationResponse, // allocated node ID
TraceAllocationActivity, // source node ID of the message
Trace12,
// 40
TraceDiscoveryNewNodeFound, // node ID
TraceDiscoveryCommitCacheUpdated, // node ID marked as committed
TraceDiscoveryNodeFinalized, // node ID in lower 7 bits, bit 8 (256, 0x100) is set if unique ID is known
TraceDiscoveryGetNodeInfoFailure, // node ID
TraceDiscoveryTimerStart, // interval in microseconds
// 45
TraceDiscoveryTimerStop, // reason code (see sources for details)
TraceDiscoveryGetNodeInfoRequest, // target node ID
TraceDiscoveryNodeRestartDetected, // node ID
TraceDiscoveryNodeRemoved, // node ID
Trace22,
// 50
NumTraceCodes
};
/**
* This interface allows the application to trace events that happen in the server.
*/
class UAVCAN_EXPORT IEventTracer
{
public:
#if UAVCAN_TOSTRING
/**
* It is safe to call this function with any argument.
* If the event code is out of range, an assertion failure will be triggered and an error text will be returned.
*/
static const char* getEventName(TraceCode code)
{
// import re;m = lambda s:',\n'.join('"%s"' % x for x in re.findall(r'\ {4}Trace[0-9]*([A-Za-z0-9]*),',s))
static const char* const Strings[] =
{
"Error",
"RaftLogLastIndexRestored",
"RaftLogAppend",
"RaftLogRemove",
"RaftCurrentTermRestored",
"RaftCurrentTermUpdate",
"RaftVotedForRestored",
"RaftVotedForUpdate",
"RaftDiscoveryBroadcast",
"RaftNewServerDiscovered",
"RaftDiscoveryReceived",
"RaftClusterSizeInited",
"RaftBadClusterSizeReceived",
"RaftCoreInited",
"RaftStateSwitch",
"",
"RaftNewLogEntry",
"RaftRequestIgnored",
"RaftVoteRequestReceived",
"RaftVoteRequestSucceeded",
"RaftVoteRequestInitiation",
"RaftPersistStateUpdateError",
"RaftCommitIndexUpdate",
"RaftNewerTermInResponse",
"RaftNewEntryCommitted",
"RaftAppendEntriesCallFailure",
"RaftElectionComplete",
"RaftAppendEntriesRespUnsucfl",
"",
"",
"AllocationFollowupResponse",
"AllocationFollowupDenied",
"AllocationFollowupTimeout",
"AllocationBadRequest",
"AllocationUnexpectedStage",
"AllocationRequestAccepted",
"AllocationExchangeComplete",
"AllocationResponse",
"AllocationActivity",
"",
"DiscoveryNewNodeFound",
"DiscoveryCommitCacheUpdated",
"DiscoveryNodeFinalized",
"DiscoveryGetNodeInfoFailure",
"DiscoveryTimerStart",
"DiscoveryTimerStop",
"DiscoveryGetNodeInfoRequest",
"DiscoveryNodeRestartDetected",
"DiscoveryNodeRemoved",
""
};
uavcan::StaticAssert<sizeof(Strings) / sizeof(Strings[0]) == NumTraceCodes>::check();
UAVCAN_ASSERT(code < NumTraceCodes);
return (code < NumTraceCodes) ? Strings[static_cast<unsigned>(code)] : "INVALID_EVENT_CODE";
}
#endif
/**
* The server invokes this method every time it believes that a noteworthy event has happened.
* It is guaranteed that event code values will never change, but new ones can be added in future. This ensures
* full backward compatibility.
* @param event_code Event code, see the sources for the enum with values.
* @param event_argument Value associated with the event; its meaning depends on the event code.
*/
virtual void onEvent(TraceCode event_code, int64_t event_argument) = 0;
virtual ~IEventTracer() { }
};
}
}
#endif // Include guard
libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp
+350
View File
@@ -0,0 +1,350 @@
/*
* Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_NODE_DISCOVERER_HPP_INCLUDED
#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_NODE_DISCOVERER_HPP_INCLUDED
#include <uavcan/build_config.hpp>
#include <uavcan/util/map.hpp>
#include <uavcan/util/method_binder.hpp>
#include <uavcan/util/bitset.hpp>
#include <uavcan/node/timer.hpp>
#include <uavcan/node/subscriber.hpp>
#include <uavcan/node/service_client.hpp>
#include <uavcan/protocol/dynamic_node_id_server/types.hpp>
#include <uavcan/protocol/dynamic_node_id_server/event.hpp>
#include <cassert>
// UAVCAN types
#include <uavcan/protocol/NodeStatus.hpp>
#include <uavcan/protocol/GetNodeInfo.hpp>
namespace uavcan
{
namespace dynamic_node_id_server
{
/**
* The allocator must implement this interface.
*/
class INodeDiscoveryHandler
{
public:
/**
* In order to avoid bus congestion, normally only leader can discover new nodes.
*/
virtual bool canDiscoverNewNodes() const = 0;
/**
* These values represent the level of awareness of a certain node by the server.
*/
enum NodeAwareness
{
NodeAwarenessUnknown,
NodeAwarenessKnownButNotCommitted,
NodeAwarenessKnownAndCommitted
};
/**
* It is OK to do full log traversal in this method, because the unique ID collector will cache the
* result when possible.
*/
virtual NodeAwareness checkNodeAwareness(NodeID node_id) const = 0;
/**
* This method will be called when a new node responds to GetNodeInfo request.
* If this method fails to register the node, the node will be queried again later and this method will be
* invoked again.
* Unique ID will be NULL if the node is assumed to not implement the GetNodeInfo service.
*/
virtual void handleNewNodeDiscovery(const UniqueID* unique_id_or_null, NodeID node_id) = 0;
virtual ~INodeDiscoveryHandler() { }
};
/**
* This class listens to NodeStatus messages from other nodes and retrieves their unique ID if they are not
* known to the allocator.
*/
class NodeDiscoverer : TimerBase
{
typedef MethodBinder<NodeDiscoverer*, void (NodeDiscoverer::*)(const ServiceCallResult<protocol::GetNodeInfo>&)>
GetNodeInfoResponseCallback;
typedef MethodBinder<NodeDiscoverer*, void (NodeDiscoverer::*)(const ReceivedDataStructure<protocol::NodeStatus>&)>
NodeStatusCallback;
struct NodeData
{
uint32_t last_seen_uptime;
uint8_t num_get_node_info_attempts;
NodeData()
: last_seen_uptime(0)
, num_get_node_info_attempts(0)
{ }
};
typedef Map<NodeID, NodeData, 10> NodeMap;
/**
* When this number of attempts has been made, the discoverer will give up and assume that the node
* does not implement this service.
*/
enum { MaxAttemptsToGetNodeInfo = 5 };
enum { TimerPollIntervalMs = 170 }; // ~ ceil(500 ms service timeout / 3)
enum { NumNodeStatusStaticReceivers = 64 };
/*
* States
*/
INodeDiscoveryHandler& handler_;
IEventTracer& tracer_;
BitSet<NodeID::Max + 1> committed_node_mask_; ///< Nodes that are marked will not be queried
NodeMap node_map_; ///< Will not work in UAVCAN_TINY
ServiceClient<protocol::GetNodeInfo, GetNodeInfoResponseCallback> get_node_info_client_;
Subscriber<protocol::NodeStatus, NodeStatusCallback, NumNodeStatusStaticReceivers, 0> node_status_sub_;
/*
* Methods
*/
void trace(TraceCode code, int64_t argument) { tracer_.onEvent(code, argument); }
INode& getNode() { return node_status_sub_.getNode(); }
void removeNode(const NodeID node_id)
{
node_map_.remove(node_id);
trace(TraceDiscoveryNodeRemoved, node_id.get());
}
NodeID pickNextNodeToQuery() const
{
// This essentially means that we pick first available node. Remember that the map is unordered.
const NodeMap::KVPair* const pair = node_map_.getByIndex(0);
return (pair == NULL) ? NodeID() : pair->key;
}
bool needToQuery(NodeID node_id)
{
UAVCAN_ASSERT(node_id.isUnicast());
/*
* Fast check
*/
if (committed_node_mask_[node_id.get()])
{
return false;
}
/*
* Slow check - may involve full log search
*/
const INodeDiscoveryHandler::NodeAwareness awareness = handler_.checkNodeAwareness(node_id);
if (awareness == INodeDiscoveryHandler::NodeAwarenessUnknown)
{
return true;
}
else if (awareness == INodeDiscoveryHandler::NodeAwarenessKnownButNotCommitted)
{
removeNode(node_id);
return false;
}
else if (awareness == INodeDiscoveryHandler::NodeAwarenessKnownAndCommitted)
{
trace(TraceDiscoveryCommitCacheUpdated, node_id.get());
committed_node_mask_[node_id.get()] = true;
removeNode(node_id);
return false;
}
else
{
UAVCAN_ASSERT(0);
return false;
}
}
NodeID pickNextNodeToQueryAndCleanupMap()
{
NodeID node_id;
do
{
node_id = pickNextNodeToQuery();
if (node_id.isUnicast())
{
if (needToQuery(node_id))
{
return node_id;
}
else
{
removeNode(node_id);
}
}
}
while (node_id.isUnicast());
return NodeID();
}
void finalizeNodeDiscovery(const UniqueID* unique_id_or_null, NodeID node_id)
{
trace(TraceDiscoveryNodeFinalized, node_id.get() | ((unique_id_or_null == NULL) ? 0U : 0x100U));
removeNode(node_id);
/*
* It is paramount to check if the server is still interested to receive this data.
* Otherwise, if the node appeared in the log while we were waiting for response, we'd end up with
* duplicate node ID in the log.
*/
if (needToQuery(node_id))
{
handler_.handleNewNodeDiscovery(unique_id_or_null, node_id);
}
}
void handleGetNodeInfoResponse(const ServiceCallResult<protocol::GetNodeInfo>& result)
{
if (result.isSuccessful())
{
UAVCAN_TRACE("dynamic_node_id_server::NodeDiscoverer", "GetNodeInfo response from %d",
int(result.getCallID().server_node_id.get()));
finalizeNodeDiscovery(&result.getResponse().hardware_version.unique_id, result.getCallID().server_node_id);
}
else
{
trace(TraceDiscoveryGetNodeInfoFailure, result.getCallID().server_node_id.get());
NodeData* const data = node_map_.access(result.getCallID().server_node_id);
if (data == NULL)
{
return; // Probably it is a known node now
}
UAVCAN_TRACE("dynamic_node_id_server::NodeDiscoverer",
"GetNodeInfo request to %d has timed out, %d attempts",
int(result.getCallID().server_node_id.get()), int(data->num_get_node_info_attempts));
data->num_get_node_info_attempts++;
if (data->num_get_node_info_attempts >= MaxAttemptsToGetNodeInfo)
{
finalizeNodeDiscovery(NULL, result.getCallID().server_node_id);
// Warning: data pointer is invalidated now
}
}
}
void handleTimerEvent(const TimerEvent&)
{
if (get_node_info_client_.hasPendingCalls())
{
return;
}
const NodeID node_id = pickNextNodeToQueryAndCleanupMap();
if (!node_id.isUnicast())
{
trace(TraceDiscoveryTimerStop, 0);
stop();
return;
}
if (!handler_.canDiscoverNewNodes())
{
return; // Timer must continue to run in order to not stuck when it unlocks
}
trace(TraceDiscoveryGetNodeInfoRequest, node_id.get());
UAVCAN_TRACE("dynamic_node_id_server::NodeDiscoverer", "Requesting GetNodeInfo from node %d",
int(node_id.get()));
const int res = get_node_info_client_.call(node_id, protocol::GetNodeInfo::Request());
if (res < 0)
{
getNode().registerInternalFailure("NodeDiscoverer GetNodeInfo call");
}
}
void handleNodeStatus(const ReceivedDataStructure<protocol::NodeStatus>& msg)
{
if (!needToQuery(msg.getSrcNodeID()))
{
return;
}
NodeData* data = node_map_.access(msg.getSrcNodeID());
if (data == NULL)
{
trace(TraceDiscoveryNewNodeFound, msg.getSrcNodeID().get());
data = node_map_.insert(msg.getSrcNodeID(), NodeData());
if (data == NULL)
{
getNode().registerInternalFailure("NodeDiscoverer OOM");
return;
}
}
UAVCAN_ASSERT(data != NULL);
if (msg.uptime_sec < data->last_seen_uptime)
{
trace(TraceDiscoveryNodeRestartDetected, msg.getSrcNodeID().get());
data->num_get_node_info_attempts = 0;
}
data->last_seen_uptime = msg.uptime_sec;
if (!isRunning())
{
startPeriodic(MonotonicDuration::fromMSec(TimerPollIntervalMs));
trace(TraceDiscoveryTimerStart, getPeriod().toUSec());
}
}
public:
NodeDiscoverer(INode& node, IEventTracer& tracer, INodeDiscoveryHandler& handler)
: TimerBase(node)
, handler_(handler)
, tracer_(tracer)
, node_map_(node.getAllocator())
, get_node_info_client_(node)
, node_status_sub_(node)
{ }
int init()
{
int res = get_node_info_client_.init();
if (res < 0)
{
return res;
}
get_node_info_client_.setCallback(
GetNodeInfoResponseCallback(this, &NodeDiscoverer::handleGetNodeInfoResponse));
res = node_status_sub_.start(NodeStatusCallback(this, &NodeDiscoverer::handleNodeStatus));
if (res < 0)
{
return res;
}
// Note: the timer starts ad-hoc from the node status callback, not here.
return 0;
}
/**
* Returns true if there's at least one node with pending GetNodeInfo.
*/
bool hasUnknownNodes() const { return !node_map_.isEmpty(); }
/**
* Returns number of nodes that are being queried at the moment.
* This method is needed for testing and state visualization.
*/
uint8_t getNumUnknownNodes() const { return static_cast<uint8_t>(node_map_.getSize()); }
};
}
}
#endif // Include guard
libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_id_selector.hpp
+71
View File
@@ -0,0 +1,71 @@
/*
* Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_NODE_ID_SELECTOR_HPP_INCLUDED
#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_NODE_ID_SELECTOR_HPP_INCLUDED
#include <uavcan/build_config.hpp>
#include <cassert>
namespace uavcan
{
namespace dynamic_node_id_server
{
/**
* Node ID allocation logic
*/
template <typename Owner>
class NodeIDSelector
{
typedef bool (Owner::*IsNodeIDTakenMethod)(const NodeID node_id) const;
const Owner* const owner_;
const IsNodeIDTakenMethod is_node_id_taken_;
public:
NodeIDSelector(const Owner* owner, IsNodeIDTakenMethod is_node_id_taken)
: owner_(owner)
, is_node_id_taken_(is_node_id_taken)
{
UAVCAN_ASSERT(owner_ != NULL);
UAVCAN_ASSERT(is_node_id_taken_ != NULL);
}
/**
* Reutrns a default-constructed (invalid) node ID if a free one could not be found.
*/
NodeID findFreeNodeID(const NodeID preferred) const
{
uint8_t candidate = preferred.isUnicast() ? preferred.get() : NodeID::MaxRecommendedForRegularNodes;
// Up
while (candidate <= NodeID::MaxRecommendedForRegularNodes)
{
if (!(owner_->*is_node_id_taken_)(candidate))
{
return candidate;
}
candidate++;
}
candidate = preferred.isUnicast() ? preferred.get() : NodeID::MaxRecommendedForRegularNodes;
// Down
while (candidate > 0)
{
if (!(owner_->*is_node_id_taken_)(candidate))
{
return candidate;
}
candidate--;
}
return NodeID();
}
};
}
}
#endif // Include guard
libuavcan/include/uavcan/protocol/dynamic_node_id_server/storage_backend.hpp
+62
View File
@@ -0,0 +1,62 @@
/*
* Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_STORAGE_BACKEND_HPP_INCLUDED
#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_STORAGE_BACKEND_HPP_INCLUDED
#include <uavcan/build_config.hpp>
#include <uavcan/marshal/types.hpp>
namespace uavcan
{
namespace dynamic_node_id_server
{
/**
* This interface is used by the server to read and write stable storage.
* The storage is represented as a key-value container, where keys and values are ASCII strings up to 32
* characters long, not including the termination byte. Fixed block size allows for absolutely straightforward
* and efficient implementation of storage backends, e.g. based on text files.
* Keys and values may contain only non-whitespace, non-formatting printable characters.
*/
class UAVCAN_EXPORT IStorageBackend
{
public:
/**
* Maximum length of keys and values. One pair takes twice as much space.
*/
enum { MaxStringLength = 32 };
/**
* It is guaranteed that the server will never require more than this number of key/value pairs.
* Total storage space needed is (MaxKeyValuePairs * MaxStringLength * 2), not including storage overhead.
*/
enum { MaxKeyValuePairs = 512 };
/**
* This type is used to exchange data chunks with the backend.
* It doesn't use any dynamic memory; please refer to the Array<> class for details.
*/
typedef MakeString<MaxStringLength>::Type String;
/**
* Read one value from the storage.
* If such key does not exist, or if read failed, an empty string will be returned.
* This method should not block for more than 50 ms.
*/
virtual String get(const String& key) const = 0;
/**
* Create or update value for the given key. Empty value should be regarded as a request to delete the key.
* This method should not block for more than 50 ms.
* Failures will be ignored.
*/
virtual void set(const String& key, const String& value) = 0;
virtual ~IStorageBackend() { }
};
}
}
#endif // Include guard
libuavcan/include/uavcan/protocol/dynamic_node_id_server/storage_marshaller.hpp
+181
View File
@@ -0,0 +1,181 @@
/*
* Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_STORAGE_MARSHALLER_HPP_INCLUDED
#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_STORAGE_MARSHALLER_HPP_INCLUDED
#include <uavcan/build_config.hpp>
#include <uavcan/debug.hpp>
#include <uavcan/protocol/dynamic_node_id_server/storage_backend.hpp>
#include <uavcan/protocol/dynamic_node_id_server/types.hpp>
#include <cstdlib>
#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
# include <cerrno>
#endif
namespace uavcan
{
namespace dynamic_node_id_server
{
/**
* This class extends the storage backend interface with serialization/deserialization functionality.
*/
class StorageMarshaller
{
IStorageBackend& storage_;
static uint8_t convertLowerCaseHexCharToNibble(char ch)
{
const uint8_t ret = (ch > '9') ? static_cast<uint8_t>(ch - 'a' + 10) : static_cast<uint8_t>(ch - '0');
UAVCAN_ASSERT(ret < 16);
return ret;
}
public:
StorageMarshaller(IStorageBackend& storage)
: storage_(storage)
{ }
/**
* These methods set the value and then immediately read it back.
* 1. Serialize the value.
* 2. Update the value on the backend.
* 3. Call get() with the same value argument.
* The caller then is supposed to check whether the argument has the desired value.
*/
int setAndGetBack(const IStorageBackend::String& key, uint32_t& inout_value)
{
IStorageBackend::String serialized;
serialized.appendFormatted("%llu", static_cast<unsigned long long>(inout_value));
UAVCAN_TRACE("StorageMarshaller", "Set %s = %s", key.c_str(), serialized.c_str());
storage_.set(key, serialized);
return get(key, inout_value);
}
int setAndGetBack(const IStorageBackend::String& key, UniqueID& inout_value)
{
IStorageBackend::String serialized;
for (uint8_t i = 0; i < UniqueID::MaxSize; i++)
{
serialized.appendFormatted("%02x", inout_value.at(i));
}
UAVCAN_ASSERT(serialized.size() == UniqueID::MaxSize * 2);
UAVCAN_TRACE("StorageMarshaller", "Set %s = %s", key.c_str(), serialized.c_str());
storage_.set(key, serialized);
return get(key, inout_value);
}
/**
* Getters simply read and deserialize the value.
* 1. Read the value back from the backend; return false if read fails.
* 2. Deserealize the newly read value; return false if deserialization fails.
* 3. Update the argument with deserialized value.
* 4. Return true.
*/
int get(const IStorageBackend::String& key, uint32_t& out_value) const
{
/*
* Reading the storage
*/
const IStorageBackend::String val = storage_.get(key);
if (val.empty())
{
return -ErrFailure;
}
/*
* Per MISRA C++ recommendations, checking the inputs instead of relying solely on errno.
* The value must contain only numeric characters.
*/
for (IStorageBackend::String::const_iterator it = val.begin(); it != val.end(); ++it)
{
if (static_cast<char>(*it) < '0' || static_cast<char>(*it) > '9')
{
return -ErrFailure;
}
}
if (val.size() > 10) // len(str(0xFFFFFFFF))
{
return -ErrFailure;
}
/*
* Conversion is carried out here
*/
#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
errno = 0;
#endif
#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
const unsigned long long x = std::strtoull(val.c_str(), NULL, 10);
#else
// There was no strtoull() before C++11, so we need to resort to strtoul()
StaticAssert<(sizeof(unsigned long) >= sizeof(uint32_t))>::check();
const unsigned long x = std::strtoul(val.c_str(), NULL, 10);
#endif
#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
if (errno != 0)
{
return -ErrFailure;
}
#endif
out_value = static_cast<uint32_t>(x);
return 0;
}
int get(const IStorageBackend::String& key, UniqueID& out_value) const
{
static const uint8_t NumBytes = UniqueID::MaxSize;
/*
* Reading the storage
*/
IStorageBackend::String val = storage_.get(key);
if (val.size() != NumBytes * 2)
{
return -ErrFailure;
}
/*
* The value must contain only hexadecimal numbers.
*/
val.convertToLowerCaseASCII();
for (IStorageBackend::String::const_iterator it = val.begin(); it != val.end(); ++it)
{
if ((static_cast<char>(*it) < '0' || static_cast<char>(*it) > '9') &&
(static_cast<char>(*it) < 'a' || static_cast<char>(*it) > 'f'))
{
return -ErrFailure;
}
}
/*
* Conversion is carried out here
*/
IStorageBackend::String::const_iterator it = val.begin();
for (uint8_t byte_index = 0; byte_index < NumBytes; byte_index++)
{
out_value[byte_index] =
static_cast<uint8_t>(convertLowerCaseHexCharToNibble(static_cast<char>(*it++)) << 4);
out_value[byte_index] =
static_cast<uint8_t>(convertLowerCaseHexCharToNibble(static_cast<char>(*it++)) | out_value[byte_index]);
}
return 0;
}
};
}
}
#endif // Include guard
libuavcan/include/uavcan/protocol/dynamic_node_id_server/types.hpp
+27
View File
@@ -0,0 +1,27 @@
/*
* Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_TYPES_HPP_INCLUDED
#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_TYPES_HPP_INCLUDED
#include <uavcan/build_config.hpp>
// UAVCAN types
#include <uavcan/protocol/dynamic_node_id/server/Entry.hpp>
namespace uavcan
{
namespace dynamic_node_id_server
{
using namespace ::uavcan::protocol::dynamic_node_id;
/**
* Node Unique ID
*/
typedef protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id UniqueID;
}
}
#endif // Include guard
libuavcan/include/uavcan/protocol/file_server.hpp
+268
View File
@@ -0,0 +1,268 @@
/*
* Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#ifndef UAVCAN_PROTOCOL_FILE_SERVER_HPP_INCLUDED
#define UAVCAN_PROTOCOL_FILE_SERVER_HPP_INCLUDED
#include <uavcan/build_config.hpp>
#include <uavcan/debug.hpp>
#include <uavcan/node/service_server.hpp>
#include <uavcan/util/method_binder.hpp>
// UAVCAN types
#include <uavcan/protocol/file/GetInfo.hpp>
#include <uavcan/protocol/file/GetDirectoryEntryInfo.hpp>
#include <uavcan/protocol/file/Read.hpp>
#include <uavcan/protocol/file/Write.hpp>
#include <uavcan/protocol/file/Delete.hpp>
namespace uavcan
{
/**
* The file server backend should implement this interface.
* Note that error codes returned by these methods are defined in uavcan.protocol.file.Error; these are
* not the same as libuavcan-internal error codes defined in uavcan.error.hpp.
*/
class UAVCAN_EXPORT IFileServerBackend
{
public:
typedef protocol::file::Path::FieldTypes::path Path;
typedef protocol::file::EntryType EntryType;
typedef protocol::file::Error Error;
/**
* All read operations must return this number of bytes, unless end of file is reached.
*/
enum { ReadSize = protocol::file::Read::Response::FieldTypes::data::MaxSize };
/**
* Shortcut for uavcan.protocol.file.Path.SEPARATOR.
*/
static char getPathSeparator() { return static_cast<char>(protocol::file::Path::SEPARATOR); }
/**
* Backend for uavcan.protocol.file.GetInfo.
* Refer to uavcan.protocol.file.EntryType for the list of available bit flags.
* Implementation of this method is required.
* On success the method must return zero.
*/
virtual int16_t getInfo(const Path& path, uint64_t& out_size, EntryType& out_type) = 0;
/**
* Backend for uavcan.protocol.file.Read.
* Implementation of this method is required.
* @ref inout_size is set to @ref ReadSize; read operation is required to return exactly this amount, except
* if the end of file is reached.
* On success the method must return zero.
*/
virtual int16_t read(const Path& path, const uint64_t offset, uint8_t* out_buffer, uint16_t& inout_size) = 0;
// Methods below are optional.
/**
* Backend for uavcan.protocol.file.Write.
* Implementation of this method is NOT required; by default it returns uavcan.protocol.file.Error.NOT_IMPLEMENTED.
* On success the method must return zero.
*/
virtual int16_t write(const Path& path, const uint64_t offset, const uint8_t* buffer, const uint16_t size)
{
(void)path;
(void)offset;
(void)buffer;
(void)size;
return Error::NOT_IMPLEMENTED;
}
/**
* Backend for uavcan.protocol.file.Delete. ('delete' is a C++ keyword, so 'remove' is used instead)
* Implementation of this method is NOT required; by default it returns uavcan.protocol.file.Error.NOT_IMPLEMENTED.
* On success the method must return zero.
*/
virtual int16_t remove(const Path& path)
{
(void)path;
return Error::NOT_IMPLEMENTED;
}
/**
* Backend for uavcan.protocol.file.GetDirectoryEntryInfo.
* Refer to uavcan.protocol.file.EntryType for the list of available bit flags.
* Implementation of this method is NOT required; by default it returns uavcan.protocol.file.Error.NOT_IMPLEMENTED.
* On success the method must return zero.
*/
virtual int16_t getDirectoryEntryInfo(const Path& directory_path, const uint32_t entry_index,
EntryType& out_type, Path& out_entry_full_path)
{
(void)directory_path;
(void)entry_index;
(void)out_type;
(void)out_entry_full_path;
return Error::NOT_IMPLEMENTED;
}
virtual ~IFileServerBackend() { }
};
/**
* Basic file server implements only the following services:
* uavcan.protocol.file.GetInfo
* uavcan.protocol.file.Read
* Also see @ref IFileServerBackend.
*/
class BasicFileServer
{
typedef MethodBinder<BasicFileServer*,
void (BasicFileServer::*)(const protocol::file::GetInfo::Request&, protocol::file::GetInfo::Response&)>
GetInfoCallback;
typedef MethodBinder<BasicFileServer*,
void (BasicFileServer::*)(const protocol::file::Read::Request&, protocol::file::Read::Response&)>
ReadCallback;
ServiceServer<protocol::file::GetInfo, GetInfoCallback> get_info_srv_;
ServiceServer<protocol::file::Read, ReadCallback> read_srv_;
void handleGetInfo(const protocol::file::GetInfo::Request& req, protocol::file::GetInfo::Response& resp)
{
resp.error.value = backend_.getInfo(req.path.path, resp.size, resp.entry_type);
}
void handleRead(const protocol::file::Read::Request& req, protocol::file::Read::Response& resp)
{
uint16_t inout_size = resp.data.capacity();
resp.data.resize(inout_size);
resp.error.value = backend_.read(req.path.path, req.offset, resp.data.begin(), inout_size);
if (resp.error.value != protocol::file::Error::OK)
{
inout_size = 0;
}
if (inout_size > resp.data.capacity())
{
UAVCAN_ASSERT(0);
resp.error.value = protocol::file::Error::UNKNOWN_ERROR;
}
else
{
resp.data.resize(inout_size);
}
}
protected:
IFileServerBackend& backend_; ///< Derived types can use it
public:
BasicFileServer(INode& node, IFileServerBackend& backend)
: get_info_srv_(node)
, read_srv_(node)
, backend_(backend)
{ }
int start()
{
int res = get_info_srv_.start(GetInfoCallback(this, &BasicFileServer::handleGetInfo));
if (res < 0)
{
return res;
}
res = read_srv_.start(ReadCallback(this, &BasicFileServer::handleRead));
if (res < 0)
{
return res;
}
return 0;
}
};
/**
* Full file server implements all file services:
* uavcan.protocol.file.GetInfo
* uavcan.protocol.file.Read
* uavcan.protocol.file.Write
* uavcan.protocol.file.Delete
* uavcan.protocol.file.GetDirectoryEntryInfo
* Also see @ref IFileServerBackend.
*/
class FileServer : protected BasicFileServer
{
typedef MethodBinder<FileServer*,
void (FileServer::*)(const protocol::file::Write::Request&, protocol::file::Write::Response&)>
WriteCallback;
typedef MethodBinder<FileServer*,
void (FileServer::*)(const protocol::file::Delete::Request&, protocol::file::Delete::Response&)>
DeleteCallback;
typedef MethodBinder<FileServer*,
void (FileServer::*)(const protocol::file::GetDirectoryEntryInfo::Request&,
protocol::file::GetDirectoryEntryInfo::Response&)>
GetDirectoryEntryInfoCallback;
ServiceServer<protocol::file::Write, WriteCallback> write_srv_;
ServiceServer<protocol::file::Delete, DeleteCallback> delete_srv_;
ServiceServer<protocol::file::GetDirectoryEntryInfo, GetDirectoryEntryInfoCallback> get_directory_entry_info_srv_;
void handleWrite(const protocol::file::Write::Request& req, protocol::file::Write::Response& resp)
{
resp.error.value = backend_.write(req.path.path, req.offset, req.data.begin(), req.data.size());
}
void handleDelete(const protocol::file::Delete::Request& req, protocol::file::Delete::Response& resp)
{
resp.error.value = backend_.remove(req.path.path);
}
void handleGetDirectoryEntryInfo(const protocol::file::GetDirectoryEntryInfo::Request& req,
protocol::file::GetDirectoryEntryInfo::Response& resp)
{
resp.error.value = backend_.getDirectoryEntryInfo(req.directory_path.path, req.entry_index,
resp.entry_type, resp.entry_full_path.path);
}
public:
FileServer(INode& node, IFileServerBackend& backend)
: BasicFileServer(node, backend)
, write_srv_(node)
, delete_srv_(node)
, get_directory_entry_info_srv_(node)
{ }
int start()
{
int res = BasicFileServer::start();
if (res < 0)
{
return res;
}
res = write_srv_.start(WriteCallback(this, &FileServer::handleWrite));
if (res < 0)
{
return res;
}
res = delete_srv_.start(DeleteCallback(this, &FileServer::handleDelete));
if (res < 0)
{
return res;
}
res = get_directory_entry_info_srv_.start(
GetDirectoryEntryInfoCallback(this, &FileServer::handleGetDirectoryEntryInfo));
if (res < 0)
{
return res;
}
return 0;
}
};
}
#endif // Include guard
libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp
+476
View File
@@ -0,0 +1,476 @@
/*
* Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#ifndef UAVCAN_PROTOCOL_FIRMWARE_UPDATE_TRIGGER_HPP_INCLUDED
#define UAVCAN_PROTOCOL_FIRMWARE_UPDATE_TRIGGER_HPP_INCLUDED
#include <uavcan/build_config.hpp>
#include <uavcan/debug.hpp>
#include <uavcan/node/service_client.hpp>
#include <uavcan/util/method_binder.hpp>
#include <uavcan/util/map.hpp>
#include <uavcan/protocol/node_info_retriever.hpp>
// UAVCAN types
#include <uavcan/protocol/file/BeginFirmwareUpdate.hpp>
namespace uavcan
{
/**
* Application-specific firmware version checking logic.
* Refer to @ref FirmwareUpdateTrigger for details.
*/
class IFirmwareVersionChecker
{
public:
/**
* This value is limited by the pool block size minus some extra data. Please refer to the Map<> documentation
* for details. If this size is set too high, the compilation will fail in the Map<> template.
*/
enum { MaxFirmwareFilePathLength = 40 };
/**
* This type is used to store path to firmware file that the target node will retrieve using the
* service uavcan.protocol.file.Read. Note that the maximum length is limited due to some specifics of
* libuavcan (@ref MaxFirmwareFilePathLength), this is NOT a protocol-level limitation.
*/
typedef MakeString<MaxFirmwareFilePathLength>::Type FirmwareFilePath;
/**
* This method will be invoked when the class obtains a response to GetNodeInfo request.
*
* @param node_id Node ID that this GetNodeInfo response was received from.
*
* @param node_info Actual node info structure; refer to uavcan.protocol.GetNodeInfo for details.
*
* @param out_firmware_file_path The implementation should return the firmware image path via this argument.
* Note that this path must be reachable via uavcan.protocol.file.Read service.
* Refer to @ref FileServer and @ref BasicFileServer for details.
*
* @return True - the class will begin sending update requests.
* False - the node will be ignored, no request will be sent.
*/
virtual bool shouldRequestFirmwareUpdate(NodeID node_id, const protocol::GetNodeInfo::Response& node_info,
FirmwareFilePath& out_firmware_file_path) = 0;
/**
* This method will be invoked when a node responds to the update request with an error. If the request simply
* times out, this method will not be invoked.
* Note that if by the time of arrival of the response the node is already removed, this method will not be called.
*
* SPECIAL CASE: If the node responds with ERROR_IN_PROGRESS, the class will assume that further requesting
* is not needed anymore. This method will not be invoked.
*
* @param node_id Node ID that returned this error.
*
* @param error_response Contents of the error response. It contains error code and text.
*
* @param out_firmware_file_path New firmware path if a retry is needed. Note that this argument will be
* initialized with old path, so if the same path needs to be reused, this
* argument should be left unchanged.
*
* @return True - the class will continue sending update requests with new firmware path.
* False - the node will be forgotten, new requests will not be sent.
*/
virtual bool shouldRetryFirmwareUpdate(NodeID node_id,
const protocol::file::BeginFirmwareUpdate::Response& error_response,
FirmwareFilePath& out_firmware_file_path) = 0;
/**
* This node is invoked when the node responds to the update request with confirmation.
* Note that if by the time of arrival of the response the node is already removed, this method will not be called.
*
* Implementation is optional; default one does nothing.
*
* @param node_id Node ID that confirmed the request.
*
* @param response Actual response.
*/
virtual void handleFirmwareUpdateConfirmation(NodeID node_id,
const protocol::file::BeginFirmwareUpdate::Response& response)
{
(void)node_id;
(void)response;
}
virtual ~IFirmwareVersionChecker() { }
};
/**
* This class subscribes to updates from @ref NodeInfoRetriever in order to detect nodes that need firmware
* updates. The decision process of whether a firmware update is needed is relayed to the application via
* @ref IFirmwareVersionChecker. If the application confirms that the update is needed, this class will begin
* sending uavcan.protocol.file.BeginFirmwareUpdate periodically (period is configurable) to every node that
* needs an update in a round-robin fashion. There are the following termination conditions for the periodical
* sending process:
*
* - The node responds with confirmation. In this case the class will forget about the node on the assumption
* that its job is done here. Confirmation will be reported to the application via the interface.
*
* - The node responds with an error, and the error code is not ERROR_IN_PROGRESS. In this case the class will
* request the application via the interface mentioned above about its further actions - either give up or
* retry, possibly with a different firmware.
*
* - The node responds with error ERROR_IN_PROGRESS. In this case the class behaves exactly in the same way as if
* response was successful (because the firmware is alredy being updated, so the goal is fulfilled).
* Confirmation will be reported to the application via the interface.
*
* - The node goes offline or restarts. In this case the node will be immediately forgotten, and the process
* will repeat again later because the node info retriever re-queries GetNodeInfo every time when a node restarts.
*
* Since the target node (i.e. node that is being updated) will try to retrieve the specified firmware file using
* the file services (uavcan.protocol.file.*), the provided firmware path must be reachable for the file server
* (@ref FileServer, @ref BasicFileServer). Normally, an application that serves as UAVCAN firmware update server
* will include at least the following components:
* - this firmware update trigger;
* - dynamic node ID allocation server;
* - file server.
*
* Implementation details: the class uses memory pool to keep the list of nodes that have not responded yet, which
* limits the maximum length of the path to the firmware file, which is covered in @ref IFirmwareVersionChecker.
* To somewhat relieve the maximum path length limitation, the class can be supplied with a common prefix that
* will be prepended to firmware pathes before sending requests.
* Interval at which requests are being sent is configurable, but the default value should cover the needs of
* virtually all use cases (as always).
*/
class FirmwareUpdateTrigger : public INodeInfoListener,
private TimerBase
{
typedef MethodBinder<FirmwareUpdateTrigger*,
void (FirmwareUpdateTrigger::*)(const ServiceCallResult<protocol::file::BeginFirmwareUpdate>&)>
BeginFirmwareUpdateResponseCallback;
typedef IFirmwareVersionChecker::FirmwareFilePath FirmwareFilePath;
enum { DefaultRequestIntervalMs = 1000 }; ///< Shall not be less than default service response timeout.
struct NextNodeIDSearchPredicate : ::uavcan::Noncopyable
{
enum { DefaultOutput = 0xFFU };
const FirmwareUpdateTrigger& owner;
uint8_t output;
NextNodeIDSearchPredicate(const FirmwareUpdateTrigger& arg_owner)
: owner(arg_owner)
, output(DefaultOutput)
{ }
bool operator()(const NodeID node_id, const FirmwareFilePath&)
{
if (node_id.get() > owner.last_queried_node_id_ &&
!owner.begin_fw_update_client_.hasPendingCallToServer(node_id))
{
output = min(output, node_id.get());
}
return false;
}
};
/*
* State
*/
ServiceClient<protocol::file::BeginFirmwareUpdate, BeginFirmwareUpdateResponseCallback> begin_fw_update_client_;
IFirmwareVersionChecker& checker_;
NodeInfoRetriever* node_info_retriever_;
Map<NodeID, FirmwareFilePath> pending_nodes_;
MonotonicDuration request_interval_;
FirmwareFilePath common_path_prefix_;
mutable uint8_t last_queried_node_id_;
/*
* Methods of INodeInfoListener
*/
virtual void handleNodeInfoUnavailable(NodeID node_id)
{
UAVCAN_TRACE("FirmwareUpdateTrigger", "Node ID %d could not provide GetNodeInfo response", int(node_id.get()));
pending_nodes_.remove(node_id); // For extra paranoia
}
virtual void handleNodeInfoRetrieved(const NodeID node_id, const protocol::GetNodeInfo::Response& node_info)
{
FirmwareFilePath firmware_file_path;
const bool update_needed = checker_.shouldRequestFirmwareUpdate(node_id, node_info, firmware_file_path);
if (update_needed)
{
UAVCAN_TRACE("FirmwareUpdateTrigger", "Node ID %d requires update; file path: %s",
int(node_id.get()), firmware_file_path.c_str());
trySetPendingNode(node_id, firmware_file_path);
}
else
{
UAVCAN_TRACE("FirmwareUpdateTrigger", "Node ID %d does not need update", int(node_id.get()));
pending_nodes_.remove(node_id);
}
}
virtual void handleNodeStatusChange(const NodeStatusMonitor::NodeStatusChangeEvent& event)
{
if (event.status.status_code == protocol::NodeStatus::STATUS_OFFLINE ||
!event.status.known)
{
pending_nodes_.remove(event.node_id);
UAVCAN_TRACE("FirmwareUpdateTrigger", "Node ID %d is offline hence forgotten", int(event.node_id.get()));
}
}
/*
* Own methods
*/
INode& getNode() { return begin_fw_update_client_.getNode(); }
void trySetPendingNode(const NodeID node_id, const FirmwareFilePath& path)
{
if (NULL != pending_nodes_.insert(node_id, path))
{
if (!TimerBase::isRunning())
{
TimerBase::startPeriodic(request_interval_);
UAVCAN_TRACE("FirmwareUpdateTrigger", "Timer started");
}
}
else
{
getNode().registerInternalFailure("FirmwareUpdateTrigger OOM");
}
}
NodeID pickNextNodeID() const
{
// We can't do index search because indices are unstable in Map<>
// First try - from the current node up
NextNodeIDSearchPredicate s1(*this);
(void)pending_nodes_.find<NextNodeIDSearchPredicate&>(s1);
if (s1.output != NextNodeIDSearchPredicate::DefaultOutput)
{
last_queried_node_id_ = s1.output;
}
else if (last_queried_node_id_ != 0)
{
// Nothing was found, resetting the selector and trying again
UAVCAN_TRACE("FirmwareUpdateTrigger", "Node selector reset, last value: %d", int(last_queried_node_id_));
last_queried_node_id_ = 0;
NextNodeIDSearchPredicate s2(*this);
(void)pending_nodes_.find<NextNodeIDSearchPredicate&>(s2);
if (s2.output != NextNodeIDSearchPredicate::DefaultOutput)
{
last_queried_node_id_ = s2.output;
}
}
else
{
; // Hopeless
}
UAVCAN_TRACE("FirmwareUpdateTrigger", "Next node ID to query: %d, pending nodes: %u, pending calls: %u",
int(last_queried_node_id_), pending_nodes_.getSize(),
begin_fw_update_client_.getNumPendingCalls());
return last_queried_node_id_;
}
void handleBeginFirmwareUpdateResponse(const ServiceCallResult<protocol::file::BeginFirmwareUpdate>& result)
{
if (!result.isSuccessful())
{
UAVCAN_TRACE("FirmwareUpdateTrigger", "Request to %d has timed out, will retry",
int(result.getCallID().server_node_id.get()));
return;
}
FirmwareFilePath* const old_path = pending_nodes_.access(result.getCallID().server_node_id);
if (old_path == NULL)
{
// The entry has been removed, assuming that it's not needed anymore
return;
}
const bool confirmed =
result.getResponse().error == protocol::file::BeginFirmwareUpdate::Response::ERROR_OK ||
result.getResponse().error == protocol::file::BeginFirmwareUpdate::Response::ERROR_IN_PROGRESS;
if (confirmed)
{
UAVCAN_TRACE("FirmwareUpdateTrigger", "Node %d confirmed the update request",
int(result.getCallID().server_node_id.get()));
pending_nodes_.remove(result.getCallID().server_node_id);
checker_.handleFirmwareUpdateConfirmation(result.getCallID().server_node_id, result.getResponse());
}
else
{
UAVCAN_ASSERT(old_path != NULL);
UAVCAN_ASSERT(TimerBase::isRunning());
// We won't have to call trySetPendingNode(), because we'll directly update the old path via the pointer
const bool update_needed =
checker_.shouldRetryFirmwareUpdate(result.getCallID().server_node_id, result.getResponse(), *old_path);
if (!update_needed)
{
UAVCAN_TRACE("FirmwareUpdateTrigger", "Node %d does not need retry",
int(result.getCallID().server_node_id.get()));
pending_nodes_.remove(result.getCallID().server_node_id);
}
}
}
virtual void handleTimerEvent(const TimerEvent&)
{
if (pending_nodes_.isEmpty())
{
TimerBase::stop();
UAVCAN_TRACE("FirmwareUpdateTrigger", "Timer stopped");
return;
}
const NodeID node_id = pickNextNodeID();
if (!node_id.isUnicast())
{
return;
}
FirmwareFilePath* const path = pending_nodes_.access(node_id);
if (path == NULL)
{
UAVCAN_ASSERT(0); // pickNextNodeID() returned a node ID that is not present in the map
return;
}
protocol::file::BeginFirmwareUpdate::Request req;
req.source_node_id = getNode().getNodeID().get();
if (!common_path_prefix_.empty())
{
req.image_file_remote_path.path += common_path_prefix_.c_str();
req.image_file_remote_path.path.push_back(protocol::file::Path::SEPARATOR);
}
req.image_file_remote_path.path += path->c_str();
UAVCAN_TRACE("FirmwareUpdateTrigger", "Request to %d with path: %s",
int(node_id.get()), req.image_file_remote_path.path.c_str());
const int call_res = begin_fw_update_client_.call(node_id, req);
if (call_res < 0)
{
getNode().registerInternalFailure("FirmwareUpdateTrigger call");
}
}
public:
FirmwareUpdateTrigger(INode& node, IFirmwareVersionChecker& checker)
: TimerBase(node)
, begin_fw_update_client_(node)
, checker_(checker)
, node_info_retriever_(NULL)
, pending_nodes_(node.getAllocator())
, request_interval_(MonotonicDuration::fromMSec(DefaultRequestIntervalMs))
, last_queried_node_id_(0)
{ }
~FirmwareUpdateTrigger()
{
if (node_info_retriever_ != NULL)
{
node_info_retriever_->removeListener(this);
}
}
/**
* Starts the object. Once started, it can't be stopped unless destroyed.
*
* @param node_info_retriever The object will register itself against this retriever.
* When the destructor is called, the object will unregister itself.
*
* @param common_path_prefix If set, this path will be prefixed to all firmware pathes provided by the
* application interface. The prefix does not need to end with path separator;
* the last trailing one will be removed (so use '//' if you need '/').
* By default the prefix is empty.
*
* @return Negative error code.
*/
int start(NodeInfoRetriever& node_info_retriever,
const FirmwareFilePath& arg_common_path_prefix = FirmwareFilePath())
{
/*
* Configuring the node info retriever
*/
node_info_retriever_ = &node_info_retriever;
int res = node_info_retriever_->addListener(this);
if (res < 0)
{
return res;
}
/*
* Initializing the prefix, removing only the last trailing path separator.
*/
common_path_prefix_ = arg_common_path_prefix;
if (!common_path_prefix_.empty() &&
*(common_path_prefix_.end() - 1) == protocol::file::Path::SEPARATOR)
{
common_path_prefix_.resize(uint8_t(common_path_prefix_.size() - 1U));
}
/*
* Initializing the client
*/
res = begin_fw_update_client_.init();
if (res < 0)
{
return res;
}
begin_fw_update_client_.setCallback(
BeginFirmwareUpdateResponseCallback(this, &FirmwareUpdateTrigger::handleBeginFirmwareUpdateResponse));
// The timer will be started ad-hoc
return 0;
}
/**
* Interval at which uavcan.protocol.file.BeginFirmwareUpdate requests are being sent.
* Note that default value should be OK for any use case.
*/
MonotonicDuration getRequestInterval() const { return request_interval_; }
void setRequestInterval(const MonotonicDuration interval)
{
if (interval.isPositive())
{
request_interval_ = interval;
if (TimerBase::isRunning()) // Restarting with new interval
{
TimerBase::startPeriodic(request_interval_);
}
}
else
{
UAVCAN_ASSERT(0);
}
}
/**
* This method is mostly needed for testing.
* When triggering is not in progress, the class consumes zero CPU time.
*/
bool isTimerRunning() const { return TimerBase::isRunning(); }
unsigned getNumPendingNodes() const
{
const unsigned ret = pending_nodes_.getSize();
UAVCAN_ASSERT((ret > 0) ? isTimerRunning() : true);
return ret;
}
};
}
#endif // Include guard
libuavcan/include/uavcan/protocol/global_time_sync_master.hpp
+173 -7
View File
@@ -5,11 +5,15 @@
#ifndef UAVCAN_PROTOCOL_GLOBAL_TIME_SYNC_MASTER_HPP_INCLUDED
#define UAVCAN_PROTOCOL_GLOBAL_TIME_SYNC_MASTER_HPP_INCLUDED
#include <uavcan/build_config.hpp>
#include <uavcan/node/publisher.hpp>
#include <uavcan/node/subscriber.hpp>
#include <uavcan/util/method_binder.hpp>
#include <uavcan/util/lazy_constructor.hpp>
#include <uavcan/protocol/GlobalTimeSync.hpp>
#include <uavcan/debug.hpp>
#include <cstdlib>
#include <cassert>
namespace uavcan
{
@@ -42,11 +46,54 @@ class UAVCAN_EXPORT GlobalTimeSyncMaster : protected LoopbackFrameListenerBase
UAVCAN_ASSERT(iface_index < MaxCanIfaces);
}
int init();
int init()
{
const int res = pub_.init();
if (res >= 0)
{
pub_.getTransferSender().setIfaceMask(uint8_t(1 << iface_index_));
pub_.getTransferSender().setCanIOFlags(CanIOFlagLoopback);
pub_.setPriority(TransferPriorityHigh); // Fixed priority
}
return res;
}
void setTxTimestamp(UtcTime ts);
void setTxTimestamp(UtcTime ts)
{
if (ts.isZero())
{
UAVCAN_ASSERT(0);
pub_.getNode().registerInternalFailure("GlobalTimeSyncMaster zero TX ts");
return;
}
if (!prev_tx_utc_.isZero())
{
prev_tx_utc_ = UtcTime(); // Reset again, because there's something broken in the driver and we don't trust it
pub_.getNode().registerInternalFailure("GlobalTimeSyncMaster pub conflict");
return;
}
prev_tx_utc_ = ts;
}
int publish(TransferID tid, MonotonicTime current_time);
int publish(TransferID tid, MonotonicTime current_time)
{
UAVCAN_ASSERT(pub_.getTransferSender().getCanIOFlags() == CanIOFlagLoopback);
UAVCAN_ASSERT(pub_.getTransferSender().getIfaceMask() == (1 << iface_index_));
const MonotonicDuration since_prev_pub = current_time - iface_prev_pub_mono_;
iface_prev_pub_mono_ = current_time;
UAVCAN_ASSERT(since_prev_pub.isPositive());
const bool long_period = since_prev_pub.toMSec() >= protocol::GlobalTimeSync::MAX_PUBLICATION_PERIOD_MS;
protocol::GlobalTimeSync msg;
msg.previous_transmission_timestamp_usec = long_period ? 0 : prev_tx_utc_.toUSec();
prev_tx_utc_ = UtcTime();
UAVCAN_TRACE("GlobalTimeSyncMaster", "Publishing %llu iface=%i tid=%i",
static_cast<unsigned long long>(msg.previous_transmission_timestamp_usec),
int(iface_index_), int(tid.get()));
return pub_.broadcast(msg, tid);
}
};
INode& node_;
@@ -55,9 +102,43 @@ class UAVCAN_EXPORT GlobalTimeSyncMaster : protected LoopbackFrameListenerBase
DataTypeID dtid_;
bool initialized_;
virtual void handleLoopbackFrame(const RxFrame& frame);
virtual void handleLoopbackFrame(const RxFrame& frame)
{
const uint8_t iface = frame.getIfaceIndex();
if (initialized_ && iface < MaxCanIfaces)
{
if (frame.getDataTypeID() == dtid_ &&
frame.getTransferType() == TransferTypeMessageBroadcast &&
frame.isLast() && frame.isFirst() &&
frame.getSrcNodeID() == node_.getNodeID())
{
iface_masters_[iface]->setTxTimestamp(frame.getUtcTimestamp());
}
}
else
{
UAVCAN_ASSERT(0);
}
}
int getNextTransferID(TransferID& tid);
int getNextTransferID(TransferID& tid)
{
const MonotonicDuration max_transfer_interval =
MonotonicDuration::fromMSec(protocol::GlobalTimeSync::PUBLISHER_TIMEOUT_MS);
const OutgoingTransferRegistryKey otr_key(dtid_, TransferTypeMessageBroadcast, NodeID::Broadcast);
const MonotonicTime otr_deadline = node_.getMonotonicTime() + max_transfer_interval;
TransferID* const tid_ptr =
node_.getDispatcher().getOutgoingTransferRegistry().accessOrCreate(otr_key, otr_deadline);
if (tid_ptr == NULL)
{
return -ErrMemory;
}
tid = *tid_ptr;
tid_ptr->increment();
return 0;
}
public:
explicit GlobalTimeSyncMaster(INode& node)
@@ -71,7 +152,45 @@ public:
* Must be called before the master can be used.
* Returns negative error code.
*/
int init();
int init()
{
if (initialized_)
{
return 0;
}
// Data type ID
const DataTypeDescriptor* const desc =
GlobalDataTypeRegistry::instance().find(DataTypeKindMessage, protocol::GlobalTimeSync::getDataTypeFullName());
if (desc == NULL)
{
return -ErrUnknownDataType;
}
dtid_ = desc->getID();
// Iface master array
int res = -ErrLogic;
for (uint8_t i = 0; i < MaxCanIfaces; i++)
{
if (!iface_masters_[i].isConstructed())
{
iface_masters_[i].construct<INode&, uint8_t>(node_, i);
}
res = iface_masters_[i]->init();
if (res < 0)
{
break;
}
}
// Loopback listener
initialized_ = res >= 0;
if (initialized_)
{
LoopbackFrameListenerBase::startListening();
}
return res;
}
/**
* Whether the master instance has been initialized.
@@ -89,7 +208,54 @@ public:
*
* Returns negative error code.
*/
int publish();
int publish()
{
if (!initialized_)
{
const int res = init();
if (res < 0)
{
return res;
}
}
/*
* Enforce max frequency
*/
const MonotonicTime current_time = node_.getMonotonicTime();
{
const MonotonicDuration since_prev_pub = current_time - prev_pub_mono_;
UAVCAN_ASSERT(since_prev_pub.isPositive());
if (since_prev_pub.toMSec() < protocol::GlobalTimeSync::MIN_PUBLICATION_PERIOD_MS)
{
UAVCAN_TRACE("GlobalTimeSyncMaster", "Publication skipped");
return 0;
}
prev_pub_mono_ = current_time;
}
/*
* Obtain common Transfer ID for all masters
*/
TransferID tid;
{
const int tid_res = getNextTransferID(tid);
if (tid_res < 0)
{
return tid_res;
}
}
for (uint8_t i = 0; i < node_.getDispatcher().getCanIOManager().getNumIfaces(); i++)
{
const int res = iface_masters_[i]->publish(tid, current_time);
if (res < 0)
{
return res;
}
}
return 0;
}
};
}
libuavcan/include/uavcan/protocol/global_time_sync_slave.hpp
+97 -7
View File
@@ -8,6 +8,8 @@
#include <uavcan/node/subscriber.hpp>
#include <uavcan/util/method_binder.hpp>
#include <uavcan/protocol/GlobalTimeSync.hpp>
#include <uavcan/debug.hpp>
#include <cassert>
namespace uavcan
{
@@ -43,13 +45,93 @@ class UAVCAN_EXPORT GlobalTimeSyncSlave : Noncopyable
ISystemClock& getSystemClock() const { return sub_.getNode().getSystemClock(); }
void adjustFromMsg(const ReceivedDataStructure<protocol::GlobalTimeSync>& msg);
void adjustFromMsg(const ReceivedDataStructure<protocol::GlobalTimeSync>& msg)
{
UAVCAN_ASSERT(msg.previous_transmission_timestamp_usec > 0);
const UtcDuration adjustment = UtcTime::fromUSec(msg.previous_transmission_timestamp_usec) - prev_ts_utc_;
void updateFromMsg(const ReceivedDataStructure<protocol::GlobalTimeSync>& msg);
UAVCAN_TRACE("GlobalTimeSyncSlave", "Adjustment: usec=%lli snid=%i iface=%i suppress=%i",
static_cast<long long>(adjustment.toUSec()),
int(msg.getSrcNodeID().get()), int(msg.getIfaceIndex()), int(suppressed_));
void processMsg(const ReceivedDataStructure<protocol::GlobalTimeSync>& msg);
if (!suppressed_)
{
getSystemClock().adjustUtc(adjustment);
}
last_adjustment_ts_ = msg.getMonotonicTimestamp();
state_ = Update;
}
void handleGlobalTimeSync(const ReceivedDataStructure<protocol::GlobalTimeSync>& msg);
void updateFromMsg(const ReceivedDataStructure<protocol::GlobalTimeSync>& msg)
{
UAVCAN_TRACE("GlobalTimeSyncSlave", "Update: snid=%i iface=%i",
int(msg.getSrcNodeID().get()), int(msg.getIfaceIndex()));
prev_ts_utc_ = msg.getUtcTimestamp();
prev_ts_mono_ = msg.getMonotonicTimestamp();
master_nid_ = msg.getSrcNodeID();
prev_iface_index_ = msg.getIfaceIndex();
prev_tid_ = msg.getTransferID();
state_ = Adjust;
}
void processMsg(const ReceivedDataStructure<protocol::GlobalTimeSync>& msg)
{
const MonotonicDuration since_prev_msg = msg.getMonotonicTimestamp() - prev_ts_mono_;
UAVCAN_ASSERT(!since_prev_msg.isNegative());
const bool needs_init = !master_nid_.isValid() || prev_ts_mono_.isZero();
const bool switch_master = msg.getSrcNodeID() < master_nid_;
const bool pub_timeout = since_prev_msg.toMSec() > protocol::GlobalTimeSync::PUBLISHER_TIMEOUT_MS;
if (switch_master || pub_timeout || needs_init)
{
UAVCAN_TRACE("GlobalTimeSyncSlave", "Force update: needs_init=%i switch_master=%i pub_timeout=%i",
int(needs_init), int(switch_master), int(pub_timeout));
updateFromMsg(msg);
}
else if (msg.getIfaceIndex() == prev_iface_index_ && msg.getSrcNodeID() == master_nid_)
{
if (state_ == Adjust)
{
const bool msg_invalid = msg.previous_transmission_timestamp_usec == 0;
const bool wrong_tid = prev_tid_.computeForwardDistance(msg.getTransferID()) != 1;
const bool wrong_timing = since_prev_msg.toMSec() > protocol::GlobalTimeSync::MAX_PUBLICATION_PERIOD_MS;
if (msg_invalid || wrong_tid || wrong_timing)
{
UAVCAN_TRACE("GlobalTimeSyncSlave",
"Adjustment skipped: msg_invalid=%i wrong_tid=%i wrong_timing=%i",
int(msg_invalid), int(wrong_tid), int(wrong_timing));
state_ = Update;
}
}
if (state_ == Adjust)
{
adjustFromMsg(msg);
}
else
{
updateFromMsg(msg);
}
}
else
{
UAVCAN_TRACE("GlobalTimeSyncSlave", "Ignored: snid=%i iface=%i",
int(msg.getSrcNodeID().get()), int(msg.getIfaceIndex()));
}
}
void handleGlobalTimeSync(const ReceivedDataStructure<protocol::GlobalTimeSync>& msg)
{
if (msg.getTransferType() == TransferTypeMessageBroadcast)
{
processMsg(msg);
}
else
{
UAVCAN_TRACE("GlobalTimeSyncSlave", "Invalid transfer type %i", int(msg.getTransferType()));
}
}
public:
explicit GlobalTimeSyncSlave(INode& node)
@@ -64,7 +146,10 @@ public:
* attention from the application, other than to handle a clock adjustment request occasionally.
* Returns negative error code.
*/
int start();
int start()
{
return sub_.start(GlobalTimeSyncCallback(this, &GlobalTimeSyncSlave::handleGlobalTimeSync));
}
/**
* Enable or disable the suppressed mode.
@@ -89,13 +174,18 @@ public:
* Note that immediately after start up the slave will be INACTIVE until it finds a master.
* Please read the specs to learn more.
*/
bool isActive() const;
bool isActive() const
{
const MonotonicDuration since_prev_adj = getSystemClock().getMonotonic() - last_adjustment_ts_;
return !last_adjustment_ts_.isZero() &&
(since_prev_adj.toMSec() <= protocol::GlobalTimeSync::PUBLISHER_TIMEOUT_MS);
}
/**
* Node ID of the master the slave is currently locked on.
* Returns an invalid Node ID if there's no active master.
*/
NodeID getMasterNodeID() const;
NodeID getMasterNodeID() const { return isActive() ? master_nid_ : NodeID(); }
/**
* Last time when the local clock adjustment was performed.
libuavcan/include/uavcan/protocol/logger.hpp
+52 -5
View File
@@ -6,9 +6,11 @@
#define UAVCAN_PROTOCOL_LOGGER_HPP_INCLUDED
#include <uavcan/time.hpp>
#include <uavcan/build_config.hpp>
#include <uavcan/protocol/debug/LogMessage.hpp>
#include <uavcan/marshal/char_array_formatter.hpp>
#include <uavcan/node/publisher.hpp>
#include <cstdlib>
#if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11)
# error UAVCAN_CPP_VERSION
@@ -62,7 +64,7 @@ public:
* This value is higher than any valid severity value.
* Use it to completely suppress the output.
*/
static const LogLevel LevelAboveAll = (1 << protocol::debug::LogLevel::FieldTypes::value::BitLen) - 1;
static LogLevel getLogLevelAboveAll() { return (1U << protocol::debug::LogLevel::FieldTypes::value::BitLen) - 1U; }
private:
enum { DefaultTxTimeoutMs = 2000 };
@@ -72,7 +74,10 @@ private:
LogLevel level_;
ILogSink* external_sink_;
LogLevel getExternalSinkLevel() const;
LogLevel getExternalSinkLevel() const
{
return (external_sink_ == NULL) ? getLogLevelAboveAll() : external_sink_->getLogLevel();
}
public:
explicit Logger(INode& node)
@@ -89,7 +94,16 @@ public:
* Must be called once before use.
* Returns negative error code.
*/
int init();
int init()
{
const int res = logmsg_pub_.init();
if (res < 0)
{
return res;
}
logmsg_pub_.setPriority(TransferPriorityLow); // Fixed priority
return 0;
}
/**
* Logs one message. Please consider using helper methods instead of this one.
@@ -102,7 +116,19 @@ public:
*
* Returns negative error code.
*/
int log(const protocol::debug::LogMessage& message);
int log(const protocol::debug::LogMessage& message)
{
int retval = 0;
if (message.level.value >= getExternalSinkLevel())
{
external_sink_->log(message);
}
if (message.level.value >= level_)
{
retval = logmsg_pub_.broadcast(message);
}
return retval;
}
/**
* Severity filter for UAVCAN broadcasting.
@@ -177,7 +203,28 @@ public:
#else
int log(LogLevel level, const char* source, const char* text) UAVCAN_NOEXCEPT;
int log(LogLevel level, const char* source, const char* text) UAVCAN_NOEXCEPT
{
#if UAVCAN_EXCEPTIONS
try
#endif
{
if (level >= level_ || level >= getExternalSinkLevel())
{
msg_buf_.level.value = level;
msg_buf_.source = source;
msg_buf_.text = text;
return log(msg_buf_);
}
return 0;
}
#if UAVCAN_EXCEPTIONS
catch (...)
{
return -ErrFailure;
}
#endif
}
int logDebug(const char* source, const char* text) UAVCAN_NOEXCEPT
{
libuavcan/include/uavcan/protocol/network_compat_checker.hpp
+179 -10
View File
@@ -5,12 +5,17 @@
#ifndef UAVCAN_PROTOCOL_NETWORK_COMPAT_CHECKER_HPP_INCLUDED
#define UAVCAN_PROTOCOL_NETWORK_COMPAT_CHECKER_HPP_INCLUDED
#include <uavcan/build_config.hpp>
#include <uavcan/debug.hpp>
#include <uavcan/util/bitset.hpp>
#include <uavcan/util/method_binder.hpp>
#include <uavcan/node/subscriber.hpp>
#include <uavcan/node/publisher.hpp>
#include <uavcan/node/service_client.hpp>
#include <uavcan/protocol/ComputeAggregateTypeSignature.hpp>
#include <uavcan/protocol/NodeStatus.hpp>
#include <uavcan/protocol/GlobalDiscoveryRequest.hpp>
#include <cassert>
namespace uavcan
{
@@ -58,18 +63,143 @@ class UAVCAN_EXPORT NetworkCompatibilityChecker : Noncopyable
INode& getNode() { return ns_sub_.getNode(); }
const INode& getNode() const { return ns_sub_.getNode(); }
MonotonicDuration getNetworkDiscoveryDelay() const;
MonotonicDuration getNetworkDiscoveryDelay() const
{
// Base duration is constant - NodeStatus max publication period
MonotonicDuration dur = MonotonicDuration::fromMSec(protocol::NodeStatus::MAX_PUBLICATION_PERIOD_MS);
// Additional duration depends on the node priority - gets larger with higher Node ID
dur += MonotonicDuration::fromMSec(getNode().getNodeID().get() * 10);
return dur;
}
NodeID findNextUncheckedNode();
NodeID findNextUncheckedNode()
{
for (uint8_t i = 1; i <= NodeID::Max; i++)
{
if (nid_mask_present_.test(i) && !nid_mask_checked_.test(i))
{
nid_mask_checked_[i] = true;
return NodeID(i);
}
}
return NodeID();
}
int waitForCATSResponse();
int waitForCATSResponse()
{
while (cats_cln_.hasPendingCalls())
{
const int res = getNode().spin(MonotonicDuration::fromMSec(10));
if (res < 0 || !result_.isOk())
{
return res;
}
}
return 0;
}
void handleNodeStatus(const ReceivedDataStructure<protocol::NodeStatus>& msg);
void handleCATSResponse(ServiceCallResult<protocol::ComputeAggregateTypeSignature>& resp);
void handleNodeStatus(const ReceivedDataStructure<protocol::NodeStatus>& msg)
{
if (!nid_mask_present_.test(msg.getSrcNodeID().get()))
{
UAVCAN_TRACE("NodeInitializer", "New node nid=%i", int(msg.getSrcNodeID().get()));
nid_mask_present_[msg.getSrcNodeID().get()] = true;
}
int checkOneNodeOneDataTypeKind(NodeID nid, DataTypeKind kind);
int checkOneNode(NodeID nid);
int checkNodes();
if (msg.getSrcNodeID() == getNode().getNodeID())
{
UAVCAN_TRACE("NodeInitializer", "Node ID collision; nid=%i", int(msg.getSrcNodeID().get()));
result_.conflicting_node = msg.getSrcNodeID();
}
}
void handleCATSResponse(ServiceCallResult<protocol::ComputeAggregateTypeSignature>& resp)
{
last_cats_request_ok_ = resp.isSuccessful();
if (last_cats_request_ok_)
{
const DataTypeSignature sign = GlobalDataTypeRegistry::instance().
computeAggregateSignature(checking_dtkind_, resp.getResponse().mutually_known_ids);
UAVCAN_TRACE("NodeInitializer", "CATS response from nid=%i; local=%llu remote=%llu",
int(resp.getCallID().server_node_id.get()), static_cast<unsigned long long>(sign.get()),
static_cast<unsigned long long>(resp.getResponse().aggregate_signature));
if (sign.get() != resp.getResponse().aggregate_signature)
{
result_.conflicting_node = resp.getCallID().server_node_id;
}
}
}
int checkOneNodeOneDataTypeKind(NodeID nid, DataTypeKind kind)
{
StaticAssert<DataTypeKindMessage == int(protocol::DataTypeKind::MESSAGE)>::check();
StaticAssert<DataTypeKindService == int(protocol::DataTypeKind::SERVICE)>::check();
UAVCAN_ASSERT(nid.isUnicast());
UAVCAN_ASSERT(!cats_cln_.hasPendingCalls());
checking_dtkind_ = kind;
protocol::ComputeAggregateTypeSignature::Request request;
request.kind.value = kind;
GlobalDataTypeRegistry::instance().getDataTypeIDMask(kind, request.known_ids);
int res = cats_cln_.call(nid, request);
if (res < 0)
{
return res;
}
res = waitForCATSResponse();
if (res < 0)
{
return res;
}
if (!last_cats_request_ok_)
{
return -ErrFailure;
}
return 0;
}
int checkOneNode(NodeID nid)
{
if (nid == getNode().getNodeID())
{
result_.conflicting_node = nid; // NodeID collision
return 0;
}
const int res = checkOneNodeOneDataTypeKind(nid, DataTypeKindMessage);
if (res < 0 || !result_.isOk())
{
return res;
}
return checkOneNodeOneDataTypeKind(nid, DataTypeKindService);
}
int checkNodes()
{
(void)nid_mask_checked_.reset();
result_ = NetworkCompatibilityCheckResult();
while (result_.isOk())
{
const NodeID nid = findNextUncheckedNode();
if (nid.isValid())
{
UAVCAN_TRACE("NodeInitializer", "Checking nid=%i", int(nid.get()));
const int res = checkOneNode(nid);
if (res < 0)
{
result_.num_failed_nodes++;
}
UAVCAN_TRACE("NodeInitializer", "Checked nid=%i result=%i", int(nid.get()), res);
}
else { break; }
}
return 0;
}
public:
explicit NetworkCompatibilityChecker(INode& node)
@@ -90,7 +220,42 @@ public:
*
* Returns negative error code.
*/
int execute();
int execute()
{
int res = 0;
if (!getNode().getNodeID().isUnicast())
{
result_.conflicting_node = getNode().getNodeID();
goto exit;
}
res = ns_sub_.start(NodeStatusCallback(this, &NetworkCompatibilityChecker::handleNodeStatus));
if (res < 0)
{
goto exit;
}
cats_cln_.setCallback(CATSResponseCallback(this, &NetworkCompatibilityChecker::handleCATSResponse));
res = cats_cln_.init();
if (res < 0)
{
goto exit;
}
res = getNode().spin(getNetworkDiscoveryDelay());
if (res < 0)
{
goto exit;
}
res = checkNodes();
exit:
ns_sub_.stop();
cats_cln_.cancelAllCalls();
return res;
}
/**
* This method can return a meaningful result only if @ref execute() has been executed successfully once.
@@ -101,7 +266,11 @@ public:
* Convenience method. Should be called immediately before @ref execute().
* Returns negative error code.
*/
static int publishGlobalDiscoveryRequest(INode& node);
static int publishGlobalDiscoveryRequest(INode& node)
{
Publisher<protocol::GlobalDiscoveryRequest> pub(node);
return pub.broadcast(protocol::GlobalDiscoveryRequest());
}
};
}
libuavcan/include/uavcan/protocol/node_info_retriever.hpp
+450
View File
@@ -0,0 +1,450 @@
/*
* Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#ifndef UAVCAN_PROTOCOL_NODE_INFO_RETRIEVER_HPP_INCLUDED
#define UAVCAN_PROTOCOL_NODE_INFO_RETRIEVER_HPP_INCLUDED
#include <uavcan/build_config.hpp>
#include <uavcan/debug.hpp>
#include <uavcan/util/multiset.hpp>
#include <uavcan/node/service_client.hpp>
#include <uavcan/node/timer.hpp>
#include <uavcan/protocol/node_status_monitor.hpp>
#include <uavcan/protocol/GetNodeInfo.hpp>
namespace uavcan
{
/**
* Classes that need to receive GetNodeInfo responses should implement this interface.
*/
class UAVCAN_EXPORT INodeInfoListener
{
public:
/**
* Called when a response to GetNodeInfo request is received. This happens shortly after the node restarts or
* becomes online for the first time.
* @param node_id Node ID of the node
* @param response Node info struct
*/
virtual void handleNodeInfoRetrieved(NodeID node_id, const protocol::GetNodeInfo::Response& node_info) = 0;
/**
* Called when the retriever decides that the node does not support the GetNodeInfo service.
* This method will never be called if the number of attempts is unlimited.
*/
virtual void handleNodeInfoUnavailable(NodeID node_id) = 0;
/**
* This call is routed directly from @ref NodeStatusMonitor.
* Default implementation does nothing.
* @param event Node status change event
*/
virtual void handleNodeStatusChange(const NodeStatusMonitor::NodeStatusChangeEvent& event)
{
(void)event;
}
/**
* This call is routed directly from @ref NodeStatusMonitor.
* Default implementation does nothing.
* @param msg Node status message
*/
virtual void handleNodeStatusMessage(const ReceivedDataStructure<protocol::NodeStatus>& msg)
{
(void)msg;
}
virtual ~INodeInfoListener() { }
};
/**
* This class automatically retrieves a response to GetNodeInfo once a node appears online or restarts.
* It does a number of attempts in case if there's a communication failure before assuming that the node does not
* implement the GetNodeInfo service. All parameters are pre-configured with sensible default values that should fit
* virtually any use case, but they can be overriden if needed - refer to the setter methods below for details.
*
* Defaults are pre-configured so that the class is able to query 123 nodes (node ID 1..125, where 1 is our local
* node and 1 is one node that implements GetNodeInfo service, hence 123) of which none implements GetNodeInfo
* service in under 5 seconds. The 5 second limitation is imposed by UAVCAN-compatible bootloaders, which are
* unlikely to wait for more than that before continuing to boot. In case if this default value is not appropriate
* for the end application, the request interval can be overriden via @ref setRequestInterval().
*
* Following the above explained requirements, the default request interval is defined as follows:
* request interval [ms] = floor(5000 [ms] bootloader timeout / 123 nodes)
* Which yields 40 ms.
*
* Given default service timeout 500 ms and the defined above request frequency 40 ms, the maximum number of
* concurrent requests will be:
* max concurrent requests = ceil(500 [ms] timeout / 40 [ms] request interval)
* Which yields 13 requests.
*
* Keep the above equations in mind when changing the default request interval.
*
* Obviously, if all calls are completing in under (request interval), the number of concurrent requests will never
* exceed one. This is actually the most likely scenario.
*
* Note that all nodes are queried in a round-robin fashion, regardless of their uptime, number of requests made, etc.
*
* Events from this class can be routed to many listeners, @ref INodeInfoListener.
*/
class UAVCAN_EXPORT NodeInfoRetriever : NodeStatusMonitor
, TimerBase
{
public:
enum { MaxNumRequestAttempts = 254 };
enum { UnlimitedRequestAttempts = 0 };
private:
typedef MethodBinder<NodeInfoRetriever*,
void (NodeInfoRetriever::*)(const ServiceCallResult<protocol::GetNodeInfo>&)>
GetNodeInfoResponseCallback;
struct Entry
{
uint32_t uptime_sec;
uint8_t num_attempts_made;
bool request_needed; ///< Always false for unknown nodes
bool updated_since_last_attempt; ///< Always false for unknown nodes
Entry()
: uptime_sec(0)
, num_attempts_made(0)
, request_needed(false)
, updated_since_last_attempt(false)
{
#if UAVCAN_DEBUG
StaticAssert<sizeof(Entry) <= 8>::check();
#endif
}
};
struct NodeInfoRetrievedHandlerCaller
{
const NodeID node_id;
const protocol::GetNodeInfo::Response& node_info;
NodeInfoRetrievedHandlerCaller(NodeID arg_node_id, const protocol::GetNodeInfo::Response& arg_node_info)
: node_id(arg_node_id)
, node_info(arg_node_info)
{ }
bool operator()(INodeInfoListener* key)
{
UAVCAN_ASSERT(key != NULL);
key->handleNodeInfoRetrieved(node_id, node_info);
return false;
}
};
template <typename Event>
struct GenericHandlerCaller
{
void (INodeInfoListener::* const method)(Event);
Event event;
GenericHandlerCaller(void (INodeInfoListener::*arg_method)(Event), Event arg_event)
: method(arg_method)
, event(arg_event)
{ }
bool operator()(INodeInfoListener* key)
{
UAVCAN_ASSERT(key != NULL);
(key->*method)(event);
return false;
}
};
enum { NumStaticCalls = 2 };
enum { DefaultNumRequestAttempts = 16 };
enum { DefaultTimerIntervalMSec = 40 }; ///< Read explanation in the class documentation
/*
* State
*/
Entry entries_[NodeID::Max]; // [1, NodeID::Max]
Multiset<INodeInfoListener*, 2> listeners_;
ServiceClient<protocol::GetNodeInfo, GetNodeInfoResponseCallback, NumStaticCalls> get_node_info_client_;
MonotonicDuration request_interval_;
mutable uint8_t last_picked_node_;
uint8_t num_attempts_;
/*
* Methods
*/
const Entry& getEntry(NodeID node_id) const { return const_cast<NodeInfoRetriever*>(this)->getEntry(node_id); }
Entry& getEntry(NodeID node_id)
{
if (node_id.get() < 1 || node_id.get() > NodeID::Max)
{
handleFatalError("NodeInfoRetriever NodeID");
}
return entries_[node_id.get() - 1];
}
void startTimerIfNotRunning()
{
if (!TimerBase::isRunning())
{
TimerBase::startPeriodic(request_interval_);
UAVCAN_TRACE("NodeInfoRetriever", "Timer started, interval %s sec", request_interval_.toString().c_str());
}
}
NodeID pickNextNodeToQuery(bool& out_at_least_one_request_needed) const
{
out_at_least_one_request_needed = false;
for (unsigned iter_cnt_ = 0; iter_cnt_ < (sizeof(entries_) / sizeof(entries_[0])); iter_cnt_++) // Round-robin
{
last_picked_node_++;
if (last_picked_node_ > NodeID::Max)
{
last_picked_node_ = 1;
}
UAVCAN_ASSERT((last_picked_node_ >= 1) &&
(last_picked_node_ <= NodeID::Max));
const Entry& entry = getEntry(last_picked_node_);
if (entry.request_needed)
{
out_at_least_one_request_needed = true;
if (entry.updated_since_last_attempt &&
!get_node_info_client_.hasPendingCallToServer(last_picked_node_))
{
UAVCAN_TRACE("NodeInfoRetriever", "Next node to query: %d", int(last_picked_node_));
return NodeID(last_picked_node_);
}
}
}
return NodeID(); // No node could be found
}
virtual void handleTimerEvent(const TimerEvent&)
{
bool at_least_one_request_needed = false;
const NodeID next = pickNextNodeToQuery(at_least_one_request_needed);
if (next.isUnicast())
{
UAVCAN_ASSERT(at_least_one_request_needed);
getEntry(next).updated_since_last_attempt = false;
const int res = get_node_info_client_.call(next, protocol::GetNodeInfo::Request());
if (res < 0)
{
get_node_info_client_.getNode().registerInternalFailure("NodeInfoRetriever GetNodeInfo call");
}
}
else
{
if (!at_least_one_request_needed)
{
TimerBase::stop();
UAVCAN_TRACE("NodeInfoRetriever", "Timer stopped");
}
}
}
virtual void handleNodeStatusChange(const NodeStatusChangeEvent& event)
{
const bool was_offline = !event.old_status.known ||
(event.old_status.status_code == protocol::NodeStatus::STATUS_OFFLINE);
const bool offline_now = !event.status.known ||
(event.status.status_code == protocol::NodeStatus::STATUS_OFFLINE);
if (was_offline || offline_now)
{
Entry& entry = getEntry(event.node_id);
entry.request_needed = !offline_now;
entry.num_attempts_made = 0;
UAVCAN_TRACE("NodeInfoRetriever", "Offline status change: node ID %d, request needed: %d",
int(event.node_id.get()), int(entry.request_needed));
if (entry.request_needed)
{
startTimerIfNotRunning();
}
}
listeners_.forEach(
GenericHandlerCaller<const NodeStatusChangeEvent&>(&INodeInfoListener::handleNodeStatusChange, event));
}
virtual void handleNodeStatusMessage(const ReceivedDataStructure<protocol::NodeStatus>& msg)
{
Entry& entry = getEntry(msg.getSrcNodeID());
if (msg.uptime_sec < entry.uptime_sec)
{
entry.request_needed = true;
entry.num_attempts_made = 0;
startTimerIfNotRunning();
}
entry.uptime_sec = msg.uptime_sec;
entry.updated_since_last_attempt = true;
listeners_.forEach(GenericHandlerCaller<const ReceivedDataStructure<protocol::NodeStatus>&>(
&INodeInfoListener::handleNodeStatusMessage, msg));
}
void handleGetNodeInfoResponse(const ServiceCallResult<protocol::GetNodeInfo>& result)
{
Entry& entry = getEntry(result.getCallID().server_node_id);
if (result.isSuccessful())
{
/*
* Updating the uptime here allows to properly handle a corner case where the service response arrives
* after the device has restarted and published its new NodeStatus (although it's unlikely to happen).
*/
entry.uptime_sec = result.getResponse().status.uptime_sec;
entry.request_needed = false;
listeners_.forEach(NodeInfoRetrievedHandlerCaller(result.getCallID().server_node_id,
result.getResponse()));
}
else
{
if (num_attempts_ != UnlimitedRequestAttempts)
{
entry.num_attempts_made++;
if (entry.num_attempts_made >= num_attempts_)
{
entry.request_needed = false;
listeners_.forEach(GenericHandlerCaller<NodeID>(&INodeInfoListener::handleNodeInfoUnavailable,
result.getCallID().server_node_id));
}
}
}
}
public:
NodeInfoRetriever(INode& node)
: NodeStatusMonitor(node)
, TimerBase(node)
, listeners_(node.getAllocator())
, get_node_info_client_(node)
, request_interval_(MonotonicDuration::fromMSec(DefaultTimerIntervalMSec))
, last_picked_node_(1)
, num_attempts_(DefaultNumRequestAttempts)
{ }
/**
* Starts the retriever.
* Destroy the object to stop it.
* Returns negative error code.
*/
int start()
{
int res = NodeStatusMonitor::start();
if (res < 0)
{
return res;
}
res = get_node_info_client_.init();
if (res < 0)
{
return res;
}
get_node_info_client_.setCallback(GetNodeInfoResponseCallback(this,
&NodeInfoRetriever::handleGetNodeInfoResponse));
// Note: the timer will be started ad-hoc
return 0;
}
/**
* Adds one listener. Does nothing if such listener already exists.
* May return -ErrMemory if there's no space to add the listener.
*/
int addListener(INodeInfoListener* listener)
{
if (listener != NULL)
{
removeListener(listener);
return (NULL == listeners_.emplace(listener)) ? -ErrMemory : 0;
}
else
{
return -ErrInvalidParam;
}
}
/**
* Removes the listener.
* If the listener was not registered, nothing will be done.
*/
void removeListener(INodeInfoListener* listener)
{
if (listener != NULL)
{
listeners_.removeAll(listener);
}
else
{
UAVCAN_ASSERT(0);
}
}
unsigned getNumListeners() const { return listeners_.getSize(); }
/**
* Number of attempts to retrieve GetNodeInfo response before giving up on the assumption that the service is
* not implemented.
* Zero is a special value that can be used to set unlimited number of attempts, @ref UnlimitedRequestAttempts.
*/
uint8_t getNumRequestAttempts() const { return num_attempts_; }
void setNumRequestAttempts(const uint8_t num)
{
num_attempts_ = min(static_cast<uint8_t>(MaxNumRequestAttempts), num);
}
/**
* Request interval also implicitly defines the maximum number of concurrent requests.
* Read the class documentation for details.
*/
MonotonicDuration getRequestInterval() const { return request_interval_; }
void setRequestInterval(const MonotonicDuration interval)
{
if (interval.isPositive())
{
request_interval_ = interval;
if (TimerBase::isRunning())
{
TimerBase::startPeriodic(request_interval_);
}
}
else
{
UAVCAN_ASSERT(0);
}
}
/**
* These methods are needed mostly for testing.
*/
bool isRetrievingInProgress() const { return TimerBase::isRunning(); }
uint8_t getNumPendingRequests() const
{
const unsigned num = get_node_info_client_.getNumPendingCalls();
UAVCAN_ASSERT(num <= 0xFF);
return static_cast<uint8_t>(num);
}
};
}
#endif // Include guard
libuavcan/include/uavcan/protocol/node_status_monitor.hpp
+133 -12
View File
@@ -5,10 +5,13 @@
#ifndef UAVCAN_PROTOCOL_NODE_STATUS_MONITOR_HPP_INCLUDED
#define UAVCAN_PROTOCOL_NODE_STATUS_MONITOR_HPP_INCLUDED
#include <uavcan/debug.hpp>
#include <uavcan/util/method_binder.hpp>
#include <uavcan/node/subscriber.hpp>
#include <uavcan/node/timer.hpp>
#include <uavcan/protocol/NodeStatus.hpp>
#include <cassert>
#include <cstdlib>
namespace uavcan
{
@@ -16,7 +19,7 @@ namespace uavcan
* This class implements the core functionality of a network monitor.
* It can be extended by inheritance to add more complex logic, or used directly as is.
*/
class UAVCAN_EXPORT NodeStatusMonitor : protected TimerBase
class UAVCAN_EXPORT NodeStatusMonitor
{
public:
typedef typename StorageType<typename protocol::NodeStatus::FieldTypes::status_code>::Type NodeStatusCode;
@@ -40,12 +43,14 @@ public:
};
private:
static const unsigned TimerPeriodMs100 = 5;
enum { TimerPeriodMs100 = 4 };
typedef MethodBinder<NodeStatusMonitor*,
void (NodeStatusMonitor::*)(const ReceivedDataStructure<protocol::NodeStatus>&)>
NodeStatusCallback;
typedef MethodBinder<NodeStatusMonitor*, void (NodeStatusMonitor::*)(const TimerEvent&)> TimerCallback;
/*
* We'll be able to handle this many nodes in the network without any dynamic memory.
*/
@@ -53,6 +58,8 @@ private:
Subscriber<protocol::NodeStatus, NodeStatusCallback, NumStaticReceivers, 0> sub_;
TimerEventForwarder<TimerCallback> timer_;
struct Entry
{
NodeStatusCode status_code;
@@ -65,13 +72,72 @@ private:
mutable Entry entries_[NodeID::Max]; // [1, NodeID::Max]
Entry& getEntry(NodeID node_id) const;
Entry& getEntry(NodeID node_id) const
{
if (node_id.get() < 1 || node_id.get() > NodeID::Max)
{
handleFatalError("NodeStatusMonitor NodeID");
}
return entries_[node_id.get() - 1];
}
void changeNodeStatus(const NodeID node_id, const Entry new_entry_value);
void changeNodeStatus(const NodeID node_id, const Entry new_entry_value)
{
Entry& entry = getEntry(node_id);
if (entry.status_code != new_entry_value.status_code)
{
NodeStatusChangeEvent event;
event.node_id = node_id;
void handleNodeStatus(const ReceivedDataStructure<protocol::NodeStatus>& msg);
event.old_status.known = entry.time_since_last_update_ms100 >= 0;
event.old_status.status_code = entry.status_code;
virtual void handleTimerEvent(const TimerEvent&);
event.status.known = true;
event.status.status_code = new_entry_value.status_code;
UAVCAN_TRACE("NodeStatusMonitor", "Node %i [%s] status change: %i --> %i", int(node_id.get()),
(event.old_status.known ? "known" : "new"),
int(event.old_status.status_code), int(event.status.status_code));
handleNodeStatusChange(event);
}
entry = new_entry_value;
}
void handleNodeStatus(const ReceivedDataStructure<protocol::NodeStatus>& msg)
{
Entry new_entry_value;
new_entry_value.time_since_last_update_ms100 = 0;
new_entry_value.status_code = msg.status_code;
changeNodeStatus(msg.getSrcNodeID(), new_entry_value);
handleNodeStatusMessage(msg);
}
void handleTimerEvent(const TimerEvent&)
{
const int OfflineTimeoutMs100 = protocol::NodeStatus::OFFLINE_TIMEOUT_MS / 100;
for (uint8_t i = 1; i <= NodeID::Max; i++)
{
const NodeID nid(i);
UAVCAN_ASSERT(nid.isUnicast());
Entry& entry = getEntry(nid);
if (entry.time_since_last_update_ms100 >= 0 &&
entry.status_code != protocol::NodeStatus::STATUS_OFFLINE)
{
entry.time_since_last_update_ms100 =
int8_t(entry.time_since_last_update_ms100 + int8_t(TimerPeriodMs100));
if (entry.time_since_last_update_ms100 >= OfflineTimeoutMs100)
{
Entry new_entry_value;
new_entry_value.time_since_last_update_ms100 = OfflineTimeoutMs100;
new_entry_value.status_code = protocol::NodeStatus::STATUS_OFFLINE;
changeNodeStatus(nid, new_entry_value);
}
}
}
}
protected:
/**
@@ -96,8 +162,8 @@ protected:
public:
explicit NodeStatusMonitor(INode& node)
: TimerBase(node)
, sub_(node)
: sub_(node)
, timer_(node)
{ }
virtual ~NodeStatusMonitor() { }
@@ -107,25 +173,80 @@ public:
* Destroy the object to stop it.
* Returns negative error code.
*/
int start();
int start()
{
const int res = sub_.start(NodeStatusCallback(this, &NodeStatusMonitor::handleNodeStatus));
if (res >= 0)
{
timer_.setCallback(TimerCallback(this, &NodeStatusMonitor::handleTimerEvent));
timer_.startPeriodic(MonotonicDuration::fromMSec(TimerPeriodMs100 * 100));
}
return res;
}
/**
* Make the node unknown.
*/
void forgetNode(NodeID node_id);
void forgetNode(NodeID node_id)
{
if (node_id.isValid())
{
Entry& entry = getEntry(node_id);
entry = Entry();
}
else
{
UAVCAN_ASSERT(0);
}
}
/**
* Returns status of a given node.
* Unknown nodes are considered offline.
*/
NodeStatus getNodeStatus(NodeID node_id) const;
NodeStatus getNodeStatus(NodeID node_id) const
{
if (!node_id.isValid())
{
UAVCAN_ASSERT(0);
return NodeStatus();
}
NodeStatus status;
const Entry& entry = getEntry(node_id);
if (entry.time_since_last_update_ms100 >= 0)
{
status.known = true;
status.status_code = entry.status_code;
}
return status;
}
/**
* This helper method allows to quickly estimate the overall network health.
* Status of the local node is not considered.
* Returns an invalid Node ID value if there's no known nodes in the network.
*/
NodeID findNodeWithWorstStatus() const;
NodeID findNodeWithWorstStatus() const
{
NodeID nid_with_worst_status;
NodeStatusCode worst_status_code = protocol::NodeStatus::STATUS_OK;
for (uint8_t i = 1; i <= NodeID::Max; i++)
{
const NodeID nid(i);
UAVCAN_ASSERT(nid.isUnicast());
const Entry& entry = getEntry(nid);
if (entry.time_since_last_update_ms100 >= 0)
{
if (entry.status_code > worst_status_code || !nid_with_worst_status.isValid())
{
nid_with_worst_status = nid;
worst_status_code = entry.status_code;
}
}
}
return nid_with_worst_status;
}
};
}
libuavcan/include/uavcan/protocol/panic_broadcaster.hpp
+42 -6
View File
@@ -5,6 +5,7 @@
#ifndef UAVCAN_PROTOCOL_PANIC_BROADCASTER_HPP_INCLUDED
#define UAVCAN_PROTOCOL_PANIC_BROADCASTER_HPP_INCLUDED
#include <uavcan/debug.hpp>
#include <uavcan/node/publisher.hpp>
#include <uavcan/node/timer.hpp>
#include <uavcan/protocol/Panic.hpp>
@@ -19,9 +20,19 @@ class UAVCAN_EXPORT PanicBroadcaster : private TimerBase
Publisher<protocol::Panic> pub_;
protocol::Panic msg_;
void publishOnce();
void publishOnce()
{
const int res = pub_.broadcast(msg_);
if (res < 0)
{
pub_.getNode().registerInternalFailure("Panic pub failed");
}
}
virtual void handleTimerEvent(const TimerEvent&);
virtual void handleTimerEvent(const TimerEvent&)
{
publishOnce();
}
public:
explicit PanicBroadcaster(INode& node)
@@ -37,16 +48,41 @@ public:
* @param short_reason Short ASCII string that describes the reason of the panic, 7 characters max.
* If the string exceeds 7 characters, it will be truncated.
*/
void panic(const char* short_reason_description);
void panic(const char* short_reason_description)
{
msg_.reason_text.clear();
const char* p = short_reason_description;
while (p && *p)
{
if (msg_.reason_text.size() == msg_.reason_text.capacity())
{
break;
}
msg_.reason_text.push_back(protocol::Panic::FieldTypes::reason_text::ValueType(*p));
p++;
}
UAVCAN_TRACE("PanicBroadcaster", "Panicking with reason '%s'", getReason().c_str());
publishOnce();
startPeriodic(MonotonicDuration::fromMSec(protocol::Panic::BROADCASTING_INTERVAL_MS));
}
/**
* Stop broadcasting immediately.
*/
void dontPanic(); // Where's my towel
void dontPanic() // Where's my towel
{
stop();
msg_.reason_text.clear();
}
bool isPanicking() const;
bool isPanicking() const { return isRunning(); }
const typename protocol::Panic::FieldTypes::reason_text& getReason() const;
const typename protocol::Panic::FieldTypes::reason_text& getReason() const
{
return msg_.reason_text;
}
};
}
libuavcan/include/uavcan/protocol/param_server.hpp
+85 -4
View File
@@ -5,10 +5,13 @@
#ifndef UAVCAN_PROTOCOL_PARAM_SERVER_HPP_INCLUDED
#define UAVCAN_PROTOCOL_PARAM_SERVER_HPP_INCLUDED
#include <uavcan/debug.hpp>
#include <uavcan/protocol/param/GetSet.hpp>
#include <uavcan/protocol/param/ExecuteOpcode.hpp>
#include <uavcan/node/service_server.hpp>
#include <uavcan/util/method_binder.hpp>
#include <cassert>
#include <cstdlib>
namespace uavcan
{
@@ -101,10 +104,65 @@ class UAVCAN_EXPORT ParamServer
ServiceServer<protocol::param::ExecuteOpcode, ExecuteOpcodeCallback> save_erase_srv_;
IParamManager* manager_;
void handleGetSet(const protocol::param::GetSet::Request& request, protocol::param::GetSet::Response& response);
void handleGetSet(const protocol::param::GetSet::Request& in, protocol::param::GetSet::Response& out)
{
UAVCAN_ASSERT(manager_ != NULL);
void handleExecuteOpcode(const protocol::param::ExecuteOpcode::Request& request,
protocol::param::ExecuteOpcode::Response& response);
// Recover the name from index
if (in.name.empty())
{
manager_->getParamNameByIndex(in.index, out.name);
UAVCAN_TRACE("ParamServer", "GetSet: Index %i --> '%s'", int(in.index), out.name.c_str());
}
else
{
out.name = in.name;
}
if (out.name.empty())
{
UAVCAN_TRACE("ParamServer", "GetSet: Can't resolve parameter name, index=%i", int(in.index));
return;
}
// Assign if needed, read back
if (!IParamManager::isValueEmpty(in.value))
{
manager_->assignParamValue(out.name, in.value);
}
manager_->readParamValue(out.name, out.value);
// Check if the value is OK, otherwise reset the name to indicate that we have no idea what is it all about
if (!IParamManager::isValueEmpty(out.value))
{
manager_->readParamDefaultMaxMin(out.name, out.default_value, out.max_value, out.min_value);
}
else
{
UAVCAN_TRACE("ParamServer", "GetSet: Unknown param: index=%i name='%s'", int(in.index), out.name.c_str());
out.name.clear();
}
}
void handleExecuteOpcode(const protocol::param::ExecuteOpcode::Request& in,
protocol::param::ExecuteOpcode::Response& out)
{
UAVCAN_ASSERT(manager_ != NULL);
if (in.opcode == protocol::param::ExecuteOpcode::Request::OPCODE_SAVE)
{
out.ok = manager_->saveAllParams() >= 0;
}
else if (in.opcode == protocol::param::ExecuteOpcode::Request::OPCODE_ERASE)
{
out.ok = manager_->eraseAllParams() >= 0;
}
else
{
UAVCAN_TRACE("ParamServer", "ExecuteOpcode: invalid opcode %i", int(in.opcode));
out.ok = false;
}
}
public:
explicit ParamServer(INode& node)
@@ -117,8 +175,31 @@ public:
* Starts the parameter server with given param manager instance.
* Returns negative error code.
*/
int start(IParamManager* manager);
int start(IParamManager* manager)
{
if (manager == NULL)
{
return -ErrInvalidParam;
}
manager_ = manager;
int res = get_set_srv_.start(GetSetCallback(this, &ParamServer::handleGetSet));
if (res < 0)
{
return res;
}
res = save_erase_srv_.start(ExecuteOpcodeCallback(this, &ParamServer::handleExecuteOpcode));
if (res < 0)
{
get_set_srv_.stop();
}
return res;
}
/**
* @ref IParamManager
*/
IParamManager* getParamManager() const { return manager_; }
};
libuavcan/include/uavcan/protocol/restart_request_server.hpp
+23 -2
View File
@@ -5,6 +5,7 @@
#ifndef UAVCAN_PROTOCOL_RESTART_REQUEST_SERVER_HPP_INCLUDED
#define UAVCAN_PROTOCOL_RESTART_REQUEST_SERVER_HPP_INCLUDED
#include <uavcan/debug.hpp>
#include <uavcan/node/service_server.hpp>
#include <uavcan/util/method_binder.hpp>
#include <uavcan/protocol/RestartNode.hpp>
@@ -44,7 +45,24 @@ class UAVCAN_EXPORT RestartRequestServer : Noncopyable
IRestartRequestHandler* handler_;
void handleRestartNode(const ReceivedDataStructure<protocol::RestartNode::Request>& request,
protocol::RestartNode::Response& response) const;
protocol::RestartNode::Response& response) const
{
UAVCAN_TRACE("RestartRequestServer", "Request from snid=%i", int(request.getSrcNodeID().get()));
response.ok = false;
if (request.magic_number == protocol::RestartNode::Request::MAGIC_NUMBER)
{
if (handler_)
{
response.ok = handler_->handleRestartRequest(request.getSrcNodeID());
}
UAVCAN_TRACE("RestartRequestServer", "%s", (response.ok ? "Accepted" : "Rejected"));
}
else
{
UAVCAN_TRACE("RestartRequestServer", "Invalid magic number 0x%llx",
static_cast<unsigned long long>(request.magic_number));
}
}
public:
explicit RestartRequestServer(INode& node)
@@ -63,7 +81,10 @@ public:
* Starts the server.
* Returns negative error code.
*/
int start();
int start()
{
return srv_.start(RestartNodeCallback(this, &RestartRequestServer::handleRestartNode));
}
};
}
libuavcan/include/uavcan/protocol/transport_stats_provider.hpp
+23 -2
View File
@@ -5,6 +5,7 @@
#ifndef UAVCAN_PROTOCOL_TRANSPORT_STATS_PROVIDER_HPP_INCLUDED
#define UAVCAN_PROTOCOL_TRANSPORT_STATS_PROVIDER_HPP_INCLUDED
#include <uavcan/build_config.hpp>
#include <uavcan/node/service_server.hpp>
#include <uavcan/util/method_binder.hpp>
#include <uavcan/protocol/GetTransportStats.hpp>
@@ -25,7 +26,24 @@ class UAVCAN_EXPORT TransportStatsProvider : Noncopyable
ServiceServer<protocol::GetTransportStats, GetTransportStatsCallback> srv_;
void handleGetTransportStats(const protocol::GetTransportStats::Request&,
protocol::GetTransportStats::Response& resp) const;
protocol::GetTransportStats::Response& resp) const
{
const TransferPerfCounter& perf = srv_.getNode().getDispatcher().getTransferPerfCounter();
resp.transfer_errors = perf.getErrorCount();
resp.transfers_tx = perf.getTxTransferCount();
resp.transfers_rx = perf.getRxTransferCount();
const CanIOManager& canio = srv_.getNode().getDispatcher().getCanIOManager();
for (uint8_t i = 0; i < canio.getNumIfaces(); i++)
{
const CanIfacePerfCounters can_perf = canio.getIfacePerfCounters(i);
protocol::CANIfaceStats stats;
stats.errors = can_perf.errors;
stats.frames_tx = can_perf.frames_tx;
stats.frames_rx = can_perf.frames_rx;
resp.can_iface_stats.push_back(stats);
}
}
public:
explicit TransportStatsProvider(INode& node)
@@ -36,7 +54,10 @@ public:
* Once started, this class requires no further attention.
* Returns negative error code.
*/
int start();
int start()
{
return srv_.start(GetTransportStatsCallback(this, &TransportStatsProvider::handleGetTransportStats));
}
};
}
libuavcan/include/uavcan/transport/can_io.hpp
+2 -1
View File
@@ -154,7 +154,6 @@ class UAVCAN_EXPORT CanIOManager : Noncopyable
int sendToIface(uint8_t iface_index, const CanFrame& frame, MonotonicTime tx_deadline, CanIOFlags flags);
int sendFromTxQueue(uint8_t iface_index);
uint8_t makePendingTxMask() const;
int callSelect(CanSelectMasks& inout_masks, MonotonicTime blocking_deadline);
public:
@@ -168,6 +167,8 @@ public:
const ICanDriver& getCanDriver() const { return driver_; }
ICanDriver& getCanDriver() { return driver_; }
uint8_t makePendingTxMask() const;
/**
* Returns:
* 0 - rejected/timedout/enqueued
libuavcan/include/uavcan/transport/dispatcher.hpp
+8
View File
@@ -119,8 +119,16 @@ public:
, self_node_id_is_set_(false)
{ }
/**
* This version returns strictly when the deadline is reached.
*/
int spin(MonotonicTime deadline);
/**
* This version does not return until all available frames are processed.
*/
int spinOnce();
/**
* Refer to CanIOManager::send() for the parameter description
*/
libuavcan/include/uavcan/transport/outgoing_transfer_registry.hpp
+2 -2
View File
@@ -165,13 +165,13 @@ TransferID* OutgoingTransferRegistry<NumStaticEntries>::accessOrCreate(const Out
template <int NumStaticEntries>
bool OutgoingTransferRegistry<NumStaticEntries>::exists(DataTypeID dtid, TransferType tt) const
{
return NULL != map_.findFirstKey(ExistenceCheckingPredicate(dtid, tt));
return NULL != map_.find(ExistenceCheckingPredicate(dtid, tt));
}
template <int NumStaticEntries>
void OutgoingTransferRegistry<NumStaticEntries>::cleanup(MonotonicTime ts)
{
map_.removeWhere(DeadlineExpiredPredicate(ts));
map_.removeAllWhere(DeadlineExpiredPredicate(ts));
}
}
libuavcan/include/uavcan/transport/transfer.hpp
+1
View File
@@ -129,6 +129,7 @@ class UAVCAN_EXPORT NodeID
public:
static const uint8_t BitLen = 7U;
static const uint8_t Max = (1U << BitLen) - 1U;
static const uint8_t MaxRecommendedForRegularNodes = Max - 2;
static const NodeID Broadcast;
NodeID() : value_(ValueInvalid) { }
libuavcan/include/uavcan/transport/transfer_listener.hpp
+41 -60
View File
@@ -99,10 +99,10 @@ public:
class UAVCAN_EXPORT TransferListenerBase : public LinkedListNode<TransferListenerBase>, Noncopyable
{
const DataTypeDescriptor& data_type_;
const TransferCRC crc_base_; ///< Pre-initialized with data type hash, thus constant
MapBase<TransferBufferManagerKey, TransferReceiver>& receivers_;
ITransferBufferManager& bufmgr_;
TransferPerfCounter& perf_;
const TransferCRC crc_base_; ///< Pre-initialized with data type hash, thus constant
bool allow_anonymous_transfers_;
class TimedOutReceiverPredicate
@@ -126,10 +126,10 @@ protected:
MapBase<TransferBufferManagerKey, TransferReceiver>& receivers,
ITransferBufferManager& bufmgr)
: data_type_(data_type)
, crc_base_(data_type.getSignature().toTransferCRC())
, receivers_(receivers)
, bufmgr_(bufmgr)
, perf_(perf)
, crc_base_(data_type.getSignature().toTransferCRC())
, allow_anonymous_transfers_(false)
{ }
@@ -178,88 +178,69 @@ public:
virtual ~TransferListener()
{
// Map must be cleared before bufmgr is destructed
receivers_.removeAll();
// Map must be cleared before bufmgr is destroyed
receivers_.clear();
}
};
/**
* This class is used by transfer listener to decide if the frame should be accepted or ignored.
*/
class ITransferAcceptanceFilter
{
public:
/**
* If it returns false, the frame will be ignored, otherwise accepted.
*/
virtual bool shouldAcceptFrame(const RxFrame& frame) const = 0;
virtual ~ITransferAcceptanceFilter() { }
};
/**
* This class should be derived by callers.
*/
template <unsigned MaxBufSize, unsigned NumStaticBufs, unsigned NumStaticReceivers>
class UAVCAN_EXPORT ServiceResponseTransferListener
: public TransferListener<MaxBufSize, NumStaticBufs, NumStaticReceivers>
class UAVCAN_EXPORT TransferListenerWithFilter : public TransferListener<MaxBufSize, NumStaticBufs, NumStaticReceivers>
{
const ITransferAcceptanceFilter* filter_;
virtual void handleFrame(const RxFrame& frame);
public:
typedef TransferListener<MaxBufSize, NumStaticBufs, NumStaticReceivers> BaseType;
struct ExpectedResponseParams
{
NodeID src_node_id;
TransferID transfer_id;
ExpectedResponseParams()
{
UAVCAN_ASSERT(!src_node_id.isValid());
}
ExpectedResponseParams(NodeID arg_src_node_id, TransferID arg_transfer_id)
: src_node_id(arg_src_node_id)
, transfer_id(arg_transfer_id)
{
UAVCAN_ASSERT(src_node_id.isUnicast());
}
bool match(const RxFrame& frame) const
{
UAVCAN_ASSERT(frame.getTransferType() == TransferTypeServiceResponse);
return (frame.getSrcNodeID() == src_node_id) && (frame.getTransferID() == transfer_id);
}
};
private:
ExpectedResponseParams response_params_;
void handleFrame(const RxFrame& frame);
public:
ServiceResponseTransferListener(TransferPerfCounter& perf, const DataTypeDescriptor& data_type,
IPoolAllocator& allocator)
TransferListenerWithFilter(TransferPerfCounter& perf, const DataTypeDescriptor& data_type,
IPoolAllocator& allocator)
: BaseType(perf, data_type, allocator)
, filter_(NULL)
{ }
void setExpectedResponseParams(const ExpectedResponseParams& erp);
const ExpectedResponseParams& getExpectedResponseParams() const { return response_params_; }
void stopAcceptingAnything();
void installAcceptanceFilter(const ITransferAcceptanceFilter* acceptance_filter)
{
filter_ = acceptance_filter;
}
};
// ----------------------------------------------------------------------------
/*
* ServiceResponseTransferListener<>
* TransferListenerWithFilter<>
*/
template <unsigned MaxBufSize, unsigned NumStaticBufs, unsigned NumStaticReceivers>
void ServiceResponseTransferListener<MaxBufSize, NumStaticBufs, NumStaticReceivers>::handleFrame(const RxFrame& frame)
void TransferListenerWithFilter<MaxBufSize, NumStaticBufs, NumStaticReceivers>::handleFrame(const RxFrame& frame)
{
if (response_params_.match(frame))
if (filter_ != NULL)
{
BaseType::handleFrame(frame);
if (filter_->shouldAcceptFrame(frame))
{
BaseType::handleFrame(frame);
}
}
else
{
UAVCAN_ASSERT(0);
}
}
template <unsigned MaxBufSize, unsigned NumStaticBufs, unsigned NumStaticReceivers>
void ServiceResponseTransferListener<MaxBufSize, NumStaticBufs, NumStaticReceivers>::setExpectedResponseParams(
const ExpectedResponseParams& erp)
{
response_params_ = erp;
}
template <unsigned MaxBufSize, unsigned NumStaticBufs, unsigned NumStaticReceivers>
void ServiceResponseTransferListener<MaxBufSize, NumStaticBufs, NumStaticReceivers>::stopAcceptingAnything()
{
response_params_ = ExpectedResponseParams();
}
}
libuavcan/include/uavcan/transport/transfer_sender.hpp
+23 -8
View File
@@ -20,16 +20,17 @@ namespace uavcan
class UAVCAN_EXPORT TransferSender
{
const MonotonicDuration max_transfer_interval_;
const DataTypeDescriptor& data_type_;
Dispatcher& dispatcher_;
TransferPriority priority_;
const CanTxQueue::Qos qos_;
const TransferCRC crc_base_;
CanTxQueue::Qos qos_;
TransferCRC crc_base_;
DataTypeID data_type_id_;
CanIOFlags flags_;
uint8_t iface_mask_;
bool allow_anonymous_transfers_;
Dispatcher& dispatcher_;
void registerError() const;
public:
@@ -43,16 +44,30 @@ public:
TransferSender(Dispatcher& dispatcher, const DataTypeDescriptor& data_type, CanTxQueue::Qos qos,
MonotonicDuration max_transfer_interval = getDefaultMaxTransferInterval())
: max_transfer_interval_(max_transfer_interval)
, data_type_(data_type)
, dispatcher_(dispatcher)
, priority_(TransferPriorityNormal)
, qos_(qos)
, crc_base_(data_type.getSignature().toTransferCRC())
, qos_(CanTxQueue::Qos())
, flags_(CanIOFlags(0))
, iface_mask_(AllIfacesMask)
, allow_anonymous_transfers_(false)
{
init(data_type, qos);
}
TransferSender(Dispatcher& dispatcher, MonotonicDuration max_transfer_interval = getDefaultMaxTransferInterval())
: max_transfer_interval_(max_transfer_interval)
, dispatcher_(dispatcher)
, priority_(TransferPriorityNormal)
, qos_(CanTxQueue::Qos())
, flags_(CanIOFlags(0))
, iface_mask_(AllIfacesMask)
, allow_anonymous_transfers_(false)
{ }
void init(const DataTypeDescriptor& dtid, CanTxQueue::Qos qos);
bool isInitialized() const { return data_type_id_ != DataTypeID(); }
CanIOFlags getCanIOFlags() const { return flags_; }
void setCanIOFlags(CanIOFlags flags) { flags_ = flags; }
libuavcan/include/uavcan/util/lazy_constructor.hpp
+25 -21
View File
@@ -26,12 +26,19 @@ namespace uavcan
template <typename T>
class UAVCAN_EXPORT LazyConstructor
{
#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
struct
{
alignas(T) unsigned char pool[sizeof(T)];
} data_;
#else
union
{
unsigned char pool[sizeof(T)];
long double _aligner1;
long long _aligner2;
} data_;
#endif
T* ptr_;
@@ -51,12 +58,6 @@ class UAVCAN_EXPORT LazyConstructor
}
}
template <typename U> struct ParamType { typedef const U& Type; };
template <typename U> struct ParamType<U&> { typedef U& Type; };
#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
template <typename U> struct ParamType<U&&> { typedef U&& Type; };
#endif
public:
LazyConstructor()
: ptr_(NULL)
@@ -123,50 +124,53 @@ public:
// for nargs in range(1, MAX_ARGS + 1):
// nums = [(x + 1) for x in range(nargs)]
// l1 = ['typename P%d' % x for x in nums]
// l2 = ['typename ParamType<P%d>::Type p%d' % (x, x) for x in nums]
// l2 = ['typename ParameterType<P%d>::Type p%d' % (x, x) for x in nums]
// l3 = ['p%d' % x for x in nums]
// print(TEMPLATE % (', '.join(l1), ', '.join(l2), ', '.join(l3)))
template <typename P1>
void construct(typename ParamType<P1>::Type p1)
void construct(typename ParameterType<P1>::Type p1)
{
ensureNotConstructed();
ptr_ = new (static_cast<void*>(data_.pool)) T(p1);
}
template <typename P1, typename P2>
void construct(typename ParamType<P1>::Type p1, typename ParamType<P2>::Type p2)
template<typename P1, typename P2>
void construct(typename ParameterType<P1>::Type p1, typename ParameterType<P2>::Type p2)
{
ensureNotConstructed();
ptr_ = new (static_cast<void*>(data_.pool)) T(p1, p2);
}
template <typename P1, typename P2, typename P3>
void construct(typename ParamType<P1>::Type p1, typename ParamType<P2>::Type p2, typename ParamType<P3>::Type p3)
template<typename P1, typename P2, typename P3>
void construct(typename ParameterType<P1>::Type p1, typename ParameterType<P2>::Type p2,
typename ParameterType<P3>::Type p3)
{
ensureNotConstructed();
ptr_ = new (static_cast<void*>(data_.pool)) T(p1, p2, p3);
}
template <typename P1, typename P2, typename P3, typename P4>
void construct(typename ParamType<P1>::Type p1, typename ParamType<P2>::Type p2, typename ParamType<P3>::Type p3,
typename ParamType<P4>::Type p4)
template<typename P1, typename P2, typename P3, typename P4>
void construct(typename ParameterType<P1>::Type p1, typename ParameterType<P2>::Type p2,
typename ParameterType<P3>::Type p3, typename ParameterType<P4>::Type p4)
{
ensureNotConstructed();
ptr_ = new (static_cast<void*>(data_.pool)) T(p1, p2, p3, p4);
}
template <typename P1, typename P2, typename P3, typename P4, typename P5>
void construct(typename ParamType<P1>::Type p1, typename ParamType<P2>::Type p2, typename ParamType<P3>::Type p3,
typename ParamType<P4>::Type p4, typename ParamType<P5>::Type p5)
template<typename P1, typename P2, typename P3, typename P4, typename P5>
void construct(typename ParameterType<P1>::Type p1, typename ParameterType<P2>::Type p2,
typename ParameterType<P3>::Type p3, typename ParameterType<P4>::Type p4,
typename ParameterType<P5>::Type p5)
{
ensureNotConstructed();
ptr_ = new (static_cast<void*>(data_.pool)) T(p1, p2, p3, p4, p5);
}
template <typename P1, typename P2, typename P3, typename P4, typename P5, typename P6>
void construct(typename ParamType<P1>::Type p1, typename ParamType<P2>::Type p2, typename ParamType<P3>::Type p3,
typename ParamType<P4>::Type p4, typename ParamType<P5>::Type p5, typename ParamType<P6>::Type p6)
template<typename P1, typename P2, typename P3, typename P4, typename P5, typename P6>
void construct(typename ParameterType<P1>::Type p1, typename ParameterType<P2>::Type p2,
typename ParameterType<P3>::Type p3, typename ParameterType<P4>::Type p4,
typename ParameterType<P5>::Type p5, typename ParameterType<P6>::Type p6)
{
ensureNotConstructed();
ptr_ = new (static_cast<void*>(data_.pool)) T(p1, p2, p3, p4, p5, p6);
libuavcan/include/uavcan/util/map.hpp
+44 -31
View File
@@ -11,6 +11,7 @@
#include <uavcan/build_config.hpp>
#include <uavcan/dynamic_memory.hpp>
#include <uavcan/util/templates.hpp>
#include <uavcan/util/placement_new.hpp>
namespace uavcan
{
@@ -107,7 +108,7 @@ private:
const unsigned num_static_entries_;
#endif
KVPair* find(const Key& key);
KVPair* findKey(const Key& key);
#if !UAVCAN_TINY
void optimizeStorage();
@@ -116,7 +117,7 @@ private:
struct YesPredicate
{
bool operator()(const Key& k, const Value& v) const { (void)k; (void)v; return true; }
bool operator()(const Key&, const Value&) const { return true; }
};
protected:
@@ -136,8 +137,11 @@ protected:
}
#endif
/// Derived class destructor must call removeAll();
~MapBase() { }
/// Derived class destructor must call clear();
~MapBase()
{
UAVCAN_ASSERT(getSize() == 0);
}
public:
/**
@@ -158,10 +162,10 @@ public:
/**
* Removes entries where the predicate returns true.
* Predicate prototype:
* bool (const Key& key, const Value& value)
* bool (Key& key, Value& value)
*/
template <typename Predicate>
void removeWhere(Predicate predicate);
void removeAllWhere(Predicate predicate);
/**
* Returns first entry where the predicate returns true.
@@ -169,9 +173,12 @@ public:
* bool (const Key& key, const Value& value)
*/
template <typename Predicate>
const Key* findFirstKey(Predicate predicate) const;
const Key* find(Predicate predicate) const;
void removeAll();
/**
* Removes all items.
*/
void clear();
/**
* Returns a key-value pair located at the specified position from the beginning.
@@ -181,7 +188,10 @@ public:
KVPair* getByIndex(unsigned index);
const KVPair* getByIndex(unsigned index) const;
bool isEmpty() const;
/**
* Complexity is O(1).
*/
bool isEmpty() const { return find(YesPredicate()) == NULL; }
unsigned getSize() const;
@@ -207,7 +217,7 @@ public:
: MapBase<Key, Value>(static_, NumStaticEntries, allocator)
{ }
~Map() { this->removeAll(); }
~Map() { this->clear(); }
#endif // !UAVCAN_TINY
};
@@ -225,7 +235,7 @@ public:
#endif
{ }
~Map() { this->removeAll(); }
~Map() { this->clear(); }
};
// ----------------------------------------------------------------------------
@@ -234,7 +244,7 @@ public:
* MapBase<>
*/
template <typename Key, typename Value>
typename MapBase<Key, Value>::KVPair* MapBase<Key, Value>::find(const Key& key)
typename MapBase<Key, Value>::KVPair* MapBase<Key, Value>::findKey(const Key& key)
{
#if !UAVCAN_TINY
for (unsigned i = 0; i < num_static_entries_; i++)
@@ -344,7 +354,7 @@ template <typename Key, typename Value>
Value* MapBase<Key, Value>::access(const Key& key)
{
UAVCAN_ASSERT(!(key == Key()));
KVPair* const kv = find(key);
KVPair* const kv = findKey(key);
return kv ? &kv->value : NULL;
}
@@ -354,7 +364,7 @@ Value* MapBase<Key, Value>::insert(const Key& key, const Value& value)
UAVCAN_ASSERT(!(key == Key()));
remove(key);
KVPair* const kv = find(Key());
KVPair* const kv = findKey(Key());
if (kv)
{
*kv = KVPair(key, value);
@@ -375,7 +385,7 @@ template <typename Key, typename Value>
void MapBase<Key, Value>::remove(const Key& key)
{
UAVCAN_ASSERT(!(key == Key()));
KVPair* const kv = find(key);
KVPair* const kv = findKey(key);
if (kv)
{
*kv = KVPair();
@@ -388,7 +398,7 @@ void MapBase<Key, Value>::remove(const Key& key)
template <typename Key, typename Value>
template <typename Predicate>
void MapBase<Key, Value>::removeWhere(Predicate predicate)
void MapBase<Key, Value>::removeAllWhere(Predicate predicate)
{
unsigned num_removed = 0;
@@ -407,8 +417,10 @@ void MapBase<Key, Value>::removeWhere(Predicate predicate)
#endif
KVGroup* p = list_.get();
while (p)
while (p != NULL)
{
KVGroup* const next_group = p->getNextListNode();
for (int i = 0; i < KVGroup::NumKV; i++)
{
const KVPair* const kv = p->kvs + i;
@@ -421,7 +433,8 @@ void MapBase<Key, Value>::removeWhere(Predicate predicate)
}
}
}
p = p->getNextListNode();
p = next_group;
}
if (num_removed > 0)
@@ -435,7 +448,7 @@ void MapBase<Key, Value>::removeWhere(Predicate predicate)
template <typename Key, typename Value>
template <typename Predicate>
const Key* MapBase<Key, Value>::findFirstKey(Predicate predicate) const
const Key* MapBase<Key, Value>::find(Predicate predicate) const
{
#if !UAVCAN_TINY
for (unsigned i = 0; i < num_static_entries_; i++)
@@ -451,8 +464,10 @@ const Key* MapBase<Key, Value>::findFirstKey(Predicate predicate) const
#endif
KVGroup* p = list_.get();
while (p)
while (p != NULL)
{
KVGroup* const next_group = p->getNextListNode();
for (int i = 0; i < KVGroup::NumKV; i++)
{
const KVPair* const kv = p->kvs + i;
@@ -464,15 +479,16 @@ const Key* MapBase<Key, Value>::findFirstKey(Predicate predicate) const
}
}
}
p = p->getNextListNode();
p = next_group;
}
return NULL;
}
template <typename Key, typename Value>
void MapBase<Key, Value>::removeAll()
void MapBase<Key, Value>::clear()
{
removeWhere(YesPredicate());
removeAllWhere(YesPredicate());
}
template <typename Key, typename Value>
@@ -495,8 +511,10 @@ typename MapBase<Key, Value>::KVPair* MapBase<Key, Value>::getByIndex(unsigned i
// Slowly crawling through the dynamic storage
KVGroup* p = list_.get();
while (p)
while (p != NULL)
{
KVGroup* const next_group = p->getNextListNode();
for (int i = 0; i < KVGroup::NumKV; i++)
{
KVPair* const kv = p->kvs + i;
@@ -509,7 +527,8 @@ typename MapBase<Key, Value>::KVPair* MapBase<Key, Value>::getByIndex(unsigned i
index--;
}
}
p = p->getNextListNode();
p = next_group;
}
return NULL;
@@ -521,12 +540,6 @@ const typename MapBase<Key, Value>::KVPair* MapBase<Key, Value>::getByIndex(unsi
return const_cast<MapBase<Key, Value>*>(this)->getByIndex(index);
}
template <typename Key, typename Value>
bool MapBase<Key, Value>::isEmpty() const
{
return getSize() == 0;
}
template <typename Key, typename Value>
unsigned MapBase<Key, Value>::getSize() const
{
libuavcan/include/uavcan/util/multiset.hpp
+542
View File
@@ -0,0 +1,542 @@
/*
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#ifndef UAVCAN_UTIL_MULTISET_HPP_INCLUDED
#define UAVCAN_UTIL_MULTISET_HPP_INCLUDED
#include <cassert>
#include <cstdlib>
#include <uavcan/util/linked_list.hpp>
#include <uavcan/build_config.hpp>
#include <uavcan/dynamic_memory.hpp>
#include <uavcan/util/templates.hpp>
#include <uavcan/util/placement_new.hpp>
#if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11)
# error UAVCAN_CPP_VERSION
#endif
namespace uavcan
{
/**
* Slow but memory efficient unordered multiset. Unlike Map<>, this container does not move objects, so
* they don't have to be copyable.
*
* Items can be allocated in a static buffer or in the node's memory pool if the static buffer is exhausted.
*
* Number of static entries must not be less than 1.
*/
template <typename T, unsigned NumStaticEntries>
class UAVCAN_EXPORT Multiset : Noncopyable
{
struct Item : ::uavcan::Noncopyable
{
T* ptr;
#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
alignas(T) unsigned char pool[sizeof(T)]; ///< Memory efficient version
#else
union
{
unsigned char pool[sizeof(T)];
/*
* Such alignment does not guarantee safety for all types (only for libuavcan internal ones);
* however, increasing it is too memory inefficient. So it is recommended to use C++11, where
* this issue is resolved with alignas() (see above).
*/
void* _aligner1_;
long long _aligner2_;
};
#endif
Item()
: ptr(NULL)
{
fill_n(pool, sizeof(pool), static_cast<unsigned char>(0));
}
~Item() { destroy(); }
bool isConstructed() const { return ptr != NULL; }
void destroy()
{
if (ptr != NULL)
{
ptr->~T();
ptr = NULL;
fill_n(pool, sizeof(pool), static_cast<unsigned char>(0));
}
}
};
private:
struct Chunk : LinkedListNode<Chunk>, ::uavcan::Noncopyable
{
enum { NumItems = (MemPoolBlockSize - sizeof(LinkedListNode<Chunk>)) / sizeof(Item) };
Item items[NumItems];
Chunk()
{
StaticAssert<(static_cast<unsigned>(NumItems) > 0)>::check();
IsDynamicallyAllocatable<Chunk>::check();
UAVCAN_ASSERT(!items[0].isConstructed());
}
static Chunk* instantiate(IPoolAllocator& allocator)
{
void* const praw = allocator.allocate(sizeof(Chunk));
if (praw == NULL)
{
return NULL;
}
return new (praw) Chunk();
}
static void destroy(Chunk*& obj, IPoolAllocator& allocator)
{
if (obj != NULL)
{
obj->~Chunk();
allocator.deallocate(obj);
obj = NULL;
}
}
Item* findFreeSlot()
{
for (unsigned i = 0; i < static_cast<unsigned>(NumItems); i++)
{
if (!items[i].isConstructed())
{
return items + i;
}
}
return NULL;
}
};
/*
* Data
*/
LinkedListRoot<Chunk> list_;
IPoolAllocator& allocator_;
Item static_[NumStaticEntries];
/*
* Methods
*/
Item* findOrCreateFreeSlot();
void compact();
enum RemoveStrategy { RemoveOne, RemoveAll };
template <typename Predicate>
void removeWhere(Predicate predicate, RemoveStrategy strategy);
struct YesPredicate
{
bool operator()(const T&) const { return true; }
};
struct IndexPredicate : ::uavcan::Noncopyable
{
unsigned index;
IndexPredicate(unsigned target_index)
: index(target_index)
{ }
bool operator()(const T&)
{
return index-- == 0;
}
};
struct ComparingPredicate
{
const T& reference;
ComparingPredicate(const T& ref)
: reference(ref)
{ }
bool operator()(const T& sample)
{
return reference == sample;
}
};
template<typename Operator>
struct OperatorToFalsePredicateAdapter : ::uavcan::Noncopyable
{
Operator oper;
OperatorToFalsePredicateAdapter(Operator o)
: oper(o)
{ }
bool operator()(T& item)
{
oper(item);
return false;
}
bool operator()(const T& item) const
{
oper(item);
return false;
}
};
public:
Multiset(IPoolAllocator& allocator)
: allocator_(allocator)
{ }
~Multiset()
{
clear();
}
/**
* Creates one item in-place and returns a pointer to it.
* If creation fails due to lack of memory, NULL will be returned.
* Complexity is O(N).
*/
T* emplace()
{
Item* const item = findOrCreateFreeSlot();
if (item == NULL)
{
return NULL;
}
UAVCAN_ASSERT(item->ptr == NULL);
item->ptr = new (item->pool) T();
return item->ptr;
}
template <typename P1>
T* emplace(P1 p1)
{
Item* const item = findOrCreateFreeSlot();
if (item == NULL)
{
return NULL;
}
UAVCAN_ASSERT(item->ptr == NULL);
item->ptr = new (item->pool) T(p1);
return item->ptr;
}
template <typename P1, typename P2>
T* emplace(P1 p1, P2 p2)
{
Item* const item = findOrCreateFreeSlot();
if (item == NULL)
{
return NULL;
}
UAVCAN_ASSERT(item->ptr == NULL);
item->ptr = new (item->pool) T(p1, p2);
return item->ptr;
}
template <typename P1, typename P2, typename P3>
T* emplace(P1 p1, P2 p2, P3 p3)
{
Item* const item = findOrCreateFreeSlot();
if (item == NULL)
{
return NULL;
}
UAVCAN_ASSERT(item->ptr == NULL);
item->ptr = new (item->pool) T(p1, p2, p3);
return item->ptr;
}
/**
* Removes entries where the predicate returns true.
* Predicate prototype:
* bool (T& item)
*/
template <typename Predicate>
void removeAllWhere(Predicate predicate) { removeWhere<Predicate>(predicate, RemoveAll); }
template <typename Predicate>
void removeFirstWhere(Predicate predicate) { removeWhere<Predicate>(predicate, RemoveOne); }
void removeFirst(const T& ref) { removeFirstWhere(ComparingPredicate(ref)); }
void removeAll(const T& ref) { removeAllWhere(ComparingPredicate(ref)); }
void clear() { removeAllWhere(YesPredicate()); }
/**
* Returns first entry where the predicate returns true.
* Predicate prototype:
* bool (const T& item)
*/
template <typename Predicate>
T* find(Predicate predicate);
template <typename Predicate>
const T* find(Predicate predicate) const
{
return const_cast<Multiset*>(this)->find<Predicate>(predicate);
}
/**
* Calls Operator for each item of the set.
* Operator prototype:
* void (T& item)
* void (const T& item) - const overload
*/
template <typename Operator>
void forEach(Operator oper)
{
OperatorToFalsePredicateAdapter<Operator> adapter(oper);
(void)find<OperatorToFalsePredicateAdapter<Operator>&>(adapter);
}
template <typename Operator>
void forEach(Operator oper) const
{
const OperatorToFalsePredicateAdapter<Operator> adapter(oper);
(void)find<const OperatorToFalsePredicateAdapter<Operator>&>(adapter);
}
/**
* Returns an item located at the specified position from the beginning.
* Note that addition and removal operations invalidate indices.
* If index is greater than or equal the number of items, null pointer will be returned.
* Complexity is O(N).
*/
T* getByIndex(unsigned index)
{
IndexPredicate predicate(index);
return find<IndexPredicate&>(predicate);
}
const T* getByIndex(unsigned index) const
{
return const_cast<Multiset*>(this)->getByIndex(index);
}
/**
* Complexity is O(1).
*/
bool isEmpty() const { return find(YesPredicate()) == NULL; }
/**
* Counts number of items stored.
* Best case complexity is O(N).
*/
unsigned getSize() const { return getNumStaticItems() + getNumDynamicItems(); }
/**
* For testing, do not use directly.
*/
unsigned getNumStaticItems() const;
unsigned getNumDynamicItems() const;
};
// ----------------------------------------------------------------------------
/*
* Multiset<>
*/
template <typename T, unsigned NumStaticEntries>
typename Multiset<T, NumStaticEntries>::Item* Multiset<T, NumStaticEntries>::findOrCreateFreeSlot()
{
#if !UAVCAN_TINY
// Search in static pool
for (unsigned i = 0; i < NumStaticEntries; i++)
{
if (!static_[i].isConstructed())
{
return &static_[i];
}
}
#endif
// Search in dynamic pool
{
Chunk* p = list_.get();
while (p)
{
Item* const dyn = p->findFreeSlot();
if (dyn != NULL)
{
return dyn;
}
p = p->getNextListNode();
}
}
// Create new dynamic chunk
Chunk* const chunk = Chunk::instantiate(allocator_);
if (chunk == NULL)
{
return NULL;
}
list_.insert(chunk);
return &chunk->items[0];
}
template <typename T, unsigned NumStaticEntries>
void Multiset<T, NumStaticEntries>::compact()
{
Chunk* p = list_.get();
while (p)
{
Chunk* const next = p->getNextListNode();
bool remove_this = true;
for (int i = 0; i < Chunk::NumItems; i++)
{
if (p->items[i].isConstructed())
{
remove_this = false;
break;
}
}
if (remove_this)
{
list_.remove(p);
Chunk::destroy(p, allocator_);
}
p = next;
}
}
template <typename T, unsigned NumStaticEntries>
template <typename Predicate>
void Multiset<T, NumStaticEntries>::removeWhere(Predicate predicate, const RemoveStrategy strategy)
{
unsigned num_removed = 0;
#if !UAVCAN_TINY
for (unsigned i = 0; i < NumStaticEntries; i++)
{
if (static_[i].isConstructed())
{
if (predicate(*static_[i].ptr))
{
num_removed++;
static_[i].destroy();
if (strategy == RemoveOne)
{
break;
}
}
}
}
#endif
Chunk* p = list_.get();
while (p != NULL)
{
Chunk* const next_chunk = p->getNextListNode(); // For the case if the current entry gets modified
if ((num_removed > 0) && (strategy == RemoveOne))
{
break;
}
for (int i = 0; i < Chunk::NumItems; i++)
{
Item& item = p->items[i];
if (item.isConstructed())
{
if (predicate(*item.ptr))
{
num_removed++;
item.destroy();
if (strategy == RemoveOne)
{
break;
}
}
}
}
p = next_chunk;
}
if (num_removed > 0)
{
compact();
}
}
template <typename T, unsigned NumStaticEntries>
template <typename Predicate>
T* Multiset<T, NumStaticEntries>::find(Predicate predicate)
{
#if !UAVCAN_TINY
for (unsigned i = 0; i < NumStaticEntries; i++)
{
if (static_[i].isConstructed())
{
if (predicate(*static_[i].ptr))
{
return static_[i].ptr;
}
}
}
#endif
Chunk* p = list_.get();
while (p != NULL)
{
Chunk* const next_chunk = p->getNextListNode(); // For the case if the current entry gets modified
for (int i = 0; i < Chunk::NumItems; i++)
{
if (p->items[i].isConstructed())
{
if (predicate(*p->items[i].ptr))
{
return p->items[i].ptr;
}
}
}
p = next_chunk;
}
return NULL;
}
template <typename T, unsigned NumStaticEntries>
unsigned Multiset<T, NumStaticEntries>::getNumStaticItems() const
{
unsigned num = 0;
#if !UAVCAN_TINY
for (unsigned i = 0; i < NumStaticEntries; i++)
{
num += static_[i].isConstructed() ? 1U : 0U;
}
#endif
return num;
}
template <typename T, unsigned NumStaticEntries>
unsigned Multiset<T, NumStaticEntries>::getNumDynamicItems() const
{
unsigned num = 0;
Chunk* p = list_.get();
while (p)
{
for (int i = 0; i < Chunk::NumItems; i++)
{
num += p->items[i].isConstructed() ? 1U : 0U;
}
p = p->getNextListNode();
}
return num;
}
}
#endif // Include guard
libuavcan/include/uavcan/util/templates.hpp
+9
View File
@@ -100,6 +100,15 @@ template <typename T> struct RemoveReference<T&> { typedef T Type; };
template <typename T> struct RemoveReference<T&&> { typedef T Type; };
#endif
/**
* Parameter types
*/
template <typename U> struct ParameterType { typedef const U& Type; };
template <typename U> struct ParameterType<U&> { typedef U& Type; };
#if UAVCAN_CPP_VERSION > UAVCAN_CPP03
template <typename U> struct ParameterType<U&&> { typedef U&& Type; };
#endif
/**
* Value types
*/
libuavcan/src/node/uc_generic_publisher.cpp
+11 -20
View File
@@ -9,7 +9,7 @@ namespace uavcan
bool GenericPublisherBase::isInited() const
{
return bool(sender_);
return sender_.isInitialized();
}
int GenericPublisherBase::doInit(DataTypeKind dtkind, const char* dtname, CanTxQueue::Qos qos)
@@ -22,13 +22,14 @@ int GenericPublisherBase::doInit(DataTypeKind dtkind, const char* dtname, CanTxQ
GlobalDataTypeRegistry::instance().freeze();
const DataTypeDescriptor* const descr = GlobalDataTypeRegistry::instance().find(dtkind, dtname);
if (!descr)
if (descr == NULL)
{
UAVCAN_TRACE("GenericPublisher", "Type [%s] is not registered", dtname);
return -ErrUnknownDataType;
}
sender_.construct<Dispatcher&, const DataTypeDescriptor&, CanTxQueue::Qos, MonotonicDuration>
(node_.getDispatcher(), *descr, qos, max_transfer_interval_);
sender_.init(*descr, qos);
return 0;
}
@@ -37,31 +38,21 @@ MonotonicTime GenericPublisherBase::getTxDeadline() const
return node_.getMonotonicTime() + tx_timeout_;
}
IMarshalBuffer* GenericPublisherBase::getBuffer(unsigned byte_len)
{
return node_.getMarshalBufferProvider().getBuffer(byte_len);
}
int GenericPublisherBase::genericPublish(const IMarshalBuffer& buffer, TransferType transfer_type, NodeID dst_node_id,
TransferID* tid, MonotonicTime blocking_deadline)
int GenericPublisherBase::genericPublish(const StaticTransferBufferImpl& buffer, TransferType transfer_type,
NodeID dst_node_id, TransferID* tid, MonotonicTime blocking_deadline)
{
if (tid)
{
return sender_->send(buffer.getDataPtr(), buffer.getDataLength(), getTxDeadline(),
blocking_deadline, transfer_type, dst_node_id, *tid);
return sender_.send(buffer.getRawPtr(), buffer.getMaxWritePos(), getTxDeadline(),
blocking_deadline, transfer_type, dst_node_id, *tid);
}
else
{
return sender_->send(buffer.getDataPtr(), buffer.getDataLength(), getTxDeadline(),
blocking_deadline, transfer_type, dst_node_id);
return sender_.send(buffer.getRawPtr(), buffer.getMaxWritePos(), getTxDeadline(),
blocking_deadline, transfer_type, dst_node_id);
}
}
TransferSender* GenericPublisherBase::getTransferSender()
{
return sender_.isConstructed() ? static_cast<TransferSender*>(sender_) : NULL;
}
void GenericPublisherBase::setTxTimeout(MonotonicDuration tx_timeout)
{
tx_timeout = max(tx_timeout, getMinTxTimeout());
libuavcan/src/node/uc_scheduler.cpp
+24 -2
View File
@@ -159,7 +159,8 @@ int Scheduler::spin(MonotonicTime deadline)
UAVCAN_ASSERT(0);
return -ErrRecursiveCall;
}
inside_spin_ = true;
InsideSpinSetter iss(*this);
UAVCAN_ASSERT(inside_spin_);
int retval = 0;
while (true)
@@ -179,7 +180,28 @@ int Scheduler::spin(MonotonicTime deadline)
}
}
inside_spin_ = false;
return retval;
}
int Scheduler::spinOnce()
{
if (inside_spin_) // Preventing recursive calls
{
UAVCAN_ASSERT(0);
return -ErrRecursiveCall;
}
InsideSpinSetter iss(*this);
UAVCAN_ASSERT(inside_spin_);
const int retval = dispatcher_.spinOnce();
if (retval < 0)
{
return retval;
}
const MonotonicTime ts = deadline_scheduler_.pollAndGetMonotonicTime(getSystemClock());
pollCleanup(ts, unsigned(retval));
return retval;
}
libuavcan/src/node/uc_service_client.cpp
+27 -9
View File
@@ -6,12 +6,33 @@
namespace uavcan
{
int ServiceClientBase::prepareToCall(INode& node, const char* dtname, NodeID server_node_id,
TransferID& out_transfer_id)
/*
* ServiceClientBase::CallState
*/
void ServiceClientBase::CallState::handleDeadline(MonotonicTime)
{
pending_ = true;
UAVCAN_TRACE("ServiceClient::CallState", "Timeout from nid=%d, tid=%d, dtname=%s",
int(id_.server_node_id.get()), int(id_.transfer_id.get()),
(owner_.data_type_descriptor_ == NULL) ? "???" : owner_.data_type_descriptor_->getFullName());
/*
* What we're doing here is relaying execution from this call stack to a different one.
* We need it because call registry cannot release memory from this callback, because this will destroy the
* object method of which we're executing now.
*/
UAVCAN_ASSERT(timed_out_ == false);
timed_out_ = true;
owner_.generateDeadlineImmediately();
UAVCAN_TRACE("ServiceClient::CallState", "Relaying execution to the owner's handler via timer callback");
}
/*
* ServiceClientBase
*/
int ServiceClientBase::prepareToCall(INode& node,
const char* dtname,
NodeID server_node_id,
ServiceCallID& out_call_id)
{
/*
* Making sure we're not going to get transport error because of invalid input data
*/
@@ -20,6 +41,7 @@ int ServiceClientBase::prepareToCall(INode& node, const char* dtname, NodeID ser
UAVCAN_TRACE("ServiceClient", "Invalid Server Node ID");
return -ErrInvalidParam;
}
out_call_id.server_node_id = server_node_id;
/*
* Determining the Data Type ID
@@ -50,13 +72,9 @@ int ServiceClientBase::prepareToCall(INode& node, const char* dtname, NodeID ser
UAVCAN_TRACE("ServiceClient", "OTR access failure, dtd=%s", data_type_descriptor_->toString().c_str());
return -ErrMemory;
}
out_transfer_id = *otr_tid;
out_call_id.transfer_id = *otr_tid;
otr_tid->increment();
/*
* Registering the deadline handler
*/
DeadlineHandler::startWithDelay(request_timeout_);
return 0;
}
libuavcan/src/protocol/uc_data_type_info_provider.cpp
-140
View File
@@ -1,140 +0,0 @@
/*
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include <uavcan/protocol/data_type_info_provider.hpp>
#include <uavcan/debug.hpp>
namespace uavcan
{
bool DataTypeInfoProvider::isValidDataTypeKind(DataTypeKind kind)
{
return (kind == DataTypeKindMessage) || (kind == DataTypeKindService);
}
void DataTypeInfoProvider::handleComputeAggregateTypeSignatureRequest(
const protocol::ComputeAggregateTypeSignature::Request& request,
protocol::ComputeAggregateTypeSignature::Response& response)
{
const DataTypeKind kind = DataTypeKind(request.kind.value); // No mapping needed
if (!isValidDataTypeKind(kind))
{
UAVCAN_TRACE("DataTypeInfoProvider",
"ComputeAggregateTypeSignature request with invalid DataTypeKind %d", kind);
return;
}
UAVCAN_TRACE("DataTypeInfoProvider", "ComputeAggregateTypeSignature request for dtk=%d, len(known_ids)=%d",
int(request.kind.value), int(request.known_ids.size()));
// Correcting the mask length according to the data type kind
response.mutually_known_ids = request.known_ids;
response.mutually_known_ids.resize(static_cast<uint16_t>(DataTypeID::getMaxValueForDataTypeKind(kind).get() + 1U));
response.aggregate_signature =
GlobalDataTypeRegistry::instance().computeAggregateSignature(kind, response.mutually_known_ids).get();
}
void DataTypeInfoProvider::handleGetDataTypeInfoRequest(const protocol::GetDataTypeInfo::Request& request,
protocol::GetDataTypeInfo::Response& response)
{
/*
* Asking the Global Data Type Registry for the matching type descriptor, either by name or by ID
*/
const DataTypeDescriptor* desc = NULL;
if (request.name.empty())
{
response.id = request.id; // Pre-setting the fields so they have meaningful values even in
response.kind = request.kind; // ...case of failure.
if (!isValidDataTypeKind(DataTypeKind(request.kind.value)))
{
UAVCAN_TRACE("DataTypeInfoProvider", "GetDataTypeInfo request with invalid DataTypeKind %i",
static_cast<int>(request.kind.value));
return;
}
desc = GlobalDataTypeRegistry::instance().find(DataTypeKind(request.kind.value), request.id);
}
else
{
response.name = request.name;
desc = GlobalDataTypeRegistry::instance().find(request.name.c_str());
}
if (desc == NULL)
{
UAVCAN_TRACE("DataTypeInfoProvider",
"Cannot process GetDataTypeInfo for nonexistent type: dtid=%i dtk=%i name='%s'",
static_cast<int>(request.id), static_cast<int>(request.kind.value), request.name.c_str());
return;
}
UAVCAN_TRACE("DataTypeInfoProvider", "GetDataTypeInfo request for %s", desc->toString().c_str());
/*
* Filling the response struct
*/
response.signature = desc->getSignature().get();
response.id = desc->getID().get();
response.kind.value = desc->getKind();
response.mask = protocol::GetDataTypeInfo::Response::MASK_KNOWN;
response.name = desc->getFullName();
const Dispatcher& dispatcher = getNode().getDispatcher();
if (desc->getKind() == DataTypeKindService)
{
if (dispatcher.hasServer(desc->getID().get()))
{
response.mask |= protocol::GetDataTypeInfo::Response::MASK_SERVING;
}
}
else if (desc->getKind() == DataTypeKindMessage)
{
if (dispatcher.hasSubscriber(desc->getID().get()))
{
response.mask |= protocol::GetDataTypeInfo::Response::MASK_SUBSCRIBED;
}
if (dispatcher.hasPublisher(desc->getID().get()))
{
response.mask |= protocol::GetDataTypeInfo::Response::MASK_PUBLISHING;
}
}
else
{
UAVCAN_ASSERT(0); // That means that GDTR somehow found a type of an unknown kind. The horror.
}
}
int DataTypeInfoProvider::start()
{
int res = 0;
res = cats_srv_.start(
ComputeAggregateTypeSignatureCallback(this, &DataTypeInfoProvider::handleComputeAggregateTypeSignatureRequest));
if (res < 0)
{
goto fail;
}
res = gdti_srv_.start(GetDataTypeInfoCallback(this, &DataTypeInfoProvider::handleGetDataTypeInfoRequest));
if (res < 0)
{
goto fail;
}
UAVCAN_ASSERT(res >= 0);
return res;
fail:
UAVCAN_ASSERT(res < 0);
cats_srv_.stop();
gdti_srv_.stop();
return res;
}
}
libuavcan/src/protocol/uc_dynamic_node_id_allocation_client.cpp → libuavcan/src/protocol/uc_dynamic_node_id_client.cpp
+16 -22
View File
@@ -2,19 +2,19 @@
* Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include <uavcan/protocol/dynamic_node_id_allocation_client.hpp>
#include <uavcan/protocol/dynamic_node_id_client.hpp>
namespace uavcan
{
void DynamicNodeIDAllocationClient::terminate()
void DynamicNodeIDClient::terminate()
{
UAVCAN_TRACE("DynamicNodeIDAllocationClient", "Client terminated");
UAVCAN_TRACE("DynamicNodeIDClient", "Client terminated");
stop();
dnida_sub_.stop();
}
void DynamicNodeIDAllocationClient::handleTimerEvent(const TimerEvent&)
void DynamicNodeIDClient::handleTimerEvent(const TimerEvent&)
{
// This method implements Rule B
UAVCAN_ASSERT(preferred_node_id_.isValid());
@@ -35,17 +35,16 @@ void DynamicNodeIDAllocationClient::handleTimerEvent(const TimerEvent&)
UAVCAN_ASSERT(equal(msg.unique_id.begin(), msg.unique_id.end(), unique_id_));
// Broadcasting
UAVCAN_TRACE("DynamicNodeIDAllocationClient", "Broadcasting 1st stage: preferred ID: %d",
UAVCAN_TRACE("DynamicNodeIDClient", "Broadcasting 1st stage: preferred ID: %d",
static_cast<int>(preferred_node_id_.get()));
const int res = dnida_pub_.broadcast(msg);
if (res < 0)
{
dnida_pub_.getNode().registerInternalFailure("DynamicNodeIDAllocationClient request failed");
dnida_pub_.getNode().registerInternalFailure("DynamicNodeIDClient request failed");
}
}
void DynamicNodeIDAllocationClient::handleAllocation(
const ReceivedDataStructure<protocol::dynamic_node_id::Allocation>& msg)
void DynamicNodeIDClient::handleAllocation(const ReceivedDataStructure<protocol::dynamic_node_id::Allocation>& msg)
{
/*
* TODO This method can blow the stack easily
@@ -59,7 +58,7 @@ void DynamicNodeIDAllocationClient::handleAllocation(
}
startPeriodic(getPeriod()); // Restarting the timer - Rule C
UAVCAN_TRACE("DynamicNodeIDAllocationClient", "Request timer reset because of Allocation message from %d",
UAVCAN_TRACE("DynamicNodeIDClient", "Request timer reset because of Allocation message from %d",
static_cast<int>(msg.getSrcNodeID().get()));
// Rule D
@@ -88,13 +87,12 @@ void DynamicNodeIDAllocationClient::handleAllocation(
unique_id_ + msg.unique_id.size()));
// Broadcasting the response
UAVCAN_TRACE("DynamicNodeIDAllocationClient",
"Broadcasting 2nd stage: preferred ID: %d, size of unique ID: %d",
UAVCAN_TRACE("DynamicNodeIDClient", "Broadcasting 2nd stage: preferred ID: %d, size of unique ID: %d",
static_cast<int>(preferred_node_id_.get()), static_cast<int>(second_stage.unique_id.size()));
const int res = dnida_pub_.broadcast(second_stage);
if (res < 0)
{
dnida_pub_.getNode().registerInternalFailure("DynamicNodeIDAllocationClient request failed");
dnida_pub_.getNode().registerInternalFailure("DynamicNodeIDClient request failed");
}
}
@@ -106,16 +104,16 @@ void DynamicNodeIDAllocationClient::handleAllocation(
{
allocated_node_id_ = msg.node_id;
allocator_node_id_ = msg.getSrcNodeID();
UAVCAN_TRACE("DynamicNodeIDAllocationClient", "Allocation complete, node ID %d provided by %d",
UAVCAN_TRACE("DynamicNodeIDClient", "Allocation complete, node ID %d provided by %d",
static_cast<int>(allocated_node_id_.get()), static_cast<int>(allocator_node_id_.get()));
terminate();
UAVCAN_ASSERT(isAllocationComplete());
}
}
int DynamicNodeIDAllocationClient::start(const protocol::HardwareVersion& hardware_version,
const NodeID preferred_node_id,
const TransferPriority transfer_priority)
int DynamicNodeIDClient::start(const protocol::HardwareVersion& hardware_version,
const NodeID preferred_node_id,
const TransferPriority transfer_priority)
{
terminate();
@@ -162,13 +160,9 @@ int DynamicNodeIDAllocationClient::start(const protocol::HardwareVersion& hardwa
return res;
}
dnida_pub_.allowAnonymousTransfers();
res = dnida_pub_.setPriority(transfer_priority);
if (res < 0)
{
return res;
}
dnida_pub_.setPriority(transfer_priority);
res = dnida_sub_.start(AllocationCallback(this, &DynamicNodeIDAllocationClient::handleAllocation));
res = dnida_sub_.start(AllocationCallback(this, &DynamicNodeIDClient::handleAllocation));
if (res < 0)
{
return res;
libuavcan/src/protocol/uc_global_time_sync_master.cpp
-206
View File
@@ -1,206 +0,0 @@
/*
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include <uavcan/build_config.hpp>
#if !UAVCAN_TINY
#include <uavcan/protocol/global_time_sync_master.hpp>
#include <uavcan/debug.hpp>
#include <cstdlib>
#include <cassert>
namespace uavcan
{
/*
* GlobalTimeSyncMaster::IfaceMaster
*/
int GlobalTimeSyncMaster::IfaceMaster::init()
{
const int res = pub_.init();
if (res >= 0)
{
TransferSender* const ts = pub_.getTransferSender();
UAVCAN_ASSERT(ts != NULL);
ts->setIfaceMask(uint8_t(1 << iface_index_));
ts->setCanIOFlags(CanIOFlagLoopback);
const int prio_res = pub_.setPriority(TransferPriorityHigh); // Fixed priority
if (prio_res < 0)
{
return prio_res;
}
}
return res;
}
void GlobalTimeSyncMaster::IfaceMaster::setTxTimestamp(UtcTime ts)
{
if (ts.isZero())
{
UAVCAN_ASSERT(0);
pub_.getNode().registerInternalFailure("GlobalTimeSyncMaster zero TX ts");
return;
}
if (!prev_tx_utc_.isZero())
{
prev_tx_utc_ = UtcTime(); // Reset again, because there's something broken in the driver and we don't trust it
pub_.getNode().registerInternalFailure("GlobalTimeSyncMaster pub conflict");
return;
}
prev_tx_utc_ = ts;
}
int GlobalTimeSyncMaster::IfaceMaster::publish(TransferID tid, MonotonicTime current_time)
{
UAVCAN_ASSERT(pub_.getTransferSender()->getCanIOFlags() == CanIOFlagLoopback);
UAVCAN_ASSERT(pub_.getTransferSender()->getIfaceMask() == (1 << iface_index_));
const MonotonicDuration since_prev_pub = current_time - iface_prev_pub_mono_;
iface_prev_pub_mono_ = current_time;
UAVCAN_ASSERT(since_prev_pub.isPositive());
const bool long_period = since_prev_pub.toMSec() >= protocol::GlobalTimeSync::MAX_PUBLICATION_PERIOD_MS;
protocol::GlobalTimeSync msg;
msg.previous_transmission_timestamp_usec = long_period ? 0 : prev_tx_utc_.toUSec();
prev_tx_utc_ = UtcTime();
UAVCAN_TRACE("GlobalTimeSyncMaster", "Publishing %llu iface=%i tid=%i",
static_cast<unsigned long long>(msg.previous_transmission_timestamp_usec),
int(iface_index_), int(tid.get()));
return pub_.broadcast(msg, tid);
}
/*
* GlobalTimeSyncMaster
*/
void GlobalTimeSyncMaster::handleLoopbackFrame(const RxFrame& frame)
{
const uint8_t iface = frame.getIfaceIndex();
if (initialized_ && iface < MaxCanIfaces)
{
if (frame.getDataTypeID() == dtid_ &&
frame.getTransferType() == TransferTypeMessageBroadcast &&
frame.isLast() && frame.isFirst() &&
frame.getSrcNodeID() == node_.getNodeID())
{
iface_masters_[iface]->setTxTimestamp(frame.getUtcTimestamp());
}
}
else
{
UAVCAN_ASSERT(0);
}
}
int GlobalTimeSyncMaster::getNextTransferID(TransferID& tid)
{
const MonotonicDuration max_transfer_interval =
MonotonicDuration::fromMSec(protocol::GlobalTimeSync::PUBLISHER_TIMEOUT_MS);
const OutgoingTransferRegistryKey otr_key(dtid_, TransferTypeMessageBroadcast, NodeID::Broadcast);
const MonotonicTime otr_deadline = node_.getMonotonicTime() + max_transfer_interval;
TransferID* const tid_ptr =
node_.getDispatcher().getOutgoingTransferRegistry().accessOrCreate(otr_key, otr_deadline);
if (tid_ptr == NULL)
{
return -ErrMemory;
}
tid = *tid_ptr;
tid_ptr->increment();
return 0;
}
int GlobalTimeSyncMaster::init()
{
if (initialized_)
{
return 0;
}
// Data type ID
const DataTypeDescriptor* const desc =
GlobalDataTypeRegistry::instance().find(DataTypeKindMessage, protocol::GlobalTimeSync::getDataTypeFullName());
if (desc == NULL)
{
return -ErrUnknownDataType;
}
dtid_ = desc->getID();
// Iface master array
int res = -ErrLogic;
for (uint8_t i = 0; i < MaxCanIfaces; i++)
{
if (!iface_masters_[i].isConstructed())
{
iface_masters_[i].construct<INode&, uint8_t>(node_, i);
}
res = iface_masters_[i]->init();
if (res < 0)
{
break;
}
}
// Loopback listener
initialized_ = res >= 0;
if (initialized_)
{
LoopbackFrameListenerBase::startListening();
}
return res;
}
int GlobalTimeSyncMaster::publish()
{
if (!initialized_)
{
const int res = init();
if (res < 0)
{
return res;
}
}
/*
* Enforce max frequency
*/
const MonotonicTime current_time = node_.getMonotonicTime();
{
const MonotonicDuration since_prev_pub = current_time - prev_pub_mono_;
UAVCAN_ASSERT(since_prev_pub.isPositive());
if (since_prev_pub.toMSec() < protocol::GlobalTimeSync::MIN_PUBLICATION_PERIOD_MS)
{
UAVCAN_TRACE("GlobalTimeSyncMaster", "Publication skipped");
return 0;
}
prev_pub_mono_ = current_time;
}
/*
* Obtain common Transfer ID for all masters
*/
TransferID tid;
{
const int tid_res = getNextTransferID(tid);
if (tid_res < 0)
{
return tid_res;
}
}
for (uint8_t i = 0; i < node_.getDispatcher().getCanIOManager().getNumIfaces(); i++)
{
const int res = iface_masters_[i]->publish(tid, current_time);
if (res < 0)
{
return res;
}
}
return 0;
}
}
#endif
libuavcan/src/protocol/uc_global_time_sync_slave.cpp
-115
View File
@@ -1,115 +0,0 @@
/*
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include <uavcan/protocol/global_time_sync_slave.hpp>
#include <uavcan/debug.hpp>
#include <cassert>
namespace uavcan
{
void GlobalTimeSyncSlave::adjustFromMsg(const ReceivedDataStructure<protocol::GlobalTimeSync>& msg)
{
UAVCAN_ASSERT(msg.previous_transmission_timestamp_usec > 0);
const UtcDuration adjustment = UtcTime::fromUSec(msg.previous_transmission_timestamp_usec) - prev_ts_utc_;
UAVCAN_TRACE("GlobalTimeSyncSlave", "Adjustment: usec=%lli snid=%i iface=%i suppress=%i",
static_cast<long long>(adjustment.toUSec()),
int(msg.getSrcNodeID().get()), int(msg.getIfaceIndex()), int(suppressed_));
if (!suppressed_)
{
getSystemClock().adjustUtc(adjustment);
}
last_adjustment_ts_ = msg.getMonotonicTimestamp();
state_ = Update;
}
void GlobalTimeSyncSlave::updateFromMsg(const ReceivedDataStructure<protocol::GlobalTimeSync>& msg)
{
UAVCAN_TRACE("GlobalTimeSyncSlave", "Update: snid=%i iface=%i",
int(msg.getSrcNodeID().get()), int(msg.getIfaceIndex()));
prev_ts_utc_ = msg.getUtcTimestamp();
prev_ts_mono_ = msg.getMonotonicTimestamp();
master_nid_ = msg.getSrcNodeID();
prev_iface_index_ = msg.getIfaceIndex();
prev_tid_ = msg.getTransferID();
state_ = Adjust;
}
void GlobalTimeSyncSlave::processMsg(const ReceivedDataStructure<protocol::GlobalTimeSync>& msg)
{
const MonotonicDuration since_prev_msg = msg.getMonotonicTimestamp() - prev_ts_mono_;
UAVCAN_ASSERT(!since_prev_msg.isNegative());
const bool needs_init = !master_nid_.isValid() || prev_ts_mono_.isZero();
const bool switch_master = msg.getSrcNodeID() < master_nid_;
const bool pub_timeout = since_prev_msg.toMSec() > protocol::GlobalTimeSync::PUBLISHER_TIMEOUT_MS;
if (switch_master || pub_timeout || needs_init)
{
UAVCAN_TRACE("GlobalTimeSyncSlave", "Force update: needs_init=%i switch_master=%i pub_timeout=%i",
int(needs_init), int(switch_master), int(pub_timeout));
updateFromMsg(msg);
}
else if (msg.getIfaceIndex() == prev_iface_index_ && msg.getSrcNodeID() == master_nid_)
{
if (state_ == Adjust)
{
const bool msg_invalid = msg.previous_transmission_timestamp_usec == 0;
const bool wrong_tid = prev_tid_.computeForwardDistance(msg.getTransferID()) != 1;
const bool wrong_timing = since_prev_msg.toMSec() > protocol::GlobalTimeSync::MAX_PUBLICATION_PERIOD_MS;
if (msg_invalid || wrong_tid || wrong_timing)
{
UAVCAN_TRACE("GlobalTimeSyncSlave", "Adjustment skipped: msg_invalid=%i wrong_tid=%i wrong_timing=%i",
int(msg_invalid), int(wrong_tid), int(wrong_timing));
state_ = Update;
}
}
if (state_ == Adjust)
{
adjustFromMsg(msg);
}
else
{
updateFromMsg(msg);
}
}
else
{
UAVCAN_TRACE("GlobalTimeSyncSlave", "Ignored: snid=%i iface=%i",
int(msg.getSrcNodeID().get()), int(msg.getIfaceIndex()));
}
}
void GlobalTimeSyncSlave::handleGlobalTimeSync(const ReceivedDataStructure<protocol::GlobalTimeSync>& msg)
{
if (msg.getTransferType() == TransferTypeMessageBroadcast)
{
processMsg(msg);
}
else
{
UAVCAN_TRACE("GlobalTimeSyncSlave", "Invalid transfer type %i", int(msg.getTransferType()));
}
}
int GlobalTimeSyncSlave::start()
{
return sub_.start(GlobalTimeSyncCallback(this, &GlobalTimeSyncSlave::handleGlobalTimeSync));
}
bool GlobalTimeSyncSlave::isActive() const
{
const MonotonicDuration since_prev_adj = getSystemClock().getMonotonic() - last_adjustment_ts_;
return !last_adjustment_ts_.isZero() && since_prev_adj.toMSec() <= protocol::GlobalTimeSync::PUBLISHER_TIMEOUT_MS;
}
NodeID GlobalTimeSyncSlave::getMasterNodeID() const
{
return isActive() ? master_nid_ : NodeID();
}
}
libuavcan/src/protocol/uc_logger.cpp
-69
View File
@@ -1,69 +0,0 @@
/*
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include <uavcan/protocol/logger.hpp>
#include <cstdlib>
namespace uavcan
{
const Logger::LogLevel Logger::LevelAboveAll;
Logger::LogLevel Logger::getExternalSinkLevel() const
{
return (external_sink_ == NULL) ? LevelAboveAll : external_sink_->getLogLevel();
}
int Logger::init()
{
const int res = logmsg_pub_.init();
if (res < 0)
{
return res;
}
return logmsg_pub_.setPriority(TransferPriorityLow); // Fixed priority
}
int Logger::log(const protocol::debug::LogMessage& message)
{
int retval = 0;
if (message.level.value >= getExternalSinkLevel())
{
external_sink_->log(message);
}
if (message.level.value >= level_)
{
retval = logmsg_pub_.broadcast(message);
}
return retval;
}
#if UAVCAN_CPP_VERSION < UAVCAN_CPP11
int Logger::log(LogLevel level, const char* source, const char* text) UAVCAN_NOEXCEPT
{
#if UAVCAN_EXCEPTIONS
try
#endif
{
if (level >= level_ || level >= getExternalSinkLevel())
{
msg_buf_.level.value = level;
msg_buf_.source = source;
msg_buf_.text = text;
return log(msg_buf_);
}
return 0;
}
#if UAVCAN_EXCEPTIONS
catch (...)
{
return -ErrFailure;
}
#endif
}
#endif
}
libuavcan/src/protocol/uc_network_compat_checker.cpp
-200
View File
@@ -1,200 +0,0 @@
/*
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include <uavcan/build_config.hpp>
#if !UAVCAN_TINY
#include <uavcan/protocol/network_compat_checker.hpp>
#include <uavcan/debug.hpp>
#include <uavcan/node/publisher.hpp>
#include <uavcan/protocol/GlobalDiscoveryRequest.hpp>
#include <cassert>
namespace uavcan
{
MonotonicDuration NetworkCompatibilityChecker::getNetworkDiscoveryDelay() const
{
// Base duration is constant - NodeStatus max publication period
MonotonicDuration dur = MonotonicDuration::fromMSec(protocol::NodeStatus::MAX_PUBLICATION_PERIOD_MS);
// Additional duration depends on the node priority - gets larger with higher Node ID
dur += MonotonicDuration::fromMSec(getNode().getNodeID().get() * 10);
return dur;
}
NodeID NetworkCompatibilityChecker::findNextUncheckedNode()
{
for (uint8_t i = 1; i <= NodeID::Max; i++)
{
if (nid_mask_present_.test(i) && !nid_mask_checked_.test(i))
{
nid_mask_checked_[i] = true;
return NodeID(i);
}
}
return NodeID();
}
int NetworkCompatibilityChecker::waitForCATSResponse()
{
while (cats_cln_.isPending())
{
const int res = getNode().spin(MonotonicDuration::fromMSec(10));
if (res < 0 || !result_.isOk())
{
return res;
}
}
return 0;
}
void NetworkCompatibilityChecker::handleNodeStatus(const ReceivedDataStructure<protocol::NodeStatus>& msg)
{
if (!nid_mask_present_.test(msg.getSrcNodeID().get()))
{
UAVCAN_TRACE("NodeInitializer", "New node nid=%i", int(msg.getSrcNodeID().get()));
nid_mask_present_[msg.getSrcNodeID().get()] = true;
}
if (msg.getSrcNodeID() == getNode().getNodeID())
{
UAVCAN_TRACE("NodeInitializer", "Node ID collision; nid=%i", int(msg.getSrcNodeID().get()));
result_.conflicting_node = msg.getSrcNodeID();
}
}
void NetworkCompatibilityChecker::handleCATSResponse(ServiceCallResult<protocol::ComputeAggregateTypeSignature>& resp)
{
last_cats_request_ok_ = resp.isSuccessful();
if (last_cats_request_ok_)
{
const DataTypeSignature sign = GlobalDataTypeRegistry::instance().
computeAggregateSignature(checking_dtkind_, resp.response.mutually_known_ids);
UAVCAN_TRACE("NodeInitializer", "CATS response from nid=%i; local=%llu remote=%llu",
int(resp.server_node_id.get()), static_cast<unsigned long long>(sign.get()),
static_cast<unsigned long long>(resp.response.aggregate_signature));
if (sign.get() != resp.response.aggregate_signature)
{
result_.conflicting_node = resp.server_node_id;
}
}
}
int NetworkCompatibilityChecker::checkOneNodeOneDataTypeKind(NodeID nid, DataTypeKind kind)
{
StaticAssert<DataTypeKindMessage == int(protocol::DataTypeKind::MESSAGE)>::check();
StaticAssert<DataTypeKindService == int(protocol::DataTypeKind::SERVICE)>::check();
UAVCAN_ASSERT(nid.isUnicast());
UAVCAN_ASSERT(!cats_cln_.isPending());
checking_dtkind_ = kind;
protocol::ComputeAggregateTypeSignature::Request request;
request.kind.value = kind;
GlobalDataTypeRegistry::instance().getDataTypeIDMask(kind, request.known_ids);
int res = cats_cln_.call(nid, request);
if (res < 0)
{
return res;
}
res = waitForCATSResponse();
if (res < 0)
{
return res;
}
if (!last_cats_request_ok_)
{
return -ErrFailure;
}
return 0;
}
int NetworkCompatibilityChecker::checkOneNode(NodeID nid)
{
if (nid == getNode().getNodeID())
{
result_.conflicting_node = nid; // NodeID collision
return 0;
}
const int res = checkOneNodeOneDataTypeKind(nid, DataTypeKindMessage);
if (res < 0 || !result_.isOk())
{
return res;
}
return checkOneNodeOneDataTypeKind(nid, DataTypeKindService);
}
int NetworkCompatibilityChecker::checkNodes()
{
(void)nid_mask_checked_.reset();
result_ = NetworkCompatibilityCheckResult();
while (result_.isOk())
{
const NodeID nid = findNextUncheckedNode();
if (nid.isValid())
{
UAVCAN_TRACE("NodeInitializer", "Checking nid=%i", int(nid.get()));
const int res = checkOneNode(nid);
if (res < 0)
{
result_.num_failed_nodes++;
}
UAVCAN_TRACE("NodeInitializer", "Checked nid=%i result=%i", int(nid.get()), res);
}
else { break; }
}
return 0;
}
int NetworkCompatibilityChecker::execute()
{
int res = 0;
if (!getNode().getNodeID().isUnicast())
{
result_.conflicting_node = getNode().getNodeID();
goto exit;
}
res = ns_sub_.start(NodeStatusCallback(this, &NetworkCompatibilityChecker::handleNodeStatus));
if (res < 0)
{
goto exit;
}
cats_cln_.setCallback(CATSResponseCallback(this, &NetworkCompatibilityChecker::handleCATSResponse));
res = cats_cln_.init();
if (res < 0)
{
goto exit;
}
res = getNode().spin(getNetworkDiscoveryDelay());
if (res < 0)
{
goto exit;
}
res = checkNodes();
exit:
ns_sub_.stop();
cats_cln_.cancel();
return res;
}
int NetworkCompatibilityChecker::publishGlobalDiscoveryRequest(INode& node)
{
Publisher<protocol::GlobalDiscoveryRequest> pub(node);
return pub.broadcast(protocol::GlobalDiscoveryRequest());
}
}
#endif
libuavcan/src/protocol/uc_node_status_monitor.cpp
-143
View File
@@ -1,143 +0,0 @@
/*
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include <uavcan/protocol/node_status_monitor.hpp>
#include <uavcan/debug.hpp>
#include <cassert>
#include <cstdlib>
namespace uavcan
{
const unsigned NodeStatusMonitor::TimerPeriodMs100;
NodeStatusMonitor::Entry& NodeStatusMonitor::getEntry(NodeID node_id) const
{
if (node_id.get() < 1 || node_id.get() > NodeID::Max)
{
handleFatalError("NodeStatusMonitor NodeID");
}
return entries_[node_id.get() - 1];
}
void NodeStatusMonitor::changeNodeStatus(const NodeID node_id, const Entry new_entry_value)
{
Entry& entry = getEntry(node_id);
if (entry.status_code != new_entry_value.status_code)
{
NodeStatusChangeEvent event;
event.node_id = node_id;
event.old_status.known = entry.time_since_last_update_ms100 >= 0;
event.old_status.status_code = entry.status_code;
event.status.known = true;
event.status.status_code = new_entry_value.status_code;
UAVCAN_TRACE("NodeStatusMonitor", "Node %i [%s] status change: %i --> %i", int(node_id.get()),
(event.old_status.known ? "known" : "new"),
int(event.old_status.status_code), int(event.status.status_code));
handleNodeStatusChange(event);
}
entry = new_entry_value;
}
void NodeStatusMonitor::handleNodeStatus(const ReceivedDataStructure<protocol::NodeStatus>& msg)
{
Entry new_entry_value;
new_entry_value.time_since_last_update_ms100 = 0;
new_entry_value.status_code = msg.status_code;
changeNodeStatus(msg.getSrcNodeID(), new_entry_value);
handleNodeStatusMessage(msg);
}
void NodeStatusMonitor::handleTimerEvent(const TimerEvent&)
{
const int OfflineTimeoutMs100 = protocol::NodeStatus::OFFLINE_TIMEOUT_MS / 100;
for (uint8_t i = 1; i <= NodeID::Max; i++)
{
const NodeID nid(i);
UAVCAN_ASSERT(nid.isUnicast());
Entry& entry = getEntry(nid);
if (entry.time_since_last_update_ms100 >= 0 &&
entry.status_code != protocol::NodeStatus::STATUS_OFFLINE)
{
entry.time_since_last_update_ms100 = int8_t(entry.time_since_last_update_ms100 + int8_t(TimerPeriodMs100));
if (entry.time_since_last_update_ms100 >= OfflineTimeoutMs100)
{
Entry new_entry_value;
new_entry_value.time_since_last_update_ms100 = OfflineTimeoutMs100;
new_entry_value.status_code = protocol::NodeStatus::STATUS_OFFLINE;
changeNodeStatus(nid, new_entry_value);
}
}
}
}
int NodeStatusMonitor::start()
{
const int res = sub_.start(NodeStatusCallback(this, &NodeStatusMonitor::handleNodeStatus));
if (res >= 0)
{
TimerBase::startPeriodic(MonotonicDuration::fromMSec(TimerPeriodMs100 * 100));
}
return res;
}
void NodeStatusMonitor::forgetNode(NodeID node_id)
{
if (node_id.isValid())
{
Entry& entry = getEntry(node_id);
entry = Entry();
}
else
{
UAVCAN_ASSERT(0);
}
}
NodeStatusMonitor::NodeStatus NodeStatusMonitor::getNodeStatus(NodeID node_id) const
{
if (!node_id.isValid())
{
UAVCAN_ASSERT(0);
return NodeStatus();
}
NodeStatus status;
const Entry& entry = getEntry(node_id);
if (entry.time_since_last_update_ms100 >= 0)
{
status.known = true;
status.status_code = entry.status_code;
}
return status;
}
NodeID NodeStatusMonitor::findNodeWithWorstStatus() const
{
NodeID nid_with_worst_status;
NodeStatusCode worst_status_code = protocol::NodeStatus::STATUS_OK;
for (uint8_t i = 1; i <= NodeID::Max; i++)
{
const NodeID nid(i);
UAVCAN_ASSERT(nid.isUnicast());
const Entry& entry = getEntry(nid);
if (entry.time_since_last_update_ms100 >= 0)
{
if (entry.status_code > worst_status_code || !nid_with_worst_status.isValid())
{
nid_with_worst_status = nid;
worst_status_code = entry.status_code;
}
}
}
return nid_with_worst_status;
}
}
libuavcan/src/protocol/uc_node_status_provider.cpp
+1 -5
View File
@@ -71,11 +71,7 @@ int NodeStatusProvider::startAndPublish(TransferPriority priority)
int res = -1;
res = node_status_pub_.setPriority(priority);
if (res < 0)
{
goto fail;
}
node_status_pub_.setPriority(priority);
if (!getNode().isPassiveMode())
{
libuavcan/src/protocol/uc_panic_broadcaster.cpp
-61
View File
@@ -1,61 +0,0 @@
/*
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include <uavcan/protocol/panic_broadcaster.hpp>
#include <uavcan/debug.hpp>
namespace uavcan
{
void PanicBroadcaster::publishOnce()
{
const int res = pub_.broadcast(msg_);
if (res < 0)
{
pub_.getNode().registerInternalFailure("Panic pub failed");
}
}
void PanicBroadcaster::handleTimerEvent(const TimerEvent&)
{
publishOnce();
}
void PanicBroadcaster::panic(const char* short_reason_description)
{
msg_.reason_text.clear();
const char* p = short_reason_description;
while (p && *p)
{
if (msg_.reason_text.size() == msg_.reason_text.capacity())
{
break;
}
msg_.reason_text.push_back(protocol::Panic::FieldTypes::reason_text::ValueType(*p));
p++;
}
UAVCAN_TRACE("PanicBroadcaster", "Panicking with reason '%s'", getReason().c_str());
publishOnce();
startPeriodic(MonotonicDuration::fromMSec(protocol::Panic::BROADCASTING_INTERVAL_MS));
}
void PanicBroadcaster::dontPanic()
{
stop();
msg_.reason_text.clear();
}
bool PanicBroadcaster::isPanicking() const
{
return isRunning();
}
const typename protocol::Panic::FieldTypes::reason_text& PanicBroadcaster::getReason() const
{
return msg_.reason_text;
}
}
libuavcan/src/protocol/uc_param_server.cpp
-95
View File
@@ -1,95 +0,0 @@
/*
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include <uavcan/protocol/param_server.hpp>
#include <uavcan/debug.hpp>
#include <cassert>
#include <cstdlib>
namespace uavcan
{
void ParamServer::handleGetSet(const protocol::param::GetSet::Request& in, protocol::param::GetSet::Response& out)
{
UAVCAN_ASSERT(manager_ != NULL);
// Recover the name from index
if (in.name.empty())
{
manager_->getParamNameByIndex(in.index, out.name);
UAVCAN_TRACE("ParamServer", "GetSet: Index %i --> '%s'", int(in.index), out.name.c_str());
}
else
{
out.name = in.name;
}
if (out.name.empty())
{
UAVCAN_TRACE("ParamServer", "GetSet: Can't resolve parameter name, index=%i", int(in.index));
return;
}
// Assign if needed, read back
if (!IParamManager::isValueEmpty(in.value))
{
manager_->assignParamValue(out.name, in.value);
}
manager_->readParamValue(out.name, out.value);
// Check if the value is OK, otherwise reset the name to indicate that we have no idea what is it all about
if (!IParamManager::isValueEmpty(out.value))
{
manager_->readParamDefaultMaxMin(out.name, out.default_value, out.max_value, out.min_value);
}
else
{
UAVCAN_TRACE("ParamServer", "GetSet: Unknown param: index=%i name='%s'", int(in.index), out.name.c_str());
out.name.clear();
}
}
void ParamServer::handleExecuteOpcode(const protocol::param::ExecuteOpcode::Request& in,
protocol::param::ExecuteOpcode::Response& out)
{
UAVCAN_ASSERT(manager_ != NULL);
if (in.opcode == protocol::param::ExecuteOpcode::Request::OPCODE_SAVE)
{
out.ok = manager_->saveAllParams() >= 0;
}
else if (in.opcode == protocol::param::ExecuteOpcode::Request::OPCODE_ERASE)
{
out.ok = manager_->eraseAllParams() >= 0;
}
else
{
UAVCAN_TRACE("ParamServer", "ExecuteOpcode: invalid opcode %i", int(in.opcode));
out.ok = false;
}
}
int ParamServer::start(IParamManager* manager)
{
if (manager == NULL)
{
return -ErrInvalidParam;
}
manager_ = manager;
int res = get_set_srv_.start(GetSetCallback(this, &ParamServer::handleGetSet));
if (res < 0)
{
return res;
}
res = save_erase_srv_.start(ExecuteOpcodeCallback(this, &ParamServer::handleExecuteOpcode));
if (res < 0)
{
get_set_srv_.stop();
}
return res;
}
}
libuavcan/src/protocol/uc_restart_request_server.cpp
-36
View File
@@ -1,36 +0,0 @@
/*
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include <uavcan/protocol/restart_request_server.hpp>
#include <uavcan/debug.hpp>
namespace uavcan
{
void RestartRequestServer::handleRestartNode(const ReceivedDataStructure<protocol::RestartNode::Request>& request,
protocol::RestartNode::Response& response) const
{
UAVCAN_TRACE("RestartRequestServer", "Request from snid=%i", int(request.getSrcNodeID().get()));
response.ok = false;
if (request.magic_number == protocol::RestartNode::Request::MAGIC_NUMBER)
{
if (handler_)
{
response.ok = handler_->handleRestartRequest(request.getSrcNodeID());
}
UAVCAN_TRACE("RestartRequestServer", "%s", (response.ok ? "Accepted" : "Rejected"));
}
else
{
UAVCAN_TRACE("RestartRequestServer", "Invalid magic number 0x%llx",
static_cast<unsigned long long>(request.magic_number));
}
}
int RestartRequestServer::start()
{
return srv_.start(RestartNodeCallback(this, &RestartRequestServer::handleRestartNode));
}
}
libuavcan/src/protocol/uc_transport_stats_provider.cpp
-40
View File
@@ -1,40 +0,0 @@
/*
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include <uavcan/build_config.hpp>
#if !UAVCAN_TINY
#include <uavcan/protocol/transport_stats_provider.hpp>
namespace uavcan
{
void TransportStatsProvider::handleGetTransportStats(const protocol::GetTransportStats::Request&,
protocol::GetTransportStats::Response& resp) const
{
const TransferPerfCounter& perf = srv_.getNode().getDispatcher().getTransferPerfCounter();
resp.transfer_errors = perf.getErrorCount();
resp.transfers_tx = perf.getTxTransferCount();
resp.transfers_rx = perf.getRxTransferCount();
const CanIOManager& canio = srv_.getNode().getDispatcher().getCanIOManager();
for (uint8_t i = 0; i < canio.getNumIfaces(); i++)
{
const CanIfacePerfCounters can_perf = canio.getIfacePerfCounters(i);
protocol::CANIfaceStats stats;
stats.errors = can_perf.errors;
stats.frames_tx = can_perf.frames_tx;
stats.frames_rx = can_perf.frames_rx;
resp.can_iface_stats.push_back(stats);
}
}
int TransportStatsProvider::start()
{
return srv_.start(GetTransportStatsCallback(this, &TransportStatsProvider::handleGetTransportStats));
}
}
#endif
libuavcan/src/transport/uc_can_io.cpp
+13 -13
View File
@@ -261,19 +261,6 @@ int CanIOManager::sendFromTxQueue(uint8_t iface_index)
return res;
}
uint8_t CanIOManager::makePendingTxMask() const
{
uint8_t write_mask = 0;
for (uint8_t i = 0; i < getNumIfaces(); i++)
{
if (!tx_queues_[i]->isEmpty())
{
write_mask |= uint8_t(1 << i);
}
}
return write_mask;
}
int CanIOManager::callSelect(CanSelectMasks& inout_masks, MonotonicTime blocking_deadline)
{
const CanSelectMasks in_masks = inout_masks;
@@ -312,6 +299,19 @@ CanIOManager::CanIOManager(ICanDriver& driver, IPoolAllocator& allocator, ISyste
}
}
uint8_t CanIOManager::makePendingTxMask() const
{
uint8_t write_mask = 0;
for (uint8_t i = 0; i < getNumIfaces(); i++)
{
if (!tx_queues_[i]->isEmpty())
{
write_mask |= uint8_t(1 << i);
}
}
return write_mask;
}
CanIfacePerfCounters CanIOManager::getIfacePerfCounters(uint8_t iface_index) const
{
ICanIface* const iface = driver_.getIface(iface_index);
libuavcan/src/transport/uc_dispatcher.cpp
+34
View File
@@ -229,6 +229,40 @@ int Dispatcher::spin(MonotonicTime deadline)
return num_frames_processed;
}
int Dispatcher::spinOnce()
{
int num_frames_processed = 0;
while (true)
{
CanIOFlags flags = 0;
CanRxFrame frame;
const int res = canio_.receive(frame, MonotonicTime(), flags);
if (res < 0)
{
return res;
}
else if (res > 0)
{
if (flags & CanIOFlagLoopback)
{
handleLoopbackFrame(frame);
}
else
{
num_frames_processed++;
handleFrame(frame);
}
}
else
{
break; // No frames left
}
}
return num_frames_processed;
}
int Dispatcher::send(const Frame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline,
CanTxQueue::Qos qos, CanIOFlags flags, uint8_t iface_mask)
{
libuavcan/src/transport/uc_transfer.cpp
+1
View File
@@ -21,6 +21,7 @@ const uint8_t NodeID::ValueBroadcast;
const uint8_t NodeID::ValueInvalid;
const uint8_t NodeID::BitLen;
const uint8_t NodeID::Max;
const uint8_t NodeID::MaxRecommendedForRegularNodes;
const NodeID NodeID::Broadcast(ValueBroadcast);
/**
libuavcan/src/transport/uc_transfer_listener.cpp
+1 -1
View File
@@ -189,7 +189,7 @@ void TransferListenerBase::handleAnonymousTransferReception(const RxFrame& frame
void TransferListenerBase::cleanup(MonotonicTime ts)
{
receivers_.removeWhere(TimedOutReceiverPredicate(ts, bufmgr_));
receivers_.removeAllWhere(TimedOutReceiverPredicate(ts, bufmgr_));
UAVCAN_ASSERT(receivers_.isEmpty() ? bufmgr_.isEmpty() : 1);
}
libuavcan/src/transport/uc_transfer_sender.cpp
+13 -4
View File
@@ -15,6 +15,15 @@ void TransferSender::registerError() const
dispatcher_.getTransferPerfCounter().addError();
}
void TransferSender::init(const DataTypeDescriptor& dtid, CanTxQueue::Qos qos)
{
UAVCAN_ASSERT(!isInitialized());
qos_ = qos;
data_type_id_ = dtid.getID();
crc_base_ = dtid.getSignature().toTransferCRC();
}
int TransferSender::send(const uint8_t* payload, unsigned payload_len, MonotonicTime tx_deadline,
MonotonicTime blocking_deadline, TransferType transfer_type, NodeID dst_node_id,
TransferID tid) const
@@ -24,7 +33,7 @@ int TransferSender::send(const uint8_t* payload, unsigned payload_len, Monotonic
return -ErrTransferTooLong;
}
Frame frame(data_type_.getID(), transfer_type, dispatcher_.getNodeID(), dst_node_id, 0, tid);
Frame frame(data_type_id_, transfer_type, dispatcher_.getNodeID(), dst_node_id, 0, tid);
if (transfer_type == TransferTypeMessageBroadcast ||
transfer_type == TransferTypeMessageUnicast)
{
@@ -143,7 +152,7 @@ int TransferSender::send(const uint8_t* payload, unsigned payload_len, Monotonic
/*
* TODO: TID is not needed for anonymous transfers, this part of the code can be skipped?
*/
const OutgoingTransferRegistryKey otr_key(data_type_.getID(), transfer_type, dst_node_id);
const OutgoingTransferRegistryKey otr_key(data_type_id_, transfer_type, dst_node_id);
UAVCAN_ASSERT(!tx_deadline.isZero());
const MonotonicTime otr_deadline = tx_deadline + max_transfer_interval_;
@@ -151,8 +160,8 @@ int TransferSender::send(const uint8_t* payload, unsigned payload_len, Monotonic
TransferID* const tid = dispatcher_.getOutgoingTransferRegistry().accessOrCreate(otr_key, otr_deadline);
if (tid == NULL)
{
UAVCAN_TRACE("TransferSender", "OTR access failure, dtd=%s tt=%i",
data_type_.toString().c_str(), int(transfer_type));
UAVCAN_TRACE("TransferSender", "OTR access failure, dtid=%d tt=%i",
int(data_type_id_.get()), int(transfer_type));
return -ErrMemory;
}
libuavcan/test/marshal/array.cpp
+10
View File
@@ -768,6 +768,16 @@ TEST(Array, Strings)
ASSERT_STREQ("Our sun is dying. 123456-789", a8_2.c_str());
ASSERT_STREQ("Our sun is dying. 123456-789", a8.c_str());
ASSERT_STREQ("456", a7.c_str());
/*
* String constructor
*/
A8 a8_3("123");
A7 a7_3 = "456";
ASSERT_EQ(3, a8_3.size());
ASSERT_EQ(3, a7_3.size());
ASSERT_STREQ("123", a8_3.c_str());
ASSERT_STREQ("456", a7_3.c_str());
}
libuavcan/test/node/publisher.cpp
+3 -14
View File
@@ -18,6 +18,8 @@ TEST(Publisher, Basic)
uavcan::Publisher<uavcan::mavlink::Message> publisher(node);
ASSERT_FALSE(publisher.getTransferSender().isInitialized());
std::cout <<
"sizeof(uavcan::Publisher<uavcan::mavlink::Message>): " <<
sizeof(uavcan::Publisher<uavcan::mavlink::Message>) << std::endl;
@@ -110,18 +112,5 @@ TEST(Publisher, Basic)
* Misc
*/
ASSERT_TRUE(uavcan::GlobalDataTypeRegistry::instance().isFrozen());
ASSERT_TRUE(publisher.getTransferSender());
}
TEST(Publisher, ImplicitInitialization)
{
SystemClockMock clock_mock(100);
CanDriverMock can_driver(2, clock_mock);
TestNode node(can_driver, clock_mock, 1);
uavcan::Publisher<uavcan::mavlink::Message> publisher(node);
// Will be initialized ad-hoc
ASSERT_TRUE(publisher.getTransferSender());
ASSERT_TRUE(publisher.getTransferSender().isInitialized());
}
libuavcan/test/node/service_client.cpp
+234 -18
View File
@@ -12,6 +12,7 @@
#include <root_ns_a/StringService.hpp>
#include <root_ns_a/EmptyService.hpp>
#include <queue>
#include <sstream>
#include "test_node.hpp"
@@ -22,21 +23,31 @@ struct ServiceCallResultHandler
StatusType last_status;
uavcan::NodeID last_server_node_id;
typename DataType::Response last_response;
std::queue<typename DataType::Response> responses;
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;
last_status = result.getStatus();
last_response = result.getResponse();
last_server_node_id = result.getCallID().server_node_id;
responses.push(result.getResponse());
}
bool match(StatusType status, uavcan::NodeID server_node_id, const typename DataType::Response& response) const
{
return
status == last_status &&
if (status == last_status &&
server_node_id == last_server_node_id &&
response == last_response;
response == last_response)
{
return true;
}
else
{
std::cout << "MISMATCH: status=" << last_status << ", last_server_node_id="
<< int(last_server_node_id.get()) << ", last response:\n" << last_response << std::endl;
return false;
}
}
typedef uavcan::MethodBinder<ServiceCallResultHandler*,
@@ -47,14 +58,25 @@ struct ServiceCallResultHandler
static void stringServiceServerCallback(const uavcan::ReceivedDataStructure<root_ns_a::StringService::Request>& req,
root_ns_a::StringService::Response& rsp)
uavcan::ServiceResponseDataStructure<root_ns_a::StringService::Response>& rsp)
{
rsp.string_response = "Request string: ";
rsp.string_response += req.string_request;
}
static void rejectingStringServiceServerCallback(
const uavcan::ReceivedDataStructure<root_ns_a::StringService::Request>& req,
uavcan::ServiceResponseDataStructure<root_ns_a::StringService::Response>& rsp)
{
rsp.string_response = "Request string: ";
rsp.string_response += req.string_request;
ASSERT_TRUE(rsp.isResponseEnabled());
rsp.setResponseEnabled(false);
ASSERT_FALSE(rsp.isResponseEnabled());
}
static void emptyServiceServerCallback(const uavcan::ReceivedDataStructure<root_ns_a::EmptyService::Request>&,
root_ns_a::EmptyService::Response&)
uavcan::ServiceResponseDataStructure<root_ns_a::EmptyService::Response>&)
{
// Nothing to do - the service is empty
}
@@ -83,6 +105,12 @@ TEST(ServiceClient, Basic)
ClientType client2(nodes.b);
ClientType client3(nodes.b);
ASSERT_EQ(0, client1.getNumPendingCalls());
ASSERT_EQ(0, client2.getNumPendingCalls());
ASSERT_EQ(0, client3.getNumPendingCalls());
ASSERT_FALSE(client1.hasPendingCallToServer(1));
client1.setCallback(handler.bind());
client2.setCallback(client1.getCallback());
client3.setCallback(client1.getCallback());
@@ -94,23 +122,47 @@ TEST(ServiceClient, Basic)
root_ns_a::StringService::Request request;
request.string_request = "Hello world";
std::cout << "!!! Calling!" << std::endl;
ASSERT_LT(0, client1.call(1, request)); // OK
ASSERT_LT(0, client1.call(1, request)); // OK - second request
ASSERT_LT(0, client2.call(1, request)); // OK
ASSERT_LT(0, client3.call(99, request)); // Will timeout!
ASSERT_LT(0, client3.call(1, request)); // OK - second request
ASSERT_TRUE(client1.hasPendingCallToServer(1));
ASSERT_TRUE(client2.hasPendingCallToServer(1));
ASSERT_TRUE(client3.hasPendingCallToServer(99));
ASSERT_TRUE(client3.hasPendingCallToServer(1));
std::cout << "!!! Spinning!" << std::endl;
ASSERT_EQ(3, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Listening now!
ASSERT_TRUE(client1.isPending());
ASSERT_TRUE(client2.isPending());
ASSERT_TRUE(client3.isPending());
ASSERT_TRUE(client1.hasPendingCalls());
ASSERT_TRUE(client2.hasPendingCalls());
ASSERT_TRUE(client3.hasPendingCalls());
ASSERT_EQ(2, client1.getNumPendingCalls());
ASSERT_EQ(1, client2.getNumPendingCalls());
ASSERT_EQ(2, client3.getNumPendingCalls());
ASSERT_EQ(uavcan::NodeID(1), client2.getCallIDByIndex(0).server_node_id);
ASSERT_EQ(uavcan::NodeID(), client2.getCallIDByIndex(1).server_node_id);
nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(20));
std::cout << "!!! Spin finished!" << std::endl;
ASSERT_EQ(1, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Third is still listening!
ASSERT_FALSE(client1.isPending());
ASSERT_FALSE(client2.isPending());
ASSERT_TRUE(client3.isPending());
ASSERT_FALSE(client1.hasPendingCalls());
ASSERT_FALSE(client2.hasPendingCalls());
ASSERT_TRUE(client3.hasPendingCalls());
ASSERT_EQ(0, client1.getNumPendingCalls());
ASSERT_EQ(0, client2.getNumPendingCalls());
ASSERT_EQ(1, client3.getNumPendingCalls()); // The one addressed to 99 is still waiting
// Validating
root_ns_a::StringService::Response expected_response;
@@ -119,9 +171,9 @@ TEST(ServiceClient, Basic)
nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(200));
ASSERT_FALSE(client1.isPending());
ASSERT_FALSE(client2.isPending());
ASSERT_FALSE(client3.isPending());
ASSERT_FALSE(client1.hasPendingCalls());
ASSERT_FALSE(client2.hasPendingCalls());
ASSERT_FALSE(client3.hasPendingCalls());
ASSERT_EQ(0, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Third has timed out :(
@@ -130,7 +182,7 @@ TEST(ServiceClient, Basic)
// Stray request
ASSERT_LT(0, client3.call(99, request)); // Will timeout!
ASSERT_TRUE(client3.isPending());
ASSERT_TRUE(client3.hasPendingCalls());
ASSERT_EQ(1, nodes.b.getDispatcher().getNumServiceResponseListeners());
}
@@ -139,6 +191,170 @@ TEST(ServiceClient, Basic)
}
TEST(ServiceClient, Rejection)
{
InterlinkedTestNodesWithSysClock nodes;
// Type registration
uavcan::GlobalDataTypeRegistry::instance().reset();
uavcan::DefaultDataTypeRegistrator<root_ns_a::StringService> _registrator;
// Server
uavcan::ServiceServer<root_ns_a::StringService> server(nodes.a);
ASSERT_EQ(0, server.start(rejectingStringServiceServerCallback));
// 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(nodes.b);
client1.setRequestTimeout(uavcan::MonotonicDuration::fromMSec(100));
client1.setCallback(handler.bind());
root_ns_a::StringService::Request request;
request.string_request = "Hello world";
ASSERT_LT(0, client1.call(1, request));
ASSERT_EQ(uavcan::NodeID(1), client1.getCallIDByIndex(0).server_node_id);
ASSERT_EQ(uavcan::NodeID(), client1.getCallIDByIndex(1).server_node_id);
ASSERT_EQ(1, nodes.b.getDispatcher().getNumServiceResponseListeners());
ASSERT_TRUE(client1.hasPendingCalls());
ASSERT_TRUE(client1.hasPendingCallToServer(1));
nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(200));
ASSERT_FALSE(client1.hasPendingCalls());
ASSERT_FALSE(client1.hasPendingCallToServer(1));
ASSERT_EQ(0, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Timed out
ASSERT_TRUE(handler.match(ResultType::ErrorTimeout, 1, root_ns_a::StringService::Response()));
}
TEST(ServiceClient, ConcurrentCalls)
{
InterlinkedTestNodesWithSysClock nodes;
// Type registration
uavcan::GlobalDataTypeRegistry::instance().reset();
uavcan::DefaultDataTypeRegistrator<root_ns_a::StringService> _registrator;
// Server
uavcan::ServiceServer<root_ns_a::StringService> server(nodes.a);
ASSERT_EQ(0, 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;
/*
* Initializing
*/
ClientType client(nodes.b);
ASSERT_EQ(0, client.getNumPendingCalls());
client.setCallback(handler.bind());
ASSERT_EQ(1, nodes.a.getDispatcher().getNumServiceRequestListeners());
ASSERT_EQ(0, nodes.b.getDispatcher().getNumServiceResponseListeners()); // NOT listening!
ASSERT_FALSE(client.hasPendingCalls());
ASSERT_EQ(0, client.getNumPendingCalls());
/*
* Calling ten requests, the last one will be cancelled
* Note that there will be non-unique transfer ID values; the client must handle that correctly
*/
uavcan::ServiceCallID last_call_id;
for (int i = 0; i < 10; i++)
{
std::ostringstream os;
os << i;
root_ns_a::StringService::Request request;
request.string_request = os.str().c_str();
ASSERT_LT(0, client.call(1, request, last_call_id));
}
ASSERT_LT(0, client.call(99, root_ns_a::StringService::Request())); // Will timeout in 500 ms
client.setRequestTimeout(uavcan::MonotonicDuration::fromMSec(100));
ASSERT_LT(0, client.call(88, root_ns_a::StringService::Request())); // Will timeout in 100 ms
ASSERT_TRUE(client.hasPendingCalls());
ASSERT_EQ(12, client.getNumPendingCalls());
ASSERT_EQ(1, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Listening
/*
* Cancelling one
*/
client.cancelCall(last_call_id);
ASSERT_TRUE(client.hasPendingCalls());
ASSERT_EQ(11, client.getNumPendingCalls());
ASSERT_EQ(1, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Still listening
/*
* Spinning
*/
nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(20));
ASSERT_TRUE(client.hasPendingCalls());
ASSERT_EQ(2, client.getNumPendingCalls()); // Two still pending
ASSERT_EQ(1, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Still listening
/*
* Validating the ones that didn't timeout
*/
root_ns_a::StringService::Response last_response;
for (int i = 0; i < 9; i++)
{
std::ostringstream os;
os << "Request string: " << i;
last_response.string_response = os.str().c_str();
ASSERT_FALSE(handler.responses.empty());
ASSERT_STREQ(last_response.string_response.c_str(), handler.responses.front().string_response.c_str());
handler.responses.pop();
}
ASSERT_TRUE(handler.responses.empty());
/*
* Validating the 100 ms timeout
*/
nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100));
ASSERT_TRUE(client.hasPendingCalls());
ASSERT_EQ(1, client.getNumPendingCalls()); // One dropped
ASSERT_EQ(1, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Still listening
ASSERT_TRUE(handler.match(ResultType::ErrorTimeout, 88, root_ns_a::StringService::Response()));
/*
* Validating the 500 ms timeout
*/
nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(500));
ASSERT_FALSE(client.hasPendingCalls());
ASSERT_EQ(0, client.getNumPendingCalls()); // All finished
ASSERT_EQ(0, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Not listening
ASSERT_TRUE(handler.match(ResultType::ErrorTimeout, 99, root_ns_a::StringService::Response()));
}
TEST(ServiceClient, Empty)
{
InterlinkedTestNodesWithSysClock nodes;
libuavcan/test/node/service_server.cpp
+3
View File
@@ -144,6 +144,9 @@ TEST(ServiceServer, Empty)
uavcan::ServiceServer<root_ns_a::EmptyService, EmptyServerImpl::Binder> server(node);
std::cout << "sizeof(ServiceServer<root_ns_a::EmptyService, EmptyServerImpl::Binder>): "
<< sizeof(uavcan::ServiceServer<root_ns_a::EmptyService, EmptyServerImpl::Binder>) << std::endl;
ASSERT_EQ(0, node.getDispatcher().getNumServiceRequestListeners());
ASSERT_GE(0, server.start(impl.bind()));
ASSERT_EQ(1, node.getDispatcher().getNumServiceRequestListeners());
libuavcan/test/node/test_node.hpp
+93 -11
View File
@@ -4,22 +4,30 @@
#pragma once
#if __GNUC__
// We need auto_ptr for compatibility reasons
# pragma GCC diagnostic ignored "-Wdeprecated-declarations"
#endif
#include <uavcan/node/abstract_node.hpp>
#include <memory>
#include <set>
#include <queue>
#include "../transport/can/can.hpp"
struct TestNode : public uavcan::INode
{
uavcan::PoolAllocator<uavcan::MemPoolBlockSize * 8, uavcan::MemPoolBlockSize> pool;
uavcan::PoolAllocator<uavcan::MemPoolBlockSize * 100, uavcan::MemPoolBlockSize> pool;
uavcan::PoolManager<1> poolmgr;
uavcan::MarshalBufferProvider<> buffer_provider;
uavcan::OutgoingTransferRegistry<8> otr;
uavcan::Scheduler scheduler;
uint64_t internal_failure_count;
TestNode(uavcan::ICanDriver& can_driver, uavcan::ISystemClock& clock_driver, uavcan::NodeID self_node_id)
: otr(poolmgr)
, scheduler(can_driver, poolmgr, clock_driver, otr)
, internal_failure_count(0)
{
poolmgr.addPool(&pool);
setNodeID(self_node_id);
@@ -28,33 +36,33 @@ struct TestNode : public uavcan::INode
virtual void registerInternalFailure(const char* msg)
{
std::cout << "TestNode internal failure: " << msg << std::endl;
internal_failure_count++;
}
virtual uavcan::PoolManager<1>& getAllocator() { return poolmgr; }
virtual uavcan::Scheduler& getScheduler() { return scheduler; }
virtual const uavcan::Scheduler& getScheduler() const { return scheduler; }
virtual uavcan::IMarshalBufferProvider& getMarshalBufferProvider() { return buffer_provider; }
};
struct PairableCanDriver : public uavcan::ICanDriver, public uavcan::ICanIface
{
uavcan::ISystemClock& clock;
PairableCanDriver* other;
std::set<PairableCanDriver*> others;
std::queue<uavcan::CanFrame> read_queue;
std::queue<uavcan::CanFrame> 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)
@@ -70,7 +78,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;
@@ -88,8 +95,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<PairableCanDriver*>::iterator it = others.begin(); it != others.end(); ++it)
{
(*it)->read_queue.push(frame);
}
if (flags & uavcan::CanIOFlagLoopback)
{
loopback_queue.push(frame);
@@ -100,7 +110,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())
{
@@ -119,6 +128,11 @@ struct PairableCanDriver : public uavcan::ICanDriver, public uavcan::ICanIface
return 1;
}
void pushRxToAllIfaces(const uavcan::CanFrame& can_frame)
{
read_queue.push(can_frame);
}
virtual uavcan::int16_t configureFilters(const uavcan::CanFilterConfig*, uavcan::uint16_t) { return -1; }
virtual uavcan::uint16_t getNumFilters() const { return 0; }
virtual uavcan::uint64_t getErrorCount() const { return error_count; }
@@ -178,3 +192,71 @@ struct InterlinkedTestNodes
typedef InterlinkedTestNodes<SystemClockDriver> InterlinkedTestNodesWithSysClock;
typedef InterlinkedTestNodes<SystemClockMock> InterlinkedTestNodesWithClockMock;
template <unsigned NumNodes>
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<NodeEnvironment> nodes[NumNodes];
TestNetwork(uavcan::uint8_t first_node_id = 1)
{
for (uavcan::uint8_t i = 0; i < NumNodes; i++)
{
nodes[i].reset(new NodeEnvironment(uint8_t(first_node_id + i)));
}
for (uavcan::uint8_t i = 0; i < NumNodes; i++)
{
for (uavcan::uint8_t k = 0; k < NumNodes; k++)
{
nodes[i]->can_driver.linkTogether(&nodes[k]->can_driver);
}
}
for (uavcan::uint8_t i = 0; i < NumNodes; i++)
{
assert(nodes[i]->can_driver.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]->node.spin(uavcan::MonotonicDuration::fromMSec(1));
if (ret < 0)
{
return ret;
}
}
}
return 0;
}
TestNode& operator[](unsigned index)
{
if (index >= NumNodes)
{
throw std::out_of_range("No such test node");
}
return nodes[index]->node;
}
};
libuavcan/test/node/test_node_test.cpp
+32
View File
@@ -0,0 +1,32 @@
/*
* Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include <gtest/gtest.h>
#include "test_node.hpp"
TEST(TestNode, TestNetwork)
{
TestNetwork<4> nwk;
uavcan::CanFrame frame;
for (uint8_t 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);
}
}
libuavcan/test/protocol/data_type_info_provider.cpp
+35 -34
View File
@@ -19,7 +19,8 @@ using uavcan::DefaultDataTypeRegistrator;
using uavcan::MonotonicDuration;
template <typename DataType>
static bool validateDataTypeInfoResponse(const std::auto_ptr<ServiceCallResult<GetDataTypeInfo> >& resp, unsigned mask)
static bool validateDataTypeInfoResponse(const std::auto_ptr<ServiceCallResultCollector<GetDataTypeInfo>::Result>& resp,
unsigned mask)
{
if (!resp.get())
{
@@ -31,29 +32,29 @@ static bool validateDataTypeInfoResponse(const std::auto_ptr<ServiceCallResult<G
std::cout << "Request was not successful" << std::endl;
return false;
}
if (resp->response.name != DataType::getDataTypeFullName())
if (resp->getResponse().name != DataType::getDataTypeFullName())
{
std::cout << "Type name mismatch: '"
<< resp->response.name.c_str() << "' '"
<< resp->getResponse().name.c_str() << "' '"
<< DataType::getDataTypeFullName() << "'" << std::endl;
return false;
}
if (DataType::getDataTypeSignature().get() != resp->response.signature)
if (DataType::getDataTypeSignature().get() != resp->getResponse().signature)
{
std::cout << "Signature mismatch" << std::endl;
return false;
}
if (resp->response.mask != mask)
if (resp->getResponse().mask != mask)
{
std::cout << "Mask mismatch" << std::endl;
return false;
}
if (resp->response.kind.value != DataType::DataTypeKind)
if (resp->getResponse().kind.value != DataType::DataTypeKind)
{
std::cout << "Kind mismatch" << std::endl;
return false;
}
if (resp->response.id != DataType::DefaultDataTypeID)
if (resp->getResponse().id != DataType::DefaultDataTypeID)
{
std::cout << "DTID mismatch" << std::endl;
return false;
@@ -90,7 +91,7 @@ TEST(DataTypeInfoProvider, Basic)
ASSERT_TRUE(validateDataTypeInfoResponse<GetDataTypeInfo>(gdti_cln.collector.result,
GetDataTypeInfo::Response::MASK_KNOWN |
GetDataTypeInfo::Response::MASK_SERVING));
ASSERT_EQ(1, gdti_cln.collector.result->server_node_id.get());
ASSERT_EQ(1, gdti_cln.collector.result->getCallID().server_node_id.get());
/*
* GetDataTypeInfo request for GetDataTypeInfo by name
@@ -105,7 +106,7 @@ TEST(DataTypeInfoProvider, Basic)
ASSERT_TRUE(validateDataTypeInfoResponse<GetDataTypeInfo>(gdti_cln.collector.result,
GetDataTypeInfo::Response::MASK_KNOWN |
GetDataTypeInfo::Response::MASK_SERVING));
ASSERT_EQ(1, gdti_cln.collector.result->server_node_id.get());
ASSERT_EQ(1, gdti_cln.collector.result->getCallID().server_node_id.get());
/*
* GetDataTypeInfo request for NodeStatus - not used yet
@@ -148,11 +149,11 @@ TEST(DataTypeInfoProvider, Basic)
ASSERT_TRUE(gdti_cln.collector.result.get());
ASSERT_TRUE(gdti_cln.collector.result->isSuccessful());
ASSERT_EQ(1, gdti_cln.collector.result->server_node_id.get());
ASSERT_EQ(0, gdti_cln.collector.result->response.mask);
ASSERT_TRUE(gdti_cln.collector.result->response.name.empty()); // Empty name
ASSERT_EQ(gdti_request.id, gdti_cln.collector.result->response.id);
ASSERT_EQ(gdti_request.kind.value, gdti_cln.collector.result->response.kind.value);
ASSERT_EQ(1, gdti_cln.collector.result->getCallID().server_node_id.get());
ASSERT_EQ(0, gdti_cln.collector.result->getResponse().mask);
ASSERT_TRUE(gdti_cln.collector.result->getResponse().name.empty()); // Empty name
ASSERT_EQ(gdti_request.id, gdti_cln.collector.result->getResponse().id);
ASSERT_EQ(gdti_request.kind.value, gdti_cln.collector.result->getResponse().kind.value);
/*
* Requesting a non-existent type by name
@@ -166,11 +167,11 @@ TEST(DataTypeInfoProvider, Basic)
ASSERT_TRUE(gdti_cln.collector.result.get());
ASSERT_TRUE(gdti_cln.collector.result->isSuccessful());
ASSERT_EQ(1, gdti_cln.collector.result->server_node_id.get());
ASSERT_EQ(0, gdti_cln.collector.result->response.mask);
ASSERT_EQ("uavcan.equipment.gnss.Fix", gdti_cln.collector.result->response.name);
ASSERT_EQ(0, gdti_cln.collector.result->response.id);
ASSERT_EQ(0, gdti_cln.collector.result->response.kind.value);
ASSERT_EQ(1, gdti_cln.collector.result->getCallID().server_node_id.get());
ASSERT_EQ(0, gdti_cln.collector.result->getResponse().mask);
ASSERT_EQ("uavcan.equipment.gnss.Fix", gdti_cln.collector.result->getResponse().name);
ASSERT_EQ(0, gdti_cln.collector.result->getResponse().id);
ASSERT_EQ(0, gdti_cln.collector.result->getResponse().kind.value);
/*
* ComputeAggregateTypeSignature test for messages
@@ -184,12 +185,12 @@ TEST(DataTypeInfoProvider, Basic)
ASSERT_TRUE(cats_cln.collector.result.get());
ASSERT_TRUE(cats_cln.collector.result->isSuccessful());
ASSERT_EQ(1, cats_cln.collector.result->server_node_id.get());
ASSERT_EQ(NodeStatus::getDataTypeSignature().get(), cats_cln.collector.result->response.aggregate_signature);
ASSERT_EQ(2048, cats_cln.collector.result->response.mutually_known_ids.size());
ASSERT_TRUE(cats_cln.collector.result->response.mutually_known_ids[NodeStatus::DefaultDataTypeID]);
cats_cln.collector.result->response.mutually_known_ids[NodeStatus::DefaultDataTypeID] = false;
ASSERT_FALSE(cats_cln.collector.result->response.mutually_known_ids.any());
ASSERT_EQ(1, cats_cln.collector.result->getCallID().server_node_id.get());
ASSERT_EQ(NodeStatus::getDataTypeSignature().get(), cats_cln.collector.result->getResponse().aggregate_signature);
ASSERT_EQ(2048, cats_cln.collector.result->getResponse().mutually_known_ids.size());
ASSERT_TRUE(cats_cln.collector.result->getResponse().mutually_known_ids[NodeStatus::DefaultDataTypeID]);
cats_cln.collector.result->getResponse().mutually_known_ids[NodeStatus::DefaultDataTypeID] = false;
ASSERT_FALSE(cats_cln.collector.result->getResponse().mutually_known_ids.any());
/*
* ComputeAggregateTypeSignature test for services
@@ -203,13 +204,13 @@ TEST(DataTypeInfoProvider, Basic)
ASSERT_TRUE(cats_cln.collector.result.get());
ASSERT_TRUE(cats_cln.collector.result->isSuccessful());
ASSERT_EQ(1, cats_cln.collector.result->server_node_id.get());
ASSERT_EQ(512, cats_cln.collector.result->response.mutually_known_ids.size());
ASSERT_TRUE(cats_cln.collector.result->response.mutually_known_ids[GetDataTypeInfo::DefaultDataTypeID]);
ASSERT_TRUE(cats_cln.collector.result->response.mutually_known_ids[ComputeAggregateTypeSignature::DefaultDataTypeID]);
cats_cln.collector.result->response.mutually_known_ids[GetDataTypeInfo::DefaultDataTypeID] = false;
cats_cln.collector.result->response.mutually_known_ids[ComputeAggregateTypeSignature::DefaultDataTypeID] = false;
ASSERT_FALSE(cats_cln.collector.result->response.mutually_known_ids.any());
ASSERT_EQ(1, cats_cln.collector.result->getCallID().server_node_id.get());
ASSERT_EQ(512, cats_cln.collector.result->getResponse().mutually_known_ids.size());
ASSERT_TRUE(cats_cln.collector.result->getResponse().mutually_known_ids[GetDataTypeInfo::DefaultDataTypeID]);
ASSERT_TRUE(cats_cln.collector.result->getResponse().mutually_known_ids[ComputeAggregateTypeSignature::DefaultDataTypeID]);
cats_cln.collector.result->getResponse().mutually_known_ids[GetDataTypeInfo::DefaultDataTypeID] = false;
cats_cln.collector.result->getResponse().mutually_known_ids[ComputeAggregateTypeSignature::DefaultDataTypeID] = false;
ASSERT_FALSE(cats_cln.collector.result->getResponse().mutually_known_ids.any());
/*
* ComputeAggregateTypeSignature test for a non-existent type
@@ -221,6 +222,6 @@ TEST(DataTypeInfoProvider, Basic)
ASSERT_TRUE(cats_cln.collector.result.get());
ASSERT_TRUE(cats_cln.collector.result->isSuccessful());
ASSERT_EQ(0, cats_cln.collector.result->response.aggregate_signature);
ASSERT_FALSE(cats_cln.collector.result->response.mutually_known_ids.any());
ASSERT_EQ(0, cats_cln.collector.result->getResponse().aggregate_signature);
ASSERT_FALSE(cats_cln.collector.result->getResponse().mutually_known_ids.any());
}
libuavcan/test/protocol/dynamic_node_id_allocation_client.cpp → libuavcan/test/protocol/dynamic_node_id_client.cpp
+5 -5
View File
@@ -3,16 +3,16 @@
*/
#include <gtest/gtest.h>
#include <uavcan/protocol/dynamic_node_id_allocation_client.hpp>
#include <uavcan/protocol/dynamic_node_id_client.hpp>
#include "helpers.hpp"
TEST(DynamicNodeIDAllocationClient, Basic)
TEST(DynamicNodeIDClient, Basic)
{
// Node A is Allocator, Node B is Allocatee
InterlinkedTestNodesWithSysClock nodes(uavcan::NodeID(10), uavcan::NodeID::Broadcast);
uavcan::DynamicNodeIDAllocationClient dnidac(nodes.b);
uavcan::DynamicNodeIDClient dnidac(nodes.b);
uavcan::GlobalDataTypeRegistry::instance().reset();
uavcan::DefaultDataTypeRegistrator<uavcan::protocol::dynamic_node_id::Allocation> _reg1;
@@ -162,11 +162,11 @@ TEST(DynamicNodeIDAllocationClient, Basic)
}
TEST(DynamicNodeIDAllocationClient, NonPassiveMode)
TEST(DynamicNodeIDClient, NonPassiveMode)
{
InterlinkedTestNodesWithSysClock nodes;
uavcan::DynamicNodeIDAllocationClient dnidac(nodes.b);
uavcan::DynamicNodeIDClient dnidac(nodes.b);
uavcan::GlobalDataTypeRegistry::instance().reset();
uavcan::DefaultDataTypeRegistrator<uavcan::protocol::dynamic_node_id::Allocation> _reg1;
libuavcan/test/protocol/dynamic_node_id_server/allocation_request_manager.cpp
+117
View File
@@ -0,0 +1,117 @@
/*
* Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include <gtest/gtest.h>
#include <uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp>
#include <uavcan/protocol/dynamic_node_id_client.hpp>
#include "event_tracer.hpp"
#include "../helpers.hpp"
using uavcan::dynamic_node_id_server::UniqueID;
class AllocationRequestHandler : public uavcan::dynamic_node_id_server::IAllocationRequestHandler
{
std::vector<std::pair<UniqueID, uavcan::NodeID> > requests_;
public:
bool can_followup;
AllocationRequestHandler() : can_followup(false) { }
virtual void handleAllocationRequest(const UniqueID& unique_id, uavcan::NodeID preferred_node_id)
{
requests_.push_back(std::pair<UniqueID, uavcan::NodeID>(unique_id, preferred_node_id));
}
virtual bool canPublishFollowupAllocationResponse() const
{
return can_followup;
}
bool matchAndPopLastRequest(const UniqueID& unique_id, uavcan::NodeID preferred_node_id)
{
if (requests_.empty())
{
std::cout << "No pending requests" << std::endl;
return false;
}
const std::pair<UniqueID, uavcan::NodeID> pair = requests_.at(requests_.size() - 1U);
requests_.pop_back();
if (pair.first != unique_id)
{
std::cout << "Unique ID mismatch" << std::endl;
return false;
}
if (pair.second != preferred_node_id)
{
std::cout << "Node ID mismatch (" << pair.second.get() << ", " << preferred_node_id.get() << ")"
<< std::endl;
return false;
}
return true;
}
void reset() { requests_.clear(); }
};
TEST(dynamic_node_id_server_AllocationRequestManager, Basic)
{
using namespace uavcan::protocol::dynamic_node_id;
using namespace uavcan::protocol::dynamic_node_id::server;
using namespace uavcan::dynamic_node_id_server;
uavcan::GlobalDataTypeRegistry::instance().reset();
uavcan::DefaultDataTypeRegistrator<Allocation> _reg1;
// Node A is Allocator, Node B is Allocatee
InterlinkedTestNodesWithSysClock nodes(uavcan::NodeID(10), uavcan::NodeID::Broadcast);
uavcan::DynamicNodeIDClient client(nodes.b);
/*
* Client initialization
*/
uavcan::protocol::HardwareVersion hwver;
for (uavcan::uint8_t i = 0; i < hwver.unique_id.size(); i++)
{
hwver.unique_id[i] = i;
}
const uavcan::NodeID PreferredNodeID = 42;
ASSERT_LE(0, client.start(hwver, PreferredNodeID));
/*
* Request manager initialization
*/
EventTracer tracer;
AllocationRequestHandler handler;
handler.can_followup = true;
AllocationRequestManager manager(nodes.a, tracer, handler);
ASSERT_LE(0, manager.init());
/*
* Allocation
*/
nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2000));
ASSERT_TRUE(handler.matchAndPopLastRequest(hwver.unique_id, PreferredNodeID));
ASSERT_LE(0, manager.broadcastAllocationResponse(hwver.unique_id, PreferredNodeID));
nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100));
/*
* Checking the client
*/
ASSERT_TRUE(client.isAllocationComplete());
ASSERT_EQ(PreferredNodeID, client.getAllocatedNodeID());
}
libuavcan/test/protocol/dynamic_node_id_server/centralized/server.cpp
+78
View File
@@ -0,0 +1,78 @@
/*
* Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include <gtest/gtest.h>
#include <uavcan/protocol/dynamic_node_id_server/centralized.hpp>
#include <uavcan/protocol/dynamic_node_id_client.hpp>
#include "../../helpers.hpp"
#include "../event_tracer.hpp"
#include "../../helpers.hpp"
#include "../memory_storage_backend.hpp"
using uavcan::dynamic_node_id_server::UniqueID;
TEST(dynamic_node_id_server_centralized_Server, Basic)
{
using namespace uavcan::dynamic_node_id_server;
using namespace uavcan::protocol::dynamic_node_id;
using namespace uavcan::protocol::dynamic_node_id::server;
uavcan::GlobalDataTypeRegistry::instance().reset();
uavcan::DefaultDataTypeRegistrator<Allocation> _reg1;
uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GetNodeInfo> _reg2;
uavcan::DefaultDataTypeRegistrator<uavcan::protocol::NodeStatus> _reg3;
EventTracer tracer;
MemoryStorageBackend storage;
// Node A is Allocator, Node B is Allocatee
InterlinkedTestNodesWithSysClock nodes(uavcan::NodeID(10), uavcan::NodeID::Broadcast);
UniqueID own_unique_id;
own_unique_id[0] = 0xAA;
own_unique_id[3] = 0xCC;
own_unique_id[7] = 0xEE;
own_unique_id[9] = 0xBB;
/*
* Server
*/
uavcan::dynamic_node_id_server::CentralizedServer server(nodes.a, storage, tracer);
ASSERT_LE(0, server.init(own_unique_id));
ASSERT_EQ(1, server.getNumAllocations()); // Server's own node ID
/*
* Client
*/
uavcan::DynamicNodeIDClient client(nodes.b);
uavcan::protocol::HardwareVersion hwver;
for (uavcan::uint8_t i = 0; i < hwver.unique_id.size(); i++)
{
hwver.unique_id[i] = i;
}
const uavcan::NodeID PreferredNodeID = 42;
ASSERT_LE(0, client.start(hwver, PreferredNodeID));
/*
* Fire
*/
nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(15000));
ASSERT_TRUE(client.isAllocationComplete());
ASSERT_EQ(PreferredNodeID, client.getAllocatedNodeID());
ASSERT_EQ(2, server.getNumAllocations()); // Server's own node ID + client
}
TEST(dynamic_node_id_server_centralized, ObjectSizes)
{
using namespace uavcan::dynamic_node_id_server;
std::cout << "centralized::Storage: " << sizeof(centralized::Storage) << std::endl;
std::cout << "centralized::Server: " << sizeof(centralized::Server) << std::endl;
std::cout << "NodeDiscoverer: " << sizeof(NodeDiscoverer) << std::endl;
}
libuavcan/test/protocol/dynamic_node_id_server/centralized/storage.cpp
+174
View File
@@ -0,0 +1,174 @@
/*
* Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include <gtest/gtest.h>
#include <uavcan/protocol/dynamic_node_id_server/centralized/storage.hpp>
#include "../../helpers.hpp"
#include "../memory_storage_backend.hpp"
TEST(dynamic_node_id_server_centralized_Storage, Initialization)
{
using namespace uavcan::dynamic_node_id_server::centralized;
// No data in the storage - initializing empty
{
MemoryStorageBackend storage;
Storage stor(storage);
ASSERT_EQ(0, storage.getNumKeys());
ASSERT_LE(0, stor.init());
ASSERT_EQ(1, storage.getNumKeys());
ASSERT_EQ(0, stor.getSize());
ASSERT_FALSE(stor.findByNodeID(1));
ASSERT_FALSE(stor.findByNodeID(0));
}
// Nonempty storage, one item
{
MemoryStorageBackend storage;
Storage stor(storage);
storage.set("size", "1");
ASSERT_EQ(-uavcan::ErrFailure, stor.init()); // Expected one entry, none found
ASSERT_EQ(1, storage.getNumKeys());
storage.set("0_unique_id", "00000000000000000000000000000000");
storage.set("0_node_id", "0");
ASSERT_EQ(-uavcan::ErrFailure, stor.init()); // Invalid entry - zero Node ID
storage.set("0_node_id", "1");
ASSERT_LE(0, stor.init()); // OK now
ASSERT_EQ(3, storage.getNumKeys());
ASSERT_EQ(1, stor.getSize());
ASSERT_TRUE(stor.findByNodeID(1));
ASSERT_FALSE(stor.findByNodeID(0));
}
// Nonempty storage, broken data
{
MemoryStorageBackend storage;
Storage stor(storage);
storage.set("size", "foobar");
ASSERT_EQ(-uavcan::ErrFailure, stor.init()); // Bad value
storage.set("size", "128");
ASSERT_EQ(-uavcan::ErrFailure, stor.init()); // Bad value
storage.set("size", "1");
ASSERT_EQ(-uavcan::ErrFailure, stor.init()); // No items
ASSERT_EQ(1, storage.getNumKeys());
storage.set("0_unique_id", "00000000000000000000000000000000");
storage.set("0_node_id", "128"); // Bad value (127 max)
ASSERT_EQ(-uavcan::ErrFailure, stor.init()); // Failed
storage.set("0_node_id", "127");
ASSERT_LE(0, stor.init()); // OK now
ASSERT_EQ(1, stor.getSize());
ASSERT_TRUE(stor.findByNodeID(127));
ASSERT_FALSE(stor.findByNodeID(0));
ASSERT_EQ(3, storage.getNumKeys());
}
// Nonempty storage, many items
{
MemoryStorageBackend storage;
Storage stor(storage);
storage.set("size", "2");
storage.set("0_unique_id", "00000000000000000000000000000000");
storage.set("0_node_id", "1");
storage.set("1_unique_id", "0123456789abcdef0123456789abcdef");
storage.set("1_node_id", "127");
ASSERT_LE(0, stor.init()); // OK now
ASSERT_EQ(5, storage.getNumKeys());
ASSERT_EQ(2, stor.getSize());
ASSERT_TRUE(stor.findByNodeID(1));
ASSERT_TRUE(stor.findByNodeID(127));
ASSERT_FALSE(stor.findByNodeID(0));
uavcan::protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id uid;
uid[0] = 0x01;
uid[1] = 0x23;
uid[2] = 0x45;
uid[3] = 0x67;
uid[4] = 0x89;
uid[5] = 0xab;
uid[6] = 0xcd;
uid[7] = 0xef;
uavcan::copy(uid.begin(), uid.begin() + 8, uid.begin() + 8);
ASSERT_TRUE(stor.findByUniqueID(uid));
ASSERT_EQ(127, stor.findByUniqueID(uid)->node_id.get());
ASSERT_EQ(uid, stor.findByNodeID(127)->unique_id);
}
}
TEST(dynamic_node_id_server_centralized_Storage, Basic)
{
using namespace uavcan::dynamic_node_id_server::centralized;
MemoryStorageBackend storage;
Storage stor(storage);
ASSERT_EQ(0, storage.getNumKeys());
ASSERT_LE(0, stor.init());
storage.print();
ASSERT_EQ(1, storage.getNumKeys());
/*
* Adding one entry to the log, making sure it appears in the storage
*/
Storage::Entry entry;
entry.node_id = 1;
entry.unique_id[0] = 1;
ASSERT_LE(0, stor.add(entry.node_id, entry.unique_id));
ASSERT_EQ("1", storage.get("size"));
ASSERT_EQ("01000000000000000000000000000000", storage.get("0_unique_id"));
ASSERT_EQ("1", storage.get("0_node_id"));
ASSERT_EQ(3, storage.getNumKeys());
ASSERT_EQ(1, stor.getSize());
/*
* Adding another entry while storage is failing
*/
storage.failOnSetCalls(true);
ASSERT_EQ(3, storage.getNumKeys());
entry.node_id = 2;
entry.unique_id[0] = 2;
ASSERT_GT(0, stor.add(entry.node_id, entry.unique_id));
ASSERT_EQ(3, storage.getNumKeys()); // No new entries, we failed
ASSERT_EQ(1, stor.getSize());
/*
* Making sure add() fails when the log is full
*/
storage.failOnSetCalls(false);
while (stor.getSize() < stor.Capacity)
{
ASSERT_LE(0, stor.add(entry.node_id, entry.unique_id));
entry.node_id = uint8_t(uavcan::min(entry.node_id.get() + 1U, 127U));
entry.unique_id[0] = uint8_t(entry.unique_id[0] + 1U);
}
ASSERT_GT(0, stor.add(123, entry.unique_id)); // Failing because full
storage.print();
}
libuavcan/test/protocol/dynamic_node_id_server/distributed/cluster_manager.cpp
+263
View File
@@ -0,0 +1,263 @@
/*
* Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include <gtest/gtest.h>
#include <uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp>
#include "../event_tracer.hpp"
#include "../../helpers.hpp"
#include "../memory_storage_backend.hpp"
TEST(dynamic_node_id_server_ClusterManager, Initialization)
{
using namespace uavcan::dynamic_node_id_server::distributed;
const unsigned MaxClusterSize =
uavcan::protocol::dynamic_node_id::server::Discovery::FieldTypes::known_nodes::MaxSize;
uavcan::GlobalDataTypeRegistry::instance().reset();
uavcan::DefaultDataTypeRegistrator<uavcan::protocol::dynamic_node_id::server::Discovery> _reg1;
EventTracer tracer;
/*
* Simple initialization
*/
{
MemoryStorageBackend storage;
Log log(storage, tracer);
InterlinkedTestNodesWithSysClock nodes;
ClusterManager mgr(nodes.a, storage, log, tracer);
// Too big
ASSERT_GT(0, mgr.init(MaxClusterSize + 1));
ASSERT_EQ(0, storage.getNumKeys());
// OK
ASSERT_LE(0, mgr.init(5));
ASSERT_EQ(1, storage.getNumKeys());
ASSERT_EQ("5", storage.get("cluster_size"));
// Testing other states
ASSERT_EQ(0, mgr.getNumKnownServers());
ASSERT_EQ(5, mgr.getClusterSize());
ASSERT_EQ(3, mgr.getQuorumSize());
ASSERT_FALSE(mgr.getRemoteServerNodeIDAtIndex(0).isValid());
}
/*
* Recovery from the storage
*/
{
MemoryStorageBackend storage;
Log log(storage, tracer);
InterlinkedTestNodesWithSysClock nodes;
ClusterManager mgr(nodes.a, storage, log, tracer);
// Not configured
ASSERT_GT(0, mgr.init());
ASSERT_EQ(0, storage.getNumKeys());
// OK
storage.set("cluster_size", "5");
ASSERT_LE(0, mgr.init());
ASSERT_EQ(1, storage.getNumKeys());
}
}
TEST(dynamic_node_id_server_ClusterManager, OneServer)
{
using namespace uavcan::dynamic_node_id_server::distributed;
uavcan::GlobalDataTypeRegistry::instance().reset();
uavcan::DefaultDataTypeRegistrator<uavcan::protocol::dynamic_node_id::server::Discovery> _reg1;
EventTracer tracer;
MemoryStorageBackend storage;
Log log(storage, tracer);
InterlinkedTestNodesWithSysClock nodes;
ClusterManager mgr(nodes.a, storage, log, tracer);
/*
* Pub and sub
*/
SubscriberWithCollector<uavcan::protocol::dynamic_node_id::server::Discovery> sub(nodes.b);
uavcan::Publisher<uavcan::protocol::dynamic_node_id::server::Discovery> pub(nodes.b);
ASSERT_LE(0, sub.start());
ASSERT_LE(0, pub.init());
/*
* Starting
*/
ASSERT_LE(0, mgr.init(1));
ASSERT_EQ(0, mgr.getNumKnownServers());
ASSERT_TRUE(mgr.isClusterDiscovered());
ASSERT_EQ(0, nodes.a.internal_failure_count);
/*
* Broadcasting discovery with wrong cluster size, it will be reported as internal failure
*/
uavcan::protocol::dynamic_node_id::server::Discovery msg;
msg.configured_cluster_size = 2;
msg.known_nodes.push_back(2U);
ASSERT_LE(0, pub.broadcast(msg));
nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10));
ASSERT_EQ(1, nodes.a.internal_failure_count);
/*
* Discovery rate limiting test
*/
ASSERT_FALSE(sub.collector.msg.get());
msg = uavcan::protocol::dynamic_node_id::server::Discovery();
msg.configured_cluster_size = 1; // Correct value
ASSERT_LE(0, pub.broadcast(msg)); // List of known nodes is empty, intentionally
nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100));
ASSERT_FALSE(sub.collector.msg.get());
nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000));
ASSERT_TRUE(sub.collector.msg.get());
ASSERT_EQ(1, sub.collector.msg->configured_cluster_size);
ASSERT_EQ(1, sub.collector.msg->known_nodes.size());
ASSERT_EQ(1, sub.collector.msg->known_nodes[0]);
sub.collector.msg.reset();
// Rinse repeat
ASSERT_LE(0, pub.broadcast(msg));
nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100));
ASSERT_FALSE(sub.collector.msg.get());
nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000));
ASSERT_TRUE(sub.collector.msg.get());
ASSERT_EQ(1, sub.collector.msg->configured_cluster_size);
ASSERT_EQ(1, sub.collector.msg->known_nodes.size());
ASSERT_EQ(1, sub.collector.msg->known_nodes[0]);
sub.collector.msg.reset();
}
TEST(dynamic_node_id_server_ClusterManager, ThreeServers)
{
using namespace uavcan::dynamic_node_id_server::distributed;
uavcan::GlobalDataTypeRegistry::instance().reset();
uavcan::DefaultDataTypeRegistrator<uavcan::protocol::dynamic_node_id::server::Discovery> _reg1;
EventTracer tracer;
MemoryStorageBackend storage;
Log log(storage, tracer);
InterlinkedTestNodesWithSysClock nodes;
ClusterManager mgr(nodes.a, storage, log, tracer);
/*
* Pub and sub
*/
SubscriberWithCollector<uavcan::protocol::dynamic_node_id::server::Discovery> sub(nodes.b);
uavcan::Publisher<uavcan::protocol::dynamic_node_id::server::Discovery> pub(nodes.b);
ASSERT_LE(0, sub.start());
ASSERT_LE(0, pub.init());
/*
* Starting
*/
ASSERT_LE(0, mgr.init(3));
ASSERT_EQ(0, mgr.getNumKnownServers());
ASSERT_FALSE(mgr.isClusterDiscovered());
/*
* Discovery publishing rate check
*/
nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100));
ASSERT_FALSE(sub.collector.msg.get());
nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000));
ASSERT_TRUE(sub.collector.msg.get());
ASSERT_EQ(3, sub.collector.msg->configured_cluster_size);
ASSERT_EQ(1, sub.collector.msg->known_nodes.size());
ASSERT_EQ(1, sub.collector.msg->known_nodes[0]);
sub.collector.msg.reset();
nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100));
ASSERT_FALSE(sub.collector.msg.get());
nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000));
ASSERT_TRUE(sub.collector.msg.get());
ASSERT_EQ(3, sub.collector.msg->configured_cluster_size);
ASSERT_EQ(1, sub.collector.msg->known_nodes.size());
ASSERT_EQ(1, sub.collector.msg->known_nodes[0]);
sub.collector.msg.reset();
/*
* Discovering other nodes
*/
uavcan::protocol::dynamic_node_id::server::Discovery msg;
msg.configured_cluster_size = 3;
msg.known_nodes.push_back(2U);
ASSERT_LE(0, pub.broadcast(msg));
nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1050));
ASSERT_TRUE(sub.collector.msg.get());
ASSERT_EQ(3, sub.collector.msg->configured_cluster_size);
ASSERT_EQ(2, sub.collector.msg->known_nodes.size());
ASSERT_EQ(1, sub.collector.msg->known_nodes[0]);
ASSERT_EQ(2, sub.collector.msg->known_nodes[1]);
sub.collector.msg.reset();
ASSERT_FALSE(mgr.isClusterDiscovered());
// This will complete the discovery
msg.known_nodes.push_back(127U);
ASSERT_LE(0, pub.broadcast(msg));
nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1050));
ASSERT_TRUE(sub.collector.msg.get());
ASSERT_EQ(3, sub.collector.msg->configured_cluster_size);
ASSERT_EQ(3, sub.collector.msg->known_nodes.size());
ASSERT_EQ(1, sub.collector.msg->known_nodes[0]);
ASSERT_EQ(2, sub.collector.msg->known_nodes[1]);
ASSERT_EQ(127, sub.collector.msg->known_nodes[2]);
sub.collector.msg.reset();
// Making sure discovery is now terminated
nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1500));
ASSERT_FALSE(sub.collector.msg.get());
/*
* Checking Raft states
*/
ASSERT_EQ(uavcan::NodeID(2), mgr.getRemoteServerNodeIDAtIndex(0));
ASSERT_EQ(uavcan::NodeID(127), mgr.getRemoteServerNodeIDAtIndex(1));
ASSERT_EQ(uavcan::NodeID(), mgr.getRemoteServerNodeIDAtIndex(2));
ASSERT_EQ(0, mgr.getServerMatchIndex(2));
ASSERT_EQ(0, mgr.getServerMatchIndex(127));
ASSERT_EQ(log.getLastIndex() + 1, mgr.getServerNextIndex(2));
ASSERT_EQ(log.getLastIndex() + 1, mgr.getServerNextIndex(127));
mgr.setServerMatchIndex(2, 10);
ASSERT_EQ(10, mgr.getServerMatchIndex(2));
mgr.incrementServerNextIndexBy(2, 5);
ASSERT_EQ(log.getLastIndex() + 1 + 5, mgr.getServerNextIndex(2));
mgr.decrementServerNextIndex(2);
ASSERT_EQ(log.getLastIndex() + 1 + 5 - 1, mgr.getServerNextIndex(2));
mgr.resetAllServerIndices();
ASSERT_EQ(0, mgr.getServerMatchIndex(2));
ASSERT_EQ(0, mgr.getServerMatchIndex(127));
ASSERT_EQ(log.getLastIndex() + 1, mgr.getServerNextIndex(2));
ASSERT_EQ(log.getLastIndex() + 1, mgr.getServerNextIndex(127));
}
libuavcan/test/protocol/dynamic_node_id_server/distributed/log.cpp
+243
View File
@@ -0,0 +1,243 @@
/*
* Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include <gtest/gtest.h>
#include <uavcan/protocol/dynamic_node_id_server/distributed/log.hpp>
#include "../event_tracer.hpp"
#include "../../helpers.hpp"
#include "../memory_storage_backend.hpp"
static const unsigned NumEntriesInStorageWithEmptyLog = 4; // last index + 3 items per log entry
TEST(dynamic_node_id_server_Log, Initialization)
{
using namespace uavcan::dynamic_node_id_server::distributed;
EventTracer tracer;
// No log data in the storage - initializing empty log
{
MemoryStorageBackend storage;
uavcan::dynamic_node_id_server::distributed::Log log(storage, tracer);
ASSERT_EQ(0, storage.getNumKeys());
ASSERT_LE(0, log.init());
ASSERT_EQ(NumEntriesInStorageWithEmptyLog, storage.getNumKeys());
ASSERT_EQ(0, log.getLastIndex());
ASSERT_EQ(0, log.getEntryAtIndex(0)->term);
ASSERT_EQ(0, log.getEntryAtIndex(0)->node_id);
ASSERT_EQ(uavcan::protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id(),
log.getEntryAtIndex(0)->unique_id);
}
// Nonempty storage, one item
{
MemoryStorageBackend storage;
Log log(storage, tracer);
storage.set("log_last_index", "0");
ASSERT_LE(-uavcan::ErrFailure, log.init()); // Expected one entry, none found
ASSERT_EQ(1, storage.getNumKeys());
storage.set("log0_term", "0");
storage.set("log0_unique_id", "00000000000000000000000000000000");
storage.set("log0_node_id", "0");
ASSERT_LE(0, log.init()); // OK now
ASSERT_EQ(NumEntriesInStorageWithEmptyLog, storage.getNumKeys());
ASSERT_EQ(0, log.getLastIndex());
ASSERT_EQ(0, log.getEntryAtIndex(0)->term);
}
// Nonempty storage, broken data
{
MemoryStorageBackend storage;
Log log(storage, tracer);
storage.set("log_last_index", "foobar");
ASSERT_LE(-uavcan::ErrFailure, log.init()); // Bad value
storage.set("log_last_index", "128");
ASSERT_LE(-uavcan::ErrFailure, log.init()); // Bad value
storage.set("log_last_index", "0");
ASSERT_LE(-uavcan::ErrFailure, log.init()); // No log items
ASSERT_EQ(1, storage.getNumKeys());
storage.set("log0_term", "0");
storage.set("log0_unique_id", "00000000000000000000000000000000");
storage.set("log0_node_id", "128"); // Bad value (127 max)
ASSERT_LE(-uavcan::ErrFailure, log.init()); // Failed
ASSERT_EQ(0, log.getLastIndex());
ASSERT_EQ(0, log.getEntryAtIndex(0)->term);
ASSERT_EQ(4, storage.getNumKeys());
}
// Nonempty storage, many items
{
MemoryStorageBackend storage;
Log log(storage, tracer);
storage.set("log_last_index", "1"); // 2 items - 0, 1
storage.set("log0_term", "0");
storage.set("log0_unique_id", "00000000000000000000000000000000");
storage.set("log0_node_id", "0");
storage.set("log1_term", "1");
storage.set("log1_unique_id", "0123456789abcdef0123456789abcdef");
storage.set("log1_node_id", "127");
ASSERT_LE(0, log.init()); // OK now
ASSERT_EQ(7, storage.getNumKeys());
ASSERT_EQ(1, log.getLastIndex());
ASSERT_EQ(0, log.getEntryAtIndex(0)->term);
ASSERT_EQ(0, log.getEntryAtIndex(0)->node_id);
ASSERT_EQ(uavcan::protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id(),
log.getEntryAtIndex(0)->unique_id);
ASSERT_EQ(1, log.getEntryAtIndex(1)->term);
ASSERT_EQ(127, log.getEntryAtIndex(1)->node_id);
uavcan::protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id uid;
uid[0] = 0x01;
uid[1] = 0x23;
uid[2] = 0x45;
uid[3] = 0x67;
uid[4] = 0x89;
uid[5] = 0xab;
uid[6] = 0xcd;
uid[7] = 0xef;
uavcan::copy(uid.begin(), uid.begin() + 8, uid.begin() + 8);
ASSERT_EQ(uid, log.getEntryAtIndex(1)->unique_id);
}
}
TEST(dynamic_node_id_server_Log, Append)
{
using namespace uavcan::dynamic_node_id_server::distributed;
EventTracer tracer;
MemoryStorageBackend storage;
Log log(storage, tracer);
ASSERT_EQ(0, storage.getNumKeys());
ASSERT_LE(0, log.init());
storage.print();
ASSERT_EQ(NumEntriesInStorageWithEmptyLog, storage.getNumKeys());
/*
* Entry at the index 0 always exists, and it's always zero-initialized.
*/
ASSERT_EQ("0", storage.get("log_last_index"));
ASSERT_EQ("0", storage.get("log0_term"));
ASSERT_EQ("00000000000000000000000000000000", storage.get("log0_unique_id"));
ASSERT_EQ("0", storage.get("log0_node_id"));
/*
* Adding one entry to the log, making sure it appears in the storage
*/
uavcan::protocol::dynamic_node_id::server::Entry entry;
entry.term = 1;
entry.node_id = 1;
entry.unique_id[0] = 1;
ASSERT_LE(0, log.append(entry));
ASSERT_EQ("1", storage.get("log_last_index"));
ASSERT_EQ("1", storage.get("log1_term"));
ASSERT_EQ("01000000000000000000000000000000", storage.get("log1_unique_id"));
ASSERT_EQ("1", storage.get("log1_node_id"));
ASSERT_EQ(1, log.getLastIndex());
ASSERT_TRUE(entry == *log.getEntryAtIndex(1));
/*
* Adding another entry while storage is failing
*/
storage.failOnSetCalls(true);
ASSERT_EQ(7, storage.getNumKeys());
entry.term = 2;
entry.node_id = 2;
entry.unique_id[0] = 2;
ASSERT_GT(0, log.append(entry));
ASSERT_EQ(7, storage.getNumKeys()); // No new entries, we failed
ASSERT_EQ(1, log.getLastIndex());
/*
* Making sure append() fails when the log is full
*/
storage.failOnSetCalls(false);
while (log.getLastIndex() < (log.Capacity - 1))
{
ASSERT_LE(0, log.append(entry));
ASSERT_TRUE(entry == *log.getEntryAtIndex(log.getLastIndex()));
entry.term += 1;
entry.node_id = uint8_t(entry.node_id + 1U);
entry.unique_id[0] = uint8_t(entry.unique_id[0] + 1U);
}
ASSERT_GT(0, log.append(entry)); // Failing because full
storage.print();
}
TEST(dynamic_node_id_server_Log, Remove)
{
using namespace uavcan::dynamic_node_id_server::distributed;
EventTracer tracer;
MemoryStorageBackend storage;
Log log(storage, tracer);
/*
* Filling the log fully
*/
uavcan::protocol::dynamic_node_id::server::Entry entry;
entry.term = 1;
entry.node_id = 1;
entry.unique_id[0] = 1;
while (log.getLastIndex() < (log.Capacity - 1))
{
ASSERT_LE(0, log.append(entry));
ASSERT_TRUE(entry == *log.getEntryAtIndex(log.getLastIndex()));
entry.term += 1;
entry.node_id = uint8_t(entry.node_id + 1U);
entry.unique_id[0] = uint8_t(entry.unique_id[0] + 1U);
}
/*
* Removal will fail as the storage is failing to update
*/
storage.failOnSetCalls(true);
ASSERT_EQ(log.Capacity - 1, log.getLastIndex());
ASSERT_GT(0, log.removeEntriesWhereIndexGreaterOrEqual(60)); // Failing
ASSERT_EQ(log.Capacity - 1, log.getLastIndex());
/*
* Now removal must work
*/
storage.failOnSetCalls(false);
ASSERT_EQ(log.Capacity - 1, log.getLastIndex());
ASSERT_LE(0, log.removeEntriesWhereIndexGreaterOrEqual(60));
ASSERT_EQ(59, log.getLastIndex());
ASSERT_EQ("59", storage.get("log_last_index"));
ASSERT_LE(0, log.removeEntriesWhereIndexGreater(30));
ASSERT_EQ(30, log.getLastIndex());
ASSERT_EQ("30", storage.get("log_last_index"));
ASSERT_LE(0, log.removeEntriesWhereIndexGreaterOrEqual(1));
ASSERT_EQ(0, log.getLastIndex());
ASSERT_EQ("0", storage.get("log_last_index"));
storage.print();
}
libuavcan/test/protocol/dynamic_node_id_server/distributed/persistent_state.cpp
+209
View File
@@ -0,0 +1,209 @@
/*
* Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include <gtest/gtest.h>
#include <uavcan/protocol/dynamic_node_id_server/distributed/persistent_state.hpp>
#include "../event_tracer.hpp"
#include "../memory_storage_backend.hpp"
TEST(dynamic_node_id_server_PersistentState, Initialization)
{
using namespace uavcan::dynamic_node_id_server::distributed;
EventTracer tracer;
/*
* First initialization
*/
{
MemoryStorageBackend storage;
PersistentState pers(storage, tracer);
ASSERT_EQ(0, storage.getNumKeys());
ASSERT_LE(0, pers.init());
ASSERT_LE(3, storage.getNumKeys());
ASSERT_EQ("0", storage.get("log_last_index"));
ASSERT_EQ("0", storage.get("current_term"));
ASSERT_EQ("0", storage.get("voted_for"));
}
/*
* Partial recovery - only empty log is recovered
*/
{
MemoryStorageBackend storage;
{
// This log is used to initialize the storage
Log log(storage, tracer);
ASSERT_LE(0, log.init());
}
ASSERT_LE(1, storage.getNumKeys());
PersistentState pers(storage, tracer);
ASSERT_LE(0, pers.init());
ASSERT_LE(3, storage.getNumKeys());
ASSERT_EQ("0", storage.get("log_last_index"));
ASSERT_EQ("0", storage.get("current_term"));
ASSERT_EQ("0", storage.get("voted_for"));
}
/*
* Partial recovery - log and current term are recovered
*/
{
MemoryStorageBackend storage;
{
// This log is used to initialize the storage
Log log(storage, tracer);
ASSERT_LE(0, log.init());
}
ASSERT_LE(1, storage.getNumKeys());
storage.set("current_term", "1");
PersistentState pers(storage, tracer);
ASSERT_GT(0, pers.init()); // Fails because current term is not zero
storage.set("current_term", "0");
ASSERT_LE(0, pers.init()); // OK now
ASSERT_LE(3, storage.getNumKeys());
ASSERT_EQ("0", storage.get("log_last_index"));
ASSERT_EQ("0", storage.get("current_term"));
ASSERT_EQ("0", storage.get("voted_for"));
}
/*
* Full recovery
*/
{
MemoryStorageBackend storage;
{
// This log is used to initialize the storage
Log log(storage, tracer);
ASSERT_LE(0, log.init());
uavcan::protocol::dynamic_node_id::server::Entry entry;
entry.term = 1;
entry.node_id = 1;
entry.unique_id[0] = 1;
ASSERT_LE(0, log.append(entry));
}
ASSERT_LE(4, storage.getNumKeys());
PersistentState pers(storage, tracer);
ASSERT_GT(0, pers.init()); // Fails because log is not empty
storage.set("current_term", "0");
storage.set("voted_for", "0");
ASSERT_GT(0, pers.init()); // Fails because of bad currentTerm
storage.set("current_term", "1"); // OK
storage.set("voted_for", "128"); // Invalid value
ASSERT_GT(0, pers.init()); // Fails because of bad votedFor
storage.set("voted_for", "0"); // OK now
ASSERT_LE(0, pers.init());
ASSERT_LE(3, storage.getNumKeys());
ASSERT_EQ("1", storage.get("log_last_index"));
ASSERT_EQ("1", storage.get("current_term"));
ASSERT_EQ("0", storage.get("voted_for"));
}
}
TEST(dynamic_node_id_server_PersistentState, Basic)
{
using namespace uavcan::dynamic_node_id_server::distributed;
EventTracer tracer;
MemoryStorageBackend storage;
PersistentState pers(storage, tracer);
/*
* Initializing
*/
ASSERT_LE(0, pers.init());
ASSERT_EQ("0", storage.get("log_last_index"));
ASSERT_EQ("0", storage.get("current_term"));
ASSERT_EQ("0", storage.get("voted_for"));
/*
* Inserting some log entries
*/
uavcan::protocol::dynamic_node_id::server::Entry entry;
entry.term = 1;
entry.node_id = 1;
entry.unique_id[0] = 1;
ASSERT_LE(0, pers.getLog().append(entry));
ASSERT_EQ("1", storage.get("log_last_index"));
ASSERT_EQ("0", storage.get("current_term"));
ASSERT_EQ("0", storage.get("voted_for"));
/*
* Changing current term
*/
ASSERT_EQ(0, pers.getCurrentTerm());
ASSERT_LE(0, pers.setCurrentTerm(2));
ASSERT_EQ(2, pers.getCurrentTerm());
ASSERT_EQ("1", storage.get("log_last_index"));
ASSERT_EQ("2", storage.get("current_term"));
ASSERT_EQ("0", storage.get("voted_for"));
/*
* Changing votedFor
*/
ASSERT_FALSE(pers.isVotedForSet());
ASSERT_EQ(0, pers.getVotedFor().get());
ASSERT_LE(0, pers.setVotedFor(0));
ASSERT_EQ(0, pers.getVotedFor().get());
ASSERT_LE(0, pers.setVotedFor(45));
ASSERT_EQ(45, pers.getVotedFor().get());
ASSERT_TRUE(pers.isVotedForSet());
ASSERT_EQ("1", storage.get("log_last_index"));
ASSERT_EQ("2", storage.get("current_term"));
ASSERT_EQ("45", storage.get("voted_for"));
ASSERT_LE(0, pers.resetVotedFor());
ASSERT_EQ(0, pers.getVotedFor().get());
ASSERT_FALSE(pers.isVotedForSet());
ASSERT_EQ("0", storage.get("voted_for"));
ASSERT_LE(0, pers.setVotedFor(45));
ASSERT_TRUE(pers.isVotedForSet());
ASSERT_EQ("45", storage.get("voted_for"));
/*
* Handling errors
*/
storage.failOnSetCalls(true);
ASSERT_EQ(2, pers.getCurrentTerm());
ASSERT_GT(0, pers.setCurrentTerm(7893));
ASSERT_EQ(2, pers.getCurrentTerm());
ASSERT_EQ(45, pers.getVotedFor().get());
ASSERT_GT(0, pers.setVotedFor(78));
ASSERT_EQ(45, pers.getVotedFor().get());
ASSERT_EQ("1", storage.get("log_last_index"));
ASSERT_EQ("2", storage.get("current_term"));
ASSERT_EQ("45", storage.get("voted_for"));
/*
* Final checks
*/
ASSERT_GT(10, storage.getNumKeys()); // Making sure there's some sane number of keys in the storage
}
libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp
+195
View File
@@ -0,0 +1,195 @@
/*
* Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#if __GNUC__
// We need auto_ptr for compatibility reasons
# pragma GCC diagnostic ignored "-Wdeprecated-declarations"
#endif
#include <gtest/gtest.h>
#include <memory>
#include <uavcan/protocol/dynamic_node_id_server/distributed.hpp>
#include <uavcan/protocol/dynamic_node_id_client.hpp>
#include "../event_tracer.hpp"
#include "../../helpers.hpp"
#include "../memory_storage_backend.hpp"
using uavcan::dynamic_node_id_server::UniqueID;
class CommitHandler : public uavcan::dynamic_node_id_server::distributed::IRaftLeaderMonitor
{
const std::string id_;
virtual void handleLogCommitOnLeader(const uavcan::protocol::dynamic_node_id::server::Entry& entry)
{
std::cout << "ENTRY COMMITTED [" << id_ << "]\n" << entry << std::endl;
}
virtual void handleLocalLeadershipChange(bool local_node_is_leader)
{
std::cout << "I AM LEADER [" << id_ << "]: " << (local_node_is_leader ? "YES" : "NOT ANYMORE") << std::endl;
}
public:
CommitHandler(const std::string& id) : id_(id) { }
};
TEST(dynamic_node_id_server_RaftCore, Basic)
{
using namespace uavcan::dynamic_node_id_server::distributed;
using namespace uavcan::protocol::dynamic_node_id::server;
uavcan::GlobalDataTypeRegistry::instance().reset();
uavcan::DefaultDataTypeRegistrator<Discovery> _reg1;
uavcan::DefaultDataTypeRegistrator<AppendEntries> _reg2;
uavcan::DefaultDataTypeRegistrator<RequestVote> _reg3;
EventTracer tracer_a("a");
EventTracer tracer_b("b");
MemoryStorageBackend storage_a;
MemoryStorageBackend storage_b;
CommitHandler commit_handler_a("a");
CommitHandler commit_handler_b("b");
InterlinkedTestNodesWithSysClock nodes;
std::auto_ptr<RaftCore> raft_a(new RaftCore(nodes.a, storage_a, tracer_a, commit_handler_a));
std::auto_ptr<RaftCore> raft_b(new RaftCore(nodes.b, storage_b, tracer_b, commit_handler_b));
/*
* Initialization
*/
ASSERT_LE(0, raft_a->init(2));
ASSERT_LE(0, raft_b->init(2));
/*
* Running and trying not to fall
*/
nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(9000));
// Either must become a leader
ASSERT_TRUE(raft_a->isLeader() || raft_b->isLeader());
ASSERT_EQ(0, raft_a->getCommitIndex());
ASSERT_EQ(0, raft_b->getCommitIndex());
/*
* Adding some stuff
*/
Entry::FieldTypes::unique_id unique_id;
uavcan::fill_n(unique_id.begin(), 16, uint8_t(0xAA));
(raft_a->isLeader() ? raft_a : raft_b)->appendLog(unique_id, uavcan::NodeID(1));
nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(6000));
ASSERT_EQ(1, raft_a->getCommitIndex());
ASSERT_EQ(1, raft_b->getCommitIndex());
/*
* Terminating the leader
*/
raft_a.reset();
nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(6000));
/*
* Reinitializing the leader - current Follower will become the new Leader
*/
storage_a.reset();
raft_a.reset(new RaftCore(nodes.a, storage_a, tracer_a, commit_handler_a));
ASSERT_LE(0, raft_a->init(2));
ASSERT_EQ(0, raft_a->getCommitIndex());
nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(9000));
ASSERT_FALSE(raft_a->isLeader());
ASSERT_TRUE(raft_b->isLeader());
ASSERT_EQ(1, raft_a->getCommitIndex());
ASSERT_EQ(1, raft_b->getCommitIndex());
}
TEST(dynamic_node_id_server_Server, Basic)
{
using namespace uavcan::dynamic_node_id_server;
using namespace uavcan::protocol::dynamic_node_id;
using namespace uavcan::protocol::dynamic_node_id::server;
uavcan::GlobalDataTypeRegistry::instance().reset();
uavcan::DefaultDataTypeRegistrator<Discovery> _reg1;
uavcan::DefaultDataTypeRegistrator<AppendEntries> _reg2;
uavcan::DefaultDataTypeRegistrator<RequestVote> _reg3;
uavcan::DefaultDataTypeRegistrator<Allocation> _reg4;
uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GetNodeInfo> _reg5;
uavcan::DefaultDataTypeRegistrator<uavcan::protocol::NodeStatus> _reg6;
EventTracer tracer;
MemoryStorageBackend storage;
// Node A is Allocator, Node B is Allocatee
InterlinkedTestNodesWithSysClock nodes(uavcan::NodeID(10), uavcan::NodeID::Broadcast);
UniqueID own_unique_id;
own_unique_id[0] = 0xAA;
own_unique_id[3] = 0xCC;
own_unique_id[7] = 0xEE;
own_unique_id[9] = 0xBB;
/*
* Server
*/
DistributedServer server(nodes.a, storage, tracer);
ASSERT_LE(0, server.init(own_unique_id, 1));
ASSERT_EQ(0, server.getNumAllocations());
/*
* Client
*/
uavcan::DynamicNodeIDClient client(nodes.b);
uavcan::protocol::HardwareVersion hwver;
for (uavcan::uint8_t i = 0; i < hwver.unique_id.size(); i++)
{
hwver.unique_id[i] = i;
}
const uavcan::NodeID PreferredNodeID = 42;
ASSERT_LE(0, client.start(hwver, PreferredNodeID));
/*
* Fire
*/
nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(15000));
ASSERT_TRUE(client.isAllocationComplete());
ASSERT_EQ(PreferredNodeID, client.getAllocatedNodeID());
ASSERT_EQ(2, server.getNumAllocations()); // Server's own node ID + client
}
TEST(dynamic_node_id_server, ObjectSizes)
{
using namespace uavcan;
using namespace uavcan::protocol::dynamic_node_id::server;
using namespace uavcan::dynamic_node_id_server;
std::cout << "distributed::Log: " << sizeof(distributed::Log) << std::endl;
std::cout << "distributed::PersistentState: " << sizeof(distributed::PersistentState) << std::endl;
std::cout << "distributed::ClusterManager: " << sizeof(distributed::ClusterManager) << std::endl;
std::cout << "distributed::RaftCore: " << sizeof(distributed::RaftCore) << std::endl;
std::cout << "distributed::Server: " << sizeof(distributed::Server) << std::endl;
std::cout << "AllocationRequestManager: " << sizeof(AllocationRequestManager) << std::endl;
std::cout << "ServiceServer<AppendEntries>: " << sizeof(ServiceServer<AppendEntries>) << std::endl;
std::cout << "ServiceClient<AppendEntries>: " << sizeof(ServiceClient<AppendEntries>) << std::endl;
std::cout << "ServiceServer<RequestVote>: " << sizeof(ServiceServer<RequestVote>) << std::endl;
std::cout << "ServiceClient<RequestVote,~,5>:"
<< sizeof(ServiceClient<RequestVote, void (*)(const ServiceCallResult<RequestVote>&), 5>) << std::endl;
}
libuavcan/test/protocol/dynamic_node_id_server/event.cpp
+45
View File
@@ -0,0 +1,45 @@
/*
* Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include <gtest/gtest.h>
#include <uavcan/protocol/dynamic_node_id_server/event.hpp>
#include "event_tracer.hpp"
TEST(dynamic_node_id_server_EventTracer, EventCodeToString)
{
using namespace uavcan::dynamic_node_id_server;
// Simply checking some error codes
ASSERT_STREQ("Error", IEventTracer::getEventName(TraceError));
ASSERT_STREQ("RaftAppendEntriesCallFailure", IEventTracer::getEventName(TraceRaftAppendEntriesCallFailure));
ASSERT_STREQ("RaftDiscoveryReceived", IEventTracer::getEventName(TraceRaftDiscoveryReceived));
ASSERT_STREQ("DiscoveryNodeRestartDetected", IEventTracer::getEventName(TraceDiscoveryNodeRestartDetected));
ASSERT_STREQ("AllocationUnexpectedStage", IEventTracer::getEventName(TraceAllocationUnexpectedStage));
}
TEST(dynamic_node_id_server_EventTracer, EnvironmentSelfTest)
{
using namespace uavcan::dynamic_node_id_server;
EventTracer tracer;
ASSERT_EQ(0, tracer.getNumEvents());
tracer.onEvent(TraceRaftAppendEntriesCallFailure, 123);
ASSERT_EQ(1, tracer.getNumEvents());
tracer.onEvent(TraceRaftAppendEntriesCallFailure, -456);
ASSERT_EQ(2, tracer.getNumEvents());
tracer.onEvent(TraceError, -0xFFFFFFFFFFFFFFFLL);
ASSERT_EQ(3, tracer.getNumEvents());
ASSERT_EQ(0, tracer.countEvents(TraceAllocationActivity));
ASSERT_EQ(2, tracer.countEvents(TraceRaftAppendEntriesCallFailure));
ASSERT_EQ(1, tracer.countEvents(TraceError));
ASSERT_EQ(-456, tracer.getLastEventArgumentOrFail(TraceRaftAppendEntriesCallFailure));
ASSERT_EQ(-0xFFFFFFFFFFFFFFFLL, tracer.getLastEventArgumentOrFail(TraceError));
}
libuavcan/test/protocol/dynamic_node_id_server/event_tracer.hpp
+67
View File
@@ -0,0 +1,67 @@
/*
* Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#pragma once
#include <iostream>
#include <string>
#include <list>
#include <uavcan/protocol/dynamic_node_id_server/event.hpp>
class EventTracer : public uavcan::dynamic_node_id_server::IEventTracer
{
struct EventLogEntry
{
const uavcan::dynamic_node_id_server::TraceCode code;
const uavcan::int64_t argument;
EventLogEntry(uavcan::dynamic_node_id_server::TraceCode arg_code, uavcan::int64_t arg_argument)
: code(arg_code)
, argument(arg_argument)
{ }
};
const std::string id_;
std::list<EventLogEntry> event_log_;
public:
EventTracer() { }
EventTracer(const std::string& id) : id_(id) { }
virtual void onEvent(uavcan::dynamic_node_id_server::TraceCode code, uavcan::int64_t argument)
{
std::cout << "EVENT [" << id_ << "]\t" << code << "\t" << getEventName(code) << "\t" << argument << std::endl;
event_log_.push_back(EventLogEntry(code, argument));
}
unsigned countEvents(const uavcan::dynamic_node_id_server::TraceCode code) const
{
unsigned count = 0;
for (std::list<EventLogEntry>::const_iterator it = event_log_.begin(); it != event_log_.end(); ++it)
{
count += (it->code == code) ? 1U : 0U;
}
return count;
}
uavcan::int64_t getLastEventArgumentOrFail(const uavcan::dynamic_node_id_server::TraceCode code) const
{
for (std::list<EventLogEntry>::const_reverse_iterator it = event_log_.rbegin(); it != event_log_.rend(); ++it)
{
if (it->code == code)
{
return it->argument;
}
}
std::cout << "No such event in the event log, code " << code << ", log length " << event_log_.size()
<< std::endl;
throw std::runtime_error("EventTracer::getLastEventArgumentOrFail()");
}
unsigned getNumEvents() const { return static_cast<unsigned>(event_log_.size()); }
};

Some files were not shown because too many files have changed in this diff Show More