diff --git a/CMakeLists.txt b/CMakeLists.txt index b889336f0d..6636e188d8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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 () diff --git a/dsdl/uavcan/equipment/CoarseOrientation.uavcan b/dsdl/uavcan/equipment/CoarseOrientation.uavcan index 0aec54d582..34f6145483 100644 --- a/dsdl/uavcan/equipment/CoarseOrientation.uavcan +++ b/dsdl/uavcan/equipment/CoarseOrientation.uavcan @@ -17,4 +17,4 @@ float32 ANGLE_MULTIPLIER = 4.7746482927568605 int5[3] fixed_axis_roll_pitch_yaw -bool defined # False if the orientation is actually not defined +bool orientation_defined # False if the orientation is actually not defined diff --git a/dsdl/uavcan/protocol/debug/1789.KeyValue.uavcan b/dsdl/uavcan/protocol/debug/1789.KeyValue.uavcan index 5566c0b83a..369bb1cec7 100644 --- a/dsdl/uavcan/protocol/debug/1789.KeyValue.uavcan +++ b/dsdl/uavcan/protocol/debug/1789.KeyValue.uavcan @@ -2,7 +2,13 @@ # Generic named parameter (key/value pair). # -bool alignment -uint8[<=100] key +# +# Integers are exactly representable in the range (-2^24, 2^24) which is (-16'777'216, 16'777'216). +# +float32 value -float64[<=1] value +# +# Tail array optimization is enabled, so if key length does not exceed 4 characters, +# the whole message can fit into one CAN frame. +# +uint8[<=34] key diff --git a/dsdl/uavcan/protocol/dynamic_node_id/server/220.AppendEntries.uavcan b/dsdl/uavcan/protocol/dynamic_node_id/server/220.AppendEntries.uavcan index f650e91306..30a966eb78 100644 --- a/dsdl/uavcan/protocol/dynamic_node_id/server/220.AppendEntries.uavcan +++ b/dsdl/uavcan/protocol/dynamic_node_id/server/220.AppendEntries.uavcan @@ -3,16 +3,29 @@ # Please refer to the specification for details. # -uint16 DEFAULT_REQUEST_TIMEOUT_MS = 100 - -uint16 DEFAULT_BASE_ELECTION_TIMEOUT_MS = 1000 # request timeout * max cluster size * 2 requests +# +# 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 -Entry[<=5] entries +# +# 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 --- diff --git a/dsdl/uavcan/protocol/file/218.Read.uavcan b/dsdl/uavcan/protocol/file/218.Read.uavcan index c7a54724fa..b120a86301 100644 --- a/dsdl/uavcan/protocol/file/218.Read.uavcan +++ b/dsdl/uavcan/protocol/file/218.Read.uavcan @@ -1,11 +1,15 @@ # -# Read contents of the file from remote node. -# Empty data in response means that the offset is out of file boundaries. -# Non-empty data means the end of file is not reached yet, even if the length is less than maximum. +# Read file from a remote node. +# +# There are two possible outcomes of a successful service call: +# 1. Data array size equals its capacity. This means that the end of the file is not reached yet. +# 2. Data array size is less than its capacity, possibly zero. This means that the end of file is reached. +# # Thus, if the client needs to fetch the entire file, it should repeatedly call this service while increasing the -# offset, until the empty data is returned. +# offset, until incomplete data is returned. +# # If the object pointed by 'path' cannot be read (e.g. it is a directory or it does not exist), appropriate error code -# will be returned. +# will be returned, and data array will be empty. # uint32 offset @@ -14,4 +18,4 @@ Path path --- Error error -uint8[<=250] data +uint8[<=256] data diff --git a/dsdl/uavcan/protocol/file/Error.uavcan b/dsdl/uavcan/protocol/file/Error.uavcan index 448c0ce361..e44e2da39c 100644 --- a/dsdl/uavcan/protocol/file/Error.uavcan +++ b/dsdl/uavcan/protocol/file/Error.uavcan @@ -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 diff --git a/libuavcan/dsdl_compiler/libuavcan_dsdl_compiler/__init__.py b/libuavcan/dsdl_compiler/libuavcan_dsdl_compiler/__init__.py index 0de8fea8a9..b92627fd07 100644 --- a/libuavcan/dsdl_compiler/libuavcan_dsdl_compiler/__init__.py +++ b/libuavcan/dsdl_compiler/libuavcan_dsdl_compiler/__init__.py @@ -260,7 +260,7 @@ def make_template_expander(filename): template_text = re.sub(r'([^\$]{0,1})\$\{([^\}]+)\}', r'\1$!\2!$', template_text) # Flow control expression transformation: % foo: ==> - template_text = re.sub(r'(?m)^(\ *)\%\ *([^\:]+?):{0,1}$', r'\1', template_text) + template_text = re.sub(r'(?m)^(\ *)\%\ *(.+?):{0,1}$', r'\1', template_text) # Block termination transformation: ==> template_text = re.sub(r'\<\!--\(end[a-z]+\)--\>', r'', template_text) diff --git a/libuavcan/dsdl_compiler/libuavcan_dsdl_compiler/data_type_template.tmpl b/libuavcan/dsdl_compiler/libuavcan_dsdl_compiler/data_type_template.tmpl index 8fd4d2f726..305d715dc5 100644 --- a/libuavcan/dsdl_compiler/libuavcan_dsdl_compiler/data_type_template.tmpl +++ b/libuavcan/dsdl_compiler/libuavcan_dsdl_compiler/data_type_template.tmpl @@ -311,7 +311,7 @@ const ::uavcan::DefaultDataTypeRegistrator< ${t.cpp_full_type_name} > _uavcan_gd // No default registration % endif -% for nsc in t.cpp_namespace_components: +% for nsc in t.cpp_namespace_components[::-1]: } // Namespace ${nsc} % endfor @@ -387,8 +387,8 @@ ${define_streaming_operator(type_name=t.cpp_full_type_name + '::Response')} ${define_streaming_operator(type_name=t.cpp_full_type_name)} % endif -% for nsc in t.cpp_namespace_components: -} +% for nsc in t.cpp_namespace_components[::-1]: +} // Namespace ${nsc} % endfor #endif // ${t.include_guard} diff --git a/libuavcan/include/uavcan/marshal/array.hpp b/libuavcan/include/uavcan/marshal/array.hpp index 73277f4c1b..c7263d106e 100644 --- a/libuavcan/include/uavcan/marshal/array.hpp +++ b/libuavcan/include/uavcan/marshal/array.hpp @@ -1087,6 +1087,19 @@ inline bool operator!=(const R& rhs, const Array& 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 +class MakeString +{ + MakeString(); // This class is not instantiatable. +public: + typedef Array, ArrayModeDynamic, MaxSize> Type; +}; + /** * YAML streamer specification for any Array<> */ diff --git a/libuavcan/include/uavcan/node/abstract_node.hpp b/libuavcan/include/uavcan/node/abstract_node.hpp index cebdcec7fc..7fddbe32b6 100644 --- a/libuavcan/include/uavcan/node/abstract_node.hpp +++ b/libuavcan/include/uavcan/node/abstract_node.hpp @@ -76,6 +76,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(); + } }; } diff --git a/libuavcan/include/uavcan/node/generic_subscriber.hpp b/libuavcan/include/uavcan/node/generic_subscriber.hpp index e3d51c9440..235f5518df 100644 --- a/libuavcan/include/uavcan/node/generic_subscriber.hpp +++ b/libuavcan/include/uavcan/node/generic_subscriber.hpp @@ -189,16 +189,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,6 +222,7 @@ protected: */ void stop() { + UAVCAN_TRACE("GenericSubscriber", "Stop; dtname=%s", DataSpec::getDataTypeFullName()); GenericSubscriberBase::stop(forwarder_); } diff --git a/libuavcan/include/uavcan/node/node.hpp b/libuavcan/include/uavcan/node/node.hpp index 930ecf419d..65b478b7a7 100644 --- a/libuavcan/include/uavcan/node/node.hpp +++ b/libuavcan/include/uavcan/node/node.hpp @@ -85,11 +85,13 @@ class UAVCAN_EXPORT Node : public INode 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; @@ -111,6 +113,7 @@ public: , proto_rrs_(*this) , proto_tsp_(*this) #endif + , internal_failure_cnt_(0) , started_(false) { } @@ -139,6 +142,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. diff --git a/libuavcan/include/uavcan/node/scheduler.hpp b/libuavcan/include/uavcan/node/scheduler.hpp index 847f76b9d9..01e58eb877 100644 --- a/libuavcan/include/uavcan/node/scheduler.hpp +++ b/libuavcan/include/uavcan/node/scheduler.hpp @@ -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_; } diff --git a/libuavcan/include/uavcan/node/service_client.hpp b/libuavcan/include/uavcan/node/service_client.hpp index 067d3b890c..b8467c4694 100644 --- a/libuavcan/include/uavcan/node/service_client.hpp +++ b/libuavcan/include/uavcan/node/service_client.hpp @@ -6,6 +6,7 @@ #define UAVCAN_NODE_SERVICE_CLIENT_HPP_INCLUDED #include +#include #include #include @@ -20,12 +21,39 @@ namespace uavcan { -template +template class UAVCAN_EXPORT ServiceResponseTransferListenerInstantiationHelper { public: // so much templating it hurts typedef typename TransferListenerInstantiationHelper::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(); } }; /** @@ -33,29 +61,39 @@ public: // so much templating it hurts * Note that application ALWAYS gets this result, even when it times out or fails because of some other reason. */ template -struct UAVCAN_EXPORT ServiceCallResult +class UAVCAN_EXPORT ServiceCallResult { +public: typedef ReceivedDataStructure 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_; } + + const ResponseFieldType& getResponse() const { return response_; } + ResponseFieldType& getResponse() { return response_; } }; /** @@ -67,10 +105,11 @@ static Stream& operator<<(Stream& s, const ServiceCallResult& 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 +121,109 @@ static Stream& operator<<(Stream& s, const ServiceCallResult& 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(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. + * * @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 = UAVCAN_CPP11 - typename Callback_ = std::function&)> + typename Callback_ = std::function&)>, #else - typename Callback_ = void (*)(const ServiceCallResult&) + typename Callback_ = void (*)(const ServiceCallResult&), #endif + unsigned NumStaticCalls_ = 1 > class UAVCAN_EXPORT ServiceClient - : public GenericSubscriber::Type > + : public GenericSubscriber::Type> , public ServiceClientBase { public: @@ -149,16 +233,43 @@ public: typedef ServiceCallResult ServiceCallResultType; typedef Callback_ Callback; + enum { NumStaticCalls = NumStaticCalls_ }; + private: typedef ServiceClient SelfType; typedef GenericPublisher PublisherType; - typedef typename ServiceResponseTransferListenerInstantiationHelper::Type TransferListenerType; + typedef typename ServiceResponseTransferListenerInstantiationHelper::Type + TransferListenerType; typedef GenericSubscriber SubscriberType; + typedef Multiset 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()); + ServiceCallResultType result(ServiceCallResultType::ErrorTimeout, state.getCallID(), + owner.getReceivedStructStorage()); // Mutable! + owner.invokeCallback(result); + } + } + }; + + CallRegistry call_registry_; + PublisherType publisher_; Callback callback_; - bool isCallbackValid() const { return try_implicit_cast(callback_, true); } + virtual bool shouldAcceptFrame(const RxFrame& frame) const; // Called from the transfer listener void invokeCallback(ServiceCallResultType& result); @@ -166,6 +277,8 @@ private: virtual void handleDeadline(MonotonicTime); + int addCallState(ServiceCallID call_id); + public: /** * @param node Node instance this client will be registered with. @@ -174,6 +287,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 +297,7 @@ public: #endif } - virtual ~ServiceClient() { cancel(); } + virtual ~ServiceClient() { cancelAllCalls(); } /** * Shall be called before first use. @@ -199,21 +313,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 +353,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 +373,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 +390,10 @@ public: // ---------------------------------------------------------------------------- -template -void ServiceClient::invokeCallback(ServiceCallResultType& result) +template +void ServiceClient::invokeCallback(ServiceCallResultType& result) { - if (isCallbackValid()) + if (try_implicit_cast(callback_, true)) { callback_(result); } @@ -259,118 +403,172 @@ void ServiceClient::invokeCallback(ServiceCallResultType& } } -template -void ServiceClient::handleReceivedDataStruct(ReceivedDataStructure& response) +template +bool ServiceClient::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 +void ServiceClient:: +handleReceivedDataStruct(ReceivedDataStructure& 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 +void ServiceClient::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(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 -void ServiceClient::handleDeadline(MonotonicTime) +template +int ServiceClient::addCallState(ServiceCallID call_id) { - const TransferListenerType* const listener = SubscriberType::getTransferListener(); - if (listener) + if (call_registry_.isEmpty()) { - const typename TransferListenerType::ExpectedResponseParams erp = listener->getExpectedResponseParams(); - ReceivedDataStructure& 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(SubscriberType::getNode(), + *this, call_id)) { - UAVCAN_ASSERT(0); - cancel(); + SubscriberType::stop(); + return -ErrMemory; } + + return 0; } -template -int ServiceClient::call(NodeID server_node_id, const RequestType& request) +template +int ServiceClient::call(NodeID server_node_id, const RequestType& request) { - cancel(); - if (!isCallbackValid()) + ServiceCallID dummy; + return call(server_node_id, request, dummy); +} + +template +int ServiceClient::call(NodeID server_node_id, const RequestType& request, + ServiceCallID& out_call_id) +{ + if (!try_implicit_cast(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 -void ServiceClient::cancel() +template +void ServiceClient::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 +void ServiceClient::cancelAllCalls() +{ + call_registry_.clear(); + SubscriberType::stop(); +} + +template +bool ServiceClient::hasPendingCallToServer(NodeID server_node_id) const +{ + return NULL != call_registry_.find(ServerSearchPredicate(server_node_id)); +} + +template +ServiceCallID ServiceClient::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 diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_client.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_client.hpp index 00d654d558..ad5238ffc8 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_client.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_client.hpp @@ -26,7 +26,7 @@ namespace uavcan * * Once dynamic allocation is complete (or not needed anymore), the object can be deleted. */ -class DynamicNodeIDClient : private TimerBase +class UAVCAN_EXPORT DynamicNodeIDClient : private TimerBase { typedef MethodBinder& msg) = 0; - virtual ~IAllocationRequestHandler() { } }; @@ -132,7 +127,6 @@ class AllocationRequestManager void handleAllocation(const ReceivedDataStructure& msg) { trace(TraceAllocationActivity, msg.getSrcNodeID().get()); - handler_.handleAllocationActivityDetection(msg); if (!msg.isAnonymousTransfer()) { @@ -251,6 +245,7 @@ public: 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) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized.hpp new file mode 100644 index 0000000000..e0f9e5bcae --- /dev/null +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized.hpp @@ -0,0 +1,20 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_HPP_INCLUDED + +#include + +namespace uavcan +{ +namespace dynamic_node_id_server +{ + +typedef centralized::Server CentralizedServer; + +} +} + +#endif // UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_HPP_INCLUDED diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/server.hpp new file mode 100644 index 0000000000..de9b99ac61 --- /dev/null +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/server.hpp @@ -0,0 +1,208 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_SERVER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_SERVER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include + +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(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 diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/storage.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/storage.hpp new file mode 100644 index 0000000000..c03957d14f --- /dev/null +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/storage.hpp @@ -0,0 +1,253 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_STORAGE_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_STORAGE_HPP_INCLUDED + +#include +#include +#include +#include + +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(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(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 diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp index 50a5c5cefb..0fa18fc17d 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp @@ -68,8 +68,6 @@ private: uint8_t cluster_size_; uint8_t num_known_servers_; - bool had_discovery_activity_; - static IStorageBackend::String getStorageKeyForClusterSize() { return "cluster_size"; } INode& getNode() { return discovery_sub_.getNode(); } @@ -89,22 +87,6 @@ private: return NULL; } - void addServer(NodeID node_id) - { - UAVCAN_ASSERT((num_known_servers_ + 1) < (MaxClusterSize - 2)); - 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(num_known_servers_ + 1U); - } - else - { - UAVCAN_ASSERT(0); - } - } - virtual void handleTimerEvent(const TimerEvent&) { UAVCAN_ASSERT(num_known_servers_ < cluster_size_); @@ -167,8 +149,6 @@ private: return; } - had_discovery_activity_ = true; - /* * Updating the set of known servers */ @@ -220,7 +200,6 @@ public: , discovery_pub_(node) , cluster_size_(0) , num_known_servers_(0) - , had_discovery_activity_(false) { } /** @@ -301,6 +280,25 @@ public: 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(num_known_servers_ + 1U); + } + else + { + UAVCAN_ASSERT(0); + } + } + /** * Whether such server has been discovered. */ @@ -414,19 +412,6 @@ public: } } - /** - * This method returns true if there was at least one Discovery message received since last call. - */ - bool hadDiscoveryActivity() - { - if (had_discovery_activity_) - { - had_discovery_activity_ = false; - return true; - } - return false; - } - /** * Number of known servers can only grow, and it never exceeds the cluster size value. * This number does not include the local server. diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/log.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/log.hpp index d829d293e0..20f957b3e7 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/log.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/log.hpp @@ -149,14 +149,6 @@ public: , last_index_(0) { } - /** - * 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() { StorageMarshaller io(storage_); diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/persistent_state.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/persistent_state.hpp index 91869679c1..0152b1851e 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/persistent_state.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/persistent_state.hpp @@ -42,6 +42,14 @@ public: , 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() { /* diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp index 17887557a3..f00fd1ec2f 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp @@ -5,6 +5,7 @@ #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 #include #include #include @@ -51,29 +52,26 @@ public: * 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 - * - * Active state switch logic: - * Activation (this is default state): - * - vote request - * - allocation activity detected - * - only if leader: - * - discovery activity detected - * - log is not fully replicated (this includes the case when the cluster is not fully discovered) - * - * Deactivation: - * - switch to follower state - * - persistent state update error - * - only if leader: - * - all log entries are fully replicated */ class RaftCore : private TimerBase { +public: + enum ServerState + { + ServerStateFollower, + ServerStateCandidate, + ServerStateLeader + }; + +private: typedef MethodBinder&, ServiceResponseDataStructure&)> AppendEntriesCallback; @@ -88,13 +86,6 @@ class RaftCore : private TimerBase typedef MethodBinder&)> RequestVoteResponseCallback; - enum ServerState - { - ServerStateFollower, - ServerStateCandidate, - ServerStateLeader - }; - struct PendingAppendEntriesFields { Log::Index prev_log_index; @@ -109,8 +100,7 @@ class RaftCore : private TimerBase /* * Constants */ - const MonotonicDuration update_interval_; ///< AE requests will be issued at this rate - const MonotonicDuration base_activity_timeout_; + enum { MaxNumFollowers = ClusterManager::MaxClusterSize - 1 }; IEventTracer& tracer_; IRaftLeaderMonitor& leader_monitor_; @@ -123,7 +113,7 @@ class RaftCore : private TimerBase Log::Index commit_index_; MonotonicTime last_activity_timestamp_; - bool active_mode_; + MonotonicDuration randomized_activity_timeout_; ServerState server_state_; uint8_t next_server_index_; ///< Next server to query AE from @@ -137,10 +127,7 @@ class RaftCore : private TimerBase ServiceServer append_entries_srv_; ServiceClient append_entries_client_; ServiceServer request_vote_srv_; - - enum { NumRequestVoteClients = ClusterManager::MaxClusterSize - 1 }; - LazyConstructor > - request_vote_clients_[NumRequestVoteClients]; + ServiceClient request_vote_client_; /* * Methods @@ -150,16 +137,49 @@ class RaftCore : private TimerBase INode& getNode() { return append_entries_srv_.getNode(); } const INode& getNode() const { return append_entries_srv_.getNode(); } - void registerActivity() { last_activity_timestamp_ = getNode().getMonotonicTime(); } + 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 { - const int multiplier = static_cast(getNode().getNodeID().get()) - 1; - - const MonotonicDuration activity_timeout = - MonotonicDuration::fromUSec(base_activity_timeout_.toUSec() + update_interval_.toUSec() * multiplier); - - return getNode().getMonotonicTime() > (last_activity_timestamp_ + activity_timeout); + return getNode().getMonotonicTime() > (last_activity_timestamp_ + randomized_activity_timeout_); } void handlePersistentStateUpdateError(int error) @@ -167,13 +187,12 @@ class RaftCore : private TimerBase UAVCAN_ASSERT(error < 0); trace(TraceRaftPersistStateUpdateError, error); switchState(ServerStateFollower); - setActiveMode(false); // Goodnight sweet prince registerActivity(); // Deferring reelections } void updateFollower() { - if (active_mode_ && isActivityTimedOut()) + if (isActivityTimedOut()) { switchState(ServerStateCandidate); registerActivity(); @@ -182,8 +201,6 @@ class RaftCore : private TimerBase void updateCandidate() { - UAVCAN_ASSERT(active_mode_); - if (num_votes_received_in_this_campaign_ > 0) { trace(TraceRaftElectionComplete, num_votes_received_in_this_campaign_); @@ -218,7 +235,7 @@ class RaftCore : private TimerBase req.last_log_term = persistent_state_.getLog().getEntryAtIndex(req.last_log_index)->term; req.term = persistent_state_.getCurrentTerm(); - for (uint8_t i = 0; i < NumRequestVoteClients; i++) + for (uint8_t i = 0; i < MaxNumFollowers; i++) { const NodeID node_id = cluster_.getRemoteServerNodeIDAtIndex(i); if (!node_id.isUnicast()) @@ -230,7 +247,7 @@ class RaftCore : private TimerBase "Requesting vote from %d", int(node_id.get())); trace(TraceRaftVoteRequestInitiation, node_id.get()); - res = request_vote_clients_[i]->call(node_id, req); + res = request_vote_client_.call(node_id, req); if (res < 0) { trace(TraceError, res); @@ -241,12 +258,12 @@ class RaftCore : private TimerBase void updateLeader() { - if (cluster_.getClusterSize() == 1) + if (append_entries_client_.hasPendingCalls()) { - setActiveMode(false); // Haha + append_entries_client_.cancelAllCalls(); // Refer to the response callback to learn why } - if (active_mode_ || (next_server_index_ > 0)) + if (cluster_.getClusterSize() > 1) { const NodeID node_id = cluster_.getRemoteServerNodeIDAtIndex(next_server_index_); UAVCAN_ASSERT(node_id.isUnicast()); @@ -325,11 +342,8 @@ class RaftCore : private TimerBase next_server_index_ = 0; num_votes_received_in_this_campaign_ = 0; - for (uint8_t i = 0; i < NumRequestVoteClients; i++) - { - request_vote_clients_[i]->cancel(); - } - append_entries_client_.cancel(); + request_vote_client_.cancelAllCalls(); + append_entries_client_.cancelAllCalls(); /* * Calling the switch handler @@ -342,18 +356,6 @@ class RaftCore : private TimerBase } } - void setActiveMode(bool new_active) - { - if (active_mode_ != new_active) - { - UAVCAN_TRACE("dynamic_node_id_server::distributed::RaftCore", "Active switch: %d --> %d", - int(active_mode_), int(new_active)); - trace(TraceRaftActiveSwitch, new_active); - - active_mode_ = new_active; - } - } - void tryIncrementCurrentTermFromResponse(Term new_term) { trace(TraceRaftNewerTermInResponse, new_term); @@ -364,7 +366,6 @@ class RaftCore : private TimerBase } registerActivity(); // Deferring future elections switchState(ServerStateFollower); - setActiveMode(false); } void propagateCommitIndex() @@ -373,29 +374,12 @@ class RaftCore : private TimerBase UAVCAN_ASSERT(server_state_ == ServerStateLeader); UAVCAN_ASSERT(commit_index_ <= persistent_state_.getLog().getLastIndex()); - if (commit_index_ == persistent_state_.getLog().getLastIndex()) + if (commit_index_ < persistent_state_.getLog().getLastIndex()) { - // All local entries are committed - bool commit_index_fully_replicated = true; - - for (uint8_t i = 0; i < cluster_.getNumKnownServers(); i++) - { - const Log::Index match_index = cluster_.getServerMatchIndex(cluster_.getRemoteServerNodeIDAtIndex(i)); - if (match_index != commit_index_) - { - commit_index_fully_replicated = false; - break; - } - } - - const bool all_done = commit_index_fully_replicated && cluster_.isClusterDiscovered(); - setActiveMode(!all_done); // Enable passive mode if commit index is the same on all nodes - } - else - { - // Not all local entries are committed - setActiveMode(true); - + /* + * 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++) { @@ -421,11 +405,20 @@ class RaftCore : private TimerBase void handleAppendEntriesRequest(const ReceivedDataStructure& request, ServiceResponseDataStructure& response) { + checkInvariants(); + if (!cluster_.isKnownServer(request.getSrcNodeID())) { - trace(TraceRaftRequestIgnored, request.getSrcNodeID().get()); - response.setResponseEnabled(false); - return; + if (cluster_.isClusterDiscovered()) + { + trace(TraceRaftRequestIgnored, request.getSrcNodeID().get()); + response.setResponseEnabled(false); + return; + } + else + { + cluster_.addServer(request.getSrcNodeID()); + } } UAVCAN_ASSERT(response.isResponseEnabled()); // This is default @@ -471,7 +464,6 @@ class RaftCore : private TimerBase registerActivity(); switchState(ServerStateFollower); - setActiveMode(false); /* * Step 2 @@ -544,28 +536,31 @@ class RaftCore : private TimerBase void handleAppendEntriesResponse(const ServiceCallResult& result) { UAVCAN_ASSERT(server_state_ == ServerStateLeader); // When state switches, all requests must be cancelled + checkInvariants(); if (!result.isSuccessful()) { return; } - if (result.response.term > persistent_state_.getCurrentTerm()) + if (result.getResponse().term > persistent_state_.getCurrentTerm()) { - tryIncrementCurrentTermFromResponse(result.response.term); + tryIncrementCurrentTermFromResponse(result.getResponse().term); } else { - if (result.response.success) + if (result.getResponse().success) { - cluster_.incrementServerNextIndexBy(result.server_node_id, pending_append_entries_fields_.num_entries); - cluster_.setServerMatchIndex(result.server_node_id, + 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.server_node_id); + cluster_.decrementServerNextIndex(result.getCallID().server_node_id); + trace(TraceRaftAppendEntriesRespUnsucfl, result.getCallID().server_node_id.get()); } } @@ -576,6 +571,7 @@ class RaftCore : private TimerBase void handleRequestVoteRequest(const ReceivedDataStructure& request, ServiceResponseDataStructure& response) { + checkInvariants(); trace(TraceRaftVoteRequestReceived, request.getSrcNodeID().get()); if (!cluster_.isKnownServer(request.getSrcNodeID())) @@ -587,14 +583,14 @@ class RaftCore : private TimerBase UAVCAN_ASSERT(response.isResponseEnabled()); // This is default - setActiveMode(true); - /* * 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) { @@ -632,6 +628,7 @@ class RaftCore : private TimerBase 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()); @@ -648,21 +645,22 @@ class RaftCore : private TimerBase void handleRequestVoteResponse(const ServiceCallResult& result) { UAVCAN_ASSERT(server_state_ == ServerStateCandidate); // When state switches, all requests must be cancelled + checkInvariants(); if (!result.isSuccessful()) { return; } - trace(TraceRaftVoteRequestSucceeded, result.server_node_id.get()); + trace(TraceRaftVoteRequestSucceeded, result.getCallID().server_node_id.get()); - if (result.response.term > persistent_state_.getCurrentTerm()) + if (result.getResponse().term > persistent_state_.getCurrentTerm()) { - tryIncrementCurrentTermFromResponse(result.response.term); + tryIncrementCurrentTermFromResponse(result.getResponse().term); } else { - if (result.response.vote_granted) + if (result.getResponse().vote_granted) { num_votes_received_in_this_campaign_++; } @@ -673,10 +671,7 @@ class RaftCore : private TimerBase virtual void handleTimerEvent(const TimerEvent&) { - if (cluster_.hadDiscoveryActivity() && isLeader()) - { - setActiveMode(true); - } + checkInvariants(); switch (server_state_) { @@ -707,33 +702,24 @@ public: RaftCore(INode& node, IStorageBackend& storage, IEventTracer& tracer, - IRaftLeaderMonitor& leader_monitor, - MonotonicDuration update_interval = - MonotonicDuration::fromMSec(AppendEntries::Request::DEFAULT_REQUEST_TIMEOUT_MS), - MonotonicDuration base_activity_timeout = - MonotonicDuration::fromMSec(AppendEntries::Request::DEFAULT_BASE_ELECTION_TIMEOUT_MS)) + IRaftLeaderMonitor& leader_monitor) : TimerBase(node) - , update_interval_(update_interval) - , base_activity_timeout_(base_activity_timeout) , 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()) - , active_mode_(true) + , 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) - { - for (uint8_t i = 0; i < NumRequestVoteClients; i++) - { - request_vote_clients_[i].construct(node); - } - } + , request_vote_client_(node) + { } /** * Once started, the logic runs in the background until destructor is called. @@ -746,13 +732,13 @@ public: /* * Initializing state variables */ - last_activity_timestamp_ = getNode().getMonotonicTime(); - active_mode_ = true; server_state_ = ServerStateFollower; next_server_index_ = 0; num_votes_received_in_this_campaign_ = 0; commit_index_ = 0; + registerActivity(); + /* * Initializing internals */ @@ -787,36 +773,41 @@ public: } append_entries_client_.setCallback(AppendEntriesResponseCallback(this, &RaftCore::handleAppendEntriesResponse)); - append_entries_client_.setRequestTimeout(update_interval_); - for (uint8_t i = 0; i < NumRequestVoteClients; i++) + res = request_vote_client_.init(); + if (res < 0) { - res = request_vote_clients_[i]->init(); - if (res < 0) - { - return res; - } - request_vote_clients_[i]->setCallback(RequestVoteResponseCallback(this, - &RaftCore::handleRequestVoteResponse)); - request_vote_clients_[i]->setRequestTimeout(update_interval_); + return res; } + request_vote_client_.setCallback(RequestVoteResponseCallback(this, &RaftCore::handleRequestVoteResponse)); - startPeriodic(update_interval_); + /* + * Initializing timing constants + * Refer to the specification for the formula + */ + const uint8_t num_followers = static_cast(cluster_.getClusterSize() - 1); - trace(TraceRaftCoreInited, update_interval_.toUSec()); + const MonotonicDuration update_interval = + MonotonicDuration::fromMSec(AppendEntries::Request::DEFAULT_MIN_ELECTION_TIMEOUT_MS / + 2 / max(static_cast(2), num_followers)); + + UAVCAN_TRACE("dynamic_node_id_server::distributed::RaftCore", + "Update interval: %ld msec", static_cast(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; } - /** - * Normally should be called when there's allocation activity on the bus. - */ - void forceActiveMode() - { - setActiveMode(true); // If the current state was Follower, active mode may be toggling quickly for some time - } - /** * This function is mostly needed for testing. */ @@ -906,6 +897,16 @@ public: // 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_; } }; } diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp index 2c20fcb11d..a8affdb8a2 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp @@ -7,7 +7,6 @@ #include #include -#include #include #include #include @@ -24,9 +23,9 @@ namespace distributed /** * This class implements the top-level allocation logic and server API. */ -class Server : IAllocationRequestHandler - , INodeDiscoveryHandler - , IRaftLeaderMonitor +class UAVCAN_EXPORT Server : IAllocationRequestHandler + , INodeDiscoveryHandler + , IRaftLeaderMonitor { struct UniqueIDLogPredicate { @@ -65,6 +64,7 @@ class Server : IAllocationRequestHandler * States */ INode& node_; + IEventTracer& tracer_; RaftCore raft_core_; AllocationRequestManager allocation_request_manager_; NodeDiscoverer node_discoverer_; @@ -135,11 +135,6 @@ class Server : IAllocationRequestHandler } } - virtual void handleAllocationActivityDetection(const ReceivedDataStructure&) - { - raft_core_.forceActiveMode(); - } - /* * Methods of INodeDiscoveryHandler */ @@ -235,7 +230,8 @@ class Server : IAllocationRequestHandler const int res = allocation_request_manager_.broadcastAllocationResponse(entry.unique_id, entry.node_id); if (res < 0) { - node_.registerInternalFailure("Dynamic allocation final broadcast"); + tracer_.onEvent(TraceError, res); + node_.registerInternalFailure("Dynamic allocation response"); } } @@ -244,6 +240,7 @@ public: 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) @@ -295,6 +292,80 @@ public: } 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); + } + } + } }; } diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp index eb775ecd7b..d447e01ac6 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp @@ -16,7 +16,7 @@ namespace dynamic_node_id_server * @ref IEventTracer. * Event codes cannot be changed, only new ones can be added. */ -enum TraceCode +enum UAVCAN_EXPORT TraceCode { // Event name Argument // 0 @@ -38,7 +38,7 @@ enum TraceCode TraceRaftCoreInited, // update interval in usec TraceRaftStateSwitch, // 0 - Follower, 1 - Candidate, 2 - Leader // 15 - TraceRaftActiveSwitch, // 0 - Passive, 1 - Active + Trace0, TraceRaftNewLogEntry, // node ID value TraceRaftRequestIgnored, // node ID of the client TraceRaftVoteRequestReceived, // node ID of the client @@ -52,7 +52,7 @@ enum TraceCode // 25 TraceRaftAppendEntriesCallFailure, // error code (may be negated) TraceRaftElectionComplete, // number of votes collected - Trace1, + TraceRaftAppendEntriesRespUnsucfl, // node ID of the client Trace2, Trace3, // 30 @@ -87,7 +87,7 @@ enum TraceCode /** * This interface allows the application to trace events that happen in the server. */ -class IEventTracer +class UAVCAN_EXPORT IEventTracer { public: #if UAVCAN_TOSTRING @@ -115,7 +115,7 @@ public: "RaftBadClusterSizeReceived", "RaftCoreInited", "RaftStateSwitch", - "RaftActiveSwitch", + "", "RaftNewLogEntry", "RaftRequestIgnored", "RaftVoteRequestReceived", @@ -127,7 +127,7 @@ public: "RaftNewEntryCommitted", "RaftAppendEntriesCallFailure", "RaftElectionComplete", - "", + "RaftAppendEntriesRespUnsucfl", "", "", "AllocationFollowupResponse", diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp index 0f96708cd5..e5e4da5e1f 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp @@ -92,6 +92,8 @@ class NodeDiscoverer : TimerBase */ enum { MaxAttemptsToGetNodeInfo = 5 }; + enum { TimerPollIntervalMs = 170 }; // ~ ceil(500 ms service timeout / 3) + enum { NumNodeStatusStaticReceivers = 64 }; /* @@ -104,7 +106,7 @@ class NodeDiscoverer : TimerBase NodeMap node_map_; ///< Will not work in UAVCAN_TINY ServiceClient get_node_info_client_; - Subscriber node_status_sub_; + Subscriber node_status_sub_; /* * Methods @@ -121,34 +123,9 @@ class NodeDiscoverer : TimerBase NodeID pickNextNodeToQuery() const { - const unsigned node_map_size = node_map_.getSize(); - - // Searching the lowest number of attempts made - uint8_t lowest_number_of_attempts = static_cast(MaxAttemptsToGetNodeInfo + 1U); - for (unsigned i = 0; i < node_map_size; i++) - { - const NodeMap::KVPair* const kv = node_map_.getByIndex(i); - UAVCAN_ASSERT(kv != NULL); - lowest_number_of_attempts = min(lowest_number_of_attempts, kv->value.num_get_node_info_attempts); - } - - // Now, among nodes with this number of attempts selecting the one with highest uptime. - NodeID output; - uint32_t largest_uptime = 0; - for (unsigned i = 0; i < node_map_size; i++) - { - const NodeMap::KVPair* const kv = node_map_.getByIndex(i); - UAVCAN_ASSERT(kv != NULL); - if ((kv->value.num_get_node_info_attempts == lowest_number_of_attempts) && - (kv->value.last_seen_uptime >= largest_uptime)) - { - largest_uptime = kv->value.last_seen_uptime; - output = kv->key; - } - } - - // An invalid node ID will be returned only if there's no nodes at all. - return output; + // 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) @@ -233,14 +210,14 @@ class NodeDiscoverer : TimerBase if (result.isSuccessful()) { UAVCAN_TRACE("dynamic_node_id_server::NodeDiscoverer", "GetNodeInfo response from %d", - int(result.server_node_id.get())); - finalizeNodeDiscovery(&result.response.hardware_version.unique_id, result.server_node_id); + int(result.getCallID().server_node_id.get())); + finalizeNodeDiscovery(&result.getResponse().hardware_version.unique_id, result.getCallID().server_node_id); } else { - trace(TraceDiscoveryGetNodeInfoFailure, result.server_node_id.get()); + trace(TraceDiscoveryGetNodeInfoFailure, result.getCallID().server_node_id.get()); - NodeData* const data = node_map_.access(result.server_node_id); + NodeData* const data = node_map_.access(result.getCallID().server_node_id); if (data == NULL) { return; // Probably it is a known node now @@ -248,11 +225,11 @@ class NodeDiscoverer : TimerBase UAVCAN_TRACE("dynamic_node_id_server::NodeDiscoverer", "GetNodeInfo request to %d has timed out, %d attempts", - int(result.server_node_id.get()), int(data->num_get_node_info_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.server_node_id); + finalizeNodeDiscovery(NULL, result.getCallID().server_node_id); // Warning: data pointer is invalidated now } } @@ -260,16 +237,11 @@ class NodeDiscoverer : TimerBase void handleTimerEvent(const TimerEvent&) { - if (get_node_info_client_.isPending()) + if (get_node_info_client_.hasPendingCalls()) { return; } - if (!handler_.canDiscoverNewNodes()) - { - return; // Timer must continue to run in order to not stuck when it unlocks - } - const NodeID node_id = pickNextNodeToQueryAndCleanupMap(); if (!node_id.isUnicast()) { @@ -278,6 +250,11 @@ class NodeDiscoverer : TimerBase 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", @@ -319,7 +296,7 @@ class NodeDiscoverer : TimerBase if (!isRunning()) { - startPeriodic(get_node_info_client_.getRequestTimeout() * 2); + startPeriodic(MonotonicDuration::fromMSec(TimerPollIntervalMs)); trace(TraceDiscoveryTimerStart, getPeriod().toUSec()); } } @@ -359,6 +336,12 @@ public: * 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(node_map_.getSize()); } }; } diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_id_selector.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_id_selector.hpp index 97c86ab21b..bd0b55c0f9 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_id_selector.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_id_selector.hpp @@ -50,7 +50,6 @@ public: } candidate = preferred.isUnicast() ? preferred.get() : NodeID::MaxRecommendedForRegularNodes; - candidate--; // This has been tested already // Down while (candidate > 0) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/storage_backend.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/storage_backend.hpp index efc3459a68..9e2395c92a 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/storage_backend.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/storage_backend.hpp @@ -19,7 +19,7 @@ namespace dynamic_node_id_server * 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 IStorageBackend +class UAVCAN_EXPORT IStorageBackend { public: /** @@ -37,7 +37,7 @@ public: * 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 Array, ArrayModeDynamic, MaxStringLength> String; + typedef MakeString::Type String; /** * Read one value from the storage. diff --git a/libuavcan/include/uavcan/protocol/file_server.hpp b/libuavcan/include/uavcan/protocol/file_server.hpp new file mode 100644 index 0000000000..1e5fed04a2 --- /dev/null +++ b/libuavcan/include/uavcan/protocol/file_server.hpp @@ -0,0 +1,273 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_FILE_SERVER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_FILE_SERVER_HPP_INCLUDED + +#include +#include +#include +#include +// UAVCAN types +#include +#include +#include +#include +#include + +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; + + /** + * Use this class to compute CRC64 for uavcan.protocol.file.GetInfo. + */ + typedef DataTypeSignatureCRC FileCRC; + + /** + * 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(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_crc64, uint32_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 uint32_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 uint32_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 + GetInfoCallback; + + typedef MethodBinder + ReadCallback; + + ServiceServer get_info_srv_; + ServiceServer read_srv_; + + void handleGetInfo(const protocol::file::GetInfo::Request& req, protocol::file::GetInfo::Response& resp) + { + resp.error.value = backend_.getInfo(req.path.path, resp.crc64, 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 + WriteCallback; + + typedef MethodBinder + DeleteCallback; + + typedef MethodBinder + GetDirectoryEntryInfoCallback; + + ServiceServer write_srv_; + ServiceServer delete_srv_; + ServiceServer 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 diff --git a/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp b/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp new file mode 100644 index 0000000000..788536c07c --- /dev/null +++ b/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp @@ -0,0 +1,476 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_FIRMWARE_UPDATE_TRIGGER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_FIRMWARE_UPDATE_TRIGGER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +// UAVCAN types +#include + +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::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&)> + 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 begin_fw_update_client_; + + IFirmwareVersionChecker& checker_; + + NodeInfoRetriever* node_info_retriever_; + + Map 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(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(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& 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 diff --git a/libuavcan/include/uavcan/protocol/network_compat_checker.hpp b/libuavcan/include/uavcan/protocol/network_compat_checker.hpp index ca6635451a..461085eb05 100644 --- a/libuavcan/include/uavcan/protocol/network_compat_checker.hpp +++ b/libuavcan/include/uavcan/protocol/network_compat_checker.hpp @@ -87,7 +87,7 @@ class UAVCAN_EXPORT NetworkCompatibilityChecker : Noncopyable int waitForCATSResponse() { - while (cats_cln_.isPending()) + while (cats_cln_.hasPendingCalls()) { const int res = getNode().spin(MonotonicDuration::fromMSec(10)); if (res < 0 || !result_.isOk()) @@ -119,15 +119,15 @@ class UAVCAN_EXPORT NetworkCompatibilityChecker : Noncopyable if (last_cats_request_ok_) { const DataTypeSignature sign = GlobalDataTypeRegistry::instance(). - computeAggregateSignature(checking_dtkind_, resp.response.mutually_known_ids); + computeAggregateSignature(checking_dtkind_, resp.getResponse().mutually_known_ids); UAVCAN_TRACE("NodeInitializer", "CATS response from nid=%i; local=%llu remote=%llu", - int(resp.server_node_id.get()), static_cast(sign.get()), - static_cast(resp.response.aggregate_signature)); + int(resp.getCallID().server_node_id.get()), static_cast(sign.get()), + static_cast(resp.getResponse().aggregate_signature)); - if (sign.get() != resp.response.aggregate_signature) + if (sign.get() != resp.getResponse().aggregate_signature) { - result_.conflicting_node = resp.server_node_id; + result_.conflicting_node = resp.getCallID().server_node_id; } } } @@ -138,7 +138,7 @@ class UAVCAN_EXPORT NetworkCompatibilityChecker : Noncopyable StaticAssert::check(); UAVCAN_ASSERT(nid.isUnicast()); - UAVCAN_ASSERT(!cats_cln_.isPending()); + UAVCAN_ASSERT(!cats_cln_.hasPendingCalls()); checking_dtkind_ = kind; protocol::ComputeAggregateTypeSignature::Request request; @@ -253,7 +253,7 @@ public: exit: ns_sub_.stop(); - cats_cln_.cancel(); + cats_cln_.cancelAllCalls(); return res; } diff --git a/libuavcan/include/uavcan/protocol/node_info_retriever.hpp b/libuavcan/include/uavcan/protocol/node_info_retriever.hpp new file mode 100644 index 0000000000..017102fa40 --- /dev/null +++ b/libuavcan/include/uavcan/protocol/node_info_retriever.hpp @@ -0,0 +1,450 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_NODE_INFO_RETRIEVER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_NODE_INFO_RETRIEVER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include + +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& 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&)> + 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::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 + 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 listeners_; + + ServiceClient 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(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(&INodeInfoListener::handleNodeStatusChange, event)); + } + + virtual void handleNodeStatusMessage(const ReceivedDataStructure& 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&>( + &INodeInfoListener::handleNodeStatusMessage, msg)); + } + + void handleGetNodeInfoResponse(const ServiceCallResult& 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(&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(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(num); + } +}; + +} + +#endif // Include guard diff --git a/libuavcan/include/uavcan/protocol/node_status_monitor.hpp b/libuavcan/include/uavcan/protocol/node_status_monitor.hpp index 60350ef6ff..06f7bb7605 100644 --- a/libuavcan/include/uavcan/protocol/node_status_monitor.hpp +++ b/libuavcan/include/uavcan/protocol/node_status_monitor.hpp @@ -19,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::Type NodeStatusCode; @@ -43,12 +43,14 @@ public: }; private: - enum { TimerPeriodMs100 = 5 }; + enum { TimerPeriodMs100 = 4 }; typedef MethodBinder&)> NodeStatusCallback; + typedef MethodBinder TimerCallback; + /* * We'll be able to handle this many nodes in the network without any dynamic memory. */ @@ -56,6 +58,8 @@ private: Subscriber sub_; + TimerEventForwarder timer_; + struct Entry { NodeStatusCode status_code; @@ -110,11 +114,7 @@ private: handleNodeStatusMessage(msg); } -protected: - /** - * This event will be invoked at 2 Hz rate. It can be used by derived classes as well. - */ - virtual void handleTimerEvent(const TimerEvent&) + void handleTimerEvent(const TimerEvent&) { const int OfflineTimeoutMs100 = protocol::NodeStatus::OFFLINE_TIMEOUT_MS / 100; @@ -126,7 +126,8 @@ protected: 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)); + 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; @@ -138,6 +139,7 @@ protected: } } +protected: /** * Called when a node becomes online, changes status or goes offline. * Refer to uavcan.protocol.NodeStatus for the offline timeout value. @@ -160,8 +162,8 @@ protected: public: explicit NodeStatusMonitor(INode& node) - : TimerBase(node) - , sub_(node) + : sub_(node) + , timer_(node) { } virtual ~NodeStatusMonitor() { } @@ -176,7 +178,8 @@ public: const int res = sub_.start(NodeStatusCallback(this, &NodeStatusMonitor::handleNodeStatus)); if (res >= 0) { - TimerBase::startPeriodic(MonotonicDuration::fromMSec(TimerPeriodMs100 * 100)); + timer_.setCallback(TimerCallback(this, &NodeStatusMonitor::handleTimerEvent)); + timer_.startPeriodic(MonotonicDuration::fromMSec(TimerPeriodMs100 * 100)); } return res; } diff --git a/libuavcan/include/uavcan/transport/dispatcher.hpp b/libuavcan/include/uavcan/transport/dispatcher.hpp index a671f4ac85..d179b32193 100644 --- a/libuavcan/include/uavcan/transport/dispatcher.hpp +++ b/libuavcan/include/uavcan/transport/dispatcher.hpp @@ -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 */ diff --git a/libuavcan/include/uavcan/transport/outgoing_transfer_registry.hpp b/libuavcan/include/uavcan/transport/outgoing_transfer_registry.hpp index 114cecee22..2dbc508118 100644 --- a/libuavcan/include/uavcan/transport/outgoing_transfer_registry.hpp +++ b/libuavcan/include/uavcan/transport/outgoing_transfer_registry.hpp @@ -165,13 +165,13 @@ TransferID* OutgoingTransferRegistry::accessOrCreate(const Out template bool OutgoingTransferRegistry::exists(DataTypeID dtid, TransferType tt) const { - return NULL != map_.findFirstKey(ExistenceCheckingPredicate(dtid, tt)); + return NULL != map_.find(ExistenceCheckingPredicate(dtid, tt)); } template void OutgoingTransferRegistry::cleanup(MonotonicTime ts) { - map_.removeWhere(DeadlineExpiredPredicate(ts)); + map_.removeAllWhere(DeadlineExpiredPredicate(ts)); } } diff --git a/libuavcan/include/uavcan/transport/transfer_listener.hpp b/libuavcan/include/uavcan/transport/transfer_listener.hpp index dc0bc66ad2..6b57c25e99 100644 --- a/libuavcan/include/uavcan/transport/transfer_listener.hpp +++ b/libuavcan/include/uavcan/transport/transfer_listener.hpp @@ -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 -class UAVCAN_EXPORT ServiceResponseTransferListener - : public TransferListener +class UAVCAN_EXPORT TransferListenerWithFilter : public TransferListener { + const ITransferAcceptanceFilter* filter_; + + virtual void handleFrame(const RxFrame& frame); + public: typedef TransferListener 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 -void ServiceResponseTransferListener::handleFrame(const RxFrame& frame) +void TransferListenerWithFilter::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 -void ServiceResponseTransferListener::setExpectedResponseParams( - const ExpectedResponseParams& erp) -{ - response_params_ = erp; -} - -template -void ServiceResponseTransferListener::stopAcceptingAnything() -{ - response_params_ = ExpectedResponseParams(); } } diff --git a/libuavcan/include/uavcan/util/lazy_constructor.hpp b/libuavcan/include/uavcan/util/lazy_constructor.hpp index 93152adc71..c7ae975663 100644 --- a/libuavcan/include/uavcan/util/lazy_constructor.hpp +++ b/libuavcan/include/uavcan/util/lazy_constructor.hpp @@ -26,12 +26,19 @@ namespace uavcan template 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 struct ParamType { typedef const U& Type; }; - template struct ParamType { typedef U& Type; }; -#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 - template struct ParamType { 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::Type p%d' % (x, x) for x in nums] +// l2 = ['typename ParameterType::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 - void construct(typename ParamType::Type p1) + void construct(typename ParameterType::Type p1) { ensureNotConstructed(); ptr_ = new (static_cast(data_.pool)) T(p1); } - template - void construct(typename ParamType::Type p1, typename ParamType::Type p2) + template + void construct(typename ParameterType::Type p1, typename ParameterType::Type p2) { ensureNotConstructed(); ptr_ = new (static_cast(data_.pool)) T(p1, p2); } - template - void construct(typename ParamType::Type p1, typename ParamType::Type p2, typename ParamType::Type p3) + template + void construct(typename ParameterType::Type p1, typename ParameterType::Type p2, + typename ParameterType::Type p3) { ensureNotConstructed(); ptr_ = new (static_cast(data_.pool)) T(p1, p2, p3); } - template - void construct(typename ParamType::Type p1, typename ParamType::Type p2, typename ParamType::Type p3, - typename ParamType::Type p4) + template + void construct(typename ParameterType::Type p1, typename ParameterType::Type p2, + typename ParameterType::Type p3, typename ParameterType::Type p4) { ensureNotConstructed(); ptr_ = new (static_cast(data_.pool)) T(p1, p2, p3, p4); } - template - void construct(typename ParamType::Type p1, typename ParamType::Type p2, typename ParamType::Type p3, - typename ParamType::Type p4, typename ParamType::Type p5) + template + void construct(typename ParameterType::Type p1, typename ParameterType::Type p2, + typename ParameterType::Type p3, typename ParameterType::Type p4, + typename ParameterType::Type p5) { ensureNotConstructed(); ptr_ = new (static_cast(data_.pool)) T(p1, p2, p3, p4, p5); } - template - void construct(typename ParamType::Type p1, typename ParamType::Type p2, typename ParamType::Type p3, - typename ParamType::Type p4, typename ParamType::Type p5, typename ParamType::Type p6) + template + void construct(typename ParameterType::Type p1, typename ParameterType::Type p2, + typename ParameterType::Type p3, typename ParameterType::Type p4, + typename ParameterType::Type p5, typename ParameterType::Type p6) { ensureNotConstructed(); ptr_ = new (static_cast(data_.pool)) T(p1, p2, p3, p4, p5, p6); diff --git a/libuavcan/include/uavcan/util/map.hpp b/libuavcan/include/uavcan/util/map.hpp index 814dd517a0..ece759af0e 100644 --- a/libuavcan/include/uavcan/util/map.hpp +++ b/libuavcan/include/uavcan/util/map.hpp @@ -11,6 +11,7 @@ #include #include #include +#include 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 - 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 - 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(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 MapBase::KVPair* MapBase::find(const Key& key) +typename MapBase::KVPair* MapBase::findKey(const Key& key) { #if !UAVCAN_TINY for (unsigned i = 0; i < num_static_entries_; i++) @@ -344,7 +354,7 @@ template Value* MapBase::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::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 void MapBase::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::remove(const Key& key) template template -void MapBase::removeWhere(Predicate predicate) +void MapBase::removeAllWhere(Predicate predicate) { unsigned num_removed = 0; @@ -407,8 +417,10 @@ void MapBase::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::removeWhere(Predicate predicate) } } } - p = p->getNextListNode(); + + p = next_group; } if (num_removed > 0) @@ -435,7 +448,7 @@ void MapBase::removeWhere(Predicate predicate) template template -const Key* MapBase::findFirstKey(Predicate predicate) const +const Key* MapBase::find(Predicate predicate) const { #if !UAVCAN_TINY for (unsigned i = 0; i < num_static_entries_; i++) @@ -451,8 +464,10 @@ const Key* MapBase::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::findFirstKey(Predicate predicate) const } } } - p = p->getNextListNode(); + + p = next_group; } return NULL; } template -void MapBase::removeAll() +void MapBase::clear() { - removeWhere(YesPredicate()); + removeAllWhere(YesPredicate()); } template @@ -495,8 +511,10 @@ typename MapBase::KVPair* MapBase::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::KVPair* MapBase::getByIndex(unsigned i index--; } } - p = p->getNextListNode(); + + p = next_group; } return NULL; @@ -521,12 +540,6 @@ const typename MapBase::KVPair* MapBase::getByIndex(unsi return const_cast*>(this)->getByIndex(index); } -template -bool MapBase::isEmpty() const -{ - return getSize() == 0; -} - template unsigned MapBase::getSize() const { diff --git a/libuavcan/include/uavcan/util/multiset.hpp b/libuavcan/include/uavcan/util/multiset.hpp new file mode 100644 index 0000000000..c1a99d31ea --- /dev/null +++ b/libuavcan/include/uavcan/util/multiset.hpp @@ -0,0 +1,542 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_UTIL_MULTISET_HPP_INCLUDED +#define UAVCAN_UTIL_MULTISET_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include + +#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 +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(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(0)); + } + } + }; + +private: + struct Chunk : LinkedListNode, ::uavcan::Noncopyable + { + enum { NumItems = (MemPoolBlockSize - sizeof(LinkedListNode)) / sizeof(Item) }; + Item items[NumItems]; + + Chunk() + { + StaticAssert<(static_cast(NumItems) > 0)>::check(); + IsDynamicallyAllocatable::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(NumItems); i++) + { + if (!items[i].isConstructed()) + { + return items + i; + } + } + return NULL; + } + }; + + /* + * Data + */ + LinkedListRoot list_; + IPoolAllocator& allocator_; + Item static_[NumStaticEntries]; + + /* + * Methods + */ + Item* findOrCreateFreeSlot(); + + void compact(); + + enum RemoveStrategy { RemoveOne, RemoveAll }; + + template + 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 + 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 + 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 + 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 + 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 + void removeAllWhere(Predicate predicate) { removeWhere(predicate, RemoveAll); } + + template + void removeFirstWhere(Predicate predicate) { removeWhere(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 + T* find(Predicate predicate); + + template + const T* find(Predicate predicate) const + { + return const_cast(this)->find(predicate); + } + + /** + * Calls Operator for each item of the set. + * Operator prototype: + * void (T& item) + * void (const T& item) - const overload + */ + template + void forEach(Operator oper) + { + OperatorToFalsePredicateAdapter adapter(oper); + (void)find&>(adapter); + } + + template + void forEach(Operator oper) const + { + const OperatorToFalsePredicateAdapter adapter(oper); + (void)find&>(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(predicate); + } + + const T* getByIndex(unsigned index) const + { + return const_cast(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 Multiset::Item* Multiset::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 +void Multiset::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 +template +void Multiset::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 +template +T* Multiset::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 +unsigned Multiset::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 +unsigned Multiset::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 diff --git a/libuavcan/include/uavcan/util/templates.hpp b/libuavcan/include/uavcan/util/templates.hpp index e2572ff3d3..b32e559d87 100644 --- a/libuavcan/include/uavcan/util/templates.hpp +++ b/libuavcan/include/uavcan/util/templates.hpp @@ -100,6 +100,15 @@ template struct RemoveReference { typedef T Type; }; template struct RemoveReference { typedef T Type; }; #endif +/** + * Parameter types + */ +template struct ParameterType { typedef const U& Type; }; +template struct ParameterType { typedef U& Type; }; +#if UAVCAN_CPP_VERSION > UAVCAN_CPP03 +template struct ParameterType { typedef U&& Type; }; +#endif + /** * Value types */ diff --git a/libuavcan/src/node/uc_scheduler.cpp b/libuavcan/src/node/uc_scheduler.cpp index 94414f5f24..5901fe75c5 100644 --- a/libuavcan/src/node/uc_scheduler.cpp +++ b/libuavcan/src/node/uc_scheduler.cpp @@ -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; } diff --git a/libuavcan/src/node/uc_service_client.cpp b/libuavcan/src/node/uc_service_client.cpp index a0c6cc00e8..49c8179a3a 100644 --- a/libuavcan/src/node/uc_service_client.cpp +++ b/libuavcan/src/node/uc_service_client.cpp @@ -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; } diff --git a/libuavcan/src/transport/uc_dispatcher.cpp b/libuavcan/src/transport/uc_dispatcher.cpp index b0ece03a7a..eb41b83e52 100644 --- a/libuavcan/src/transport/uc_dispatcher.cpp +++ b/libuavcan/src/transport/uc_dispatcher.cpp @@ -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) { diff --git a/libuavcan/src/transport/uc_transfer_listener.cpp b/libuavcan/src/transport/uc_transfer_listener.cpp index a48878f0c1..38cdb726eb 100644 --- a/libuavcan/src/transport/uc_transfer_listener.cpp +++ b/libuavcan/src/transport/uc_transfer_listener.cpp @@ -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); } diff --git a/libuavcan/test/node/service_client.cpp b/libuavcan/test/node/service_client.cpp index f668f791d5..82d99a66ac 100644 --- a/libuavcan/test/node/service_client.cpp +++ b/libuavcan/test/node/service_client.cpp @@ -12,6 +12,7 @@ #include #include #include +#include #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 responses; void handleResponse(const uavcan::ServiceCallResult& 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 _registrator; + + // Server + uavcan::ServiceServer server(nodes.a); + ASSERT_EQ(0, server.start(stringServiceServerCallback)); + + // Caller + typedef uavcan::ServiceCallResult ResultType; + typedef uavcan::ServiceClient::Binder > ClientType; + ServiceCallResultHandler 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, last_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, last_response)); +} + + TEST(ServiceClient, Empty) { InterlinkedTestNodesWithSysClock nodes; diff --git a/libuavcan/test/node/test_node.hpp b/libuavcan/test/node/test_node.hpp index c8b2055834..543f99a50d 100644 --- a/libuavcan/test/node/test_node.hpp +++ b/libuavcan/test/node/test_node.hpp @@ -4,14 +4,21 @@ #pragma once +#if __GNUC__ +// We need auto_ptr for compatibility reasons +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#endif + #include #include +#include +#include #include "../transport/can/can.hpp" struct TestNode : public uavcan::INode { - uavcan::PoolAllocator pool; + uavcan::PoolAllocator pool; uavcan::PoolManager<1> poolmgr; uavcan::MarshalBufferProvider<> buffer_provider; uavcan::OutgoingTransferRegistry<8> otr; @@ -43,21 +50,21 @@ struct TestNode : public uavcan::INode struct PairableCanDriver : public uavcan::ICanDriver, public uavcan::ICanIface { uavcan::ISystemClock& clock; - PairableCanDriver* other; + std::set others; std::queue read_queue; std::queue loopback_queue; uint64_t error_count; PairableCanDriver(uavcan::ISystemClock& clock) : clock(clock) - , other(NULL) , error_count(0) { } void linkTogether(PairableCanDriver* with) { - this->other = with; - with->other = this; + this->others.insert(with); + with->others.insert(this); + others.erase(this); } virtual uavcan::ICanIface* getIface(uavcan::uint8_t iface_index) @@ -73,7 +80,6 @@ struct PairableCanDriver : public uavcan::ICanDriver, public uavcan::ICanIface virtual uavcan::int16_t select(uavcan::CanSelectMasks& inout_masks, uavcan::MonotonicTime blocking_deadline) { - assert(other); if (inout_masks.read == 1) { inout_masks.read = (!read_queue.empty() || !loopback_queue.empty()) ? 1 : 0; @@ -91,8 +97,11 @@ struct PairableCanDriver : public uavcan::ICanDriver, public uavcan::ICanIface virtual uavcan::int16_t send(const uavcan::CanFrame& frame, uavcan::MonotonicTime, uavcan::CanIOFlags flags) { - assert(other); - other->read_queue.push(frame); + assert(!others.empty()); + for (std::set::iterator it = others.begin(); it != others.end(); ++it) + { + (*it)->read_queue.push(frame); + } if (flags & uavcan::CanIOFlagLoopback) { loopback_queue.push(frame); @@ -103,7 +112,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()) { @@ -122,6 +130,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; } @@ -181,3 +194,71 @@ struct InterlinkedTestNodes typedef InterlinkedTestNodes InterlinkedTestNodesWithSysClock; typedef InterlinkedTestNodes InterlinkedTestNodesWithClockMock; + + +template +struct TestNetwork +{ + struct NodeEnvironment + { + SystemClockDriver clock; + PairableCanDriver can_driver; + TestNode node; + + NodeEnvironment(uavcan::NodeID node_id) + : can_driver(clock) + , node(can_driver, clock, node_id) + { } + }; + + std::auto_ptr nodes[NumNodes]; + + TestNetwork(uavcan::uint8_t first_node_id = 1) + { + for (uavcan::uint8_t i = 0; i < NumNodes; i++) + { + nodes[i].reset(new NodeEnvironment(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; + } +}; diff --git a/libuavcan/test/node/test_node_test.cpp b/libuavcan/test/node/test_node_test.cpp new file mode 100644 index 0000000000..b40357be21 --- /dev/null +++ b/libuavcan/test/node/test_node_test.cpp @@ -0,0 +1,32 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include "test_node.hpp" + + +TEST(TestNode, TestNetwork) +{ + TestNetwork<4> nwk; + + uavcan::CanFrame frame; + for (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); + } +} diff --git a/libuavcan/test/protocol/data_type_info_provider.cpp b/libuavcan/test/protocol/data_type_info_provider.cpp index f987b3080b..1780d5f801 100644 --- a/libuavcan/test/protocol/data_type_info_provider.cpp +++ b/libuavcan/test/protocol/data_type_info_provider.cpp @@ -31,29 +31,29 @@ static bool validateDataTypeInfoResponse(const std::auto_ptrresponse.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 +90,7 @@ TEST(DataTypeInfoProvider, Basic) ASSERT_TRUE(validateDataTypeInfoResponse(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 +105,7 @@ TEST(DataTypeInfoProvider, Basic) ASSERT_TRUE(validateDataTypeInfoResponse(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 +148,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 +166,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 +184,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 +203,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 +221,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()); } diff --git a/libuavcan/test/protocol/dynamic_node_id_server/allocation_request_manager.cpp b/libuavcan/test/protocol/dynamic_node_id_server/allocation_request_manager.cpp index 037d73bc3a..dfecf86b6a 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/allocation_request_manager.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_server/allocation_request_manager.cpp @@ -30,12 +30,6 @@ public: return can_followup; } - virtual void handleAllocationActivityDetection( - const uavcan::ReceivedDataStructure& msg) - { - std::cout << "ALLOCATION ACTIVITY\n" << msg << std::endl; - } - bool matchAndPopLastRequest(const UniqueID& unique_id, uavcan::NodeID preferred_node_id) { if (requests_.empty()) diff --git a/libuavcan/test/protocol/dynamic_node_id_server/centralized/server.cpp b/libuavcan/test/protocol/dynamic_node_id_server/centralized/server.cpp new file mode 100644 index 0000000000..5e83b19d05 --- /dev/null +++ b/libuavcan/test/protocol/dynamic_node_id_server/centralized/server.cpp @@ -0,0 +1,78 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include +#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 _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + uavcan::DefaultDataTypeRegistrator _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; +} diff --git a/libuavcan/test/protocol/dynamic_node_id_server/centralized/storage.cpp b/libuavcan/test/protocol/dynamic_node_id_server/centralized/storage.cpp new file mode 100644 index 0000000000..51ada6d104 --- /dev/null +++ b/libuavcan/test/protocol/dynamic_node_id_server/centralized/storage.cpp @@ -0,0 +1,174 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#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(); +} diff --git a/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp b/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp index e46cf27a4e..59a4178ebd 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp @@ -68,11 +68,10 @@ TEST(dynamic_node_id_server_RaftCore, Basic) /* * Running and trying not to fall */ - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(5000)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(9000)); - // The one with lower node ID must become a leader - ASSERT_TRUE(raft_a->isLeader()); - ASSERT_FALSE(raft_b->isLeader()); + // 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()); @@ -83,19 +82,19 @@ TEST(dynamic_node_id_server_RaftCore, Basic) Entry::FieldTypes::unique_id unique_id; uavcan::fill_n(unique_id.begin(), 16, uint8_t(0xAA)); - raft_a->appendLog(unique_id, uavcan::NodeID(1)); + (raft_a->isLeader() ? raft_a : raft_b)->appendLog(unique_id, uavcan::NodeID(1)); - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2000)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(6000)); ASSERT_EQ(1, raft_a->getCommitIndex()); ASSERT_EQ(1, raft_b->getCommitIndex()); /* - * Terminating the leader - the Follower will continue to sleep + * Terminating the leader */ raft_a.reset(); - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2000)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(6000)); /* * Reinitializing the leader - current Follower will become the new Leader @@ -106,7 +105,7 @@ TEST(dynamic_node_id_server_RaftCore, Basic) ASSERT_LE(0, raft_a->init(2)); ASSERT_EQ(0, raft_a->getCommitIndex()); - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(5000)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(9000)); ASSERT_FALSE(raft_a->isLeader()); ASSERT_TRUE(raft_b->isLeader()); @@ -166,7 +165,7 @@ TEST(dynamic_node_id_server_Server, Basic) /* * Fire */ - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(4000)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(15000)); ASSERT_TRUE(client.isAllocationComplete()); ASSERT_EQ(PreferredNodeID, client.getAllocatedNodeID()); diff --git a/libuavcan/test/protocol/dynamic_node_id_server/event.cpp b/libuavcan/test/protocol/dynamic_node_id_server/event.cpp index e8f2c457e6..7405f5d850 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/event.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_server/event.cpp @@ -13,7 +13,6 @@ TEST(dynamic_node_id_server_EventTracer, EventCodeToString) // Simply checking some error codes ASSERT_STREQ("Error", IEventTracer::getEventName(TraceError)); - ASSERT_STREQ("RaftActiveSwitch", IEventTracer::getEventName(TraceRaftActiveSwitch)); ASSERT_STREQ("RaftAppendEntriesCallFailure", IEventTracer::getEventName(TraceRaftAppendEntriesCallFailure)); ASSERT_STREQ("RaftDiscoveryReceived", IEventTracer::getEventName(TraceRaftDiscoveryReceived)); ASSERT_STREQ("DiscoveryNodeRestartDetected", IEventTracer::getEventName(TraceDiscoveryNodeRestartDetected)); diff --git a/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp b/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp index 427213bad1..7313bf0b0a 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp @@ -111,6 +111,7 @@ TEST(dynamic_node_id_server_NodeDiscoverer, Basic) /* * Publishing NodeStatus, discovery is disabled */ + std::cout << "!!! Publishing NodeStatus, discovery is disabled" << std::endl; handler.can_discover = false; uavcan::Publisher node_status_pub(nodes.b); @@ -132,48 +133,53 @@ TEST(dynamic_node_id_server_NodeDiscoverer, Basic) /* * Enabling discovery - the querying will continue despite the fact that NodeStatus messages are not arriving */ + std::cout << "!!! Enabling discovery" << std::endl; handler.can_discover = true; - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1900)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(650)); ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound)); ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart)); ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryTimerStop)); - ASSERT_LE(1, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest)); - ASSERT_LE(1, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure)); + ASSERT_EQ(2, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest)); + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure)); ASSERT_TRUE(disc.hasUnknownNodes()); /* * Publishing NodeStatus */ + std::cout << "!!! Publishing NodeStatus" << std::endl; + node_status.uptime_sec += 5U; ASSERT_LE(0, node_status_pub.broadcast(node_status)); - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1900)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(650)); ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound)); ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart)); ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryTimerStop)); - ASSERT_LE(2, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest)); - ASSERT_LE(2, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure)); + ASSERT_EQ(3, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest)); + ASSERT_EQ(2, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure)); ASSERT_TRUE(disc.hasUnknownNodes()); /* * Publishing NodeStatus, discovery is enabled, GetNodeInfo mock server is initialized */ + std::cout << "!!! Publishing NodeStatus, discovery is enabled, GetNodeInfo mock server is initialized" << std::endl; + GetNodeInfoMockServer get_node_info_server(nodes.b); get_node_info_server.response.hardware_version.unique_id[0] = 123; // Arbitrary data get_node_info_server.response.hardware_version.unique_id[6] = 213; get_node_info_server.response.hardware_version.unique_id[14] = 52; ASSERT_LE(0, get_node_info_server.start()); - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1900)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(500)); ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound)); ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart)); ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStop)); - ASSERT_LE(3, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest)); - ASSERT_LE(2, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure)); + ASSERT_EQ(4, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest)); + ASSERT_EQ(3, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure)); ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNodeFinalized)); ASSERT_FALSE(disc.hasUnknownNodes()); @@ -220,13 +226,13 @@ TEST(dynamic_node_id_server_NodeDiscoverer, RestartAndMaxAttempts) node_status.uptime_sec = 10; // Nonzero ASSERT_LE(0, node_status_pub.broadcast(node_status)); - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(3400)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1600)); ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound)); ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart)); ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryTimerStop)); - ASSERT_LE(3, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest)); - ASSERT_LE(3, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure)); + ASSERT_EQ(4, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest)); + ASSERT_EQ(3, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure)); ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryNodeFinalized)); ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryNodeRestartDetected)); ASSERT_TRUE(disc.hasUnknownNodes()); @@ -238,13 +244,13 @@ TEST(dynamic_node_id_server_NodeDiscoverer, RestartAndMaxAttempts) node_status.uptime_sec = 9; // Less than previous ASSERT_LE(0, node_status_pub.broadcast(node_status)); - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(3400)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1600)); ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound)); ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart)); ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryTimerStop)); - ASSERT_LE(6, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest)); - ASSERT_LE(6, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure)); + ASSERT_EQ(7, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest)); + ASSERT_EQ(6, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure)); ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryNodeFinalized)); ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNodeRestartDetected)); ASSERT_TRUE(disc.hasUnknownNodes()); @@ -252,13 +258,13 @@ TEST(dynamic_node_id_server_NodeDiscoverer, RestartAndMaxAttempts) /* * Waiting for timeout */ - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(3400)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1600)); ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound)); ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart)); ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStop)); - ASSERT_LE(8, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest)); - ASSERT_LE(8, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure)); + ASSERT_EQ(8, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest)); + ASSERT_EQ(8, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure)); ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNodeFinalized)); ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNodeRestartDetected)); ASSERT_FALSE(disc.hasUnknownNodes()); @@ -269,3 +275,16 @@ TEST(dynamic_node_id_server_NodeDiscoverer, RestartAndMaxAttempts) ASSERT_TRUE(handler.findNode(UniqueID())); ASSERT_EQ(2, handler.findNode(UniqueID())->node_id.get()); } + + +TEST(dynamic_node_id_server_NodeDiscoverer, Sizes) +{ + using namespace uavcan; + + std::cout << "BitSet: " << sizeof(BitSet) << std::endl; + std::cout << "ServiceClient: " << sizeof(ServiceClient) << std::endl; + std::cout << "protocol::GetNodeInfo::Response: " << sizeof(protocol::GetNodeInfo::Response) << std::endl; + std::cout << "Subscriber: " + << sizeof(Subscriber&), 64, 0>) << std::endl; +} diff --git a/libuavcan/test/protocol/file_server.cpp b/libuavcan/test/protocol/file_server.cpp new file mode 100644 index 0000000000..f3591e79e3 --- /dev/null +++ b/libuavcan/test/protocol/file_server.cpp @@ -0,0 +1,142 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include "helpers.hpp" + + +class TestFileServerBackend : public uavcan::IFileServerBackend +{ +public: + static const std::string file_name; + static const std::string file_data; + + virtual int16_t getInfo(const Path& path, uint64_t& out_crc64, uint32_t& out_size, EntryType& out_type) + { + if (path == file_name) + { + { + FileCRC crc; + crc.add(reinterpret_cast(file_data.c_str()), unsigned(file_data.length())); + out_crc64 = crc.get(); + } + out_size = uint16_t(file_data.length()); + out_type.flags |= EntryType::FLAG_FILE; + out_type.flags |= EntryType::FLAG_READABLE; + return 0; + } + else + { + return Error::NOT_FOUND; + } + } + + virtual int16_t read(const Path& path, const uint32_t offset, uint8_t* out_buffer, uint16_t& inout_size) + { + if (path == file_name) + { + if (offset < file_data.length()) + { + inout_size = uint16_t(file_data.length() - offset); + std::memcpy(out_buffer, file_data.c_str() + offset, inout_size); + } + else + { + inout_size = 0; + } + return 0; + } + else + { + return Error::NOT_FOUND; + } + } +}; + +const std::string TestFileServerBackend::file_name = "test"; +const std::string TestFileServerBackend::file_data = "123456789"; + +TEST(BasicFileServer, Basic) +{ + using namespace uavcan::protocol::file; + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + + InterlinkedTestNodesWithSysClock nodes; + + TestFileServerBackend backend; + + uavcan::BasicFileServer serv(nodes.a, backend); + std::cout << "sizeof(uavcan::BasicFileServer): " << sizeof(uavcan::BasicFileServer) << std::endl; + + ASSERT_LE(0, serv.start()); + + /* + * GetInfo, existing file + */ + { + ServiceClientWithCollector get_info(nodes.b); + + GetInfo::Request get_info_req; + get_info_req.path.path = "test"; + + ASSERT_LE(0, get_info.call(1, get_info_req)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)); + + ASSERT_TRUE(get_info.collector.result.get()); + ASSERT_TRUE(get_info.collector.result->isSuccessful()); + + ASSERT_EQ(0x62EC59E3F1A4F00A, get_info.collector.result->getResponse().crc64); + ASSERT_EQ(0, get_info.collector.result->getResponse().error.value); + ASSERT_EQ(9, get_info.collector.result->getResponse().size); + ASSERT_EQ(EntryType::FLAG_FILE | EntryType::FLAG_READABLE, + get_info.collector.result->getResponse().entry_type.flags); + } + + /* + * Read, existing file + */ + { + ServiceClientWithCollector read(nodes.b); + + Read::Request read_req; + read_req.path.path = "test"; + + ASSERT_LE(0, read.call(1, read_req)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)); + + ASSERT_TRUE(read.collector.result.get()); + ASSERT_TRUE(read.collector.result->isSuccessful()); + + ASSERT_EQ("123456789", read.collector.result->getResponse().data); + ASSERT_EQ(0, read.collector.result->getResponse().error.value); + } +} + + +TEST(FileServer, Basic) +{ + using namespace uavcan::protocol::file; + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + uavcan::DefaultDataTypeRegistrator _reg3; + uavcan::DefaultDataTypeRegistrator _reg4; + uavcan::DefaultDataTypeRegistrator _reg5; + + InterlinkedTestNodesWithSysClock nodes; + + TestFileServerBackend backend; + + uavcan::FileServer serv(nodes.a, backend); + std::cout << "sizeof(uavcan::FileServer): " << sizeof(uavcan::FileServer) << std::endl; + + ASSERT_LE(0, serv.start()); + + // TODO TEST +} diff --git a/libuavcan/test/protocol/firmware_update_trigger.cpp b/libuavcan/test/protocol/firmware_update_trigger.cpp new file mode 100644 index 0000000000..08ef9c4fd8 --- /dev/null +++ b/libuavcan/test/protocol/firmware_update_trigger.cpp @@ -0,0 +1,336 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include +#include "helpers.hpp" + +using namespace uavcan::protocol::file; + +struct FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker +{ + unsigned should_request_cnt; + unsigned should_retry_cnt; + unsigned confirmation_cnt; + + std::string firmware_path; + + int retry_quota; + std::string expected_node_name_to_update; + + BeginFirmwareUpdate::Response last_error_response; + + FirmwareVersionChecker() + : should_request_cnt(0) + , should_retry_cnt(0) + , confirmation_cnt(0) + , retry_quota(0) + { } + + virtual bool shouldRequestFirmwareUpdate(uavcan::NodeID node_id, + const uavcan::protocol::GetNodeInfo::Response& node_info, + FirmwareFilePath& out_firmware_file_path) + { + should_request_cnt++; + std::cout << "REQUEST? " << int(node_id.get()) << "\n" << node_info << std::endl; + out_firmware_file_path = firmware_path.c_str(); + return node_info.name == expected_node_name_to_update; + } + + virtual bool shouldRetryFirmwareUpdate(uavcan::NodeID node_id, + const BeginFirmwareUpdate::Response& error_response, + FirmwareFilePath& out_firmware_file_path) + { + last_error_response = error_response; + std::cout << "RETRY? " << int(node_id.get()) << "\n" << error_response << std::endl; + should_retry_cnt++; + + EXPECT_STREQ(firmware_path.c_str(), out_firmware_file_path.c_str()); + + if (retry_quota > 0) + { + retry_quota--; + return true; + } + else + { + return false; + } + } + + virtual void handleFirmwareUpdateConfirmation(uavcan::NodeID node_id, + const BeginFirmwareUpdate::Response& response) + { + confirmation_cnt++; + std::cout << "CONFIRMED " << int(node_id.get()) << "\n" << response << std::endl; + } +}; + +struct BeginFirmwareUpdateServer +{ + uint8_t response_error_code; + + BeginFirmwareUpdateServer() : response_error_code(0) { } + + void handleRequest(const uavcan::ReceivedDataStructure& req, + uavcan::ServiceResponseDataStructure& res) const + { + std::cout << "REQUEST\n" << req << std::endl; + res.error = response_error_code; + res.optional_error_message = "foobar"; + } + + typedef uavcan::MethodBinder&, + uavcan::ServiceResponseDataStructure&) const> Callback; + + Callback makeCallback() { return Callback(this, &BeginFirmwareUpdateServer::handleRequest); } +}; + + +TEST(FirmwareUpdateTrigger, Basic) +{ + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + uavcan::DefaultDataTypeRegistrator _reg3; + uavcan::DefaultDataTypeRegistrator _reg4; + + InterlinkedTestNodesWithSysClock nodes; + + FirmwareVersionChecker checker; + + uavcan::NodeInfoRetriever node_info_retriever(nodes.a); // On the same node + + uavcan::FirmwareUpdateTrigger trigger(nodes.a, checker); + std::cout << "sizeof(uavcan::FirmwareUpdateTrigger): " << sizeof(uavcan::FirmwareUpdateTrigger) << std::endl; + + std::auto_ptr provider(new uavcan::NodeStatusProvider(nodes.b)); // Other node + + /* + * Initializing + */ + ASSERT_LE(0, trigger.start(node_info_retriever, "/path_prefix/")); + + ASSERT_LE(0, node_info_retriever.start()); + ASSERT_EQ(1, node_info_retriever.getNumListeners()); + + uavcan::protocol::HardwareVersion hwver; + hwver.unique_id[0] = 123; + hwver.unique_id[4] = 213; + hwver.unique_id[8] = 45; + + provider->setName("Ivan"); + provider->setHardwareVersion(hwver); + + ASSERT_LE(0, provider->startAndPublish()); + + ASSERT_FALSE(trigger.isTimerRunning()); + ASSERT_EQ(0, trigger.getNumPendingNodes()); + + /* + * Updating one node + * The server that can confirm the request is not running yet + */ + checker.firmware_path = "firmware_path"; + checker.expected_node_name_to_update = "Ivan"; + checker.retry_quota = 1000; + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2000)); + + ASSERT_TRUE(trigger.isTimerRunning()); + ASSERT_EQ(1, trigger.getNumPendingNodes()); + + ASSERT_EQ(1, checker.should_request_cnt); + ASSERT_EQ(0, checker.should_retry_cnt); + ASSERT_EQ(0, checker.confirmation_cnt); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2000)); + + // Still running! + ASSERT_TRUE(trigger.isTimerRunning()); + ASSERT_EQ(1, trigger.getNumPendingNodes()); + + /* + * Starting the firmware update server that returns an error + * The checker will instruct the trigger to repeat + */ + uavcan::ServiceServer srv(nodes.b); + BeginFirmwareUpdateServer srv_impl; + + ASSERT_LE(0, srv.start(srv_impl.makeCallback())); + + srv_impl.response_error_code = BeginFirmwareUpdate::Response::ERROR_UNKNOWN; + checker.retry_quota = 1000; + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1100)); + + ASSERT_EQ(1, checker.should_request_cnt); + ASSERT_EQ(1, checker.should_retry_cnt); + ASSERT_EQ(0, checker.confirmation_cnt); + + // Still running! + ASSERT_TRUE(trigger.isTimerRunning()); + ASSERT_EQ(1, trigger.getNumPendingNodes()); + + /* + * Trying again, this time with ERROR_IN_PROGRESS + */ + srv_impl.response_error_code = BeginFirmwareUpdate::Response::ERROR_IN_PROGRESS; + checker.retry_quota = 0; + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2100)); + + ASSERT_EQ(1, checker.should_request_cnt); + ASSERT_EQ(1, checker.should_retry_cnt); + ASSERT_EQ(1, checker.confirmation_cnt); + + // Stopped! + ASSERT_FALSE(trigger.isTimerRunning()); + ASSERT_EQ(0, trigger.getNumPendingNodes()); + + /* + * Restarting the node info provider + * Now it doesn't need an update + */ + provider.reset(new uavcan::NodeStatusProvider(nodes.b)); + + provider->setName("Dmitry"); + provider->setHardwareVersion(hwver); + + ASSERT_LE(0, provider->startAndPublish()); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2100)); + + ASSERT_EQ(2, checker.should_request_cnt); + ASSERT_EQ(1, checker.should_retry_cnt); + ASSERT_EQ(1, checker.confirmation_cnt); + + // Stopped! + ASSERT_FALSE(trigger.isTimerRunning()); + ASSERT_EQ(0, trigger.getNumPendingNodes()); + + /* + * Final checks + */ + ASSERT_EQ(0, nodes.a.internal_failure_count); + ASSERT_EQ(0, nodes.b.internal_failure_count); +} + + +TEST(FirmwareUpdateTrigger, MultiNode) +{ + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + uavcan::DefaultDataTypeRegistrator _reg3; + uavcan::DefaultDataTypeRegistrator _reg4; + + TestNetwork<5> nodes; + + // The trigger node + FirmwareVersionChecker checker; + uavcan::NodeInfoRetriever node_info_retriever(nodes[0]); + uavcan::FirmwareUpdateTrigger trigger(nodes[0], checker); + + // The client nodes + std::auto_ptr provider_a(new uavcan::NodeStatusProvider(nodes[1])); + std::auto_ptr provider_b(new uavcan::NodeStatusProvider(nodes[2])); + std::auto_ptr provider_c(new uavcan::NodeStatusProvider(nodes[3])); + std::auto_ptr provider_d(new uavcan::NodeStatusProvider(nodes[4])); + + uavcan::protocol::HardwareVersion hwver; + + /* + * Initializing + */ + ASSERT_LE(0, trigger.start(node_info_retriever, "/path_prefix/")); + + ASSERT_LE(0, node_info_retriever.start()); + ASSERT_EQ(1, node_info_retriever.getNumListeners()); + + hwver.unique_id[0] = 0xAA; + provider_a->setHardwareVersion(hwver); + provider_a->setName("Victor"); + ASSERT_LE(0, provider_a->startAndPublish()); + + hwver.unique_id[0] = 0xBB; + provider_b->setHardwareVersion(hwver); + provider_b->setName("Victor"); + ASSERT_LE(0, provider_b->startAndPublish()); + + hwver.unique_id[0] = 0xCC; + provider_c->setHardwareVersion(hwver); + provider_c->setName("Alexey"); + ASSERT_LE(0, provider_c->startAndPublish()); + + hwver.unique_id[0] = 0xDD; + provider_d->setHardwareVersion(hwver); + provider_d->setName("Victor"); + ASSERT_LE(0, provider_d->startAndPublish()); + + checker.expected_node_name_to_update = "Victor"; // Victors will get updated, others will not + checker.firmware_path = "abc"; + + /* + * Running - 3 will timout, 1 will be ignored + */ + ASSERT_FALSE(trigger.isTimerRunning()); + ASSERT_EQ(0, trigger.getNumPendingNodes()); + + nodes.spinAll(uavcan::MonotonicDuration::fromMSec(4100)); + + ASSERT_TRUE(trigger.isTimerRunning()); + ASSERT_EQ(3, trigger.getNumPendingNodes()); + + ASSERT_EQ(4, checker.should_request_cnt); + ASSERT_EQ(0, checker.should_retry_cnt); + ASSERT_EQ(0, checker.confirmation_cnt); + + /* + * Initializing the BeginFirmwareUpdate servers + */ + uavcan::ServiceServer srv_a(nodes[1]); + uavcan::ServiceServer srv_b(nodes[2]); + uavcan::ServiceServer srv_c(nodes[3]); + uavcan::ServiceServer srv_d(nodes[4]); + + BeginFirmwareUpdateServer srv_a_impl; + BeginFirmwareUpdateServer srv_b_impl; + BeginFirmwareUpdateServer srv_c_impl; + BeginFirmwareUpdateServer srv_d_impl; + + ASSERT_LE(0, srv_a.start(srv_a_impl.makeCallback())); + ASSERT_LE(0, srv_b.start(srv_b_impl.makeCallback())); + ASSERT_LE(0, srv_c.start(srv_c_impl.makeCallback())); + ASSERT_LE(0, srv_d.start(srv_d_impl.makeCallback())); + + srv_a_impl.response_error_code = BeginFirmwareUpdate::Response::ERROR_INVALID_MODE; // retry + srv_b_impl.response_error_code = BeginFirmwareUpdate::Response::ERROR_INVALID_MODE; // retry + srv_c_impl.response_error_code = BeginFirmwareUpdate::Response::ERROR_INVALID_MODE; // ignore (see below) + srv_d_impl.response_error_code = BeginFirmwareUpdate::Response::ERROR_OK; // OK + + /* + * Spinning, now we're getting some errors + * This also checks correctness of the round-robin selector + */ + checker.retry_quota = 2; + nodes.spinAll(uavcan::MonotonicDuration::fromMSec(4200)); // Two will retry, one drop, one confirm + + ASSERT_TRUE(trigger.isTimerRunning()); + ASSERT_EQ(0, trigger.getNumPendingNodes()); // All removed + + EXPECT_EQ(4, checker.should_request_cnt); + EXPECT_EQ(4, checker.should_retry_cnt); + EXPECT_EQ(1, checker.confirmation_cnt); + + /* + * Waiting for the timer to stop + */ + nodes.spinAll(uavcan::MonotonicDuration::fromMSec(1100)); + + ASSERT_FALSE(trigger.isTimerRunning()); +} diff --git a/libuavcan/test/protocol/global_time_sync_master.cpp b/libuavcan/test/protocol/global_time_sync_master.cpp index 404d649027..bed4d908dc 100644 --- a/libuavcan/test/protocol/global_time_sync_master.cpp +++ b/libuavcan/test/protocol/global_time_sync_master.cpp @@ -30,9 +30,9 @@ struct GlobalTimeSyncTestNetwork , master_low(120) , master_high(8) { - slave.can.other = &master_low.can; - master_low.can.other = &slave.can; - master_high.can.other = &slave.can; + slave.can.others.insert(&master_low.can); + master_low.can.others.insert(&slave.can); + master_high.can.others.insert(&slave.can); } void spinAll(uavcan::MonotonicDuration duration = uavcan::MonotonicDuration::fromMSec(9)) diff --git a/libuavcan/test/protocol/global_time_sync_slave.cpp b/libuavcan/test/protocol/global_time_sync_slave.cpp index 5bb2e91c0d..25c4e50975 100644 --- a/libuavcan/test/protocol/global_time_sync_slave.cpp +++ b/libuavcan/test/protocol/global_time_sync_slave.cpp @@ -109,7 +109,7 @@ TEST(GlobalTimeSyncSlave, Basic) master2_clock.monotonic_auto_advance = 1000; master2_clock.preserve_utc = true; PairableCanDriver master2_can(master2_clock); - master2_can.other = &nodes.can_a; + master2_can.others.insert(&nodes.can_a); TestNode master2_node(master2_can, master2_clock, 8); uavcan::Publisher gts_pub2(master2_node); diff --git a/libuavcan/test/protocol/helpers.hpp b/libuavcan/test/protocol/helpers.hpp index df8085566e..e0fb164f2f 100644 --- a/libuavcan/test/protocol/helpers.hpp +++ b/libuavcan/test/protocol/helpers.hpp @@ -103,3 +103,28 @@ struct BackgroundSpinner : uavcan::TimerBase spinning_node.spin(uavcan::MonotonicDuration::fromMSec(1)); } }; + +template +static inline void emulateSingleFrameBroadcastTransfer(CanDriver& can, uavcan::NodeID node_id, + const MessageType& message, uavcan::TransferID tid) +{ + uavcan::StaticTransferBuffer<100> buffer; + uavcan::BitStream bitstream(buffer); + uavcan::ScalarCodec codec(bitstream); + + // Manual message publication + ASSERT_LT(0, MessageType::encode(message, codec)); + ASSERT_GE(8, buffer.getMaxWritePos()); + + // DataTypeID data_type_id, TransferType transfer_type, NodeID src_node_id, NodeID dst_node_id, + // uint_fast8_t frame_index, TransferID transfer_id, bool last_frame + uavcan::Frame frame(MessageType::DefaultDataTypeID, uavcan::TransferTypeMessageBroadcast, + node_id, uavcan::NodeID::Broadcast, 0, tid, true); + + ASSERT_EQ(buffer.getMaxWritePos(), frame.setPayload(buffer.getRawPtr(), buffer.getMaxWritePos())); + + uavcan::CanFrame can_frame; + ASSERT_TRUE(frame.compile(can_frame)); + + can.pushRxToAllIfaces(can_frame); +} diff --git a/libuavcan/test/protocol/node_info_retriever.cpp b/libuavcan/test/protocol/node_info_retriever.cpp new file mode 100644 index 0000000000..88fc4aace8 --- /dev/null +++ b/libuavcan/test/protocol/node_info_retriever.cpp @@ -0,0 +1,273 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#if __GNUC__ +// We need auto_ptr for compatibility reasons +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#endif + +#include +#include +#include +#include +#include "helpers.hpp" + +static void publishNodeStatus(PairableCanDriver& can, uavcan::NodeID node_id, uavcan::uint8_t status_code, + uavcan::uint32_t uptime_sec, uavcan::TransferID tid) +{ + uavcan::protocol::NodeStatus msg; + msg.status_code = status_code; + msg.uptime_sec = uptime_sec; + emulateSingleFrameBroadcastTransfer(can, node_id, msg, tid); +} + + +struct NodeInfoListener : public uavcan::INodeInfoListener +{ + std::auto_ptr last_node_info; + uavcan::NodeID last_node_id; + unsigned status_message_cnt; + unsigned status_change_cnt; + unsigned info_unavailable_cnt; + + NodeInfoListener() + : status_message_cnt(0) + , status_change_cnt(0) + , info_unavailable_cnt(0) + { } + + virtual void handleNodeInfoRetrieved(uavcan::NodeID node_id, + const uavcan::protocol::GetNodeInfo::Response& node_info) + { + last_node_id = node_id; + std::cout << node_info << std::endl; + last_node_info.reset(new uavcan::protocol::GetNodeInfo::Response(node_info)); + } + + virtual void handleNodeInfoUnavailable(uavcan::NodeID node_id) + { + std::cout << "NODE INFO FOR " << int(node_id.get()) << " IS UNAVAILABLE" << std::endl; + last_node_id = node_id; + info_unavailable_cnt++; + } + + virtual void handleNodeStatusChange(const uavcan::NodeStatusMonitor::NodeStatusChangeEvent& event) + { + std::cout << "NODE " << int(event.node_id.get()) << " STATUS CHANGE: " + << int(event.old_status.status_code) << " --> " << int(event.status.status_code) << std::endl; + status_change_cnt++; + } + + virtual void handleNodeStatusMessage(const uavcan::ReceivedDataStructure& msg) + { + std::cout << msg << std::endl; + status_message_cnt++; + } +}; + + +TEST(NodeInfoRetriever, Basic) +{ + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + uavcan::DefaultDataTypeRegistrator _reg3; + + InterlinkedTestNodesWithSysClock nodes; + + uavcan::NodeInfoRetriever retr(nodes.a); + std::cout << "sizeof(uavcan::NodeInfoRetriever): " << sizeof(uavcan::NodeInfoRetriever) << std::endl; + std::cout << "sizeof(uavcan::ServiceClient): " + << sizeof(uavcan::ServiceClient) << std::endl; + + std::auto_ptr provider(new uavcan::NodeStatusProvider(nodes.b)); + + NodeInfoListener listener; + + /* + * Initialization + */ + ASSERT_LE(0, retr.start()); + + retr.removeListener(&listener); // Does nothing + retr.addListener(&listener); + retr.addListener(&listener); + retr.addListener(&listener); + ASSERT_EQ(1, retr.getNumListeners()); + + uavcan::protocol::HardwareVersion hwver; + hwver.unique_id[0] = 123; + hwver.unique_id[4] = 213; + hwver.unique_id[8] = 45; + + provider->setName("Ivan"); + provider->setHardwareVersion(hwver); + + ASSERT_LE(0, provider->startAndPublish()); + + ASSERT_FALSE(retr.isRetrievingInProgress()); + ASSERT_EQ(0, retr.getNumPendingRequests()); + + EXPECT_EQ(40, retr.getRequestInterval().toMSec()); // Default + + /* + * Waiting for discovery + */ + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(50)); + ASSERT_TRUE(retr.isRetrievingInProgress()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1500)); + ASSERT_FALSE(retr.isRetrievingInProgress()); + + ASSERT_EQ(2, listener.status_message_cnt); + ASSERT_EQ(1, listener.status_change_cnt); + ASSERT_EQ(0, listener.info_unavailable_cnt); + ASSERT_TRUE(listener.last_node_info.get()); + ASSERT_EQ(uavcan::NodeID(2), listener.last_node_id); + ASSERT_EQ("Ivan", listener.last_node_info->name); + ASSERT_TRUE(hwver == listener.last_node_info->hardware_version); + + provider.reset(); // Moving the provider out of the way; its entry will timeout meanwhile + + /* + * Declaring a bunch of different nodes that don't support GetNodeInfo + */ + ASSERT_FALSE(retr.isRetrievingInProgress()); + + retr.setNumRequestAttempts(3); + + uavcan::TransferID tid; + + publishNodeStatus(nodes.can_a, uavcan::NodeID(10), 0, 10, tid); + publishNodeStatus(nodes.can_a, uavcan::NodeID(11), 0, 10, tid); + publishNodeStatus(nodes.can_a, uavcan::NodeID(12), 0, 10, tid); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); + ASSERT_LE(1, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); + ASSERT_LE(2, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); + ASSERT_EQ(3, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000)); + ASSERT_TRUE(retr.isRetrievingInProgress()); + + tid.increment(); + publishNodeStatus(nodes.can_a, uavcan::NodeID(10), 0, 11, tid); + publishNodeStatus(nodes.can_a, uavcan::NodeID(11), 0, 11, tid); + publishNodeStatus(nodes.can_a, uavcan::NodeID(12), 0, 11, tid); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); + ASSERT_LE(1, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); + ASSERT_LE(2, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); + ASSERT_EQ(3, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000)); + ASSERT_TRUE(retr.isRetrievingInProgress()); + + tid.increment(); + publishNodeStatus(nodes.can_a, uavcan::NodeID(10), 0, 12, tid); + publishNodeStatus(nodes.can_a, uavcan::NodeID(11), 0, 12, tid); + publishNodeStatus(nodes.can_a, uavcan::NodeID(12), 0, 10, tid); // Reset + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); + ASSERT_LE(1, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); + ASSERT_LE(2, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); + ASSERT_EQ(3, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000)); + ASSERT_TRUE(retr.isRetrievingInProgress()); + + EXPECT_EQ(11, listener.status_message_cnt); + EXPECT_EQ(5, listener.status_change_cnt); // node 2 online/offline + 3 test nodes above + EXPECT_EQ(2, listener.info_unavailable_cnt); + + tid.increment(); + publishNodeStatus(nodes.can_a, uavcan::NodeID(12), 0, 11, tid); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); + ASSERT_EQ(1, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); + ASSERT_EQ(1, retr.getNumPendingRequests()); // Still one because two went offline + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1200)); + ASSERT_TRUE(retr.isRetrievingInProgress()); + + tid.increment(); + publishNodeStatus(nodes.can_a, uavcan::NodeID(12), 0, 12, tid); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1200)); + ASSERT_FALSE(retr.isRetrievingInProgress()); // Out of attempts, stopping + ASSERT_EQ(0, retr.getNumPendingRequests()); + + EXPECT_EQ(13, listener.status_message_cnt); + EXPECT_EQ(7, listener.status_change_cnt); // node 2 online/offline + 2 test nodes above online/offline + 1 + EXPECT_EQ(3, listener.info_unavailable_cnt); +} + + +TEST(NodeInfoRetriever, MaxConcurrentRequests) +{ + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + uavcan::DefaultDataTypeRegistrator _reg3; + + InterlinkedTestNodesWithSysClock nodes; + + uavcan::NodeInfoRetriever retr(nodes.a); + std::cout << "sizeof(uavcan::NodeInfoRetriever): " << sizeof(uavcan::NodeInfoRetriever) << std::endl; + std::cout << "sizeof(uavcan::ServiceClient): " + << sizeof(uavcan::ServiceClient) << std::endl; + + NodeInfoListener listener; + + /* + * Initialization + */ + ASSERT_LE(0, retr.start()); + + retr.addListener(&listener); + ASSERT_EQ(1, retr.getNumListeners()); + + ASSERT_FALSE(retr.isRetrievingInProgress()); + ASSERT_EQ(0, retr.getNumPendingRequests()); + + ASSERT_EQ(40, retr.getRequestInterval().toMSec()); + + const unsigned MaxPendingRequests = 14; // See class docs + const unsigned MinPendingRequestsAtFullLoad = 12; + + /* + * Sending a lot of requests, making sure that the number of concurrent calls does not exceed the specified limit. + */ + for (uint8_t node_id = 1U; node_id <= 127U; node_id++) + { + publishNodeStatus(nodes.can_a, node_id, 0, 0, uavcan::TransferID()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)); + ASSERT_GE(MaxPendingRequests, retr.getNumPendingRequests()); + ASSERT_TRUE(retr.isRetrievingInProgress()); + } + + ASSERT_GE(MaxPendingRequests, retr.getNumPendingRequests()); + ASSERT_LE(MinPendingRequestsAtFullLoad, retr.getNumPendingRequests()); + + for (int i = 0; i < 8; i++) // Approximate + { + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(35)); + std::cout << "!!! SPIN " << i << " COMPLETE" << std::endl; + + ASSERT_GE(MaxPendingRequests, retr.getNumPendingRequests()); + ASSERT_LE(MinPendingRequestsAtFullLoad, retr.getNumPendingRequests()); + + ASSERT_TRUE(retr.isRetrievingInProgress()); + } + + ASSERT_LT(0, retr.getNumPendingRequests()); + ASSERT_TRUE(retr.isRetrievingInProgress()); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(5000)); + + ASSERT_EQ(0, retr.getNumPendingRequests()); + ASSERT_FALSE(retr.isRetrievingInProgress()); +} diff --git a/libuavcan/test/protocol/node_status_monitor.cpp b/libuavcan/test/protocol/node_status_monitor.cpp index bfb97b0053..4f47b33252 100644 --- a/libuavcan/test/protocol/node_status_monitor.cpp +++ b/libuavcan/test/protocol/node_status_monitor.cpp @@ -10,32 +10,10 @@ static void publishNodeStatus(CanDriverMock& can, uavcan::NodeID node_id, uavcan::uint8_t status_code, uavcan::uint32_t uptime_sec, uavcan::TransferID tid) { - uavcan::StaticTransferBuffer<100> buffer; - uavcan::BitStream bitstream(buffer); - uavcan::ScalarCodec codec(bitstream); - uavcan::protocol::NodeStatus msg; msg.status_code = status_code; msg.uptime_sec = uptime_sec; - - // Manual message publication - ASSERT_LT(0, uavcan::protocol::NodeStatus::encode(msg, codec)); - ASSERT_GE(8, buffer.getMaxWritePos()); - - // DataTypeID data_type_id, TransferType transfer_type, NodeID src_node_id, NodeID dst_node_id, - // uint_fast8_t frame_index, TransferID transfer_id, bool last_frame - uavcan::Frame frame(uavcan::protocol::NodeStatus::DefaultDataTypeID, uavcan::TransferTypeMessageBroadcast, - node_id, uavcan::NodeID::Broadcast, 0, tid, true); - - ASSERT_EQ(buffer.getMaxWritePos(), frame.setPayload(buffer.getRawPtr(), buffer.getMaxWritePos())); - - uavcan::CanFrame can_frame; - ASSERT_TRUE(frame.compile(can_frame)); - - for (uint8_t i = 0; i < can.getNumIfaces(); i++) - { - can.ifaces.at(i).pushRx(can_frame); - } + emulateSingleFrameBroadcastTransfer(can, node_id, msg, tid); } diff --git a/libuavcan/test/protocol/node_status_provider.cpp b/libuavcan/test/protocol/node_status_provider.cpp index 31c4e5cc20..5c9feacdf6 100644 --- a/libuavcan/test/protocol/node_status_provider.cpp +++ b/libuavcan/test/protocol/node_status_provider.cpp @@ -102,14 +102,15 @@ TEST(NodeStatusProvider, Basic) ASSERT_TRUE(gni_cln.collector.result.get()); // Response must have been delivered ASSERT_TRUE(gni_cln.collector.result->isSuccessful()); - ASSERT_EQ(1, gni_cln.collector.result->server_node_id.get()); + ASSERT_EQ(1, gni_cln.collector.result->getCallID().server_node_id.get()); - ASSERT_EQ(uavcan::protocol::NodeStatus::STATUS_CRITICAL, gni_cln.collector.result->response.status.status_code); + ASSERT_EQ(uavcan::protocol::NodeStatus::STATUS_CRITICAL, + gni_cln.collector.result->getResponse().status.status_code); - ASSERT_TRUE(hwver == gni_cln.collector.result->response.hardware_version); - ASSERT_TRUE(swver == gni_cln.collector.result->response.software_version); + ASSERT_TRUE(hwver == gni_cln.collector.result->getResponse().hardware_version); + ASSERT_TRUE(swver == gni_cln.collector.result->getResponse().software_version); - ASSERT_EQ("superluminal_communication_unit", gni_cln.collector.result->response.name); + ASSERT_EQ("superluminal_communication_unit", gni_cln.collector.result->getResponse().name); /* * GlobalDiscoveryRequest diff --git a/libuavcan/test/protocol/param_server.cpp b/libuavcan/test/protocol/param_server.cpp index 245864a83f..bcc38e1f1d 100644 --- a/libuavcan/test/protocol/param_server.cpp +++ b/libuavcan/test/protocol/param_server.cpp @@ -113,16 +113,16 @@ TEST(ParamServer, Basic) save_erase_rq.opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_SAVE; doCall(save_erase_cln, save_erase_rq, nodes); ASSERT_TRUE(save_erase_cln.collector.result.get()); - ASSERT_TRUE(save_erase_cln.collector.result->response.ok); + ASSERT_TRUE(save_erase_cln.collector.result->getResponse().ok); save_erase_rq.opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_ERASE; doCall(save_erase_cln, save_erase_rq, nodes); - ASSERT_TRUE(save_erase_cln.collector.result->response.ok); + ASSERT_TRUE(save_erase_cln.collector.result->getResponse().ok); // Invalid opcode save_erase_rq.opcode = 0xFF; doCall(save_erase_cln, save_erase_rq, nodes); - ASSERT_FALSE(save_erase_cln.collector.result->response.ok); + ASSERT_FALSE(save_erase_cln.collector.result->getResponse().ok); /* * Get/set @@ -131,17 +131,17 @@ TEST(ParamServer, Basic) get_set_rq.name = "nonexistent_parameter"; doCall(get_set_cln, get_set_rq, nodes); ASSERT_TRUE(get_set_cln.collector.result.get()); - ASSERT_TRUE(get_set_cln.collector.result->response.name.empty()); + ASSERT_TRUE(get_set_cln.collector.result->getResponse().name.empty()); // No such variable, shall return empty name/value get_set_rq.index = 0; get_set_rq.name.clear(); get_set_rq.value.value_int.push_back(0xDEADBEEF); doCall(get_set_cln, get_set_rq, nodes); - ASSERT_TRUE(get_set_cln.collector.result->response.name.empty()); - ASSERT_TRUE(get_set_cln.collector.result->response.value.value_bool.empty()); - ASSERT_TRUE(get_set_cln.collector.result->response.value.value_int.empty()); - ASSERT_TRUE(get_set_cln.collector.result->response.value.value_float.empty()); + ASSERT_TRUE(get_set_cln.collector.result->getResponse().name.empty()); + ASSERT_TRUE(get_set_cln.collector.result->getResponse().value.value_bool.empty()); + ASSERT_TRUE(get_set_cln.collector.result->getResponse().value.value_int.empty()); + ASSERT_TRUE(get_set_cln.collector.result->getResponse().value.value_float.empty()); mgr.kv["foobar"] = 123.456; // New param @@ -149,10 +149,10 @@ TEST(ParamServer, Basic) get_set_rq = uavcan::protocol::param::GetSet::Request(); get_set_rq.name = "foobar"; doCall(get_set_cln, get_set_rq, nodes); - ASSERT_STREQ("foobar", get_set_cln.collector.result->response.name.c_str()); - ASSERT_TRUE(get_set_cln.collector.result->response.value.value_bool.empty()); - ASSERT_TRUE(get_set_cln.collector.result->response.value.value_int.empty()); - ASSERT_FLOAT_EQ(123.456F, get_set_cln.collector.result->response.value.value_float[0]); + ASSERT_STREQ("foobar", get_set_cln.collector.result->getResponse().name.c_str()); + ASSERT_TRUE(get_set_cln.collector.result->getResponse().value.value_bool.empty()); + ASSERT_TRUE(get_set_cln.collector.result->getResponse().value.value_int.empty()); + ASSERT_FLOAT_EQ(123.456F, get_set_cln.collector.result->getResponse().value.value_float[0]); // Set by index get_set_rq = uavcan::protocol::param::GetSet::Request(); @@ -163,13 +163,13 @@ TEST(ParamServer, Basic) get_set_rq.value.value_string.push_back(str); } doCall(get_set_cln, get_set_rq, nodes); - ASSERT_STREQ("foobar", get_set_cln.collector.result->response.name.c_str()); - ASSERT_FLOAT_EQ(424242, get_set_cln.collector.result->response.value.value_float[0]); + ASSERT_STREQ("foobar", get_set_cln.collector.result->getResponse().name.c_str()); + ASSERT_FLOAT_EQ(424242, get_set_cln.collector.result->getResponse().value.value_float[0]); // Get by index get_set_rq = uavcan::protocol::param::GetSet::Request(); get_set_rq.index = 0; doCall(get_set_cln, get_set_rq, nodes); - ASSERT_STREQ("foobar", get_set_cln.collector.result->response.name.c_str()); - ASSERT_FLOAT_EQ(424242, get_set_cln.collector.result->response.value.value_float[0]); + ASSERT_STREQ("foobar", get_set_cln.collector.result->getResponse().name.c_str()); + ASSERT_FLOAT_EQ(424242, get_set_cln.collector.result->getResponse().value.value_float[0]); } diff --git a/libuavcan/test/protocol/restart_request_server.cpp b/libuavcan/test/protocol/restart_request_server.cpp index 7b27621bf2..8229ed8ce6 100644 --- a/libuavcan/test/protocol/restart_request_server.cpp +++ b/libuavcan/test/protocol/restart_request_server.cpp @@ -44,7 +44,7 @@ TEST(RestartRequestServer, Basic) ASSERT_TRUE(rrs_cln.collector.result.get()); ASSERT_TRUE(rrs_cln.collector.result->isSuccessful()); - ASSERT_FALSE(rrs_cln.collector.result->response.ok); + ASSERT_FALSE(rrs_cln.collector.result->getResponse().ok); /* * Accepted @@ -57,7 +57,7 @@ TEST(RestartRequestServer, Basic) nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)); ASSERT_TRUE(rrs_cln.collector.result->isSuccessful()); - ASSERT_TRUE(rrs_cln.collector.result->response.ok); + ASSERT_TRUE(rrs_cln.collector.result->getResponse().ok); /* * Rejected by handler @@ -68,7 +68,7 @@ TEST(RestartRequestServer, Basic) nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)); ASSERT_TRUE(rrs_cln.collector.result->isSuccessful()); - ASSERT_FALSE(rrs_cln.collector.result->response.ok); + ASSERT_FALSE(rrs_cln.collector.result->getResponse().ok); /* * Rejected because of invalid magic number @@ -79,5 +79,5 @@ TEST(RestartRequestServer, Basic) nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)); ASSERT_TRUE(rrs_cln.collector.result->isSuccessful()); - ASSERT_FALSE(rrs_cln.collector.result->response.ok); + ASSERT_FALSE(rrs_cln.collector.result->getResponse().ok); } diff --git a/libuavcan/test/protocol/transport_stats_provider.cpp b/libuavcan/test/protocol/transport_stats_provider.cpp index e309977d2f..6c8ed9da69 100644 --- a/libuavcan/test/protocol/transport_stats_provider.cpp +++ b/libuavcan/test/protocol/transport_stats_provider.cpp @@ -28,13 +28,13 @@ TEST(TransportStatsProvider, Basic) ASSERT_TRUE(tsp_cln.collector.result.get()); ASSERT_TRUE(tsp_cln.collector.result->isSuccessful()); - ASSERT_EQ(0, tsp_cln.collector.result->response.transfer_errors); - ASSERT_EQ(1, tsp_cln.collector.result->response.transfers_rx); - ASSERT_EQ(0, tsp_cln.collector.result->response.transfers_tx); - ASSERT_EQ(1, tsp_cln.collector.result->response.can_iface_stats.size()); - ASSERT_EQ(0, tsp_cln.collector.result->response.can_iface_stats[0].errors); - ASSERT_EQ(1, tsp_cln.collector.result->response.can_iface_stats[0].frames_rx); - ASSERT_EQ(0, tsp_cln.collector.result->response.can_iface_stats[0].frames_tx); + ASSERT_EQ(0, tsp_cln.collector.result->getResponse().transfer_errors); + ASSERT_EQ(1, tsp_cln.collector.result->getResponse().transfers_rx); + ASSERT_EQ(0, tsp_cln.collector.result->getResponse().transfers_tx); + ASSERT_EQ(1, tsp_cln.collector.result->getResponse().can_iface_stats.size()); + ASSERT_EQ(0, tsp_cln.collector.result->getResponse().can_iface_stats[0].errors); + ASSERT_EQ(1, tsp_cln.collector.result->getResponse().can_iface_stats[0].frames_rx); + ASSERT_EQ(0, tsp_cln.collector.result->getResponse().can_iface_stats[0].frames_tx); /* * Second request @@ -43,13 +43,13 @@ TEST(TransportStatsProvider, Basic) ASSERT_LE(0, nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10))); ASSERT_TRUE(tsp_cln.collector.result.get()); - ASSERT_EQ(0, tsp_cln.collector.result->response.transfer_errors); - ASSERT_EQ(2, tsp_cln.collector.result->response.transfers_rx); - ASSERT_EQ(1, tsp_cln.collector.result->response.transfers_tx); - ASSERT_EQ(1, tsp_cln.collector.result->response.can_iface_stats.size()); - ASSERT_EQ(0, tsp_cln.collector.result->response.can_iface_stats[0].errors); - ASSERT_EQ(2, tsp_cln.collector.result->response.can_iface_stats[0].frames_rx); - ASSERT_EQ(6, tsp_cln.collector.result->response.can_iface_stats[0].frames_tx); + ASSERT_EQ(0, tsp_cln.collector.result->getResponse().transfer_errors); + ASSERT_EQ(2, tsp_cln.collector.result->getResponse().transfers_rx); + ASSERT_EQ(1, tsp_cln.collector.result->getResponse().transfers_tx); + ASSERT_EQ(1, tsp_cln.collector.result->getResponse().can_iface_stats.size()); + ASSERT_EQ(0, tsp_cln.collector.result->getResponse().can_iface_stats[0].errors); + ASSERT_EQ(2, tsp_cln.collector.result->getResponse().can_iface_stats[0].frames_rx); + ASSERT_EQ(6, tsp_cln.collector.result->getResponse().can_iface_stats[0].frames_tx); /* * Sending a malformed frame, it must be registered as tranfer error @@ -74,11 +74,11 @@ TEST(TransportStatsProvider, Basic) ASSERT_LE(0, nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10))); ASSERT_TRUE(tsp_cln.collector.result.get()); - ASSERT_EQ(1, tsp_cln.collector.result->response.transfer_errors); // That broken frame - ASSERT_EQ(3, tsp_cln.collector.result->response.transfers_rx); - ASSERT_EQ(2, tsp_cln.collector.result->response.transfers_tx); - ASSERT_EQ(1, tsp_cln.collector.result->response.can_iface_stats.size()); - ASSERT_EQ(72, tsp_cln.collector.result->response.can_iface_stats[0].errors); - ASSERT_EQ(4, tsp_cln.collector.result->response.can_iface_stats[0].frames_rx); // Same here - ASSERT_EQ(12, tsp_cln.collector.result->response.can_iface_stats[0].frames_tx); + ASSERT_EQ(1, tsp_cln.collector.result->getResponse().transfer_errors); // That broken frame + ASSERT_EQ(3, tsp_cln.collector.result->getResponse().transfers_rx); + ASSERT_EQ(2, tsp_cln.collector.result->getResponse().transfers_tx); + ASSERT_EQ(1, tsp_cln.collector.result->getResponse().can_iface_stats.size()); + ASSERT_EQ(72, tsp_cln.collector.result->getResponse().can_iface_stats[0].errors); + ASSERT_EQ(4, tsp_cln.collector.result->getResponse().can_iface_stats[0].frames_rx); // Same here + ASSERT_EQ(12, tsp_cln.collector.result->getResponse().can_iface_stats[0].frames_tx); } diff --git a/libuavcan/test/transport/can/can.hpp b/libuavcan/test/transport/can/can.hpp index 6557d16533..00a038df9e 100644 --- a/libuavcan/test/transport/can/can.hpp +++ b/libuavcan/test/transport/can/can.hpp @@ -169,6 +169,14 @@ public: , select_failure(false) { } + void pushRxToAllIfaces(const uavcan::CanFrame& can_frame) + { + for (uint8_t i = 0; i < getNumIfaces(); i++) + { + ifaces.at(i).pushRx(can_frame); + } + } + virtual uavcan::int16_t select(uavcan::CanSelectMasks& inout_masks, uavcan::MonotonicTime deadline) { assert(this); diff --git a/libuavcan/test/transport/dispatcher.cpp b/libuavcan/test/transport/dispatcher.cpp index 95bc5c359b..ce30738e21 100644 --- a/libuavcan/test/transport/dispatcher.cpp +++ b/libuavcan/test/transport/dispatcher.cpp @@ -163,7 +163,7 @@ TEST(Dispatcher, Reception) while (true) { - const int res = dispatcher.spin(tsMono(0)); + const int res = dispatcher.spinOnce(); ASSERT_LE(0, res); clockmock.advance(100); if (res == 0) @@ -298,7 +298,7 @@ TEST(Dispatcher, Spin) ASSERT_EQ(100, clockmock.monotonic); ASSERT_EQ(0, dispatcher.spin(tsMono(1000))); ASSERT_LE(1000, clockmock.monotonic); - ASSERT_EQ(0, dispatcher.spin(tsMono(0))); + ASSERT_EQ(0, dispatcher.spinOnce()); ASSERT_LE(1000, clockmock.monotonic); ASSERT_EQ(0, dispatcher.spin(tsMono(1100))); ASSERT_LE(1100, clockmock.monotonic); diff --git a/libuavcan/test/util/map.cpp b/libuavcan/test/util/map.cpp index 3f8671ede9..e394415b00 100644 --- a/libuavcan/test/util/map.cpp +++ b/libuavcan/test/util/map.cpp @@ -105,18 +105,18 @@ TEST(Map, Basic) ASSERT_EQ("D", *map->access("4")); // Finding some keys - ASSERT_EQ("1", *map->findFirstKey(KeyFindPredicate("1"))); - ASSERT_EQ("2", *map->findFirstKey(KeyFindPredicate("2"))); - ASSERT_EQ("3", *map->findFirstKey(KeyFindPredicate("3"))); - ASSERT_EQ("4", *map->findFirstKey(KeyFindPredicate("4"))); - ASSERT_FALSE(map->findFirstKey(KeyFindPredicate("nonexistent_key"))); + ASSERT_EQ("1", *map->find(KeyFindPredicate("1"))); + ASSERT_EQ("2", *map->find(KeyFindPredicate("2"))); + ASSERT_EQ("3", *map->find(KeyFindPredicate("3"))); + ASSERT_EQ("4", *map->find(KeyFindPredicate("4"))); + ASSERT_FALSE(map->find(KeyFindPredicate("nonexistent_key"))); // Finding some values - ASSERT_EQ("1", *map->findFirstKey(ValueFindPredicate("A"))); - ASSERT_EQ("2", *map->findFirstKey(ValueFindPredicate("B"))); - ASSERT_EQ("3", *map->findFirstKey(ValueFindPredicate("C"))); - ASSERT_EQ("4", *map->findFirstKey(ValueFindPredicate("D"))); - ASSERT_FALSE(map->findFirstKey(KeyFindPredicate("nonexistent_value"))); + ASSERT_EQ("1", *map->find(ValueFindPredicate("A"))); + ASSERT_EQ("2", *map->find(ValueFindPredicate("B"))); + ASSERT_EQ("3", *map->find(ValueFindPredicate("C"))); + ASSERT_EQ("4", *map->find(ValueFindPredicate("D"))); + ASSERT_FALSE(map->find(KeyFindPredicate("nonexistent_value"))); // Removing one static map->remove("1"); // One of dynamics now migrates to the static storage @@ -176,7 +176,7 @@ TEST(Map, Basic) // Removing odd values - nearly half of them ASSERT_EQ(2, map->getNumStaticPairs()); const unsigned num_dynamics_old = map->getNumDynamicPairs(); - map->removeWhere(oddValuePredicate); + map->removeAllWhere(oddValuePredicate); ASSERT_EQ(2, map->getNumStaticPairs()); const unsigned num_dynamics_new = map->getNumDynamicPairs(); std::cout << "Num of dynamic pairs reduced from " << num_dynamics_old << " to " << num_dynamics_new << std::endl; diff --git a/libuavcan/test/util/multiset.cpp b/libuavcan/test/util/multiset.cpp new file mode 100644 index 0000000000..8ff43e69b9 --- /dev/null +++ b/libuavcan/test/util/multiset.cpp @@ -0,0 +1,308 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include +#include + + +static std::string toString(long x) +{ + char buf[80]; + std::snprintf(buf, sizeof(buf), "%li", x); + return std::string(buf); +} + +static bool oddValuePredicate(const std::string& value) +{ + EXPECT_FALSE(value.empty()); + const int num = atoi(value.c_str()); + return num & 1; +} + +struct FindPredicate +{ + const std::string target; + FindPredicate(const std::string& target) : target(target) { } + bool operator()(const std::string& value) const { return value == target; } +}; + +struct NoncopyableWithCounter : uavcan::Noncopyable +{ + static int num_objects; + long long value; + + NoncopyableWithCounter() : value(0) { num_objects++; } + NoncopyableWithCounter(long long x) : value(x) { num_objects++; } + ~NoncopyableWithCounter() { num_objects--; } + + static bool isNegative(const NoncopyableWithCounter& val) + { + return val.value < 0; + } + + bool operator==(const NoncopyableWithCounter& ref) const { return ref.value == value; } +}; + +int NoncopyableWithCounter::num_objects = 0; + +template +struct SummationOperator : uavcan::Noncopyable +{ + T accumulator; + SummationOperator() : accumulator() { } + void operator()(const T& x) { accumulator += x; } +}; + +struct ClearingOperator +{ + template + void operator()(T& x) const { x = T(); } +}; + + +TEST(Multiset, Basic) +{ + using uavcan::Multiset; + + static const int POOL_BLOCKS = 3; + uavcan::PoolAllocator pool; + uavcan::PoolManager<2> poolmgr; + poolmgr.addPool(&pool); + + typedef Multiset MultisetType; + std::auto_ptr mset(new MultisetType(poolmgr)); + + typedef SummationOperator StringConcatenationOperator; + + // Empty + mset->removeFirst("foo"); + ASSERT_EQ(0, pool.getNumUsedBlocks()); + ASSERT_FALSE(mset->getByIndex(0)); + ASSERT_FALSE(mset->getByIndex(1)); + ASSERT_FALSE(mset->getByIndex(10000)); + + // Static addion + ASSERT_EQ("1", *mset->emplace("1")); + ASSERT_EQ("2", *mset->emplace("2")); + ASSERT_EQ(0, pool.getNumUsedBlocks()); + ASSERT_EQ(2, mset->getNumStaticItems()); + ASSERT_EQ(0, mset->getNumDynamicItems()); + + // Ordering + ASSERT_TRUE(*mset->getByIndex(0) == "1"); + ASSERT_TRUE(*mset->getByIndex(1) == "2"); + + { + StringConcatenationOperator op; + mset->forEach(op); + ASSERT_EQ("12", op.accumulator); + } + + // Dynamic addition + ASSERT_EQ("3", *mset->emplace("3")); + ASSERT_EQ("3", *mset->getByIndex(2)); + ASSERT_EQ(1, pool.getNumUsedBlocks()); + + ASSERT_EQ("4", *mset->emplace("4")); + ASSERT_LE(1, pool.getNumUsedBlocks()); // One or more + ASSERT_EQ(2, mset->getNumStaticItems()); + ASSERT_EQ(2, mset->getNumDynamicItems()); + + // Making sure everything is here + ASSERT_EQ("1", *mset->getByIndex(0)); + ASSERT_EQ("2", *mset->getByIndex(1)); + // 2 and 3 are not tested because their placement depends on number of items per dynamic block + ASSERT_FALSE(mset->getByIndex(100)); + ASSERT_FALSE(mset->getByIndex(4)); + + const std::string data_at_pos2 = *mset->getByIndex(2); + const std::string data_at_pos3 = *mset->getByIndex(3); + + // Finding some items + ASSERT_EQ("1", *mset->find(FindPredicate("1"))); + ASSERT_EQ("2", *mset->find(FindPredicate("2"))); + ASSERT_EQ("3", *mset->find(FindPredicate("3"))); + ASSERT_EQ("4", *mset->find(FindPredicate("4"))); + ASSERT_FALSE(mset->find(FindPredicate("nonexistent"))); + + { + StringConcatenationOperator op; + mset->forEach(op); + std::cout << "Accumulator: " << op.accumulator << std::endl; + ASSERT_EQ(4, op.accumulator.size()); + } + + // Removing one static; ordering will be preserved + mset->removeFirst("1"); + mset->removeFirst("foo"); // There's no such thing anyway + ASSERT_LE(1, pool.getNumUsedBlocks()); + ASSERT_EQ(1, mset->getNumStaticItems()); + ASSERT_EQ(2, mset->getNumDynamicItems()); // This container does not move items + + // Ordering has not changed + ASSERT_EQ("2", *mset->getByIndex(0)); // Entry "1" was here + ASSERT_EQ(data_at_pos2, *mset->getByIndex(1)); + ASSERT_EQ(data_at_pos3, *mset->getByIndex(2)); + + // Removing another static + mset->removeFirst("2"); + ASSERT_EQ(0, mset->getNumStaticItems()); + ASSERT_EQ(2, mset->getNumDynamicItems()); + ASSERT_LE(1, pool.getNumUsedBlocks()); + + // Adding some new items + unsigned max_value_integer = 0; + for (int i = 0; i < 100; i++) + { + const std::string value = toString(i); + std::string* res = mset->emplace(value); // Will NOT override above + if (res == NULL) + { + ASSERT_LT(2, i); + break; + } + else + { + ASSERT_EQ(value, *res); + } + max_value_integer = unsigned(i); + } + std::cout << "Max value: " << max_value_integer << std::endl; + + // Making sure there is true OOM + ASSERT_EQ(0, pool.getNumFreeBlocks()); + ASSERT_FALSE(mset->emplace("nonexistent")); + + // Removing odd values - nearly half of them + mset->removeAllWhere(oddValuePredicate); + + // Making sure there's no odd values left + for (unsigned kv_int = 0; kv_int <= max_value_integer; kv_int++) + { + const std::string* val = mset->find(FindPredicate(toString(kv_int))); + if (val) + { + ASSERT_FALSE(kv_int & 1); + } + else + { + ASSERT_TRUE(kv_int & 1); + } + } + + // Clearing all strings + { + StringConcatenationOperator op; + mset->forEach(op); + std::cout << "Accumulator before clearing: " << op.accumulator << std::endl; + } + mset->forEach(ClearingOperator()); + { + StringConcatenationOperator op; + mset->forEach(op); + std::cout << "Accumulator after clearing: " << op.accumulator << std::endl; + ASSERT_TRUE(op.accumulator.empty()); + } + + // Making sure the memory will be released + mset.reset(); + ASSERT_EQ(0, pool.getNumUsedBlocks()); +} + + +TEST(Multiset, PrimitiveKey) +{ + using uavcan::Multiset; + + static const int POOL_BLOCKS = 3; + uavcan::PoolAllocator pool; + uavcan::PoolManager<2> poolmgr; + poolmgr.addPool(&pool); + + typedef Multiset MultisetType; + std::auto_ptr mset(new MultisetType(poolmgr)); + + // Empty + mset->removeFirst(8); + ASSERT_EQ(0, pool.getNumUsedBlocks()); + ASSERT_EQ(0, mset->getSize()); + ASSERT_FALSE(mset->getByIndex(0)); + + // Insertion + ASSERT_EQ(1, *mset->emplace(1)); + ASSERT_EQ(1, mset->getSize()); + ASSERT_EQ(2, *mset->emplace(2)); + ASSERT_EQ(2, mset->getSize()); + ASSERT_EQ(3, *mset->emplace(3)); + ASSERT_EQ(4, *mset->emplace(4)); + ASSERT_EQ(4, mset->getSize()); + +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + // Only C++11 because C++03 uses one entry per pool block which breaks ordering + ASSERT_EQ(1, *mset->getByIndex(0)); + ASSERT_EQ(2, *mset->getByIndex(1)); + ASSERT_EQ(3, *mset->getByIndex(2)); + ASSERT_EQ(4, *mset->getByIndex(3)); + ASSERT_FALSE(mset->getByIndex(5)); + ASSERT_FALSE(mset->getByIndex(1000)); +#endif + + // Summation and clearing + { + SummationOperator summation_operator; + mset->forEach&>(summation_operator); + ASSERT_EQ(1 + 2 + 3 + 4, summation_operator.accumulator); + } + mset->forEach(ClearingOperator()); + { + SummationOperator summation_operator; + mset->forEach&>(summation_operator); + ASSERT_EQ(0, summation_operator.accumulator); + } +} + + +TEST(Multiset, NoncopyableWithCounter) +{ + using uavcan::Multiset; + + static const int POOL_BLOCKS = 3; + uavcan::PoolAllocator pool; + uavcan::PoolManager<2> poolmgr; + poolmgr.addPool(&pool); + + typedef Multiset MultisetType; + std::auto_ptr mset(new MultisetType(poolmgr)); + + ASSERT_EQ(0, NoncopyableWithCounter::num_objects); + ASSERT_EQ(0, mset->emplace()->value); + ASSERT_EQ(1, NoncopyableWithCounter::num_objects); + ASSERT_EQ(123, mset->emplace(123)->value); + ASSERT_EQ(2, NoncopyableWithCounter::num_objects); + ASSERT_EQ(-456, mset->emplace(-456)->value); + ASSERT_EQ(3, NoncopyableWithCounter::num_objects); + ASSERT_EQ(456, mset->emplace(456)->value); + ASSERT_EQ(4, NoncopyableWithCounter::num_objects); + ASSERT_EQ(-789, mset->emplace(-789)->value); + ASSERT_EQ(5, NoncopyableWithCounter::num_objects); + + mset->removeFirst(NoncopyableWithCounter(0)); + ASSERT_EQ(4, NoncopyableWithCounter::num_objects); + ASSERT_EQ(123, mset->getByIndex(0)->value); + + mset->removeFirstWhere(&NoncopyableWithCounter::isNegative); + ASSERT_EQ(3, NoncopyableWithCounter::num_objects); + ASSERT_EQ(456, mset->getByIndex(1)->value); // -456 is now removed + + mset->removeAllWhere(&NoncopyableWithCounter::isNegative); + ASSERT_EQ(2, NoncopyableWithCounter::num_objects); // Only 1 and 2 are left + + mset.reset(); + + ASSERT_EQ(0, pool.getNumUsedBlocks()); + ASSERT_EQ(0, NoncopyableWithCounter::num_objects); // All destroyed +} diff --git a/libuavcan_drivers/linux/CMakeLists.txt b/libuavcan_drivers/linux/CMakeLists.txt index a88c7da13a..b33af01b8e 100644 --- a/libuavcan_drivers/linux/CMakeLists.txt +++ b/libuavcan_drivers/linux/CMakeLists.txt @@ -25,6 +25,8 @@ if (TARGET uavcan) set(UAVCAN_LIB uavcan) include_directories(${libuavcan_SOURCE_DIR}/include ${libuavcan_SOURCE_DIR}/include/dsdlc_generated) + message(STATUS "POSIX source dir: ${libuavcan_posix_SOURCE_DIR}") + include_directories(${libuavcan_posix_SOURCE_DIR}/include) else () message(STATUS "Using installed uavcan library") find_library(UAVCAN_LIB uavcan REQUIRED) @@ -55,6 +57,12 @@ target_link_libraries(test_time_sync ${UAVCAN_LIB} rt ${CMAKE_THREAD_LIBS_INIT}) add_executable(test_system_utils apps/test_system_utils.cpp) target_link_libraries(test_system_utils ${UAVCAN_LIB} rt ${CMAKE_THREAD_LIBS_INIT}) +add_executable(test_posix apps/test_posix.cpp) +target_link_libraries(test_posix ${UAVCAN_LIB} rt ${CMAKE_THREAD_LIBS_INIT}) + +add_executable(test_dynamic_node_id_client apps/test_dynamic_node_id_client.cpp) +target_link_libraries(test_dynamic_node_id_client ${UAVCAN_LIB} rt ${CMAKE_THREAD_LIBS_INIT}) + # # Tools # @@ -64,7 +72,11 @@ target_link_libraries(uavcan_status_monitor ${UAVCAN_LIB} rt ${CMAKE_THREAD_LIBS add_executable(uavcan_nodetool apps/uavcan_nodetool.cpp) target_link_libraries(uavcan_nodetool ${UAVCAN_LIB} rt ${CMAKE_THREAD_LIBS_INIT}) +add_executable(uavcan_dynamic_node_id_server apps/uavcan_dynamic_node_id_server.cpp) +target_link_libraries(uavcan_dynamic_node_id_server ${UAVCAN_LIB} rt ${CMAKE_THREAD_LIBS_INIT}) + install(TARGETS uavcan_status_monitor uavcan_nodetool + uavcan_dynamic_node_id_server RUNTIME DESTINATION bin) \ No newline at end of file diff --git a/libuavcan_drivers/linux/apps/test_dynamic_node_id_client.cpp b/libuavcan_drivers/linux/apps/test_dynamic_node_id_client.cpp new file mode 100644 index 0000000000..8743577730 --- /dev/null +++ b/libuavcan_drivers/linux/apps/test_dynamic_node_id_client.cpp @@ -0,0 +1,121 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include "debug.hpp" +#include +#include + +namespace +{ + +uavcan_linux::NodePtr initNodeWithDynamicID(const std::vector& ifaces, + const std::uint8_t instance_id, + const uavcan::NodeID preferred_node_id, + const std::string& name) +{ + /* + * Initializing the node object + */ + auto node = uavcan_linux::makeNode(ifaces); + + node->setName(name.c_str()); + node->getLogger().setLevel(uavcan::protocol::debug::LogLevel::DEBUG); + + { + const auto app_id = uavcan_linux::makeApplicationID(uavcan_linux::MachineIDReader().read(), name, instance_id); + + uavcan::protocol::HardwareVersion hwver; + std::copy(app_id.begin(), app_id.end(), hwver.unique_id.begin()); + std::cout << hwver << std::endl; + + node->setHardwareVersion(hwver); + } + + /* + * Starting the node + */ + const int start_res = node->start(); + ENFORCE(0 == start_res); + + /* + * Running the dynamic node ID client until it's done + */ + uavcan::DynamicNodeIDClient client(*node); + + ENFORCE(0 <= client.start(node->getNodeStatusProvider().getHardwareVersion(), preferred_node_id)); + + std::cout << "Waiting for dynamic node ID allocation..." << std::endl; + + while (!client.isAllocationComplete()) + { + const int res = node->spin(uavcan::MonotonicDuration::fromMSec(100)); + if (res < 0) + { + std::cerr << "Spin error: " << res << std::endl; + } + } + + std::cout << "Node ID " << int(client.getAllocatedNodeID().get()) + << " allocated by " << int(client.getAllocatorNodeID().get()) << std::endl; + + /* + * Finishing the node initialization + */ + node->setNodeID(client.getAllocatedNodeID()); + + node->setStatusOk(); + + return node; +} + +void runForever(const uavcan_linux::NodePtr& node) +{ + while (true) + { + const int res = node->spin(uavcan::MonotonicDuration::fromMSec(100)); + if (res < 0) + { + std::cerr << "Spin error: " << res << std::endl; + } + } +} + +} + +int main(int argc, const char** argv) +{ + try + { + if (argc < 3) + { + std::cerr << "Usage:\n\t" + << argv[0] << " [can-iface-name-N...]\n" + << "Where is used to augment the unique node ID and also indicates\n" + << "the preferred node ID value. Valid range is [0, 127]." + << std::endl; + return 1; + } + + const int instance_id = std::stoi(argv[1]); + if (instance_id < 0 || instance_id > 127) + { + std::cerr << "Invalid instance ID: " << instance_id << std::endl; + std::exit(1); + } + + uavcan_linux::NodePtr node = initNodeWithDynamicID(std::vector(argv + 2, argv + argc), + std::uint8_t(instance_id), + std::uint8_t(instance_id), + "org.uavcan.linux_test_dynamic_node_id_client"); + runForever(node); + + return 0; + } + catch (const std::exception& ex) + { + std::cerr << "Error: " << ex.what() << std::endl; + return 1; + } +} diff --git a/libuavcan_drivers/linux/apps/test_posix.cpp b/libuavcan_drivers/linux/apps/test_posix.cpp new file mode 100644 index 0000000000..a59aa1c3ef --- /dev/null +++ b/libuavcan_drivers/linux/apps/test_posix.cpp @@ -0,0 +1,73 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include +#include +#include +#include "debug.hpp" + +int main(int argc, const char** argv) +{ + (void)argc; + (void)argv; + try + { + ENFORCE(0 == std::system("mkdir -p /tmp/uavcan_posix/dynamic_node_id_server")); + + /* + * Event tracer test + */ + { + using namespace uavcan::dynamic_node_id_server; + + const std::string event_log_file("/tmp/uavcan_posix/dynamic_node_id_server/event.log"); + + uavcan_posix::dynamic_node_id_server::FileEventTracer tracer; + ENFORCE(0 <= tracer.init(event_log_file.c_str())); + + // Adding a line + static_cast(tracer).onEvent(TraceError, 123456); + ENFORCE(0 == std::system(("cat " + event_log_file).c_str())); + + // Removing the log file + ENFORCE(0 == std::system(("rm -f " + event_log_file).c_str())); + + // Adding another line + static_cast(tracer).onEvent(TraceError, 789123); + ENFORCE(0 == std::system(("cat " + event_log_file).c_str())); + } + + /* + * Storage backend test + */ + { + using namespace uavcan::dynamic_node_id_server; + + uavcan_posix::dynamic_node_id_server::FileStorageBackend backend; + ENFORCE(0 <= backend.init("/tmp/uavcan_posix/dynamic_node_id_server/storage")); + + auto print_key = [&](const char* key) { + std::cout << static_cast(backend).get(key).c_str() << std::endl; + }; + + print_key("foobar"); + + static_cast(backend).set("foobar", "0123456789abcdef0123456789abcdef"); + static_cast(backend).set("the_answer", "42"); + + print_key("foobar"); + print_key("the_answer"); + print_key("nonexistent"); + } + + return 0; + } + catch (const std::exception& ex) + { + std::cerr << "Exception: " << ex.what() << std::endl; + return 1; + } +} diff --git a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp new file mode 100644 index 0000000000..39634e313a --- /dev/null +++ b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp @@ -0,0 +1,676 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "debug.hpp" +// UAVCAN +#include +// UAVCAN Linux drivers +#include +// UAVCAN POSIX drivers +#include +#include + +namespace +{ + +constexpr int MaxNumLastEvents = 30; +constexpr int MinUpdateInterval = 100; + +uavcan_linux::NodePtr initNode(const std::vector& ifaces, uavcan::NodeID nid, const std::string& name) +{ + auto node = uavcan_linux::makeNode(ifaces); + + node->setNodeID(nid); + node->setName(name.c_str()); + node->getLogger().setLevel(uavcan::protocol::debug::LogLevel::DEBUG); + + { + const auto app_id = uavcan_linux::makeApplicationID(uavcan_linux::MachineIDReader().read(), name, nid.get()); + + uavcan::protocol::HardwareVersion hwver; + std::copy(app_id.begin(), app_id.end(), hwver.unique_id.begin()); + std::cout << hwver << std::endl; + + node->setHardwareVersion(hwver); + } + + const int start_res = node->start(); + ENFORCE(0 == start_res); + + uavcan::NetworkCompatibilityCheckResult check_result; + ENFORCE(0 == node->checkNetworkCompatibility(check_result)); + if (!check_result.isOk()) + { + throw std::runtime_error("Network conflict with node " + std::to_string(check_result.conflicting_node.get())); + } + + node->setStatusOk(); + + return node; +} + + +class EventTracer : public uavcan_posix::dynamic_node_id_server::FileEventTracer +{ +public: + struct RecentEvent + { + const uavcan::MonotonicDuration time_since_startup; + const uavcan::UtcTime utc_timestamp; + const uavcan::dynamic_node_id_server::TraceCode code; + const std::int64_t argument; + + RecentEvent(uavcan::MonotonicDuration arg_time_since_startup, + uavcan::UtcTime arg_utc_timestamp, + uavcan::dynamic_node_id_server::TraceCode arg_code, + std::int64_t arg_argument) + : time_since_startup(arg_time_since_startup) + , utc_timestamp(arg_utc_timestamp) + , code(arg_code) + , argument(arg_argument) + { } + + uavcan::MakeString<81>::Type toString() const // Heapless return + { + char timebuf[11] = { }; + { + const std::time_t rawtime = utc_timestamp.toUSec() * 1e-6; + const auto tm = localtime(&rawtime); + std::strftime(timebuf, 10, "%H:%M:%S.", tm); + std::snprintf(&timebuf[9], 3, "%02u", static_cast((utc_timestamp.toMSec() % 1000U) / 10U)); + } + + decltype(toString()) out; + out.resize(out.capacity()); + (void)std::snprintf(reinterpret_cast(out.begin()), out.size() - 1U, + "%-11s %-28s %-20lld %016llx", + timebuf, + getEventName(code), + static_cast(argument), + static_cast(argument)); + return out; + } + + static const char* getTableHeader() + { + // Matches the string format above + return "Timestamp Event name Argument (dec) Argument (hex)"; + } + }; + + struct EventStatisticsRecord + { + std::uint64_t count; + uavcan::MonotonicTime last_occurence; + + EventStatisticsRecord() + : count(0) + { } + + void hit(uavcan::MonotonicTime ts) + { + count++; + last_occurence = ts; + } + }; + +private: + struct EnumKeyHash + { + template + std::size_t operator()(T t) const { return static_cast(t); } + }; + + uavcan_linux::SystemClock clock_; + const uavcan::MonotonicTime started_at_ = clock_.getMonotonic(); + const unsigned num_last_events_; + + std::deque last_events_; + std::unordered_map event_counters_; + + bool had_events_ = false; + + virtual void onEvent(uavcan::dynamic_node_id_server::TraceCode code, std::int64_t argument) + { + uavcan_posix::dynamic_node_id_server::FileEventTracer::onEvent(code, argument); + + had_events_ = true; + + const auto ts_m = clock_.getMonotonic(); + const auto ts_utc = clock_.getUtc(); + const auto time_since_startup = ts_m - started_at_; + + last_events_.emplace_front(time_since_startup, ts_utc, code, argument); + if (last_events_.size() > num_last_events_) + { + last_events_.pop_back(); + } + + event_counters_[code].hit(ts_m); + } + +public: + EventTracer(unsigned num_last_events_to_keep) + : num_last_events_(num_last_events_to_keep) + { } + + using uavcan_posix::dynamic_node_id_server::FileEventTracer::init; + + const RecentEvent& getEventByIndex(unsigned index) const { return last_events_.at(index); } + + unsigned getNumEvents() const { return last_events_.size(); } + + const decltype(event_counters_)& getEventCounters() const { return event_counters_; } + + bool hadEvents() + { + if (had_events_) + { + had_events_ = false; + return true; + } + return false; + } +}; + + +::winsize getTerminalSize() +{ + auto w = ::winsize(); + ENFORCE(0 >= ioctl(STDOUT_FILENO, TIOCGWINSZ, &w)); + ENFORCE(w.ws_col > 0 && w.ws_row > 0); + return w; +} + + +std::vector> +collectRelevantEvents(const EventTracer& event_tracer, const unsigned num_events) +{ + // First, creating a vector of pairs (event code, count) + typedef std::pair Pair; + const auto counters = event_tracer.getEventCounters(); + std::vector pairs(counters.size()); + std::copy(counters.begin(), counters.end(), pairs.begin()); + + // Now, sorting the pairs so that the most recent ones are on top of the list + std::sort(pairs.begin(), pairs.end(), [](const Pair& a, const Pair& b) { + return a.second.last_occurence > b.second.last_occurence; + }); + + // Cutting the oldest events away + pairs.resize(std::min(num_events, unsigned(pairs.size()))); + + // Sorting so that the most frequent events are on top of the list + std::stable_sort(pairs.begin(), pairs.end(), [](const Pair& a, const Pair& b) { + return a.second.count > b.second.count; + }); + + return pairs; +} + +enum class CLIColor : unsigned +{ + Red = 31, + Green = 32, + Yellow = 33, + Blue = 34, + Magenta = 35, + Cyan = 36, + White = 37, + Default = 39 +}; + +CLIColor getColorHash(unsigned value) { return CLIColor(31 + value % 7); } + +class CLIColorizer +{ + const CLIColor color_; +public: + explicit CLIColorizer(CLIColor c) : color_(c) + { + std::printf("\033[%um", static_cast(color_)); + } + + ~CLIColorizer() + { + std::printf("\033[%um", static_cast(CLIColor::Default)); + } +}; + + +void redraw(const uavcan_linux::NodePtr& node, + const uavcan::MonotonicTime timestamp, + const EventTracer& event_tracer, + const uavcan::dynamic_node_id_server::DistributedServer& server) +{ + using uavcan::dynamic_node_id_server::distributed::RaftCore; + + /* + * Constants that are permanent for the designed UI layout + */ + constexpr unsigned NumRelevantEvents = 16; + constexpr unsigned NumRowsWithoutEvents = 3; + + /* + * Collecting the data + */ + const unsigned num_rows = getTerminalSize().ws_row; + + const auto relevant_events = collectRelevantEvents(event_tracer, NumRelevantEvents); + + const uavcan::dynamic_node_id_server::distributed::StateReport report(server); + + const auto time_since_last_activity = timestamp - report.last_activity_timestamp; + + /* + * Basic rendering functions + */ + unsigned next_relevant_event_index = 0; + + const auto render_next_event_counter = [&]() + { + const char* event_name = ""; + char event_count_str[10] = { }; + CLIColor event_color = CLIColor::Default; + + if (next_relevant_event_index < relevant_events.size()) + { + const auto e = relevant_events[next_relevant_event_index]; + event_name = uavcan::dynamic_node_id_server::IEventTracer::getEventName(e.first); + std::snprintf(event_count_str, sizeof(event_count_str) - 1U, "%llu", + static_cast(e.second.count)); + event_color = getColorHash(static_cast(e.first)); + } + next_relevant_event_index++; + + std::printf(" | "); + CLIColorizer izer(event_color); + std::printf("%-29s %-9s\n", event_name, event_count_str); + }; + + const auto render_top_str = [&](const char* local_state_name, const char* local_state_value, CLIColor color) + { + { + CLIColorizer izer(color); + std::printf("%-20s %-16s", local_state_name, local_state_value); + } + render_next_event_counter(); + }; + + const auto render_top_int = [&](const char* local_state_name, long long local_state_value, CLIColor color) + { + char buf[21]; + std::snprintf(buf, sizeof(buf) - 1U, "%lld", local_state_value); + render_top_str(local_state_name, buf, color); + }; + + const auto raft_state_to_string = [](uavcan::dynamic_node_id_server::distributed::RaftCore::ServerState s) + { + switch (s) + { + case RaftCore::ServerStateFollower: return "Follower"; + case RaftCore::ServerStateCandidate: return "Candidate"; + case RaftCore::ServerStateLeader: return "Leader"; + default: return "BADSTATE"; + } + }; + + const auto duration_to_string = [](uavcan::MonotonicDuration dur) + { + uavcan::MakeString<16>::Type str; // This is much faster than std::string + str.appendFormatted("%.1f", dur.toUSec() / 1e6); + return str; + }; + + const auto colorize_if = [](bool condition, CLIColor color) + { + return condition ? color : CLIColor::Default; + }; + + /* + * Rendering the data to the CLI + */ + std::printf("\x1b[1J"); // Clear screen from the current cursor position to the beginning + std::printf("\x1b[H"); // Move cursor to the coordinates 1,1 + + // Local state and relevant event counters - two columns + std::printf(" Local state | Event counters\n"); + + render_top_int("Node ID", + node->getNodeID().get(), + CLIColor::Default); + + render_top_str("State", + raft_state_to_string(report.state), + (report.state == RaftCore::ServerStateCandidate) ? CLIColor::Magenta : + (report.state == RaftCore::ServerStateLeader) ? CLIColor::Green : + CLIColor::Default); + + render_top_int("Last log index", + report.last_log_index, + CLIColor::Default); + + render_top_int("Commit index", + report.commit_index, + colorize_if(report.commit_index != report.last_log_index, CLIColor::Magenta)); + + render_top_int("Last log term", + report.last_log_term, + CLIColor::Default); + + render_top_int("Current term", + report.current_term, + CLIColor::Default); + + render_top_int("Voted for", + report.voted_for.get(), + CLIColor::Default); + + render_top_str("Since activity", + duration_to_string(time_since_last_activity).c_str(), + CLIColor::Default); + + render_top_str("Random timeout", + duration_to_string(report.randomized_timeout).c_str(), + CLIColor::Default); + + render_top_int("Unknown nodes", + report.num_unknown_nodes, + colorize_if(report.num_unknown_nodes != 0, CLIColor::Magenta)); + + render_top_int("Node failures", + node->getInternalFailureCount(), + colorize_if(node->getInternalFailureCount() != 0, CLIColor::Magenta)); + + // Empty line before the next block + std::printf(" "); + render_next_event_counter(); + + // Followers block + std::printf(" Followers "); + render_next_event_counter(); + + const auto render_followers_state = [&](const char* name, + const std::function value_getter, + const std::function color_getter) + { + std::printf("%-17s", name); + for (std::uint8_t i = 0; i < 4; i++) + { + if (i < (report.cluster_size - 1)) + { + CLIColorizer colorizer(color_getter(i)); + const auto value = value_getter(i); + if (value >= 0) + { + std::printf("%-5d", value); + } + else + { + std::printf("N/A "); + } + } + else + { + std::printf(" "); + } + } + render_next_event_counter(); + }; + + const auto follower_color_getter = [&](std::uint8_t i) + { + if (report.state != RaftCore::ServerStateLeader) { return CLIColor::Default; } + if (!report.followers[i].node_id.isValid()) { return CLIColor::Red; } + if (report.followers[i].match_index != report.last_log_index || + report.followers[i].next_index <= report.last_log_index) + { + return CLIColor::Magenta; + } + return CLIColor::Default; + }; + + render_followers_state("Node ID", [&](std::uint8_t i) + { + const auto nid = report.followers[i].node_id; + return nid.isValid() ? nid.get() : -1; + }, + follower_color_getter); + + render_followers_state("Next index", + [&](std::uint8_t i) { return report.followers[i].next_index; }, + follower_color_getter); + + render_followers_state("Match index", + [&](std::uint8_t i) { return report.followers[i].match_index; }, + follower_color_getter); + + assert(next_relevant_event_index == NumRelevantEvents); // Ensuring that all events can be printed + + // Separator + std::printf("--------------------------------------+----------------------------------------\n"); + + // Event log + std::printf("%s\n", EventTracer::RecentEvent::getTableHeader()); + const int num_events_to_render = static_cast(num_rows) - + static_cast(next_relevant_event_index) - + static_cast(NumRowsWithoutEvents) - + 1; // This allows to keep the last line empty for stdout or UAVCAN_TRACE() + for (int i = 0; + i < num_events_to_render && i < static_cast(event_tracer.getNumEvents()); + i++) + { + const auto e = event_tracer.getEventByIndex(i); + CLIColorizer colorizer(getColorHash(static_cast(e.code))); + std::printf("%s\n", e.toString().c_str()); + } + + std::fflush(stdout); +} + + +void runForever(const uavcan_linux::NodePtr& node, + const std::uint8_t cluster_size, + const std::string& event_log_file, + const std::string& persistent_storage_path) +{ + /* + * Event tracer + */ + EventTracer event_tracer(MaxNumLastEvents); + ENFORCE(0 <= event_tracer.init(event_log_file.c_str())); + + /* + * Storage backend + */ + uavcan_posix::dynamic_node_id_server::FileStorageBackend storage_backend; + ENFORCE(0 <= storage_backend.init(persistent_storage_path.c_str())); + + /* + * Server + */ + uavcan::dynamic_node_id_server::DistributedServer server(*node, storage_backend, event_tracer); + + const int server_init_res = server.init(node->getNodeStatusProvider().getHardwareVersion().unique_id, cluster_size); + if (server_init_res < 0) + { + throw std::runtime_error("Failed to start the server; error " + std::to_string(server_init_res)); + } + + /* + * Preparing the CLI + */ + std::printf("\x1b[2J"); // Clear entire screen; this will preserve initialization output in the scrollback + + /* + * Spinning the node + */ + uavcan::MonotonicTime last_redraw_at; + + while (true) + { + const int res = node->spin(uavcan::MonotonicDuration::fromMSec(MinUpdateInterval)); + if (res < 0) + { + std::cerr << "Spin error: " << res << std::endl; + } + + const auto ts = node->getMonotonicTime(); + + if (event_tracer.hadEvents() || (ts - last_redraw_at).toMSec() > 1000) + { + last_redraw_at = ts; + redraw(node, ts, event_tracer, server); + } + } +} + +struct Options +{ + uavcan::NodeID node_id; + std::vector ifaces; + std::uint8_t cluster_size = 0; + std::string storage_path; +}; + +Options parseOptions(int argc, const char** argv) +{ + const char* const executable_name = *argv++; + argc--; + + const auto enforce = [executable_name](bool condition, const char* error_text) { + if (!condition) + { + std::cerr << error_text << "\n" + << "Usage:\n\t" + << executable_name + << " [can-iface-name-N...] [-c ] -s ]" + << std::endl; + std::exit(1); + } + }; + + enforce(argc >= 3, "Not enough arguments"); + + /* + * Node ID is always at the first position + */ + argc--; + const int node_id = std::stoi(*argv++); + enforce(node_id >= 1 && node_id <= 127, "Invalid node ID"); + + Options out; + out.node_id = uavcan::NodeID(std::uint8_t(node_id)); + + while (argc --> 0) + { + const std::string token(*argv++); + + if (token[0] != '-') + { + out.ifaces.push_back(token); + } + else if (token[1] == 'c') + { + int cluster_size = 0; + if (token.length() > 2) // -c2 + { + cluster_size = std::stoi(token.c_str() + 2); + } + else // -c 2 + { + enforce(argc --> 0, "Expected cluster size"); + cluster_size = std::stoi(*argv++); + } + enforce(cluster_size >= 1 && + cluster_size <= uavcan::dynamic_node_id_server::distributed::ClusterManager::MaxClusterSize, + "Invalid cluster size"); + out.cluster_size = std::uint8_t(cluster_size); + } + else if (token[1] == 's') + { + if (token.length() > 2) // -s/foo/bar + { + out.storage_path = token.c_str() + 2; + } + else // -s /foo/bar + { + enforce(argc --> 0, "Expected storage path"); + out.storage_path = *argv++; + } + } + else + { + enforce(false, "Unexpected argument"); + } + } + + enforce(!out.storage_path.empty(), "Invalid storage path"); + + return out; +} + +} + +int main(int argc, const char** argv) +{ + try + { + std::srand(std::time(nullptr)); + + if (isatty(STDOUT_FILENO) != 1) + { + std::cerr << "This application cannot run if stdout is not associated with a terminal" << std::endl; + std::exit(1); + } + + auto options = parseOptions(argc, argv); + + std::cout << "Self node ID: " << int(options.node_id.get()) << "\n" + "Cluster size: " << int(options.cluster_size) << "\n" + "Storage path: " << options.storage_path << "\n" + "Num ifaces: " << options.ifaces.size() << "\n" +#ifdef NDEBUG + "Build mode: Release" +#else + "Build mode: Debug" +#endif + << std::endl; + + /* + * Preparing the storage directory + */ + options.storage_path += "/node_" + std::to_string(options.node_id.get()); + + (void)std::system(("mkdir -p '" + options.storage_path + "' &>/dev/null").c_str()); + + const auto event_log_file = options.storage_path + "/events.log"; + const auto storage_path = options.storage_path + "/storage/"; + + /* + * Starting the node + */ + auto node = initNode(options.ifaces, options.node_id, "org.uavcan.linux_app.dynamic_node_id_server"); + runForever(node, options.cluster_size, event_log_file, storage_path); + return 0; + } + catch (const std::exception& ex) + { + std::cerr << "Error: " << ex.what() << std::endl; + return 1; + } +} diff --git a/libuavcan_drivers/linux/apps/uavcan_status_monitor.cpp b/libuavcan_drivers/linux/apps/uavcan_status_monitor.cpp index 540c68dce6..db05275a54 100644 --- a/libuavcan_drivers/linux/apps/uavcan_status_monitor.cpp +++ b/libuavcan_drivers/linux/apps/uavcan_status_monitor.cpp @@ -94,7 +94,8 @@ class Monitor : public uavcan::NodeStatusMonitor void redraw(const uavcan::TimerEvent&) { - std::cout << "\x1b\x5b\x48" << "\x1b\x5b\x32\x4a" + std::cout << "\x1b[1J" // Clear screen from the current cursor position to the beginning + << "\x1b[H" // Move cursor to the coordinates 1,1 << " NID | Status | Uptime (sec) | Vendor-specific status (hex/bin/dec)\n" << "-----+---------------+--------------+--------------------------------------\n"; for (unsigned i = 1; i <= uavcan::NodeID::Max; i++) diff --git a/libuavcan_drivers/linux/include/uavcan_linux/helpers.hpp b/libuavcan_drivers/linux/include/uavcan_linux/helpers.hpp index 608330fb48..d27cad4833 100644 --- a/libuavcan_drivers/linux/include/uavcan_linux/helpers.hpp +++ b/libuavcan_drivers/linux/include/uavcan_linux/helpers.hpp @@ -42,7 +42,7 @@ class BlockingServiceClient : public uavcan::ServiceClient void callback(const uavcan::ServiceCallResult& res) { - response_ = res.response; + response_ = res.getResponse(); call_was_successful_ = res.isSuccessful(); } @@ -85,7 +85,7 @@ public: const int call_res = Super::call(server_node_id, request); if (call_res >= 0) { - while (Super::isPending()) + while (Super::hasPendingCalls()) { const int spin_res = Super::getNode().spin(SpinDuration); if (spin_res < 0) diff --git a/libuavcan_drivers/linux/include/uavcan_linux/system_utils.hpp b/libuavcan_drivers/linux/include/uavcan_linux/system_utils.hpp index 14aa91ad63..b4d6ce0a5c 100644 --- a/libuavcan_drivers/linux/include/uavcan_linux/system_utils.hpp +++ b/libuavcan_drivers/linux/include/uavcan_linux/system_utils.hpp @@ -14,6 +14,7 @@ #include #include #include +#include namespace uavcan_linux { @@ -132,5 +133,45 @@ public: } }; +/** + * This class computes unique ID for a UAVCAN node in a Linux application. + * It takes the following inputs: + * - Unique machine ID + * - Node name string (e.g. "org.uavcan.linux_app.dynamic_node_id_server") + * - Instance ID byte, e.g. node ID + */ +std::array makeApplicationID(const MachineIDReader::MachineID& machine_id, + const std::string& node_name, + const std::uint8_t instance_id) +{ + union HalfID + { + std::uint64_t num; + std::uint8_t bytes[8]; + + HalfID(std::uint64_t arg_num) : num(arg_num) { } + }; + + std::array out; + + // First 8 bytes of the application ID are CRC64 of the machine ID in native byte order + { + uavcan::DataTypeSignatureCRC crc; + crc.add(machine_id.data(), static_cast(machine_id.size())); + HalfID half(crc.get()); + std::copy_n(half.bytes, 8, out.begin()); + } + + // Last 8 bytes of the application ID are CRC64 of the node name and optionally node ID + { + uavcan::DataTypeSignatureCRC crc; + crc.add(reinterpret_cast(node_name.c_str()), static_cast(node_name.length())); + crc.add(instance_id); + HalfID half(crc.get()); + std::copy_n(half.bytes, 8, out.begin() + 8); + } + + return out; +} } diff --git a/libuavcan_drivers/posix/CMakeLists.txt b/libuavcan_drivers/posix/CMakeLists.txt new file mode 100644 index 0000000000..49fbbfb8e1 --- /dev/null +++ b/libuavcan_drivers/posix/CMakeLists.txt @@ -0,0 +1,12 @@ +# +# Copyright (C) 2015 Pavel Kirienko +# + +cmake_minimum_required(VERSION 2.8) + +project(libuavcan_posix) + +# +# Library (header only) +# +install(DIRECTORY include/uavcan_posix DESTINATION include) diff --git a/libuavcan_drivers/posix_tools/include.mk b/libuavcan_drivers/posix/include.mk similarity index 100% rename from libuavcan_drivers/posix_tools/include.mk rename to libuavcan_drivers/posix/include.mk diff --git a/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp b/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp new file mode 100644 index 0000000000..32226c3b38 --- /dev/null +++ b/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp @@ -0,0 +1,154 @@ +/**************************************************************************** +* +* Copyright (c) 2015 PX4 Development Team. All rights reserved. +* Author: Pavel Kirienko +* David Sidrane +* +****************************************************************************/ + +#ifndef UAVCAN_POSIX_BASIC_FILE_SERVER_BACKEND_HPP_INCLUDED +#define UAVCAN_POSIX_BASIC_FILE_SERVER_BACKEND_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace uavcan_posix +{ +/** + * This interface implements a POSIX compliant IFileServerBackend interface + */ +class BasicFileSeverBackend : public uavcan::IFileServerBackend +{ + enum { FilePermissions = 438 }; ///< 0o666 + +protected: + /** + * Back-end for uavcan.protocol.file.GetInfo. + * Implementation of this method is required. + * On success the method must return zero. + */ + virtual int16_t getInfo(const Path& path, uint64_t& out_crc64, uint32_t& out_size, EntryType& out_type) + { + int rv = uavcan::protocol::file::Error::INVALID_VALUE; + FileCRC crc; + if (path.size() > 0) + { + using namespace std; + out_size = 0; + out_crc64 = 0; + + rv = -ENOENT; + uint8_t buffer[512]; + + int fd = ::open(path.c_str(), O_RDONLY); + + if (fd >= 0) + { + int len = 0; + + do + { + + len = ::read(fd, buffer, sizeof(buffer)); + + if (len > 0) + { + + out_size += len; + crc.add(buffer, len); + + } + else if (len < 0) + { + rv = EIO; + goto out_close; + } + + } + while(len > 0); + + out_crc64 = crc.get(); + + // We can assume the path is to a file and the file is readable. + out_type.flags = uavcan::protocol::file::EntryType::FLAG_READABLE | + uavcan::protocol::file::EntryType::FLAG_FILE; + + // TODO Using fixed flag FLAG_READABLE until we add file permission checks to return actual value. + // TODO Check whether the object pointed by path is a file or a directory + // On could ad call to stat() to determine if the path is to a file or a directory but the + // what are the return parameters in this case? + + rv = 0; + out_close: + close(fd); + } + } + return rv; + } + + /** + * Back-end 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 uint32_t offset, uint8_t* out_buffer, uint16_t& inout_size) + { + int rv = uavcan::protocol::file::Error::INVALID_VALUE; + + if (path.size() > 0) + { + int fd = open(path.c_str(), O_RDONLY); + + if (fd < 0) + { + rv = errno; + } + else + { + if (::lseek(fd, offset, SEEK_SET) < 0) + { + rv = errno; + } + else + { + // TODO use a read at offset to fill on EAGAIN + ssize_t len = ::read(fd, out_buffer, inout_size); + + if (len < 0) + { + rv = errno; + } + else + { + + inout_size = len; + rv = 0; + } + } + (void)close(fd); + } + } + return rv; + } + +}; + +} + +#endif // Include guard diff --git a/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp new file mode 100644 index 0000000000..45a6d11c12 --- /dev/null +++ b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp @@ -0,0 +1,89 @@ +/**************************************************************************** +* +* Copyright (c) 2015 PX4 Development Team. All rights reserved. +* Author: Pavel Kirienko +* David Sidrane +* +****************************************************************************/ + +#ifndef UAVCAN_POSIX_DYNAMIC_NODE_ID_SERVER_FILE_EVENT_TRACER_HPP_INCLUDED +#define UAVCAN_POSIX_DYNAMIC_NODE_ID_SERVER_FILE_EVENT_TRACER_HPP_INCLUDED + +#include +#include +#include +#include +#include + +namespace uavcan_posix +{ +namespace dynamic_node_id_server +{ +/** + * This interface implements a POSIX compliant file based IEventTracer interface + */ +class FileEventTracer : public uavcan::dynamic_node_id_server::IEventTracer +{ + /** + * Maximum length of full path to log file + */ + enum { MaxPathLength = 128 }; + + enum { FilePermissions = 438 }; ///< 0o666 + + /** + * This type is used for the path + */ + typedef uavcan::MakeString::Type PathString; + + PathString path_; + +protected: + virtual void onEvent(uavcan::dynamic_node_id_server::TraceCode code, uavcan::int64_t argument) + { + using namespace std; + + timespec ts = timespec(); // If clock_gettime() fails, zero time will be used + (void)clock_gettime(CLOCK_REALTIME, &ts); + + int fd = open(path_.c_str(), O_WRONLY | O_CREAT | O_APPEND, FilePermissions); + if (fd >= 0) + { + const int FormatBufferLength = 63; + char buffer[FormatBufferLength + 1]; + int n = snprintf(buffer, FormatBufferLength, "%ld.%06ld\t%d\t%lld\n", + static_cast(ts.tv_sec), static_cast(ts.tv_nsec / 1000L), + static_cast(code), static_cast(argument)); + (void)write(fd, buffer, n); // TODO FIXME Write loop + (void)close(fd); + } + } + +public: + /** + * Initializes the file based event tracer. + */ + int init(const PathString & path) + { + using namespace std; + + int rv = -uavcan::ErrInvalidParam; + + if (path.size() > 0) + { + rv = 0; + path_ = path.c_str(); + int fd = open(path_.c_str(), O_RDWR | O_CREAT | O_TRUNC, FilePermissions); + if (fd >= 0) + { + (void)close(fd); + } + } + return rv; + } +}; + +} +} + +#endif // Include guard diff --git a/libuavcan_drivers/posix_tools/include/posix_tools/file_storage_backend.hpp b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp similarity index 77% rename from libuavcan_drivers/posix_tools/include/posix_tools/file_storage_backend.hpp rename to libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp index e5233420cf..76c515a8cb 100644 --- a/libuavcan_drivers/posix_tools/include/posix_tools/file_storage_backend.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp @@ -6,15 +6,17 @@ * ****************************************************************************/ -#pragma once +#ifndef UAVCAN_POSIX_DYNAMIC_NODE_ID_SERVER_FILE_STORAGE_BACKEND_HPP_INCLUDED +#define UAVCAN_POSIX_DYNAMIC_NODE_ID_SERVER_FILE_STORAGE_BACKEND_HPP_INCLUDED #include -#include +#include #include #include -#include #include #include +#include +#include #include @@ -30,24 +32,18 @@ class FileStorageBackend : public uavcan::dynamic_node_id_server::IStorageBacken /** * Maximum length of full path including / and key max */ - enum { MaxPathLength = 128 }; + enum { FilePermissions = 438 }; ///< 0o666 /** * This type is used for the path */ - typedef uavcan::Array, - uavcan::ArrayModeDynamic, MaxPathLength> PathString; - + typedef uavcan::MakeString::Type PathString; PathString base_path; -public: - - FileStorageBackend() { } - - +protected: virtual String get(const String& key) const { using namespace std; @@ -58,9 +54,9 @@ public: if (fd >= 0) { char buffer[MaxStringLength + 1]; - memset(buffer, 0, sizeof(buffer)); + (void)memset(buffer, 0, sizeof(buffer)); int len = read(fd, buffer, MaxStringLength); - close(fd); + (void)close(fd); if (len > 0) { for (int i = 0; i < len; i++) @@ -75,7 +71,6 @@ public: } } return value; - } virtual void set(const String& key, const String& value) @@ -83,24 +78,24 @@ public: using namespace std; PathString path = base_path.c_str(); path += key; - int fd = open(path.c_str(), O_WRONLY | O_CREAT | O_TRUNC); + int fd = open(path.c_str(), O_WRONLY | O_CREAT | O_TRUNC, FilePermissions); if (fd >= 0) { - write(fd, value.c_str(), value.size()); - close(fd); + (void)write(fd, value.c_str(), value.size()); // TODO FIXME Write loop + (void)fsync(fd); + (void)close(fd); } } +public: /** - * Initializes the File based back end storage by passing to a path to + * Initializes the file based backend storage by passing a path to * the directory where the key named files will be stored. - * This the return result should be 0 on success. + * The return value should be 0 on success. * If it is -ErrInvalidConfiguration then the the path name is too long to - * Accommodate the trailing slash and max key length; - * + * accommodate the trailing slash and max key length. */ - - int init(const PathString & path) + int init(const PathString& path) { using namespace std; @@ -108,7 +103,6 @@ public: if (path.size() > 0) { - base_path = path.c_str(); if (base_path.back() == '/') @@ -132,10 +126,10 @@ public: } } return rv; - } - }; } } + +#endif // Include guard diff --git a/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp b/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp new file mode 100644 index 0000000000..d38bf1a1f2 --- /dev/null +++ b/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp @@ -0,0 +1,424 @@ +/**************************************************************************** +* +* Copyright (c) 2015 PX4 Development Team. All rights reserved. +* Author: Pavel Kirienko +* David Sidrane +* +****************************************************************************/ + +#ifndef UAVCAN_POSIX_FIRMWARE_VERSION_CHECKER_HPP_INCLUDED +#define UAVCAN_POSIX_FIRMWARE_VERSION_CHECKER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include + +#include + +#if !defined(DIRENT_ISFILE) && defined(DT_REG) +# define DIRENT_ISFILE(dtype) ((dtype) == DT_REG) +#endif + +namespace uavcan_posix +{ +/** + * Firmware version checking logic. + * Refer to @ref FirmwareUpdateTrigger for details. + */ +class FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker +{ + enum { FilePermissions = 438 }; ///< 0o666 + + enum { MaxBasePathLength = 128 }; + + /** + * This type is used for the base path + */ + typedef uavcan::MakeString::Type BasePathString; + + /** + * Maximum length of full path including / the file name + */ + enum { MaxPathLength = uavcan::protocol::file::Path::FieldTypes::path::MaxSize + MaxBasePathLength }; + + /** + * This type is used internally for the full path to file + */ + typedef uavcan::MakeString::Type PathString; + + + BasePathString base_path_; + BasePathString cache_path_; + + /** + * The folder where the files will be copied and read from + */ + static const char* getCacheDir() { return "c"; } + + static void addSlash(BasePathString& path) + { + if (path.back() != getPathSeparator()) + { + path.push_back(getPathSeparator()); + } + } + + static void removeSlash(BasePathString& path) + { + if (path.back() == getPathSeparator()) + { + path.pop_back(); + } + } + + void setFirmwareBasePath(const char* path) + { + base_path_ = path; + } + + void setFirmwareCachePath(const char* path) + { + cache_path_ = path; + } + + int copyIfNot(const char* srcpath, const char* destpath) + { + // Does the file exist + int rv = 0; + int dfd = open(destpath, O_RDONLY, 0); + + if (dfd >= 0) + { + // Close it and exit 0 + (void)close(dfd); + } + else + { + uint8_t buffer[512]; + + dfd = open(destpath, O_WRONLY | O_CREAT, FilePermissions); + if (dfd < 0) + { + rv = -errno; + } + else + { + int sfd = open(srcpath, O_RDONLY, 0); + if (sfd < 0) + { + rv = -errno; + } + else + { + ssize_t size; + do + { + size = ::read(sfd, buffer, sizeof(buffer)); + if (size != 0) + { + if (size < 0) + { + + rv = -errno; + + } + else + { + + if (size != write(dfd, buffer, size)) + { + rv = -errno; + } + } + } + } + while (rv == 0 && size); + + (void)close(sfd); + } + (void)close(dfd); + } + } + return rv; + } + + struct AppDescriptor + { + uint8_t signature[sizeof(uavcan::uint64_t)]; + uint64_t image_crc; + uint32_t image_size; + uint32_t vcs_commit; + uint8_t major_version; + uint8_t minor_version; + uint8_t reserved[6]; + }; + + + static int getFileInfo(const char* path, AppDescriptor & descriptor) + { + using namespace std; + + const unsigned MaxChunk = 512 / sizeof(uint64_t); + + uint64_t signature = 0; + std::memcpy(&signature, "APDesc00", 8); + + + int rv = -ENOENT; + uint64_t chunk[MaxChunk]; + int fd = open(path, O_RDONLY); + + if (fd >= 0) + { + AppDescriptor* pdescriptor = NULL; + + while (pdescriptor == NULL) + { + int len = read(fd, chunk, sizeof(chunk)); + + if (len == 0) + { + break; + } + + if (len < 0) + { + rv = -errno; + goto out_close; + } + + uint64_t* p = &chunk[0]; + + do + { + if (*p == signature) + { + pdescriptor = (AppDescriptor*) p; + descriptor = *pdescriptor; + rv = 0; + break; + } + } + while (p++ <= &chunk[MaxChunk - (sizeof(AppDescriptor) / sizeof(chunk[0]))]); + } + + out_close: + (void)close(fd); + } + return rv; + } + +public: + + const BasePathString& getFirmwareBasePath() const { return base_path_; } + + const BasePathString& getFirmwareCachePath() const { return cache_path_; } + + static char getPathSeparator() + { + return static_cast(uavcan::protocol::file::Path::SEPARATOR); + } + + /** + * Creates the Directories were the files will be stored + * + * This is directory structure is in support of a workaround + * for the issues that FirmwareFilePath is 40 + * + * It creates a path structure: + * +---(base_path) + * +-c <----------- Files are cached here. + */ + int createFwPaths(const char* base_path) + { + using namespace std; + int rv = -uavcan::ErrInvalidParam; + + if (base_path) + { + const int len = strlen(base_path); + + if (len > 0 && len < base_path_.MaxSize) + { + setFirmwareBasePath(base_path); + removeSlash(base_path_); + const char* path = getFirmwareBasePath().c_str(); + + setFirmwareCachePath(path); + addSlash(cache_path_); + cache_path_ += getCacheDir(); + + rv = 0; + struct stat sb; + if (stat(path, &sb) != 0 || !S_ISDIR(sb.st_mode)) + { + rv = mkdir(path, S_IRWXU | S_IRWXG | S_IRWXO); + } + + path = getFirmwareCachePath().c_str(); + + if (rv == 0 && (stat(path, &sb) != 0 || !S_ISDIR(sb.st_mode))) + { + rv = mkdir(path, S_IRWXU | S_IRWXG | S_IRWXO); + } + + addSlash(base_path_); + addSlash(cache_path_); + + if (rv >= 0) + { + if ((getFirmwareCachePath().size() + uavcan::protocol::file::Path::FieldTypes::path::MaxSize) > + MaxPathLength) + { + rv = -uavcan::ErrInvalidConfiguration; + } + } + } + } + return rv; + } + + + +protected: + /** + * 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(uavcan::NodeID node_id, + const uavcan::protocol::GetNodeInfo::Response& node_info, + FirmwareFilePath& out_firmware_file_path) + { + using namespace std; + + /* This is a work around for two issues. + * 1) FirmwareFilePath is 40 + * 2) OK using is using 32 for max file names. + * + * So for the file: + * org.pixhawk.px4cannode-v1-0.1.59efc137.uavcan.bin + * +---fw + * +-c <----------- Files are cashed here. + * +--- 59efc137.bin <---------- A unknown Firmware file + * +---org.pixhawk.px4cannode-v1 <---------- node_info.name + * +---1.0 <-------------------------------- node_info.name's hardware_version.major,minor + * + - 59efc137.bin <----------- A well known file must match the name + * in the root fw folder, so if it does not exist + * it is copied up + */ + bool rv = false; + + char fname_root[MaxBasePathLength + 1]; + int n = snprintf(fname_root, sizeof(fname_root), "%s%s/%d.%d", + getFirmwareBasePath().c_str(), + node_info.name.c_str(), + node_info.hardware_version.major, + node_info.hardware_version.minor); + + if (n > 0 && n < (int) sizeof(fname_root) - 2) + { + DIR* const fwdir = opendir(fname_root); + + fname_root[n++] = getPathSeparator(); + fname_root[n++] = '\0'; + + if (fwdir != NULL) + { + struct dirent* pfile = NULL; + while ((pfile = readdir(fwdir)) != NULL) + { + // TODO: This is not POSIX compliant + if (DIRENT_ISFILE(pfile->d_type)) + { + // Open any bin file in there. + if (strstr(pfile->d_name, ".bin") != NULL) + { + PathString full_src_path = fname_root; + full_src_path += pfile->d_name; + + PathString full_dst_path = getFirmwareCachePath().c_str(); + full_dst_path += pfile->d_name; + + // ease the burden on the user + int cr = copyIfNot(full_src_path.c_str(), full_dst_path.c_str()); + + // We have a file, is it a valid image + AppDescriptor descriptor; + + std::memset(&descriptor, 0, sizeof(descriptor)); + + if (cr == 0 && getFileInfo(full_dst_path.c_str(), descriptor) == 0) + { + volatile AppDescriptor descriptorC = descriptor; + descriptorC.reserved[1]++; + + if (node_info.software_version.image_crc == 0 || + (node_info.software_version.major == 0 && node_info.software_version.minor == 0) || + descriptor.image_crc != node_info.software_version.image_crc) + { + rv = true; + out_firmware_file_path = pfile->d_name; + } + break; + } + } + } + } + (void)closedir(fwdir); + } + } + return rv; + } + + /** + * 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(uavcan::NodeID, + const uavcan::protocol::file::BeginFirmwareUpdate::Response&, + FirmwareFilePath&) + { + // TODO: Limit the number of attempts per node + return true; + } + +public: + + const char* getFirmwarePath() const + { + return getFirmwareCachePath().c_str(); + } +}; + +} + +#endif // Include guard diff --git a/libuavcan_drivers/posix_tools/include/posix_tools/file_event_tracer.hpp b/libuavcan_drivers/posix_tools/include/posix_tools/file_event_tracer.hpp deleted file mode 100644 index 719e34694d..0000000000 --- a/libuavcan_drivers/posix_tools/include/posix_tools/file_event_tracer.hpp +++ /dev/null @@ -1,80 +0,0 @@ -/* - * Copyright (C) 2015 Pavel Kirienko - */ - -#pragma once - -#include -#include - -namespace uavcan_posix -{ -namespace dynamic_node_id_server -{ -/** - * This interface implements a POSIX compliant file based IEventTracer interface - */ -class FileEventTracer : public uavcan::dynamic_node_id_server::IEventTracer -{ - /** - * Maximum length of full path to log file - */ - - enum { MaxPathLength = 128, FormatBufferLength = 64 }; - - - /** - * This type is used for the path - */ - typedef uavcan::Array, - uavcan::ArrayModeDynamic, MaxPathLength> PathString; - - - PathString path_; - - -public: - - FileEventTracer() { } - - virtual void onEvent(uavcan::dynamic_node_id_server::TraceCode code, uavcan::int64_t argument) - { - struct timespec ts; - clock_gettime(CLOCK_REALTIME, &ts); - int fd = open(path_.c_str(), O_WRONLY | O_CREAT | O_APPEND); - if (fd >= 0 ) - { - char buffer[FormatBufferLength + 1]; - int n = snprintf(buffer, FormatBufferLength, "%d.%ld,%d,%lld\n", ts.tv_sec, ts.tv_nsec, code, argument); - write(fd, buffer, n); - close(fd); - } - } - /** - * Initializes the File based event trace - * - */ - - int init(const PathString & path) - { - using namespace std; - - int rv = -uavcan::ErrInvalidParam; - - if (path.size() > 0) - { - rv = 0; - path_ = path.c_str(); - int fd = open(path_.c_str(), O_RDWR | O_CREAT | O_TRUNC); - if ( fd >= 0) - { - close(fd); - } - } - return rv; - } - -}; - -} -}