From b6639d922b5deda22fa174af240dba89ad07a226 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 11 May 2015 20:48:35 +0300 Subject: [PATCH 001/143] Fixed naming in CoarseOrientation 'defined' cannot be used because it's a keyword in C/C++. --- dsdl/uavcan/equipment/CoarseOrientation.uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 From 1c52527f97d0607d15815c41881dad9aa662f794 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 12 May 2015 12:29:15 +0300 Subject: [PATCH 002/143] Fixed KeyValue definition TAO is enabled, value is only float32 --- dsdl/uavcan/protocol/debug/1789.KeyValue.uavcan | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) 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 From a548d8311ce9c72562327bc92cd02b7ee2f38f2c Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 13 May 2015 16:39:22 +0300 Subject: [PATCH 003/143] Testing framework: added emulateSingleFrameBroadcastTransfer() --- libuavcan/test/protocol/helpers.hpp | 28 +++++++++++++++++++ .../test/protocol/node_status_monitor.cpp | 24 +--------------- 2 files changed, 29 insertions(+), 23 deletions(-) diff --git a/libuavcan/test/protocol/helpers.hpp b/libuavcan/test/protocol/helpers.hpp index df8085566e..171fb66871 100644 --- a/libuavcan/test/protocol/helpers.hpp +++ b/libuavcan/test/protocol/helpers.hpp @@ -103,3 +103,31 @@ struct BackgroundSpinner : uavcan::TimerBase spinning_node.spin(uavcan::MonotonicDuration::fromMSec(1)); } }; + +template +static inline void emulateSingleFrameBroadcastTransfer(CanDriverMock& 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)); + + for (uint8_t i = 0; i < can.getNumIfaces(); i++) + { + can.ifaces.at(i).pushRx(can_frame); + } +} 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); } From bc00899e7080d5cc7f5e25c4d4c6d41482b33005 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 13 May 2015 17:50:45 +0300 Subject: [PATCH 004/143] Testing tooling fix --- libuavcan/test/node/test_node.hpp | 5 +++++ libuavcan/test/protocol/helpers.hpp | 9 +++------ libuavcan/test/transport/can/can.hpp | 8 ++++++++ 3 files changed, 16 insertions(+), 6 deletions(-) diff --git a/libuavcan/test/node/test_node.hpp b/libuavcan/test/node/test_node.hpp index c8b2055834..5aaa60288d 100644 --- a/libuavcan/test/node/test_node.hpp +++ b/libuavcan/test/node/test_node.hpp @@ -122,6 +122,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; } diff --git a/libuavcan/test/protocol/helpers.hpp b/libuavcan/test/protocol/helpers.hpp index 171fb66871..e0fb164f2f 100644 --- a/libuavcan/test/protocol/helpers.hpp +++ b/libuavcan/test/protocol/helpers.hpp @@ -104,8 +104,8 @@ struct BackgroundSpinner : uavcan::TimerBase } }; -template -static inline void emulateSingleFrameBroadcastTransfer(CanDriverMock& can, uavcan::NodeID node_id, +template +static inline void emulateSingleFrameBroadcastTransfer(CanDriver& can, uavcan::NodeID node_id, const MessageType& message, uavcan::TransferID tid) { uavcan::StaticTransferBuffer<100> buffer; @@ -126,8 +126,5 @@ static inline void emulateSingleFrameBroadcastTransfer(CanDriverMock& can, uavca 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); - } + can.pushRxToAllIfaces(can_frame); } 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); From 842319a2905cb8cb2778b92be6a44feeca6ae084 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 13 May 2015 21:59:43 +0300 Subject: [PATCH 005/143] Initial implementation of NodeInfoRetriever; fixes pending --- .../uavcan/protocol/node_info_retriever.hpp | 340 ++++++++++++++++++ .../test/protocol/node_info_retriever.cpp | 139 +++++++ 2 files changed, 479 insertions(+) create mode 100644 libuavcan/include/uavcan/protocol/node_info_retriever.hpp create mode 100644 libuavcan/test/protocol/node_info_retriever.cpp 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..542210e113 --- /dev/null +++ b/libuavcan/include/uavcan/protocol/node_info_retriever.hpp @@ -0,0 +1,340 @@ +/* + * 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 + +namespace uavcan +{ +/** + * Classes that need to receive GetNodeInfo responses should implement this interface. + */ +class 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. + * Events from this class can be routed to many subscribers. + */ +class NodeInfoRetriever : protected NodeStatusMonitor +{ +public: + enum { MaxNumRequestAttempts = 254 }; + enum { DefaultNumRequestAttempts = 30 }; + 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 + } + }; + + /* + * Callers are used with removeWhere() predicate. They don't actually remove anything. + */ + 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, bool) + { + 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, bool) + { + UAVCAN_ASSERT(key != NULL); + (key->*method)(event); + return false; + } + }; + + /* + * State + */ + Entry entries_[NodeID::Max]; // [1, NodeID::Max] + + Map listeners_; // Only keys are used + + ServiceClient get_node_info_client_; + + 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]; + } + + NodeID pickNextNodeToQuery() const + { + 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 && entry.updated_since_last_attempt) + { + 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& event) // 2 Hz + { + NodeStatusMonitor::handleTimerEvent(event); + + if (!get_node_info_client_.isPending()) // If request is pending, this condition will fail every second time + { + const NodeID next = pickNextNodeToQuery(); // Typ. 1 Hz + if (next.isUnicast()) + { + 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"); + } + } + } + } + + 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)); + } + + listeners_.removeWhere( + 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; + } + entry.uptime_sec = msg.uptime_sec; + entry.updated_since_last_attempt = true; + + listeners_.removeWhere(GenericHandlerCaller&>( + &INodeInfoListener::handleNodeStatusMessage, msg)); + } + + void handleGetNodeInfoResponse(const ServiceCallResult& result) + { + Entry& entry = getEntry(result.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.response.status.uptime_sec; + entry.request_needed = false; + listeners_.removeWhere(NodeInfoRetrievedHandlerCaller(result.server_node_id, result.response)); + } + else + { + if (num_attempts_ != UnlimitedRequestAttempts) + { + entry.num_attempts_made++; + if (entry.num_attempts_made >= num_attempts_) + { + entry.request_needed = false; + listeners_.removeWhere(GenericHandlerCaller(&INodeInfoListener::handleNodeInfoUnavailable, + result.server_node_id)); + } + } + } + } + +public: + NodeInfoRetriever(INode& node) + : NodeStatusMonitor(node) + , listeners_(node.getAllocator()) + , get_node_info_client_(node) + , 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)); + + return 0; + } + + /** + * Adds one listener to the set. + * May return -ErrMemory if there's no space to add the listener. + */ + int addListener(INodeInfoListener* listener) + { + UAVCAN_ASSERT(listener != NULL); + bool* value = listeners_.insert(listener, true); + return (value == NULL) ? -ErrMemory : 0; + } + + /** + * Removes the listener. + * If the listener was not registered, nothing will be done. + */ + void removeListener(INodeInfoListener* listener) + { + UAVCAN_ASSERT(listener != NULL); + listeners_.remove(listener); + } + + /** + * 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); + } +}; + +} + +#endif // Include guard diff --git a/libuavcan/test/protocol/node_info_retriever.cpp b/libuavcan/test/protocol/node_info_retriever.cpp new file mode 100644 index 0000000000..04f2d346ac --- /dev/null +++ b/libuavcan/test/protocol/node_info_retriever.cpp @@ -0,0 +1,139 @@ +/* + * 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) + { + (void)event; + 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); + + 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()); + + /* + * Waiting for discovery + */ + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1600)); + + 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 + */ + 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(2100)); + + 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); + + // TODO finish the test when the logic is fixed +} From 2b0d669d7fe6476d381326e47ef4210c0a3ad370 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 13 May 2015 22:02:02 +0300 Subject: [PATCH 006/143] Revert "NodeStatusMonitor API update" This reverts commit 08d96ef329fe2e12ee6149787b4aca69b1f48df1. --- libuavcan/include/uavcan/protocol/node_status_monitor.hpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/node_status_monitor.hpp b/libuavcan/include/uavcan/protocol/node_status_monitor.hpp index 60350ef6ff..cf0dd7fd9b 100644 --- a/libuavcan/include/uavcan/protocol/node_status_monitor.hpp +++ b/libuavcan/include/uavcan/protocol/node_status_monitor.hpp @@ -110,10 +110,6 @@ 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&) { const int OfflineTimeoutMs100 = protocol::NodeStatus::OFFLINE_TIMEOUT_MS / 100; @@ -138,6 +134,7 @@ protected: } } +protected: /** * Called when a node becomes online, changes status or goes offline. * Refer to uavcan.protocol.NodeStatus for the offline timeout value. From e2ef4a4518a0064a9e86b3427b7f83c4cdb322f1 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 13 May 2015 22:32:23 +0300 Subject: [PATCH 007/143] Restructured NodeStatusMonitor and NodeInfoRetriever - TimerBase is not inherited by the monitor now because that was a suboptimal solution for a class designed for inheritance --- .../uavcan/protocol/node_info_retriever.hpp | 62 +++++++++++++++---- .../uavcan/protocol/node_status_monitor.hpp | 20 +++--- 2 files changed, 62 insertions(+), 20 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/node_info_retriever.hpp b/libuavcan/include/uavcan/protocol/node_info_retriever.hpp index 542210e113..4c4245b6c8 100644 --- a/libuavcan/include/uavcan/protocol/node_info_retriever.hpp +++ b/libuavcan/include/uavcan/protocol/node_info_retriever.hpp @@ -9,6 +9,7 @@ #include #include #include +#include #include #include @@ -17,7 +18,7 @@ namespace uavcan /** * Classes that need to receive GetNodeInfo responses should implement this interface. */ -class INodeInfoListener +class UAVCAN_EXPORT INodeInfoListener { public: /** @@ -63,7 +64,8 @@ public: * implement the GetNodeInfo service. * Events from this class can be routed to many subscribers. */ -class NodeInfoRetriever : protected NodeStatusMonitor +class UAVCAN_EXPORT NodeInfoRetriever : NodeStatusMonitor + , TimerBase { public: enum { MaxNumRequestAttempts = 254 }; @@ -150,6 +152,8 @@ private: /* * Methods */ + static MonotonicDuration getTimerPollInterval() { return MonotonicDuration::fromMSec(100); } + const Entry& getEntry(NodeID node_id) const { return const_cast(this)->getEntry(node_id); } Entry& getEntry(NodeID node_id) { @@ -160,6 +164,14 @@ private: return entries_[node_id.get() - 1]; } + void startTimerIfNotRunning() + { + if (!TimerBase::isRunning()) + { + TimerBase::startPeriodic(getTimerPollInterval()); + } + } + NodeID pickNextNodeToQuery() const { for (unsigned iter_cnt_ = 0; iter_cnt_ < (sizeof(entries_) / sizeof(entries_[0])); iter_cnt_++) // Round-robin @@ -182,22 +194,38 @@ private: return NodeID(); // No node could be found } - virtual void handleTimerEvent(const TimerEvent& event) // 2 Hz + virtual void handleTimerEvent(const TimerEvent&) { - NodeStatusMonitor::handleTimerEvent(event); - - if (!get_node_info_client_.isPending()) // If request is pending, this condition will fail every second time + if (get_node_info_client_.isPending()) // If request is pending, this condition will fail every second time { - const NodeID next = pickNextNodeToQuery(); // Typ. 1 Hz - if (next.isUnicast()) + return; + } + + const NodeID next = pickNextNodeToQuery(); + if (next.isUnicast()) + { + getEntry(next).updated_since_last_attempt = false; + const int res = get_node_info_client_.call(next, protocol::GetNodeInfo::Request()); + if (res < 0) { - 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 + { + bool requests_needed = false; + for (uint8_t i = 1; i <= NodeID::Max; i++) + { + if (getEntry(i).request_needed) { - get_node_info_client_.getNode().registerInternalFailure("NodeInfoRetriever GetNodeInfo call"); + requests_needed = true; + break; } } + if (!requests_needed) + { + TimerBase::stop(); + } } } @@ -218,6 +246,11 @@ private: 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_.removeWhere( @@ -232,6 +265,8 @@ private: { entry.request_needed = true; entry.num_attempts_made = 0; + + startTimerIfNotRunning(); } entry.uptime_sec = msg.uptime_sec; entry.updated_since_last_attempt = true; @@ -272,6 +307,7 @@ private: public: NodeInfoRetriever(INode& node) : NodeStatusMonitor(node) + , TimerBase(node) , listeners_(node.getAllocator()) , get_node_info_client_(node) , last_picked_node_(1) @@ -298,7 +334,7 @@ public: } get_node_info_client_.setCallback(GetNodeInfoResponseCallback(this, &NodeInfoRetriever::handleGetNodeInfoResponse)); - + // Note: the timer will be started ad-hoc return 0; } diff --git a/libuavcan/include/uavcan/protocol/node_status_monitor.hpp b/libuavcan/include/uavcan/protocol/node_status_monitor.hpp index cf0dd7fd9b..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,7 +114,7 @@ private: handleNodeStatusMessage(msg); } - virtual void handleTimerEvent(const TimerEvent&) + void handleTimerEvent(const TimerEvent&) { const int OfflineTimeoutMs100 = protocol::NodeStatus::OFFLINE_TIMEOUT_MS / 100; @@ -122,7 +126,8 @@ private: 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; @@ -157,8 +162,8 @@ protected: public: explicit NodeStatusMonitor(INode& node) - : TimerBase(node) - , sub_(node) + : sub_(node) + , timer_(node) { } virtual ~NodeStatusMonitor() { } @@ -173,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; } From 3db54cd6af14037d5116dd07159664b5a3e8221b Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 13 May 2015 23:23:03 +0300 Subject: [PATCH 008/143] Improved NodeDiscoverer logic --- .../node_discoverer.hpp | 54 +++++++++---------- .../node_discoverer.cpp | 6 +-- 2 files changed, 30 insertions(+), 30 deletions(-) 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..93e78d8a9e 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 @@ -86,6 +86,27 @@ class NodeDiscoverer : TimerBase typedef Map NodeMap; + class HighestUptimeSearcher : ::uavcan::Noncopyable + { + uint32_t highest_uptime_sec_; + NodeID node_id_; + + public: + HighestUptimeSearcher() : highest_uptime_sec_(0) { } + + bool operator()(const NodeID& key, const NodeData& value) + { + if (value.last_seen_uptime >= highest_uptime_sec_) + { + highest_uptime_sec_ = value.last_seen_uptime; + node_id_ = key; + } + return false; + } + + NodeID getNodeWithHighestUptime() const { return node_id_; } + }; + /** * When this number of attempts has been made, the discoverer will give up and assume that the node * does not implement this service. @@ -121,34 +142,13 @@ class NodeDiscoverer : TimerBase NodeID pickNextNodeToQuery() const { - const unsigned node_map_size = node_map_.getSize(); + HighestUptimeSearcher searcher; - // 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); - } + const NodeID* const out = node_map_.findFirstKey(searcher); + (void)out; + UAVCAN_ASSERT(out == NULL); - // 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; + return searcher.getNodeWithHighestUptime(); } bool needToQuery(NodeID node_id) @@ -319,7 +319,7 @@ class NodeDiscoverer : TimerBase if (!isRunning()) { - startPeriodic(get_node_info_client_.getRequestTimeout() * 2); + startPeriodic(get_node_info_client_.getRequestTimeout()); trace(TraceDiscoveryTimerStart, getPeriod().toUSec()); } } 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..4c28113ead 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp @@ -134,7 +134,7 @@ TEST(dynamic_node_id_server_NodeDiscoverer, Basic) */ handler.can_discover = true; - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1900)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1400)); ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound)); ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart)); @@ -149,7 +149,7 @@ TEST(dynamic_node_id_server_NodeDiscoverer, Basic) 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(1400)); ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound)); ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart)); @@ -167,7 +167,7 @@ TEST(dynamic_node_id_server_NodeDiscoverer, Basic) 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(1400)); ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound)); ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart)); From 509ef85048c889afe8ee2882ccbab965b6a16dba Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 14 May 2015 00:02:43 +0300 Subject: [PATCH 009/143] Properly defining poll interval of NodeDiscoverer --- .../node_discoverer.hpp | 4 +- .../node_discoverer.cpp | 42 +++++++++++-------- 2 files changed, 27 insertions(+), 19 deletions(-) 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 93e78d8a9e..6d351d6176 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 @@ -113,6 +113,8 @@ class NodeDiscoverer : TimerBase */ enum { MaxAttemptsToGetNodeInfo = 5 }; + enum { TimerPollIntervalMs = 170 }; // ~ ceil(500 ms service timeout / 3) + enum { NumNodeStatusStaticReceivers = 64 }; /* @@ -319,7 +321,7 @@ class NodeDiscoverer : TimerBase if (!isRunning()) { - startPeriodic(get_node_info_client_.getRequestTimeout()); + startPeriodic(MonotonicDuration::fromMSec(TimerPollIntervalMs)); trace(TraceDiscoveryTimerStart, getPeriod().toUSec()); } } 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 4c28113ead..9a469c4461 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(1400)); + 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(1400)); + 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(1400)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(400)); 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(1650)); 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(1650)); 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(1650)); 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()); From fb155d8fc9b56466c146d26ddbf936c827ddb7a0 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 14 May 2015 00:08:22 +0300 Subject: [PATCH 010/143] Fixed missing UAVCAN_EXPORT declaration --- .../include/uavcan/protocol/dynamic_node_id_client.hpp | 2 +- .../protocol/dynamic_node_id_server/distributed/server.hpp | 6 +++--- .../uavcan/protocol/dynamic_node_id_server/event.hpp | 4 ++-- .../protocol/dynamic_node_id_server/storage_backend.hpp | 2 +- 4 files changed, 7 insertions(+), 7 deletions(-) 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 Date: Fri, 15 May 2015 00:02:06 +0300 Subject: [PATCH 011/143] dsdlc: Fixed comment generation --- libuavcan/dsdl_compiler/libuavcan_dsdl_compiler/__init__.py | 2 +- .../libuavcan_dsdl_compiler/data_type_template.tmpl | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) 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} From ee761eebad9a450c45ff799f77e04fcb311bb567 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 15 May 2015 15:29:31 +0300 Subject: [PATCH 012/143] Multiset<> --- libuavcan/include/uavcan/util/map.hpp | 7 +- libuavcan/include/uavcan/util/multiset.hpp | 570 +++++++++++++++++++++ libuavcan/test/util/multiset.cpp | 225 ++++++++ 3 files changed, 800 insertions(+), 2 deletions(-) create mode 100644 libuavcan/include/uavcan/util/multiset.hpp create mode 100644 libuavcan/test/util/multiset.cpp diff --git a/libuavcan/include/uavcan/util/map.hpp b/libuavcan/include/uavcan/util/map.hpp index 814dd517a0..dcbdf638a0 100644 --- a/libuavcan/include/uavcan/util/map.hpp +++ b/libuavcan/include/uavcan/util/map.hpp @@ -137,7 +137,10 @@ protected: #endif /// Derived class destructor must call removeAll(); - ~MapBase() { } + ~MapBase() + { + UAVCAN_ASSERT(getSize() == 0); + } public: /** @@ -158,7 +161,7 @@ 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); diff --git a/libuavcan/include/uavcan/util/multiset.hpp b/libuavcan/include/uavcan/util/multiset.hpp new file mode 100644 index 0000000000..311f4775f0 --- /dev/null +++ b/libuavcan/include/uavcan/util/multiset.hpp @@ -0,0 +1,570 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_UTIL_MULTISET_HPP_INCLUDED +#define UAVCAN_UTIL_MULTISET_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ +/** + * Slow but memory efficient unordered set. + * + * Items can be allocated in a static buffer or in the node's memory pool if the static buffer is exhausted. + * When an item is deleted from the static buffer, one pair from the memory pool will be moved in the free + * slot of the static buffer, so the use of the memory pool is minimized. + * + * Please be aware that this container does not perform any speed optimizations to minimize memory footprint, + * so the complexity of most operations is O(N). + * + * Type requirements: + * T must be copyable, assignable and default constructible. + * T must implement a comparison operator. + * T's default constructor must initialize the object into invalid state. + * Size of T must not exceed MemPoolBlockSize. + */ +template +class UAVCAN_EXPORT MultisetBase : Noncopyable +{ + template friend class Multiset; + +protected: + /* + * Purpose of this type is to enforce default initialization of T + */ + struct Item + { + T value; + Item() : value() { } + Item(const T& v) : value(v) { } + bool operator==(const Item& rhs) const { return rhs.value == value; } + bool operator!=(const Item& rhs) const { return !operator==(rhs); } + operator T() const { return value; } + }; + + struct Chunk : LinkedListNode + { + enum { NumItems = (MemPoolBlockSize - sizeof(LinkedListNode)) / sizeof(Item) }; + Item items[NumItems]; + + Chunk() + { + StaticAssert<(static_cast(NumItems) > 0)>::check(); + IsDynamicallyAllocatable::check(); + UAVCAN_ASSERT(items[0].value == T()); + } + + 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* find(const Item& item) + { + for (unsigned i = 0; i < static_cast(NumItems); i++) + { + if (items[i] == item) + { + return items + i; + } + } + return NULL; + } + }; + +private: + LinkedListRoot list_; + IPoolAllocator& allocator_; +#if !UAVCAN_TINY + Item* const static_; + const unsigned num_static_entries_; +#endif + + Item* find(const Item& item); + +#if !UAVCAN_TINY + void optimizeStorage(); +#endif + void compact(); + + struct YesPredicate + { + bool operator()(const T&) const { return true; } + }; + +protected: +#if UAVCAN_TINY + MultisetBase(IPoolAllocator& allocator) + : allocator_(allocator) + { + UAVCAN_ASSERT(Item() == Item()); + } +#else + MultisetBase(Item* static_buf, unsigned num_static_entries, IPoolAllocator& allocator) + : allocator_(allocator) + , static_(static_buf) + , num_static_entries_(num_static_entries) + { + UAVCAN_ASSERT(Item() == Item()); + } +#endif + + /// Derived class destructor must call removeAll(); + ~MultisetBase() + { + UAVCAN_ASSERT(getSize() == 0); + } + +public: + /** + * Adds one item and returns a pointer to it. + * If add fails due to lack of memory, NULL will be returned. + */ + T* add(const T& item); + + /** + * Does nothing if there's no such item. + */ + void remove(const T& item); + + /** + * Removes entries where the predicate returns true. + * Predicate prototype: + * bool (T& item) + */ + template + void removeWhere(Predicate predicate); + + /** + * Returns first entry where the predicate returns true. + * Predicate prototype: + * bool (const T& item) + */ + template + const T* findFirst(Predicate predicate) const; + + /** + * Removes all items; all pool memory will be released. + */ + void removeAll(); + + /** + * Returns an item located at the specified position from the beginning. + * Note that any insertion or deletion may greatly disturb internal ordering, so use with care. + * If index is greater than or equal the number of items, null pointer will be returned. + */ + T* getByIndex(unsigned index); + const T* getByIndex(unsigned index) const; + + bool isEmpty() const; + + /** + * Counts number of items stored. + * Best case complexity is O(N). + */ + unsigned getSize() const; + + /** + * For testing, do not use directly. + */ + unsigned getNumStaticItems() const; + unsigned getNumDynamicItems() const; +}; + + +template +class UAVCAN_EXPORT Multiset : public MultisetBase +{ + typename MultisetBase::Item static_[NumStaticEntries]; + +public: + +#if !UAVCAN_TINY + + // This instantiation will not be valid in UAVCAN_TINY mode + explicit Multiset(IPoolAllocator& allocator) + : MultisetBase(static_, NumStaticEntries, allocator) + { + UAVCAN_ASSERT(static_[0].value == T()); + } + + ~Multiset() { this->removeAll(); } + +#endif // !UAVCAN_TINY +}; + + +template +class UAVCAN_EXPORT Multiset : public MultisetBase +{ +public: + explicit Multiset(IPoolAllocator& allocator) +#if UAVCAN_TINY + : MultisetBase(allocator) +#else + : MultisetBase(NULL, 0, allocator) +#endif + { } + + ~Multiset() { this->removeAll(); } +}; + +// ---------------------------------------------------------------------------- + +/* + * MultisetBase<> + */ +template +typename MultisetBase::Item* MultisetBase::find(const Item& item) +{ +#if !UAVCAN_TINY + for (unsigned i = 0; i < num_static_entries_; i++) + { + if (static_[i] == item) + { + return static_ + i; + } + } +#endif + + Chunk* p = list_.get(); + while (p) + { + Item* const dyn = p->find(item); + if (dyn != NULL) + { + return dyn; + } + p = p->getNextListNode(); + } + return NULL; +} + +#if !UAVCAN_TINY + +template +void MultisetBase::optimizeStorage() +{ + while (true) + { + // Looking for first EMPTY static entry + Item* stat = NULL; + for (unsigned i = 0; i < num_static_entries_; i++) + { + if (static_[i] == Item()) + { + stat = static_ + i; + break; + } + } + if (stat == NULL) + { + break; + } + + // Looking for the first NON-EMPTY dynamic entry, erasing immediately + Chunk* p = list_.get(); + Item dyn; + UAVCAN_ASSERT(dyn == Item()); + while (p) + { + bool stop = false; + for (int i = 0; i < Chunk::NumItems; i++) + { + if (p->items[i] != Item()) // Non empty + { + dyn = p->items[i]; // Copy by value + p->items[i] = Item(); // Erase immediately + stop = true; + break; + } + } + if (stop) + { + break; + } + p = p->getNextListNode(); + } + if (dyn == Item()) + { + break; + } + + // Migrating + *stat = dyn; + } +} + +#endif // !UAVCAN_TINY + +template +void MultisetBase::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] != Item()) + { + remove_this = false; + break; + } + } + if (remove_this) + { + list_.remove(p); + Chunk::destroy(p, allocator_); + } + p = next; + } +} + +template +T* MultisetBase::add(const T& value) +{ + UAVCAN_ASSERT(!(value == T())); + remove(value); + + Item* const item = find(Item()); + if (item) + { + *item = Item(value); + return &item->value; + } + + Chunk* const itemg = Chunk::instantiate(allocator_); + if (itemg == NULL) + { + return NULL; + } + list_.insert(itemg); + itemg->items[0] = Item(value); + return &itemg->items[0].value; +} + +template +void MultisetBase::remove(const T& value) +{ + UAVCAN_ASSERT(!(value == T())); + Item* const item = find(Item(value)); + if (item != NULL) + { + *item = Item(); +#if !UAVCAN_TINY + optimizeStorage(); +#endif + compact(); + } +} + +template +template +void MultisetBase::removeWhere(Predicate predicate) +{ + unsigned num_removed = 0; + +#if !UAVCAN_TINY + for (unsigned i = 0; i < num_static_entries_; i++) + { + if (static_[i] != Item()) + { + if (predicate(static_[i].value)) + { + num_removed++; + static_[i] = Item(); + } + } + } +#endif + + Chunk* p = list_.get(); + while (p) + { + for (int i = 0; i < Chunk::NumItems; i++) + { + const Item* const item = p->items + i; + if ((*item) != Item()) + { + if (predicate(item->value)) + { + num_removed++; + p->items[i] = Item(); + } + } + } + p = p->getNextListNode(); + } + + if (num_removed > 0) + { +#if !UAVCAN_TINY + optimizeStorage(); +#endif + compact(); + } +} + +template +template +const T* MultisetBase::findFirst(Predicate predicate) const +{ +#if !UAVCAN_TINY + for (unsigned i = 0; i < num_static_entries_; i++) + { + if (static_[i] != Item()) + { + if (predicate(static_[i].value)) + { + return &static_[i].value; + } + } + } +#endif + + Chunk* p = list_.get(); + while (p) + { + for (int i = 0; i < Chunk::NumItems; i++) + { + const Item* const item = p->items + i; + if ((*item) != Item()) + { + if (predicate(item->value)) + { + return &p->items[i].value; + } + } + } + p = p->getNextListNode(); + } + return NULL; +} + +template +void MultisetBase::removeAll() +{ + removeWhere(YesPredicate()); +} + +template +T* MultisetBase::getByIndex(unsigned index) +{ +#if !UAVCAN_TINY + // Checking the static storage + for (unsigned i = 0; i < num_static_entries_; i++) + { + if (static_[i] != Item()) + { + if (index == 0) + { + return &static_[i].value; + } + index--; + } + } +#endif + + // Slowly crawling through the dynamic storage + Chunk* p = list_.get(); + while (p) + { + for (int i = 0; i < Chunk::NumItems; i++) + { + Item* const item = p->items + i; + if ((*item) != Item()) + { + if (index == 0) + { + return &item->value; + } + index--; + } + } + p = p->getNextListNode(); + } + + return NULL; +} + +template +const T* MultisetBase::getByIndex(unsigned index) const +{ + return const_cast*>(this)->getByIndex(index); +} + +template +bool MultisetBase::isEmpty() const +{ + return getSize() == 0; +} + +template +unsigned MultisetBase::getSize() const +{ + return getNumStaticItems() + getNumDynamicItems(); +} + +template +unsigned MultisetBase::getNumStaticItems() const +{ + unsigned num = 0; +#if !UAVCAN_TINY + for (unsigned i = 0; i < num_static_entries_; i++) + { + if (static_[i] != Item()) + { + num++; + } + } +#endif + return num; +} + +template +unsigned MultisetBase::getNumDynamicItems() const +{ + unsigned num = 0; + Chunk* p = list_.get(); + while (p) + { + for (int i = 0; i < Chunk::NumItems; i++) + { + const Item* const item = p->items + i; + if ((*item) != Item()) + { + num++; + } + } + p = p->getNextListNode(); + } + return num; +} + +} + +#endif // Include guard diff --git a/libuavcan/test/util/multiset.cpp b/libuavcan/test/util/multiset.cpp new file mode 100644 index 0000000000..a6cb3c71ea --- /dev/null +++ b/libuavcan/test/util/multiset.cpp @@ -0,0 +1,225 @@ +/* + * 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; } +}; + + +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)); + + // Empty + mset->remove("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->add("1")); + ASSERT_EQ("2", *mset->add("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"); + + // Dynamic addion + ASSERT_EQ("3", *mset->add("3")); + ASSERT_EQ(1, pool.getNumUsedBlocks()); + + ASSERT_EQ("4", *mset->add("4")); + ASSERT_EQ(1, pool.getNumUsedBlocks()); // Assuming that at least 2 items fit one block + 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)); + ASSERT_EQ("3", *mset->getByIndex(2)); + ASSERT_EQ("4", *mset->getByIndex(3)); + ASSERT_FALSE(mset->getByIndex(100)); + ASSERT_FALSE(mset->getByIndex(4)); + + // Finding some items + ASSERT_EQ("1", *mset->findFirst(FindPredicate("1"))); + ASSERT_EQ("2", *mset->findFirst(FindPredicate("2"))); + ASSERT_EQ("3", *mset->findFirst(FindPredicate("3"))); + ASSERT_EQ("4", *mset->findFirst(FindPredicate("4"))); + ASSERT_FALSE(mset->findFirst(FindPredicate("nonexistent"))); + + // Removing one static + mset->remove("1"); // One of dynamics now migrates to the static storage + mset->remove("foo"); // There's no such thing anyway + ASSERT_EQ(1, pool.getNumUsedBlocks()); + ASSERT_EQ(2, mset->getNumStaticItems()); + ASSERT_EQ(1, mset->getNumDynamicItems()); + + // Ordering has not changed - first dynamic entry has moved to the first static slot + ASSERT_EQ("3", *mset->getByIndex(0)); + ASSERT_EQ("2", *mset->getByIndex(1)); + ASSERT_EQ("4", *mset->getByIndex(2)); + + // Removing another static + mset->remove("2"); + ASSERT_EQ(2, mset->getNumStaticItems()); + ASSERT_EQ(0, mset->getNumDynamicItems()); + ASSERT_EQ(0, pool.getNumUsedBlocks()); // No dynamic entries left + + // Adding some new dynamics + unsigned max_value_integer = 0; + for (int i = 0; i < 100; i++) + { + const std::string value = toString(i); + std::string* res = mset->add(value); // Will override some from the 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; + ASSERT_LT(4, max_value_integer); + + // Making sure there is true OOM + ASSERT_EQ(0, pool.getNumFreeBlocks()); + ASSERT_FALSE(mset->add("nonexistent")); + + // Removing odd values - nearly half of them + ASSERT_EQ(2, mset->getNumStaticItems()); + const unsigned num_dynamics_old = mset->getNumDynamicItems(); + mset->removeWhere(oddValuePredicate); + ASSERT_EQ(2, mset->getNumStaticItems()); + const unsigned num_dynamics_new = mset->getNumDynamicItems(); + std::cout << "Num of dynamic pairs reduced from " << num_dynamics_old << " to " << num_dynamics_new << std::endl; + ASSERT_LT(num_dynamics_new, num_dynamics_old); + + // 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->findFirst(FindPredicate(toString(kv_int))); + if (val) + { + ASSERT_FALSE(kv_int & 1); + } + else + { + ASSERT_TRUE(kv_int & 1); + } + } + + // Making sure the memory will be released + mset.reset(); + ASSERT_EQ(0, pool.getNumUsedBlocks()); +} + + +TEST(Multiset, NoStatic) +{ + 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->remove("foo"); + ASSERT_EQ(0, pool.getNumUsedBlocks()); + ASSERT_FALSE(mset->getByIndex(0)); + + // Insertion + ASSERT_EQ("a", *mset->add("a")); + ASSERT_EQ("b", *mset->add("b")); + ASSERT_EQ(1, pool.getNumUsedBlocks()); + ASSERT_EQ(0, mset->getNumStaticItems()); + ASSERT_EQ(2, mset->getNumDynamicItems()); + + // Ordering + ASSERT_EQ("a", *mset->getByIndex(0)); + ASSERT_EQ("b", *mset->getByIndex(1)); + ASSERT_FALSE(mset->getByIndex(3)); + ASSERT_FALSE(mset->getByIndex(1000)); +} + + +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->remove(8); + ASSERT_EQ(0, pool.getNumUsedBlocks()); + ASSERT_EQ(0, mset->getSize()); + ASSERT_FALSE(mset->getByIndex(0)); + + // Insertion + ASSERT_EQ(1, *mset->add(1)); + ASSERT_EQ(1, mset->getSize()); + ASSERT_EQ(2, *mset->add(2)); + ASSERT_EQ(2, mset->getSize()); + ASSERT_EQ(3, *mset->add(3)); + ASSERT_EQ(4, *mset->add(4)); + ASSERT_EQ(4, mset->getSize()); + + // 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)); +} From 282b995c1e94d4080cdba2eaeab270900daae596 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 15 May 2015 18:41:38 +0300 Subject: [PATCH 013/143] Partially refactored ServiceClient, tests are failing, the code is totally broken --- .../include/uavcan/node/service_client.hpp | 354 ++++++++++++------ .../distributed/raft_core.hpp | 25 +- .../node_discoverer.hpp | 14 +- .../protocol/network_compat_checker.hpp | 16 +- .../uavcan/protocol/node_info_retriever.hpp | 13 +- .../uavcan/transport/transfer_listener.hpp | 97 ++--- libuavcan/include/uavcan/util/multiset.hpp | 1 + libuavcan/src/node/uc_service_client.cpp | 29 +- libuavcan/test/node/service_client.cpp | 30 +- .../test/protocol/data_type_info_provider.cpp | 66 ++-- .../test/protocol/node_status_provider.cpp | 11 +- libuavcan/test/protocol/param_server.cpp | 32 +- .../test/protocol/restart_request_server.cpp | 8 +- .../protocol/transport_stats_provider.cpp | 42 +-- .../linux/include/uavcan_linux/helpers.hpp | 4 +- 15 files changed, 435 insertions(+), 307 deletions(-) diff --git a/libuavcan/include/uavcan/node/service_client.hpp b/libuavcan/include/uavcan/node/service_client.hpp index 067d3b890c..2aefd9c1f3 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,42 +121,64 @@ static Stream& operator<<(Stream& s, const ServiceCallResult& scr) /** * Do not use directly. */ -class ServiceClientBase : protected DeadlineHandler +class ServiceClientBase : public ITransferAcceptanceFilter, Noncopyable { 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_; - explicit ServiceClientBase(INode& node) - : DeadlineHandler(node.getScheduler()) - , data_type_descriptor_(NULL) + virtual void handleDeadline(MonotonicTime); + + public: + CallState(INode& node, ServiceClientBase& owner, ServiceCallID call_id) + : DeadlineHandler(node.getScheduler()) + , owner_(owner) + , id_(call_id) + { + UAVCAN_ASSERT(id_.isValid()); + DeadlineHandler::startWithDelay(owner_.request_timeout_); + } + + bool doesMatch(ServiceCallID call_id) const { return call_id == id_; } + + 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.doesMatch(id); } + }; + + MonotonicDuration request_timeout_; + + ServiceClientBase() + : 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); + + virtual void handleTimeout(ServiceCallID call_id) = 0; 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; }; /** @@ -132,14 +193,17 @@ public: */ 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,22 +213,32 @@ 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; +#if 0 + typedef Multiset CallRegistry; + CallRegistry call_registry_; +#endif + 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); virtual void handleReceivedDataStruct(ReceivedDataStructure& response); - virtual void handleDeadline(MonotonicTime); + virtual void handleTimeout(ServiceCallID call_id); + + int addCallState(ServiceCallID call_id); public: /** @@ -173,7 +247,6 @@ public: */ explicit ServiceClient(INode& node, const Callback& callback = Callback()) : SubscriberType(node) - , ServiceClientBase(node) , publisher_(node, getDefaultRequestTimeout()) , callback_(callback) { @@ -183,7 +256,7 @@ public: #endif } - virtual ~ServiceClient() { cancel(); } + virtual ~ServiceClient() { cancelAll(); } /** * Shall be called before first use. @@ -202,18 +275,24 @@ public: * Note that the callback will ALWAYS be called even if the service call has timed 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. */ - void cancel(); + int call(NodeID server_node_id, const RequestType& request, ServiceCallID& out_call_id); + + /** + * Cancels certain call referred via call ID structure. + */ + void cancel(ServiceCallID call_id); + + /** + * Cancels all pending calls. + */ + void cancelAll(); /** * Service response callback must be set prior service call. @@ -221,6 +300,16 @@ public: const Callback& getCallback() const { return callback_; } void setCallback(const Callback& cb) { callback_ = cb; } +#if 0 + unsigned getNumPendingCalls() const { return call_registry_.getSize(); } +#endif + +#if 0 + bool hasPendingCalls() const { return !call_registry_.isEmpty(); } +#else + bool hasPendingCalls() const { return false; } +#endif + /** * Returns the number of failed attempts to decode received response. Generally, a failed attempt means either: * - Transient failure in the transport layer. @@ -246,10 +335,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,55 +348,86 @@ 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 + +#if 0 + return call_registry_.findFirst(CallStateMatchingPredicate(ServiceCallID(frame.getSrcNodeID(), + frame.getTransferID()))) != NULL; +#else + (void)frame; + return false; +#endif +} + +template +void ServiceClient:: +handleReceivedDataStruct(ReceivedDataStructure& response) { UAVCAN_ASSERT(response.getTransferType() == TransferTypeServiceResponse); - const TransferListenerType* const listener = SubscriberType::getTransferListener(); - if (listener) - { - const typename TransferListenerType::ExpectedResponseParams erp = listener->getExpectedResponseParams(); - ServiceCallResultType result(ServiceCallResultType::Success, erp.src_node_id, response); - cancel(); - invokeCallback(result); - } - else - { - UAVCAN_ASSERT(0); - cancel(); - } + + ServiceCallID call_id(response.getSrcNodeID(), response.getTransferID()); + cancel(call_id); + ServiceCallResultType result(ServiceCallResultType::Success, call_id, response); // Mutable! + invokeCallback(result); } -template -void ServiceClient::handleDeadline(MonotonicTime) -{ - const TransferListenerType* const listener = SubscriberType::getTransferListener(); - if (listener) - { - const typename TransferListenerType::ExpectedResponseParams erp = listener->getExpectedResponseParams(); - ReceivedDataStructure& ref = SubscriberType::getReceivedStructStorage(); - ServiceCallResultType result(ServiceCallResultType::ErrorTimeout, erp.src_node_id, ref); - UAVCAN_TRACE("ServiceClient", "Timeout from nid=%i, dtname=%s", - erp.src_node_id.get(), DataType::getDataTypeFullName()); - cancel(); - invokeCallback(result); - } - else - { - UAVCAN_ASSERT(0); - cancel(); - } +template +void ServiceClient::handleTimeout(ServiceCallID call_id) +{ + cancel(call_id); + ServiceCallResultType result(ServiceCallResultType::ErrorTimeout, call_id, + SubscriberType::getReceivedStructStorage()); // Mutable! + invokeCallback(result); } -template -int ServiceClient::call(NodeID server_node_id, const RequestType& request) +template +int ServiceClient::addCallState(ServiceCallID call_id) { - cancel(); - if (!isCallbackValid()) +#if 0 + if (call_registry_.isEmpty()) + { + 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; + } + } + + if (call_registry_.add(CallState(SubscriberType::getNode(), *this, call_id)) == NULL) + { + SubscriberType::stop(); + return -ErrMemory; + } + + return 0; +#else + (void)call_id; + return 0; +#endif +} + +template +int ServiceClient::call(NodeID server_node_id, + const RequestType& request) +{ + 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; } /* @@ -315,37 +435,35 @@ int ServiceClient::call(NodeID server_node_id, const Reque */ 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(); + cancel(out_call_id); return -ErrLogic; } - const typename TransferListenerType::ExpectedResponseParams erp(server_node_id, transfer_id); - tl->setExpectedResponseParams(erp); + tl->installAcceptanceFilter(this); /* * Publishing the request @@ -353,22 +471,34 @@ int ServiceClient::call(NodeID server_node_id, const Reque const int publisher_res = publisher_.publish(request, TransferTypeServiceRequest, server_node_id, transfer_id); if (publisher_res < 0) { - cancel(); + cancel(out_call_id); + return publisher_res; } - return publisher_res; + + return 0; } -template -void ServiceClient::cancel() +template +void ServiceClient::cancel(ServiceCallID call_id) { - pending_ = false; - SubscriberType::stop(); - DeadlineHandler::stop(); - TransferListenerType* const tl = SubscriberType::getTransferListener(); - if (tl) +#if 0 + call_registry_.remove(call_id); + if (call_registry_.isEmpty()) { - tl->stopAcceptingAnything(); + SubscriberType::stop(); } +#else + (void)call_id; +#endif +} + +template +void ServiceClient::cancelAll() +{ +#if 0 + call_registry_.removeAll(); +#endif + SubscriberType::stop(); } } 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..634bd002bd 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 @@ -327,9 +327,9 @@ class RaftCore : private TimerBase for (uint8_t i = 0; i < NumRequestVoteClients; i++) { - request_vote_clients_[i]->cancel(); + request_vote_clients_[i]->cancelAll(); // TODO FIXME Concurrent calls!! } - append_entries_client_.cancel(); + append_entries_client_.cancelAll(); /* * Calling the switch handler @@ -550,22 +550,23 @@ class RaftCore : private TimerBase 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); } } @@ -654,15 +655,15 @@ class RaftCore : private TimerBase 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_++; } 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 6d351d6176..74280ac9f4 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 @@ -235,14 +235,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 @@ -250,11 +250,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 } } @@ -262,7 +262,7 @@ class NodeDiscoverer : TimerBase void handleTimerEvent(const TimerEvent&) { - if (get_node_info_client_.isPending()) + if (get_node_info_client_.hasPendingCalls()) { return; } diff --git a/libuavcan/include/uavcan/protocol/network_compat_checker.hpp b/libuavcan/include/uavcan/protocol/network_compat_checker.hpp index ca6635451a..3ac0ae0264 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_.cancelAll(); return res; } diff --git a/libuavcan/include/uavcan/protocol/node_info_retriever.hpp b/libuavcan/include/uavcan/protocol/node_info_retriever.hpp index 4c4245b6c8..c2656b645c 100644 --- a/libuavcan/include/uavcan/protocol/node_info_retriever.hpp +++ b/libuavcan/include/uavcan/protocol/node_info_retriever.hpp @@ -196,9 +196,9 @@ private: virtual void handleTimerEvent(const TimerEvent&) { - if (get_node_info_client_.isPending()) // If request is pending, this condition will fail every second time + if (get_node_info_client_.hasPendingCalls()) // If request is pending, this condition will fail every second time { - return; + return; // TODO FIXME Concurrent calls!! } const NodeID next = pickNextNodeToQuery(); @@ -277,7 +277,7 @@ private: void handleGetNodeInfoResponse(const ServiceCallResult& result) { - Entry& entry = getEntry(result.server_node_id); + Entry& entry = getEntry(result.getCallID().server_node_id); if (result.isSuccessful()) { @@ -285,9 +285,10 @@ private: * 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.response.status.uptime_sec; + entry.uptime_sec = result.getResponse().status.uptime_sec; entry.request_needed = false; - listeners_.removeWhere(NodeInfoRetrievedHandlerCaller(result.server_node_id, result.response)); + listeners_.removeWhere(NodeInfoRetrievedHandlerCaller(result.getCallID().server_node_id, + result.getResponse())); } else { @@ -298,7 +299,7 @@ private: { entry.request_needed = false; listeners_.removeWhere(GenericHandlerCaller(&INodeInfoListener::handleNodeInfoUnavailable, - result.server_node_id)); + result.getCallID().server_node_id)); } } } diff --git a/libuavcan/include/uavcan/transport/transfer_listener.hpp b/libuavcan/include/uavcan/transport/transfer_listener.hpp index dc0bc66ad2..62be67d64b 100644 --- a/libuavcan/include/uavcan/transport/transfer_listener.hpp +++ b/libuavcan/include/uavcan/transport/transfer_listener.hpp @@ -178,88 +178,71 @@ public: virtual ~TransferListener() { - // Map must be cleared before bufmgr is destructed + // Map must be cleared before bufmgr is destroyed receivers_.removeAll(); } }; +/** + * 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) + { + UAVCAN_ASSERT(filter_ == NULL); + filter_ = acceptance_filter; + UAVCAN_ASSERT(filter_ != NULL); + } }; // ---------------------------------------------------------------------------- /* - * 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/multiset.hpp b/libuavcan/include/uavcan/util/multiset.hpp index 311f4775f0..7cce1e2a17 100644 --- a/libuavcan/include/uavcan/util/multiset.hpp +++ b/libuavcan/include/uavcan/util/multiset.hpp @@ -146,6 +146,7 @@ public: /** * Does nothing if there's no such item. + * Only the first matching item will be removed. */ void remove(const T& item); diff --git a/libuavcan/src/node/uc_service_client.cpp b/libuavcan/src/node/uc_service_client.cpp index a0c6cc00e8..f686db20c5 100644 --- a/libuavcan/src/node/uc_service_client.cpp +++ b/libuavcan/src/node/uc_service_client.cpp @@ -6,12 +6,26 @@ 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_ASSERT(id_.isValid()); + UAVCAN_TRACE("ServiceClient", "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()); + owner_.handleTimeout(id_); +} +/* + * 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 +34,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 +65,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/test/node/service_client.cpp b/libuavcan/test/node/service_client.cpp index f668f791d5..fa08b7214a 100644 --- a/libuavcan/test/node/service_client.cpp +++ b/libuavcan/test/node/service_client.cpp @@ -26,9 +26,9 @@ struct ServiceCallResultHandler 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; } bool match(StatusType status, uavcan::NodeID server_node_id, const typename DataType::Response& response) const @@ -111,17 +111,17 @@ TEST(ServiceClient, Basic) ASSERT_EQ(3, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Listening now! - ASSERT_TRUE(client1.isPending()); - ASSERT_TRUE(client2.isPending()); - ASSERT_TRUE(client3.isPending()); + ASSERT_TRUE(client1.hasPendingCalls()); + ASSERT_TRUE(client2.hasPendingCalls()); + ASSERT_TRUE(client3.hasPendingCalls()); nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(20)); ASSERT_EQ(1, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Third is still listening! - ASSERT_FALSE(client1.isPending()); - ASSERT_FALSE(client2.isPending()); - ASSERT_TRUE(client3.isPending()); + ASSERT_FALSE(client1.hasPendingCalls()); + ASSERT_FALSE(client2.hasPendingCalls()); + ASSERT_TRUE(client3.hasPendingCalls()); // Validating root_ns_a::StringService::Response expected_response; @@ -130,9 +130,9 @@ TEST(ServiceClient, Basic) nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(200)); - ASSERT_FALSE(client1.isPending()); - ASSERT_FALSE(client2.isPending()); - ASSERT_FALSE(client3.isPending()); + ASSERT_FALSE(client1.hasPendingCalls()); + ASSERT_FALSE(client2.hasPendingCalls()); + ASSERT_FALSE(client3.hasPendingCalls()); ASSERT_EQ(0, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Third has timed out :( @@ -141,7 +141,7 @@ TEST(ServiceClient, Basic) // Stray request ASSERT_LT(0, client3.call(99, request)); // Will timeout! - ASSERT_TRUE(client3.isPending()); + ASSERT_TRUE(client3.hasPendingCalls()); ASSERT_EQ(1, nodes.b.getDispatcher().getNumServiceResponseListeners()); } @@ -178,10 +178,10 @@ TEST(ServiceClient, Rejection) ASSERT_LT(0, client1.call(1, request)); ASSERT_EQ(1, nodes.b.getDispatcher().getNumServiceResponseListeners()); - ASSERT_TRUE(client1.isPending()); + ASSERT_TRUE(client1.hasPendingCalls()); nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(200)); - ASSERT_FALSE(client1.isPending()); + ASSERT_FALSE(client1.hasPendingCalls()); ASSERT_EQ(0, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Timed out 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/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_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) From 0d85d672c7860c3fab38780b0b29487ff1a61a9b Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 15 May 2015 18:45:37 +0300 Subject: [PATCH 014/143] Temporary fix for assertion failure in tests; 12 tests are failing --- libuavcan/include/uavcan/node/service_client.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libuavcan/include/uavcan/node/service_client.hpp b/libuavcan/include/uavcan/node/service_client.hpp index 2aefd9c1f3..8a9dced967 100644 --- a/libuavcan/include/uavcan/node/service_client.hpp +++ b/libuavcan/include/uavcan/node/service_client.hpp @@ -407,7 +407,7 @@ int ServiceClient::addCallState(ServiceCa return 0; #else (void)call_id; - return 0; + return -ErrNotInited; #endif } From 048e0a33eebc0e1ebd843bbc0fbbec4f178ece19 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 15 May 2015 21:32:08 +0300 Subject: [PATCH 015/143] Non-moving multiset, tests are failing in C++03 mode --- libuavcan/include/uavcan/util/map.hpp | 1 + libuavcan/include/uavcan/util/multiset.hpp | 440 +++++++++------------ libuavcan/test/util/multiset.cpp | 82 ++-- 3 files changed, 238 insertions(+), 285 deletions(-) diff --git a/libuavcan/include/uavcan/util/map.hpp b/libuavcan/include/uavcan/util/map.hpp index dcbdf638a0..ed5535266d 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 { diff --git a/libuavcan/include/uavcan/util/multiset.hpp b/libuavcan/include/uavcan/util/multiset.hpp index 7cce1e2a17..1c62dd3fdf 100644 --- a/libuavcan/include/uavcan/util/multiset.hpp +++ b/libuavcan/include/uavcan/util/multiset.hpp @@ -11,45 +11,62 @@ #include #include #include +#include + +#if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11) +# error UAVCAN_CPP_VERSION +#endif namespace uavcan { /** - * Slow but memory efficient unordered set. + * 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. - * When an item is deleted from the static buffer, one pair from the memory pool will be moved in the free - * slot of the static buffer, so the use of the memory pool is minimized. - * - * Please be aware that this container does not perform any speed optimizations to minimize memory footprint, - * so the complexity of most operations is O(N). - * - * Type requirements: - * T must be copyable, assignable and default constructible. - * T must implement a comparison operator. - * T's default constructor must initialize the object into invalid state. - * Size of T must not exceed MemPoolBlockSize. */ template class UAVCAN_EXPORT MultisetBase : Noncopyable { - template friend class Multiset; - protected: - /* - * Purpose of this type is to enforce default initialization of T - */ - struct Item + struct Item : ::uavcan::Noncopyable { - T value; - Item() : value() { } - Item(const T& v) : value(v) { } - bool operator==(const Item& rhs) const { return rhs.value == value; } - bool operator!=(const Item& rhs) const { return !operator==(rhs); } - operator T() const { return value; } + 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)]; + long double _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)); + } + } }; - struct Chunk : LinkedListNode +private: + struct Chunk : LinkedListNode, ::uavcan::Noncopyable { enum { NumItems = (MemPoolBlockSize - sizeof(LinkedListNode)) / sizeof(Item) }; Item items[NumItems]; @@ -58,7 +75,7 @@ protected: { StaticAssert<(static_cast(NumItems) > 0)>::check(); IsDynamicallyAllocatable::check(); - UAVCAN_ASSERT(items[0].value == T()); + UAVCAN_ASSERT(!items[0].isConstructed()); } static Chunk* instantiate(IPoolAllocator& allocator) @@ -81,11 +98,11 @@ protected: } } - Item* find(const Item& item) + Item* findFreeSlot() { for (unsigned i = 0; i < static_cast(NumItems); i++) { - if (items[i] == item) + if (!items[i].isConstructed()) { return items + i; } @@ -94,7 +111,6 @@ protected: } }; -private: LinkedListRoot list_; IPoolAllocator& allocator_; #if !UAVCAN_TINY @@ -102,11 +118,8 @@ private: const unsigned num_static_entries_; #endif - Item* find(const Item& item); + Item* findOrCreateFreeSlot(); -#if !UAVCAN_TINY - void optimizeStorage(); -#endif void compact(); struct YesPredicate @@ -114,6 +127,20 @@ private: 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; } + }; + protected: #if UAVCAN_TINY MultisetBase(IPoolAllocator& allocator) @@ -126,9 +153,7 @@ protected: : allocator_(allocator) , static_(static_buf) , num_static_entries_(num_static_entries) - { - UAVCAN_ASSERT(Item() == Item()); - } + { } #endif /// Derived class destructor must call removeAll(); @@ -138,17 +163,70 @@ protected: } public: + /** + * This is needed for testing. + */ + enum { NumItemsPerDynamicChunk = Chunk::NumItems }; + /** * Adds one item and returns a pointer to it. * If add fails due to lack of memory, NULL will be returned. */ - T* add(const T& item); + T* add() + { + 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* add(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* add(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* add(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; + } /** - * Does nothing if there's no such item. - * Only the first matching item will be removed. + * @ref removeMatching() */ - void remove(const T& item); + enum RemoveStrategy { RemoveOne, RemoveAll }; /** * Removes entries where the predicate returns true. @@ -156,7 +234,15 @@ public: * bool (T& item) */ template - void removeWhere(Predicate predicate); + void removeMatching(Predicate predicate, RemoveStrategy strategy); + + template + void removeAllMatching(Predicate predicate) { removeMatching(predicate, RemoveAll); } + + template + void removeFirstMatching(Predicate predicate) { removeMatching(predicate, RemoveOne); } + + void removeFirst(const T& ref) { removeFirstMatching(ComparingPredicate(ref)); } /** * Returns first entry where the predicate returns true. @@ -164,28 +250,45 @@ public: * bool (const T& item) */ template - const T* findFirst(Predicate predicate) const; + T* find(Predicate predicate); + + template + const T* find(Predicate predicate) const + { + return const_cast*>(this)->find(predicate); + } /** * Removes all items; all pool memory will be released. */ - void removeAll(); + void removeAll() { removeAllMatching(YesPredicate()); } /** * Returns an item located at the specified position from the beginning. * Note that any insertion or deletion may greatly disturb internal ordering, so use with care. * If index is greater than or equal the number of items, null pointer will be returned. */ - T* getByIndex(unsigned index); - const T* getByIndex(unsigned index) const; + T* getByIndex(unsigned index) + { + IndexPredicate predicate(index); + return find(predicate); + } - bool isEmpty() const; + const T* getByIndex(unsigned index) const + { + return const_cast*>(this)->getByIndex(index); + } + + /** + * This is O(1) + */ + bool isEmpty() const { return find(YesPredicate()) == NULL; } /** * Counts number of items stored. * Best case complexity is O(N). */ - unsigned getSize() const; + unsigned getSize() const { return getNumStaticItems() + getNumDynamicItems(); } /** * For testing, do not use directly. @@ -207,9 +310,7 @@ public: // This instantiation will not be valid in UAVCAN_TINY mode explicit Multiset(IPoolAllocator& allocator) : MultisetBase(static_, NumStaticEntries, allocator) - { - UAVCAN_ASSERT(static_[0].value == T()); - } + { } ~Multiset() { this->removeAll(); } @@ -238,87 +339,42 @@ public: * MultisetBase<> */ template -typename MultisetBase::Item* MultisetBase::find(const Item& item) +typename MultisetBase::Item* MultisetBase::findOrCreateFreeSlot() { #if !UAVCAN_TINY + // Search in static pool for (unsigned i = 0; i < num_static_entries_; i++) { - if (static_[i] == item) + if (!static_[i].isConstructed()) { - return static_ + i; + return &static_[i]; } } #endif - Chunk* p = list_.get(); - while (p) + // Search in dynamic pool { - Item* const dyn = p->find(item); - if (dyn != NULL) - { - return dyn; - } - p = p->getNextListNode(); - } - return NULL; -} - -#if !UAVCAN_TINY - -template -void MultisetBase::optimizeStorage() -{ - while (true) - { - // Looking for first EMPTY static entry - Item* stat = NULL; - for (unsigned i = 0; i < num_static_entries_; i++) - { - if (static_[i] == Item()) - { - stat = static_ + i; - break; - } - } - if (stat == NULL) - { - break; - } - - // Looking for the first NON-EMPTY dynamic entry, erasing immediately Chunk* p = list_.get(); - Item dyn; - UAVCAN_ASSERT(dyn == Item()); while (p) { - bool stop = false; - for (int i = 0; i < Chunk::NumItems; i++) + Item* const dyn = p->findFreeSlot(); + if (dyn != NULL) { - if (p->items[i] != Item()) // Non empty - { - dyn = p->items[i]; // Copy by value - p->items[i] = Item(); // Erase immediately - stop = true; - break; - } - } - if (stop) - { - break; + return dyn; } p = p->getNextListNode(); } - if (dyn == Item()) - { - break; - } - - // Migrating - *stat = dyn; } -} -#endif // !UAVCAN_TINY + // Create new dynamic chunk + Chunk* const chunk = Chunk::instantiate(allocator_); + if (chunk == NULL) + { + return NULL; + } + list_.insert(chunk); + return &chunk->items[0]; +} template void MultisetBase::compact() @@ -330,7 +386,7 @@ void MultisetBase::compact() bool remove_this = true; for (int i = 0; i < Chunk::NumItems; i++) { - if (p->items[i] != Item()) + if (p->items[i].isConstructed()) { remove_this = false; break; @@ -345,103 +401,73 @@ void MultisetBase::compact() } } -template -T* MultisetBase::add(const T& value) -{ - UAVCAN_ASSERT(!(value == T())); - remove(value); - - Item* const item = find(Item()); - if (item) - { - *item = Item(value); - return &item->value; - } - - Chunk* const itemg = Chunk::instantiate(allocator_); - if (itemg == NULL) - { - return NULL; - } - list_.insert(itemg); - itemg->items[0] = Item(value); - return &itemg->items[0].value; -} - -template -void MultisetBase::remove(const T& value) -{ - UAVCAN_ASSERT(!(value == T())); - Item* const item = find(Item(value)); - if (item != NULL) - { - *item = Item(); -#if !UAVCAN_TINY - optimizeStorage(); -#endif - compact(); - } -} - template template -void MultisetBase::removeWhere(Predicate predicate) +void MultisetBase::removeMatching(Predicate predicate, const RemoveStrategy strategy) { unsigned num_removed = 0; #if !UAVCAN_TINY for (unsigned i = 0; i < num_static_entries_; i++) { - if (static_[i] != Item()) + if (static_[i].isConstructed()) { - if (predicate(static_[i].value)) + if (predicate(*static_[i].ptr)) { num_removed++; - static_[i] = Item(); + static_[i].destroy(); } } + + if ((num_removed > 0) && (strategy == RemoveOne)) + { + break; + } } #endif Chunk* p = list_.get(); while (p) { + if ((num_removed > 0) && (strategy == RemoveOne)) + { + break; + } + for (int i = 0; i < Chunk::NumItems; i++) { - const Item* const item = p->items + i; - if ((*item) != Item()) + Item& item = p->items[i]; + if (item.isConstructed()) { - if (predicate(item->value)) + if (predicate(*item.ptr)) { num_removed++; - p->items[i] = Item(); + item.destroy(); } } } + p = p->getNextListNode(); } if (num_removed > 0) { -#if !UAVCAN_TINY - optimizeStorage(); -#endif compact(); } } template template -const T* MultisetBase::findFirst(Predicate predicate) const +T* MultisetBase::find(Predicate predicate) { #if !UAVCAN_TINY for (unsigned i = 0; i < num_static_entries_; i++) { - if (static_[i] != Item()) + if (static_[i].isConstructed()) { - if (predicate(static_[i].value)) + if (predicate(*static_[i].ptr)) { - return &static_[i].value; + return static_[i].ptr; } } } @@ -452,12 +478,11 @@ const T* MultisetBase::findFirst(Predicate predicate) const { for (int i = 0; i < Chunk::NumItems; i++) { - const Item* const item = p->items + i; - if ((*item) != Item()) + if (p->items[i].isConstructed()) { - if (predicate(item->value)) + if (predicate(*p->items[i].ptr)) { - return &p->items[i].value; + return p->items[i].ptr; } } } @@ -466,70 +491,6 @@ const T* MultisetBase::findFirst(Predicate predicate) const return NULL; } -template -void MultisetBase::removeAll() -{ - removeWhere(YesPredicate()); -} - -template -T* MultisetBase::getByIndex(unsigned index) -{ -#if !UAVCAN_TINY - // Checking the static storage - for (unsigned i = 0; i < num_static_entries_; i++) - { - if (static_[i] != Item()) - { - if (index == 0) - { - return &static_[i].value; - } - index--; - } - } -#endif - - // Slowly crawling through the dynamic storage - Chunk* p = list_.get(); - while (p) - { - for (int i = 0; i < Chunk::NumItems; i++) - { - Item* const item = p->items + i; - if ((*item) != Item()) - { - if (index == 0) - { - return &item->value; - } - index--; - } - } - p = p->getNextListNode(); - } - - return NULL; -} - -template -const T* MultisetBase::getByIndex(unsigned index) const -{ - return const_cast*>(this)->getByIndex(index); -} - -template -bool MultisetBase::isEmpty() const -{ - return getSize() == 0; -} - -template -unsigned MultisetBase::getSize() const -{ - return getNumStaticItems() + getNumDynamicItems(); -} - template unsigned MultisetBase::getNumStaticItems() const { @@ -537,10 +498,7 @@ unsigned MultisetBase::getNumStaticItems() const #if !UAVCAN_TINY for (unsigned i = 0; i < num_static_entries_; i++) { - if (static_[i] != Item()) - { - num++; - } + num += static_[i].isConstructed() ? 1U : 0U; } #endif return num; @@ -555,11 +513,7 @@ unsigned MultisetBase::getNumDynamicItems() const { for (int i = 0; i < Chunk::NumItems; i++) { - const Item* const item = p->items + i; - if ((*item) != Item()) - { - num++; - } + num += p->items[i].isConstructed() ? 1U : 0U; } p = p->getNextListNode(); } diff --git a/libuavcan/test/util/multiset.cpp b/libuavcan/test/util/multiset.cpp index a6cb3c71ea..21f74500cd 100644 --- a/libuavcan/test/util/multiset.cpp +++ b/libuavcan/test/util/multiset.cpp @@ -44,7 +44,7 @@ TEST(Multiset, Basic) std::auto_ptr mset(new MultisetType(poolmgr)); // Empty - mset->remove("foo"); + mset->removeFirst("foo"); ASSERT_EQ(0, pool.getNumUsedBlocks()); ASSERT_FALSE(mset->getByIndex(0)); ASSERT_FALSE(mset->getByIndex(1)); @@ -61,54 +61,57 @@ TEST(Multiset, Basic) ASSERT_TRUE(*mset->getByIndex(0) == "1"); ASSERT_TRUE(*mset->getByIndex(1) == "2"); - // Dynamic addion + // Dynamic addition ASSERT_EQ("3", *mset->add("3")); + ASSERT_EQ("3", *mset->getByIndex(2)); ASSERT_EQ(1, pool.getNumUsedBlocks()); ASSERT_EQ("4", *mset->add("4")); - ASSERT_EQ(1, pool.getNumUsedBlocks()); // Assuming that at least 2 items fit one block + 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)); - ASSERT_EQ("3", *mset->getByIndex(2)); - ASSERT_EQ("4", *mset->getByIndex(3)); + // 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->findFirst(FindPredicate("1"))); - ASSERT_EQ("2", *mset->findFirst(FindPredicate("2"))); - ASSERT_EQ("3", *mset->findFirst(FindPredicate("3"))); - ASSERT_EQ("4", *mset->findFirst(FindPredicate("4"))); - ASSERT_FALSE(mset->findFirst(FindPredicate("nonexistent"))); + 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"))); - // Removing one static - mset->remove("1"); // One of dynamics now migrates to the static storage - mset->remove("foo"); // There's no such thing anyway - ASSERT_EQ(1, pool.getNumUsedBlocks()); - ASSERT_EQ(2, mset->getNumStaticItems()); - ASSERT_EQ(1, mset->getNumDynamicItems()); + // 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 - first dynamic entry has moved to the first static slot - ASSERT_EQ("3", *mset->getByIndex(0)); - ASSERT_EQ("2", *mset->getByIndex(1)); - ASSERT_EQ("4", *mset->getByIndex(2)); + // 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->remove("2"); - ASSERT_EQ(2, mset->getNumStaticItems()); - ASSERT_EQ(0, mset->getNumDynamicItems()); - ASSERT_EQ(0, pool.getNumUsedBlocks()); // No dynamic entries left + mset->removeFirst("2"); + ASSERT_EQ(0, mset->getNumStaticItems()); + ASSERT_EQ(2, mset->getNumDynamicItems()); + ASSERT_LE(1, pool.getNumUsedBlocks()); - // Adding some new dynamics + // 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->add(value); // Will override some from the above + std::string* res = mset->add(value); // Will NOT override above if (res == NULL) { ASSERT_LT(2, i); @@ -121,25 +124,18 @@ TEST(Multiset, Basic) max_value_integer = unsigned(i); } std::cout << "Max value: " << max_value_integer << std::endl; - ASSERT_LT(4, max_value_integer); // Making sure there is true OOM ASSERT_EQ(0, pool.getNumFreeBlocks()); ASSERT_FALSE(mset->add("nonexistent")); // Removing odd values - nearly half of them - ASSERT_EQ(2, mset->getNumStaticItems()); - const unsigned num_dynamics_old = mset->getNumDynamicItems(); - mset->removeWhere(oddValuePredicate); - ASSERT_EQ(2, mset->getNumStaticItems()); - const unsigned num_dynamics_new = mset->getNumDynamicItems(); - std::cout << "Num of dynamic pairs reduced from " << num_dynamics_old << " to " << num_dynamics_new << std::endl; - ASSERT_LT(num_dynamics_new, num_dynamics_old); + mset->removeAllMatching(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->findFirst(FindPredicate(toString(kv_int))); + const std::string* val = mset->find(FindPredicate(toString(kv_int))); if (val) { ASSERT_FALSE(kv_int & 1); @@ -160,16 +156,17 @@ TEST(Multiset, NoStatic) { using uavcan::Multiset; - static const int POOL_BLOCKS = 3; - uavcan::PoolAllocator pool; + uavcan::PoolAllocator<1024, 128> pool; // Large enough to keep everything uavcan::PoolManager<2> poolmgr; poolmgr.addPool(&pool); typedef Multiset MultisetType; std::auto_ptr mset(new MultisetType(poolmgr)); + ASSERT_LE(2, MultisetType::NumItemsPerDynamicChunk); + // Empty - mset->remove("foo"); + mset->removeFirst("foo"); ASSERT_EQ(0, pool.getNumUsedBlocks()); ASSERT_FALSE(mset->getByIndex(0)); @@ -192,16 +189,17 @@ TEST(Multiset, PrimitiveKey) { using uavcan::Multiset; - static const int POOL_BLOCKS = 3; - uavcan::PoolAllocator pool; + uavcan::PoolAllocator<1024, 128> pool; // Large enough to keep everything uavcan::PoolManager<2> poolmgr; poolmgr.addPool(&pool); - typedef Multiset MultisetType; + typedef Multiset MultisetType; std::auto_ptr mset(new MultisetType(poolmgr)); + ASSERT_LE(2, MultisetType::NumItemsPerDynamicChunk); + // Empty - mset->remove(8); + mset->removeFirst(8); ASSERT_EQ(0, pool.getNumUsedBlocks()); ASSERT_EQ(0, mset->getSize()); ASSERT_FALSE(mset->getByIndex(0)); From 5e7f81c11b57b65bfd96af244b4319d8a9826ace Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 16 May 2015 02:36:45 +0300 Subject: [PATCH 016/143] Fixed Multiset tests --- libuavcan/test/util/multiset.cpp | 20 +++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/libuavcan/test/util/multiset.cpp b/libuavcan/test/util/multiset.cpp index 21f74500cd..f89f61fd0a 100644 --- a/libuavcan/test/util/multiset.cpp +++ b/libuavcan/test/util/multiset.cpp @@ -156,15 +156,14 @@ TEST(Multiset, NoStatic) { using uavcan::Multiset; - uavcan::PoolAllocator<1024, 128> pool; // Large enough to keep everything + 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_LE(2, MultisetType::NumItemsPerDynamicChunk); - // Empty mset->removeFirst("foo"); ASSERT_EQ(0, pool.getNumUsedBlocks()); @@ -173,15 +172,17 @@ TEST(Multiset, NoStatic) // Insertion ASSERT_EQ("a", *mset->add("a")); ASSERT_EQ("b", *mset->add("b")); - ASSERT_EQ(1, pool.getNumUsedBlocks()); + ASSERT_LE(1, pool.getNumUsedBlocks()); ASSERT_EQ(0, mset->getNumStaticItems()); ASSERT_EQ(2, mset->getNumDynamicItems()); - // Ordering +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + // Only C++11 because C++03 uses one entry per pool block which breaks ordering ASSERT_EQ("a", *mset->getByIndex(0)); ASSERT_EQ("b", *mset->getByIndex(1)); ASSERT_FALSE(mset->getByIndex(3)); ASSERT_FALSE(mset->getByIndex(1000)); +#endif } @@ -189,15 +190,14 @@ TEST(Multiset, PrimitiveKey) { using uavcan::Multiset; - uavcan::PoolAllocator<1024, 128> pool; // Large enough to keep everything + 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_LE(2, MultisetType::NumItemsPerDynamicChunk); - // Empty mset->removeFirst(8); ASSERT_EQ(0, pool.getNumUsedBlocks()); @@ -213,11 +213,13 @@ TEST(Multiset, PrimitiveKey) ASSERT_EQ(4, *mset->add(4)); ASSERT_EQ(4, mset->getSize()); - // Ordering +#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 } From 24f0ec56f4030289c4abc4aa666376e7d990d2bd Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 16 May 2015 03:14:46 +0300 Subject: [PATCH 017/143] Multiset fixes and more tests --- libuavcan/include/uavcan/util/multiset.hpp | 70 ++++++++--------- libuavcan/test/util/multiset.cpp | 87 ++++++++++++++++++---- 2 files changed, 107 insertions(+), 50 deletions(-) diff --git a/libuavcan/include/uavcan/util/multiset.hpp b/libuavcan/include/uavcan/util/multiset.hpp index 1c62dd3fdf..d39b31b7c5 100644 --- a/libuavcan/include/uavcan/util/multiset.hpp +++ b/libuavcan/include/uavcan/util/multiset.hpp @@ -122,6 +122,11 @@ private: void compact(); + enum RemoveStrategy { RemoveOne, RemoveAll }; + + template + void removeWhere(Predicate predicate, RemoveStrategy strategy); + struct YesPredicate { bool operator()(const T&) const { return true; } @@ -156,7 +161,7 @@ protected: { } #endif - /// Derived class destructor must call removeAll(); + /// Derived class destructor must call clear(); ~MultisetBase() { UAVCAN_ASSERT(getSize() == 0); @@ -164,15 +169,11 @@ protected: public: /** - * This is needed for testing. + * 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). */ - enum { NumItemsPerDynamicChunk = Chunk::NumItems }; - - /** - * Adds one item and returns a pointer to it. - * If add fails due to lack of memory, NULL will be returned. - */ - T* add() + T* emplace() { Item* const item = findOrCreateFreeSlot(); if (item == NULL) @@ -185,7 +186,7 @@ public: } template - T* add(P1 p1) + T* emplace(P1 p1) { Item* const item = findOrCreateFreeSlot(); if (item == NULL) @@ -198,7 +199,7 @@ public: } template - T* add(P1 p1, P2 p2) + T* emplace(P1 p1, P2 p2) { Item* const item = findOrCreateFreeSlot(); if (item == NULL) @@ -211,7 +212,7 @@ public: } template - T* add(P1 p1, P2 p2, P3 p3) + T* emplace(P1 p1, P2 p2, P3 p3) { Item* const item = findOrCreateFreeSlot(); if (item == NULL) @@ -223,26 +224,22 @@ public: return item->ptr; } - /** - * @ref removeMatching() - */ - enum RemoveStrategy { RemoveOne, RemoveAll }; - /** * Removes entries where the predicate returns true. * Predicate prototype: * bool (T& item) */ template - void removeMatching(Predicate predicate, RemoveStrategy strategy); + void removeAllWhere(Predicate predicate) { removeWhere(predicate, RemoveAll); } template - void removeAllMatching(Predicate predicate) { removeMatching(predicate, RemoveAll); } + void removeFirstWhere(Predicate predicate) { removeWhere(predicate, RemoveOne); } - template - void removeFirstMatching(Predicate predicate) { removeMatching(predicate, RemoveOne); } + void removeFirst(const T& ref) { removeFirstWhere(ComparingPredicate(ref)); } - void removeFirst(const T& ref) { removeFirstMatching(ComparingPredicate(ref)); } + void removeAll(const T& ref) { removeAllWhere(ComparingPredicate(ref)); } + + void clear() { removeAllWhere(YesPredicate()); } /** * Returns first entry where the predicate returns true. @@ -258,15 +255,11 @@ public: return const_cast*>(this)->find(predicate); } - /** - * Removes all items; all pool memory will be released. - */ - void removeAll() { removeAllMatching(YesPredicate()); } - /** * Returns an item located at the specified position from the beginning. - * Note that any insertion or deletion may greatly disturb internal ordering, so use with care. + * 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) { @@ -280,7 +273,7 @@ public: } /** - * This is O(1) + * Complexity is O(N). */ bool isEmpty() const { return find(YesPredicate()) == NULL; } @@ -312,7 +305,7 @@ public: : MultisetBase(static_, NumStaticEntries, allocator) { } - ~Multiset() { this->removeAll(); } + ~Multiset() { this->clear(); } #endif // !UAVCAN_TINY }; @@ -330,7 +323,7 @@ public: #endif { } - ~Multiset() { this->removeAll(); } + ~Multiset() { this->clear(); } }; // ---------------------------------------------------------------------------- @@ -403,7 +396,7 @@ void MultisetBase::compact() template template -void MultisetBase::removeMatching(Predicate predicate, const RemoveStrategy strategy) +void MultisetBase::removeWhere(Predicate predicate, const RemoveStrategy strategy) { unsigned num_removed = 0; @@ -416,13 +409,12 @@ void MultisetBase::removeMatching(Predicate predicate, const RemoveStrategy s { num_removed++; static_[i].destroy(); + if (strategy == RemoveOne) + { + break; + } } } - - if ((num_removed > 0) && (strategy == RemoveOne)) - { - break; - } } #endif @@ -443,6 +435,10 @@ void MultisetBase::removeMatching(Predicate predicate, const RemoveStrategy s { num_removed++; item.destroy(); + if (strategy == RemoveOne) + { + break; + } } } } diff --git a/libuavcan/test/util/multiset.cpp b/libuavcan/test/util/multiset.cpp index f89f61fd0a..2c49e77122 100644 --- a/libuavcan/test/util/multiset.cpp +++ b/libuavcan/test/util/multiset.cpp @@ -30,6 +30,25 @@ struct FindPredicate 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; + TEST(Multiset, Basic) { @@ -51,8 +70,8 @@ TEST(Multiset, Basic) ASSERT_FALSE(mset->getByIndex(10000)); // Static addion - ASSERT_EQ("1", *mset->add("1")); - ASSERT_EQ("2", *mset->add("2")); + 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()); @@ -62,11 +81,11 @@ TEST(Multiset, Basic) ASSERT_TRUE(*mset->getByIndex(1) == "2"); // Dynamic addition - ASSERT_EQ("3", *mset->add("3")); + ASSERT_EQ("3", *mset->emplace("3")); ASSERT_EQ("3", *mset->getByIndex(2)); ASSERT_EQ(1, pool.getNumUsedBlocks()); - ASSERT_EQ("4", *mset->add("4")); + ASSERT_EQ("4", *mset->emplace("4")); ASSERT_LE(1, pool.getNumUsedBlocks()); // One or more ASSERT_EQ(2, mset->getNumStaticItems()); ASSERT_EQ(2, mset->getNumDynamicItems()); @@ -111,7 +130,7 @@ TEST(Multiset, Basic) for (int i = 0; i < 100; i++) { const std::string value = toString(i); - std::string* res = mset->add(value); // Will NOT override above + std::string* res = mset->emplace(value); // Will NOT override above if (res == NULL) { ASSERT_LT(2, i); @@ -127,10 +146,10 @@ TEST(Multiset, Basic) // Making sure there is true OOM ASSERT_EQ(0, pool.getNumFreeBlocks()); - ASSERT_FALSE(mset->add("nonexistent")); + ASSERT_FALSE(mset->emplace("nonexistent")); // Removing odd values - nearly half of them - mset->removeAllMatching(oddValuePredicate); + mset->removeAllWhere(oddValuePredicate); // Making sure there's no odd values left for (unsigned kv_int = 0; kv_int <= max_value_integer; kv_int++) @@ -170,8 +189,8 @@ TEST(Multiset, NoStatic) ASSERT_FALSE(mset->getByIndex(0)); // Insertion - ASSERT_EQ("a", *mset->add("a")); - ASSERT_EQ("b", *mset->add("b")); + ASSERT_EQ("a", *mset->emplace("a")); + ASSERT_EQ("b", *mset->emplace("b")); ASSERT_LE(1, pool.getNumUsedBlocks()); ASSERT_EQ(0, mset->getNumStaticItems()); ASSERT_EQ(2, mset->getNumDynamicItems()); @@ -205,12 +224,12 @@ TEST(Multiset, PrimitiveKey) ASSERT_FALSE(mset->getByIndex(0)); // Insertion - ASSERT_EQ(1, *mset->add(1)); + ASSERT_EQ(1, *mset->emplace(1)); ASSERT_EQ(1, mset->getSize()); - ASSERT_EQ(2, *mset->add(2)); + ASSERT_EQ(2, *mset->emplace(2)); ASSERT_EQ(2, mset->getSize()); - ASSERT_EQ(3, *mset->add(3)); - ASSERT_EQ(4, *mset->add(4)); + ASSERT_EQ(3, *mset->emplace(3)); + ASSERT_EQ(4, *mset->emplace(4)); ASSERT_EQ(4, mset->getSize()); #if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 @@ -223,3 +242,45 @@ TEST(Multiset, PrimitiveKey) ASSERT_FALSE(mset->getByIndex(1000)); #endif } + + +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 +} From eb370b08dd42696fdf7d8e7b8594aef8bf772e44 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 16 May 2015 03:31:18 +0300 Subject: [PATCH 018/143] Refactored Map<> --- .../node_discoverer.hpp | 2 +- .../uavcan/protocol/node_info_retriever.hpp | 14 +++--- .../transport/outgoing_transfer_registry.hpp | 4 +- .../uavcan/transport/transfer_listener.hpp | 2 +- libuavcan/include/uavcan/util/map.hpp | 46 +++++++++---------- .../src/transport/uc_transfer_listener.cpp | 2 +- libuavcan/test/util/map.cpp | 22 ++++----- 7 files changed, 46 insertions(+), 46 deletions(-) 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 74280ac9f4..03b4145759 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 @@ -146,7 +146,7 @@ class NodeDiscoverer : TimerBase { HighestUptimeSearcher searcher; - const NodeID* const out = node_map_.findFirstKey(searcher); + const NodeID* const out = node_map_.find(searcher); (void)out; UAVCAN_ASSERT(out == NULL); diff --git a/libuavcan/include/uavcan/protocol/node_info_retriever.hpp b/libuavcan/include/uavcan/protocol/node_info_retriever.hpp index c2656b645c..29391a15a6 100644 --- a/libuavcan/include/uavcan/protocol/node_info_retriever.hpp +++ b/libuavcan/include/uavcan/protocol/node_info_retriever.hpp @@ -253,7 +253,7 @@ private: } } - listeners_.removeWhere( + listeners_.removeAllWhere( GenericHandlerCaller(&INodeInfoListener::handleNodeStatusChange, event)); } @@ -271,8 +271,8 @@ private: entry.uptime_sec = msg.uptime_sec; entry.updated_since_last_attempt = true; - listeners_.removeWhere(GenericHandlerCaller&>( - &INodeInfoListener::handleNodeStatusMessage, msg)); + listeners_.removeAllWhere(GenericHandlerCaller&>( + &INodeInfoListener::handleNodeStatusMessage, msg)); } void handleGetNodeInfoResponse(const ServiceCallResult& result) @@ -287,8 +287,8 @@ private: */ entry.uptime_sec = result.getResponse().status.uptime_sec; entry.request_needed = false; - listeners_.removeWhere(NodeInfoRetrievedHandlerCaller(result.getCallID().server_node_id, - result.getResponse())); + listeners_.removeAllWhere(NodeInfoRetrievedHandlerCaller(result.getCallID().server_node_id, + result.getResponse())); } else { @@ -298,8 +298,8 @@ private: if (entry.num_attempts_made >= num_attempts_) { entry.request_needed = false; - listeners_.removeWhere(GenericHandlerCaller(&INodeInfoListener::handleNodeInfoUnavailable, - result.getCallID().server_node_id)); + listeners_.removeAllWhere(GenericHandlerCaller( + &INodeInfoListener::handleNodeInfoUnavailable, result.getCallID().server_node_id)); } } } 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 62be67d64b..428ec1489b 100644 --- a/libuavcan/include/uavcan/transport/transfer_listener.hpp +++ b/libuavcan/include/uavcan/transport/transfer_listener.hpp @@ -179,7 +179,7 @@ public: virtual ~TransferListener() { // Map must be cleared before bufmgr is destroyed - receivers_.removeAll(); + receivers_.clear(); } }; diff --git a/libuavcan/include/uavcan/util/map.hpp b/libuavcan/include/uavcan/util/map.hpp index ed5535266d..1f805fa0a2 100644 --- a/libuavcan/include/uavcan/util/map.hpp +++ b/libuavcan/include/uavcan/util/map.hpp @@ -108,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(); @@ -117,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: @@ -137,7 +137,7 @@ protected: } #endif - /// Derived class destructor must call removeAll(); + /// Derived class destructor must call clear(); ~MapBase() { UAVCAN_ASSERT(getSize() == 0); @@ -165,7 +165,7 @@ public: * bool (Key& key, Value& value) */ template - void removeWhere(Predicate predicate); + void removeAllWhere(Predicate predicate); /** * Returns first entry where the predicate returns true. @@ -173,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. @@ -185,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; @@ -211,7 +217,7 @@ public: : MapBase(static_, NumStaticEntries, allocator) { } - ~Map() { this->removeAll(); } + ~Map() { this->clear(); } #endif // !UAVCAN_TINY }; @@ -229,7 +235,7 @@ public: #endif { } - ~Map() { this->removeAll(); } + ~Map() { this->clear(); } }; // ---------------------------------------------------------------------------- @@ -238,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++) @@ -348,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; } @@ -358,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); @@ -379,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(); @@ -392,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; @@ -439,7 +445,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++) @@ -474,9 +480,9 @@ const Key* MapBase::findFirstKey(Predicate predicate) const } template -void MapBase::removeAll() +void MapBase::clear() { - removeWhere(YesPredicate()); + removeAllWhere(YesPredicate()); } template @@ -525,12 +531,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/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/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; From f713ef5e00864d80915e20bcadd871c10db86890 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 16 May 2015 03:36:01 +0300 Subject: [PATCH 019/143] LazyConstructor memory optimization --- libuavcan/include/uavcan/util/lazy_constructor.hpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/libuavcan/include/uavcan/util/lazy_constructor.hpp b/libuavcan/include/uavcan/util/lazy_constructor.hpp index 93152adc71..08ff9ab5f9 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_; From be5bcf9084cbb30cda0c507fd48cd43199da8d2e Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 16 May 2015 13:21:36 +0300 Subject: [PATCH 020/143] ParameterType<> template --- .../include/uavcan/util/lazy_constructor.hpp | 39 +++++++++---------- libuavcan/include/uavcan/util/templates.hpp | 9 +++++ 2 files changed, 27 insertions(+), 21 deletions(-) diff --git a/libuavcan/include/uavcan/util/lazy_constructor.hpp b/libuavcan/include/uavcan/util/lazy_constructor.hpp index 08ff9ab5f9..c7ae975663 100644 --- a/libuavcan/include/uavcan/util/lazy_constructor.hpp +++ b/libuavcan/include/uavcan/util/lazy_constructor.hpp @@ -58,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) @@ -130,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/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 */ From 713ec48ce9d41074ef556f90e83b0ad83ea0a692 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 16 May 2015 13:38:42 +0300 Subject: [PATCH 021/143] Multiset<>::forEach() --- libuavcan/include/uavcan/util/multiset.hpp | 63 ++++++++++++++++++++-- libuavcan/test/util/multiset.cpp | 56 +++++++++++++++++++ 2 files changed, 115 insertions(+), 4 deletions(-) diff --git a/libuavcan/include/uavcan/util/multiset.hpp b/libuavcan/include/uavcan/util/multiset.hpp index d39b31b7c5..0f8b0799f8 100644 --- a/libuavcan/include/uavcan/util/multiset.hpp +++ b/libuavcan/include/uavcan/util/multiset.hpp @@ -135,15 +135,50 @@ private: struct IndexPredicate : ::uavcan::Noncopyable { unsigned index; - IndexPredicate(unsigned target_index) : index(target_index) { } - bool operator()(const T&) { return index--==0; } + 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; } + + ComparingPredicate(const T& ref) + : reference(ref) + { } + + bool operator()(const T& sample) + { + return reference == sample; + } + }; + + template + struct OperatorToFalsePredicateAdapter : ::uavcan::Noncopyable + { + const typename ParameterType::Type oper; + + OperatorToFalsePredicateAdapter(typename ParameterType::Type o) + : oper(o) + { } + + bool operator()(T& item) + { + oper(item); + return false; + } + + bool operator()(const T& item) const + { + oper(item); + return false; + } }; protected: @@ -255,6 +290,26 @@ public: 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. diff --git a/libuavcan/test/util/multiset.cpp b/libuavcan/test/util/multiset.cpp index 2c49e77122..35e7376e75 100644 --- a/libuavcan/test/util/multiset.cpp +++ b/libuavcan/test/util/multiset.cpp @@ -49,6 +49,20 @@ struct NoncopyableWithCounter : uavcan::Noncopyable 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) { @@ -62,6 +76,8 @@ TEST(Multiset, Basic) typedef Multiset MultisetType; std::auto_ptr mset(new MultisetType(poolmgr)); + typedef SummationOperator StringConcatenationOperator; + // Empty mset->removeFirst("foo"); ASSERT_EQ(0, pool.getNumUsedBlocks()); @@ -80,6 +96,12 @@ TEST(Multiset, Basic) 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)); @@ -107,6 +129,13 @@ TEST(Multiset, Basic) 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 @@ -165,6 +194,20 @@ TEST(Multiset, Basic) } } + // 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()); @@ -241,6 +284,19 @@ TEST(Multiset, PrimitiveKey) 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); + } } From 861315d1c375c4e5552f6269ef6d690a5b9506b0 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 16 May 2015 13:44:03 +0300 Subject: [PATCH 022/143] Typo --- libuavcan/include/uavcan/util/multiset.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libuavcan/include/uavcan/util/multiset.hpp b/libuavcan/include/uavcan/util/multiset.hpp index 0f8b0799f8..b9a9b4b07e 100644 --- a/libuavcan/include/uavcan/util/multiset.hpp +++ b/libuavcan/include/uavcan/util/multiset.hpp @@ -328,7 +328,7 @@ public: } /** - * Complexity is O(N). + * Complexity is O(1). */ bool isEmpty() const { return find(YesPredicate()) == NULL; } From 39b924cd8ad6d6e0400cb078911647a00706ba86 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 16 May 2015 14:06:32 +0300 Subject: [PATCH 023/143] Multiset storage alignment fix --- libuavcan/include/uavcan/util/multiset.hpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/libuavcan/include/uavcan/util/multiset.hpp b/libuavcan/include/uavcan/util/multiset.hpp index b9a9b4b07e..d796b1bff6 100644 --- a/libuavcan/include/uavcan/util/multiset.hpp +++ b/libuavcan/include/uavcan/util/multiset.hpp @@ -39,7 +39,12 @@ protected: union { unsigned char pool[sizeof(T)]; - long double _aligner1_; + /* + * 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 From 40e68d41033b29ac39f16e627a0a44f715114c39 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 16 May 2015 14:09:15 +0300 Subject: [PATCH 024/143] TransferListenerWithFilter - NULL checks removed as they were conflicting with ServiceClient<>'s logic --- libuavcan/include/uavcan/transport/transfer_listener.hpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/libuavcan/include/uavcan/transport/transfer_listener.hpp b/libuavcan/include/uavcan/transport/transfer_listener.hpp index 428ec1489b..6b57c25e99 100644 --- a/libuavcan/include/uavcan/transport/transfer_listener.hpp +++ b/libuavcan/include/uavcan/transport/transfer_listener.hpp @@ -218,9 +218,7 @@ public: void installAcceptanceFilter(const ITransferAcceptanceFilter* acceptance_filter) { - UAVCAN_ASSERT(filter_ == NULL); filter_ = acceptance_filter; - UAVCAN_ASSERT(filter_ != NULL); } }; From b2b7693ee66a14fbfedbdfdcbece241972252f71 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 16 May 2015 14:19:48 +0300 Subject: [PATCH 025/143] Partially implemented and fixed ServiceClient<>, 7 tests are failing --- .../include/uavcan/node/service_client.hpp | 51 +++++++------------ 1 file changed, 17 insertions(+), 34 deletions(-) diff --git a/libuavcan/include/uavcan/node/service_client.hpp b/libuavcan/include/uavcan/node/service_client.hpp index 8a9dced967..feb10702f9 100644 --- a/libuavcan/include/uavcan/node/service_client.hpp +++ b/libuavcan/include/uavcan/node/service_client.hpp @@ -222,10 +222,8 @@ private: TransferListenerType; typedef GenericSubscriber SubscriberType; -#if 0 typedef Multiset CallRegistry; CallRegistry call_registry_; -#endif PublisherType publisher_; Callback callback_; @@ -247,6 +245,7 @@ public: */ explicit ServiceClient(INode& node, const Callback& callback = Callback()) : SubscriberType(node) + , call_registry_(node.getAllocator()) , publisher_(node, getDefaultRequestTimeout()) , callback_(callback) { @@ -300,15 +299,15 @@ public: const Callback& getCallback() const { return callback_; } void setCallback(const Callback& cb) { callback_ = cb; } -#if 0 + /** + * Complexity is O(N) of number of pending calls. + */ unsigned getNumPendingCalls() const { return call_registry_.getSize(); } -#endif -#if 0 + /** + * Complexity is O(1). + */ bool hasPendingCalls() const { return !call_registry_.isEmpty(); } -#else - bool hasPendingCalls() const { return false; } -#endif /** * Returns the number of failed attempts to decode received response. Generally, a failed attempt means either: @@ -353,13 +352,9 @@ bool ServiceClient::shouldAcceptFrame(con { UAVCAN_ASSERT(frame.getTransferType() == TransferTypeServiceResponse); // Other types filtered out by dispatcher -#if 0 - return call_registry_.findFirst(CallStateMatchingPredicate(ServiceCallID(frame.getSrcNodeID(), - frame.getTransferID()))) != NULL; -#else - (void)frame; - return false; -#endif + return NULL != call_registry_.find(CallStateMatchingPredicate(ServiceCallID(frame.getSrcNodeID(), + frame.getTransferID()))); + } template @@ -387,7 +382,6 @@ void ServiceClient::handleTimeout(Service template int ServiceClient::addCallState(ServiceCallID call_id) { -#if 0 if (call_registry_.isEmpty()) { const int subscriber_res = SubscriberType::startAsServiceResponseListener(); @@ -398,30 +392,25 @@ int ServiceClient::addCallState(ServiceCa } } - if (call_registry_.add(CallState(SubscriberType::getNode(), *this, call_id)) == NULL) + if (NULL == call_registry_.template emplace(SubscriberType::getNode(), + *this, call_id)) { SubscriberType::stop(); return -ErrMemory; } return 0; -#else - (void)call_id; - return -ErrNotInited; -#endif } template -int ServiceClient::call(NodeID server_node_id, - const RequestType& request) +int ServiceClient::call(NodeID server_node_id, const RequestType& request) { ServiceCallID dummy; return call(server_node_id, request, dummy); } template -int ServiceClient::call(NodeID server_node_id, - const RequestType& request, +int ServiceClient::call(NodeID server_node_id, const RequestType& request, ServiceCallID& out_call_id) { if (!try_implicit_cast(callback_, true)) @@ -475,29 +464,23 @@ int ServiceClient::call(NodeID server_nod return publisher_res; } - return 0; + return publisher_res; } template void ServiceClient::cancel(ServiceCallID call_id) { -#if 0 - call_registry_.remove(call_id); + call_registry_.removeFirstWhere(CallStateMatchingPredicate(call_id)); if (call_registry_.isEmpty()) { SubscriberType::stop(); } -#else - (void)call_id; -#endif } template void ServiceClient::cancelAll() { -#if 0 - call_registry_.removeAll(); -#endif + call_registry_.clear(); SubscriberType::stop(); } From da98060a5876a392d6e2a0a268125477bcda4df4 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 16 May 2015 15:03:53 +0300 Subject: [PATCH 026/143] Nasty bug in ServiceClient<>::call() --- libuavcan/include/uavcan/node/service_client.hpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/libuavcan/include/uavcan/node/service_client.hpp b/libuavcan/include/uavcan/node/service_client.hpp index feb10702f9..b0ba225baa 100644 --- a/libuavcan/include/uavcan/node/service_client.hpp +++ b/libuavcan/include/uavcan/node/service_client.hpp @@ -422,7 +422,6 @@ int ServiceClient::call(NodeID server_nod /* * 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, out_call_id); if (prep_res < 0) @@ -457,13 +456,15 @@ int ServiceClient::call(NodeID server_nod /* * 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(out_call_id); return publisher_res; } + UAVCAN_ASSERT(server_node_id == out_call_id.server_node_id); return publisher_res; } From e921f4da026f6823b30d48b6acb133fed8e387ee Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 16 May 2015 15:07:21 +0300 Subject: [PATCH 027/143] More debug outputs --- libuavcan/include/uavcan/node/generic_subscriber.hpp | 6 ++++++ libuavcan/test/node/service_client.cpp | 6 ++++++ 2 files changed, 12 insertions(+) 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/test/node/service_client.cpp b/libuavcan/test/node/service_client.cpp index fa08b7214a..bef7ea69c1 100644 --- a/libuavcan/test/node/service_client.cpp +++ b/libuavcan/test/node/service_client.cpp @@ -105,10 +105,14 @@ TEST(ServiceClient, Basic) root_ns_a::StringService::Request request; request.string_request = "Hello world"; + std::cout << "!!! Calling!" << std::endl; + ASSERT_LT(0, client1.call(1, request)); // OK ASSERT_LT(0, client2.call(1, request)); // OK ASSERT_LT(0, client3.call(99, request)); // Will timeout! + std::cout << "!!! Spinning!" << std::endl; + ASSERT_EQ(3, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Listening now! ASSERT_TRUE(client1.hasPendingCalls()); @@ -117,6 +121,8 @@ TEST(ServiceClient, Basic) nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(20)); + std::cout << "!!! Spin finished!" << std::endl; + ASSERT_EQ(1, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Third is still listening! ASSERT_FALSE(client1.hasPendingCalls()); From cbbb3bd9bebea5541d04db4ade2837d3a0d90ca1 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 16 May 2015 15:12:10 +0300 Subject: [PATCH 028/143] All tests are passing --- .../test/protocol/dynamic_node_id_server/node_discoverer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 9a469c4461..7d1d692976 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp @@ -173,7 +173,7 @@ TEST(dynamic_node_id_server_NodeDiscoverer, Basic) get_node_info_server.response.hardware_version.unique_id[14] = 52; ASSERT_LE(0, get_node_info_server.start()); - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(400)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(500)); ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound)); ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart)); From 34200c18bed6d9eaab34cf1c84980a3568fa32d9 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 16 May 2015 16:07:52 +0300 Subject: [PATCH 029/143] New logic of file.Read --- dsdl/uavcan/protocol/file/218.Read.uavcan | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) 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 From 81533eda46b7d3bed851cac0da193bfd8cd26cf0 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 16 May 2015 21:04:25 +0300 Subject: [PATCH 030/143] Method to generate immediate deadlines in DeadlineHandler --- libuavcan/include/uavcan/node/scheduler.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/libuavcan/include/uavcan/node/scheduler.hpp b/libuavcan/include/uavcan/node/scheduler.hpp index 847f76b9d9..0d1d82553c 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(); From 9ba6050af15548701f5cff0c99c6a27c252046fc Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 16 May 2015 21:12:15 +0300 Subject: [PATCH 031/143] ServiceClient<>: proper destruction of CallState objects via execution relaying --- .../include/uavcan/node/service_client.hpp | 69 +++++++++++++++---- libuavcan/src/node/uc_service_client.cpp | 13 +++- 2 files changed, 66 insertions(+), 16 deletions(-) diff --git a/libuavcan/include/uavcan/node/service_client.hpp b/libuavcan/include/uavcan/node/service_client.hpp index b0ba225baa..1ebd2f5bcb 100644 --- a/libuavcan/include/uavcan/node/service_client.hpp +++ b/libuavcan/include/uavcan/node/service_client.hpp @@ -121,7 +121,8 @@ static Stream& operator<<(Stream& s, const ServiceCallResult& scr) /** * Do not use directly. */ -class ServiceClientBase : public ITransferAcceptanceFilter, Noncopyable +class ServiceClientBase : protected ITransferAcceptanceFilter + , protected DeadlineHandler { const DataTypeDescriptor* data_type_descriptor_; ///< This will be initialized at the time of first call @@ -130,6 +131,7 @@ protected: { ServiceClientBase& owner_; const ServiceCallID id_; + bool timed_out_; virtual void handleDeadline(MonotonicTime); @@ -138,12 +140,17 @@ protected: : DeadlineHandler(node.getScheduler()) , owner_(owner) , id_(call_id) + , timed_out_(false) { UAVCAN_ASSERT(id_.isValid()); DeadlineHandler::startWithDelay(owner_.request_timeout_); } - bool doesMatch(ServiceCallID call_id) const { return call_id == id_; } + 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 { @@ -155,13 +162,14 @@ protected: { const ServiceCallID id; CallStateMatchingPredicate(ServiceCallID reference) : id(reference) { } - bool operator()(const CallState& state) const { return state.doesMatch(id); } + bool operator()(const CallState& state) const { return state.getCallID() == id; } }; MonotonicDuration request_timeout_; - ServiceClientBase() - : data_type_descriptor_(NULL) + ServiceClientBase(INode& node) + : DeadlineHandler(node.getScheduler()) + , data_type_descriptor_(NULL) , request_timeout_(getDefaultRequestTimeout()) { } @@ -169,8 +177,6 @@ protected: int prepareToCall(INode& node, const char* dtname, NodeID server_node_id, ServiceCallID& out_call_id); - virtual void handleTimeout(ServiceCallID call_id) = 0; - public: /** * It's not recommended to override default timeouts. @@ -223,6 +229,27 @@ private: 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_; @@ -234,7 +261,7 @@ private: virtual void handleReceivedDataStruct(ReceivedDataStructure& response); - virtual void handleTimeout(ServiceCallID call_id); + virtual void handleDeadline(MonotonicTime); int addCallState(ServiceCallID call_id); @@ -245,6 +272,7 @@ public: */ explicit ServiceClient(INode& node, const Callback& callback = Callback()) : SubscriberType(node) + , ServiceClientBase(node) , call_registry_(node.getAllocator()) , publisher_(node, getDefaultRequestTimeout()) , callback_(callback) @@ -371,12 +399,27 @@ handleReceivedDataStruct(ReceivedDataStructure& response) template -void ServiceClient::handleTimeout(ServiceCallID call_id) +void ServiceClient::handleDeadline(MonotonicTime) { - cancel(call_id); - ServiceCallResultType result(ServiceCallResultType::ErrorTimeout, call_id, - SubscriberType::getReceivedStructStorage()); // Mutable! - invokeCallback(result); + 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()) + { + SubscriberType::stop(); + } } template diff --git a/libuavcan/src/node/uc_service_client.cpp b/libuavcan/src/node/uc_service_client.cpp index f686db20c5..49c8179a3a 100644 --- a/libuavcan/src/node/uc_service_client.cpp +++ b/libuavcan/src/node/uc_service_client.cpp @@ -11,11 +11,18 @@ namespace uavcan */ void ServiceClientBase::CallState::handleDeadline(MonotonicTime) { - UAVCAN_ASSERT(id_.isValid()); - UAVCAN_TRACE("ServiceClient", "Timeout from nid=%d, tid=%d, dtname=%s", + 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()); - owner_.handleTimeout(id_); + /* + * 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"); } /* From 90d60688b3d32242aea25e5944d561aab42bd1c7 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 16 May 2015 21:19:54 +0300 Subject: [PATCH 032/143] ServiceClient<>: renaming and a minor logic fix --- .../include/uavcan/node/service_client.hpp | 20 ++++++++++--------- .../distributed/raft_core.hpp | 4 ++-- .../protocol/network_compat_checker.hpp | 2 +- 3 files changed, 14 insertions(+), 12 deletions(-) diff --git a/libuavcan/include/uavcan/node/service_client.hpp b/libuavcan/include/uavcan/node/service_client.hpp index 1ebd2f5bcb..576ea47bcb 100644 --- a/libuavcan/include/uavcan/node/service_client.hpp +++ b/libuavcan/include/uavcan/node/service_client.hpp @@ -162,7 +162,7 @@ protected: { const ServiceCallID id; CallStateMatchingPredicate(ServiceCallID reference) : id(reference) { } - bool operator()(const CallState& state) const { return state.getCallID() == id; } + bool operator()(const CallState& state) const { return (state.getCallID() == id) && !state.hasTimedOut(); } }; MonotonicDuration request_timeout_; @@ -283,7 +283,7 @@ public: #endif } - virtual ~ServiceClient() { cancelAll(); } + virtual ~ServiceClient() { cancelAllCalls(); } /** * Shall be called before first use. @@ -314,12 +314,12 @@ public: /** * Cancels certain call referred via call ID structure. */ - void cancel(ServiceCallID call_id); + void cancelCall(ServiceCallID call_id); /** * Cancels all pending calls. */ - void cancelAll(); + void cancelAllCalls(); /** * Service response callback must be set prior service call. @@ -329,11 +329,13 @@ public: /** * 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(); } @@ -392,7 +394,7 @@ handleReceivedDataStruct(ReceivedDataStructure& response) UAVCAN_ASSERT(response.getTransferType() == TransferTypeServiceResponse); ServiceCallID call_id(response.getSrcNodeID(), response.getTransferID()); - cancel(call_id); + cancelCall(call_id); ServiceCallResultType result(ServiceCallResultType::Success, call_id, response); // Mutable! invokeCallback(result); } @@ -491,7 +493,7 @@ int ServiceClient::call(NodeID server_nod if (tl == NULL) { UAVCAN_ASSERT(0); // Must have been created - cancel(out_call_id); + cancelCall(out_call_id); return -ErrLogic; } tl->installAcceptanceFilter(this); @@ -503,7 +505,7 @@ int ServiceClient::call(NodeID server_nod out_call_id.transfer_id); if (publisher_res < 0) { - cancel(out_call_id); + cancelCall(out_call_id); return publisher_res; } @@ -512,7 +514,7 @@ int ServiceClient::call(NodeID server_nod } template -void ServiceClient::cancel(ServiceCallID call_id) +void ServiceClient::cancelCall(ServiceCallID call_id) { call_registry_.removeFirstWhere(CallStateMatchingPredicate(call_id)); if (call_registry_.isEmpty()) @@ -522,7 +524,7 @@ void ServiceClient::cancel(ServiceCallID } template -void ServiceClient::cancelAll() +void ServiceClient::cancelAllCalls() { call_registry_.clear(); SubscriberType::stop(); 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 634bd002bd..89f84c2260 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 @@ -327,9 +327,9 @@ class RaftCore : private TimerBase for (uint8_t i = 0; i < NumRequestVoteClients; i++) { - request_vote_clients_[i]->cancelAll(); // TODO FIXME Concurrent calls!! + request_vote_clients_[i]->cancelAllCalls(); // TODO FIXME Concurrent calls!! } - append_entries_client_.cancelAll(); + append_entries_client_.cancelAllCalls(); /* * Calling the switch handler diff --git a/libuavcan/include/uavcan/protocol/network_compat_checker.hpp b/libuavcan/include/uavcan/protocol/network_compat_checker.hpp index 3ac0ae0264..461085eb05 100644 --- a/libuavcan/include/uavcan/protocol/network_compat_checker.hpp +++ b/libuavcan/include/uavcan/protocol/network_compat_checker.hpp @@ -253,7 +253,7 @@ public: exit: ns_sub_.stop(); - cats_cln_.cancelAll(); + cats_cln_.cancelAllCalls(); return res; } From dbf3d1622ead3ba134892a647e24b269cbbedac5 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 16 May 2015 21:25:27 +0300 Subject: [PATCH 033/143] Improved test of ServiceClient<> --- libuavcan/test/node/service_client.cpp | 31 ++++++++++++++++++++++---- 1 file changed, 27 insertions(+), 4 deletions(-) diff --git a/libuavcan/test/node/service_client.cpp b/libuavcan/test/node/service_client.cpp index bef7ea69c1..ea1c339a62 100644 --- a/libuavcan/test/node/service_client.cpp +++ b/libuavcan/test/node/service_client.cpp @@ -33,10 +33,18 @@ struct ServiceCallResultHandler 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 Date: Sat, 16 May 2015 22:17:14 +0300 Subject: [PATCH 034/143] ServiceClient<>: test of concurrent call logic --- libuavcan/test/node/service_client.cpp | 123 +++++++++++++++++++++++++ libuavcan/test/node/test_node.hpp | 2 +- 2 files changed, 124 insertions(+), 1 deletion(-) diff --git a/libuavcan/test/node/service_client.cpp b/libuavcan/test/node/service_client.cpp index ea1c339a62..4022de062e 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,6 +23,7 @@ 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) { @@ -29,6 +31,7 @@ struct ServiceCallResultHandler 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 @@ -218,6 +221,126 @@ TEST(ServiceClient, Rejection) } +TEST(ServiceClient, ConcurrentCalls) +{ + InterlinkedTestNodesWithSysClock nodes; + + // Type registration + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _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 5aaa60288d..418159aa33 100644 --- a/libuavcan/test/node/test_node.hpp +++ b/libuavcan/test/node/test_node.hpp @@ -11,7 +11,7 @@ struct TestNode : public uavcan::INode { - uavcan::PoolAllocator pool; + uavcan::PoolAllocator pool; uavcan::PoolManager<1> poolmgr; uavcan::MarshalBufferProvider<> buffer_provider; uavcan::OutgoingTransferRegistry<8> otr; From 16a9d206c6ab285ca370a044e56a4e2d477a9084 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 16 May 2015 22:26:04 +0300 Subject: [PATCH 035/143] ServiceClient documentation --- libuavcan/include/uavcan/node/service_client.hpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/libuavcan/include/uavcan/node/service_client.hpp b/libuavcan/include/uavcan/node/service_client.hpp index 576ea47bcb..47035056a1 100644 --- a/libuavcan/include/uavcan/node/service_client.hpp +++ b/libuavcan/include/uavcan/node/service_client.hpp @@ -190,12 +190,19 @@ public: /** * 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 @@ -299,7 +306,7 @@ 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. * * Returns negative error code. @@ -308,6 +315,7 @@ public: /** * 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. */ int call(NodeID server_node_id, const RequestType& request, ServiceCallID& out_call_id); @@ -347,7 +355,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. */ From 02fe76cd6f09b8b872713fbbd79cdfc6c968a236 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 16 May 2015 22:38:42 +0300 Subject: [PATCH 036/143] Simplified Multiset<> --- libuavcan/include/uavcan/util/multiset.hpp | 122 +++++++-------------- libuavcan/test/util/multiset.cpp | 34 ------ 2 files changed, 39 insertions(+), 117 deletions(-) diff --git a/libuavcan/include/uavcan/util/multiset.hpp b/libuavcan/include/uavcan/util/multiset.hpp index d796b1bff6..aae7d295f4 100644 --- a/libuavcan/include/uavcan/util/multiset.hpp +++ b/libuavcan/include/uavcan/util/multiset.hpp @@ -24,11 +24,12 @@ namespace uavcan * 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 MultisetBase : Noncopyable +template +class UAVCAN_EXPORT Multiset : Noncopyable { -protected: struct Item : ::uavcan::Noncopyable { T* ptr; @@ -116,13 +117,16 @@ private: } }; + /* + * Data + */ LinkedListRoot list_; IPoolAllocator& allocator_; -#if !UAVCAN_TINY - Item* const static_; - const unsigned num_static_entries_; -#endif + Item static_[NumStaticEntries]; + /* + * Methods + */ Item* findOrCreateFreeSlot(); void compact(); @@ -186,28 +190,16 @@ private: } }; -protected: -#if UAVCAN_TINY - MultisetBase(IPoolAllocator& allocator) - : allocator_(allocator) - { - UAVCAN_ASSERT(Item() == Item()); - } -#else - MultisetBase(Item* static_buf, unsigned num_static_entries, IPoolAllocator& allocator) - : allocator_(allocator) - , static_(static_buf) - , num_static_entries_(num_static_entries) - { } -#endif - - /// Derived class destructor must call clear(); - ~MultisetBase() - { - UAVCAN_ASSERT(getSize() == 0); - } - 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. @@ -292,7 +284,7 @@ public: template const T* find(Predicate predicate) const { - return const_cast*>(this)->find(predicate); + return const_cast(this)->find(predicate); } /** @@ -329,7 +321,7 @@ public: const T* getByIndex(unsigned index) const { - return const_cast*>(this)->getByIndex(index); + return const_cast(this)->getByIndex(index); } /** @@ -350,53 +342,17 @@ public: unsigned getNumDynamicItems() const; }; - -template -class UAVCAN_EXPORT Multiset : public MultisetBase -{ - typename MultisetBase::Item static_[NumStaticEntries]; - -public: - -#if !UAVCAN_TINY - - // This instantiation will not be valid in UAVCAN_TINY mode - explicit Multiset(IPoolAllocator& allocator) - : MultisetBase(static_, NumStaticEntries, allocator) - { } - - ~Multiset() { this->clear(); } - -#endif // !UAVCAN_TINY -}; - - -template -class UAVCAN_EXPORT Multiset : public MultisetBase -{ -public: - explicit Multiset(IPoolAllocator& allocator) -#if UAVCAN_TINY - : MultisetBase(allocator) -#else - : MultisetBase(NULL, 0, allocator) -#endif - { } - - ~Multiset() { this->clear(); } -}; - // ---------------------------------------------------------------------------- /* - * MultisetBase<> + * Multiset<> */ -template -typename MultisetBase::Item* MultisetBase::findOrCreateFreeSlot() +template +typename Multiset::Item* Multiset::findOrCreateFreeSlot() { #if !UAVCAN_TINY // Search in static pool - for (unsigned i = 0; i < num_static_entries_; i++) + for (unsigned i = 0; i < NumStaticEntries; i++) { if (!static_[i].isConstructed()) { @@ -429,8 +385,8 @@ typename MultisetBase::Item* MultisetBase::findOrCreateFreeSlot() return &chunk->items[0]; } -template -void MultisetBase::compact() +template +void Multiset::compact() { Chunk* p = list_.get(); while (p) @@ -454,14 +410,14 @@ void MultisetBase::compact() } } -template +template template -void MultisetBase::removeWhere(Predicate predicate, const RemoveStrategy strategy) +void Multiset::removeWhere(Predicate predicate, const RemoveStrategy strategy) { unsigned num_removed = 0; #if !UAVCAN_TINY - for (unsigned i = 0; i < num_static_entries_; i++) + for (unsigned i = 0; i < NumStaticEntries; i++) { if (static_[i].isConstructed()) { @@ -512,12 +468,12 @@ void MultisetBase::removeWhere(Predicate predicate, const RemoveStrategy stra } } -template +template template -T* MultisetBase::find(Predicate predicate) +T* Multiset::find(Predicate predicate) { #if !UAVCAN_TINY - for (unsigned i = 0; i < num_static_entries_; i++) + for (unsigned i = 0; i < NumStaticEntries; i++) { if (static_[i].isConstructed()) { @@ -547,12 +503,12 @@ T* MultisetBase::find(Predicate predicate) return NULL; } -template -unsigned MultisetBase::getNumStaticItems() const +template +unsigned Multiset::getNumStaticItems() const { unsigned num = 0; #if !UAVCAN_TINY - for (unsigned i = 0; i < num_static_entries_; i++) + for (unsigned i = 0; i < NumStaticEntries; i++) { num += static_[i].isConstructed() ? 1U : 0U; } @@ -560,8 +516,8 @@ unsigned MultisetBase::getNumStaticItems() const return num; } -template -unsigned MultisetBase::getNumDynamicItems() const +template +unsigned Multiset::getNumDynamicItems() const { unsigned num = 0; Chunk* p = list_.get(); diff --git a/libuavcan/test/util/multiset.cpp b/libuavcan/test/util/multiset.cpp index 35e7376e75..8ff43e69b9 100644 --- a/libuavcan/test/util/multiset.cpp +++ b/libuavcan/test/util/multiset.cpp @@ -214,40 +214,6 @@ TEST(Multiset, Basic) } -TEST(Multiset, NoStatic) -{ - 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("foo"); - ASSERT_EQ(0, pool.getNumUsedBlocks()); - ASSERT_FALSE(mset->getByIndex(0)); - - // Insertion - ASSERT_EQ("a", *mset->emplace("a")); - ASSERT_EQ("b", *mset->emplace("b")); - ASSERT_LE(1, pool.getNumUsedBlocks()); - ASSERT_EQ(0, mset->getNumStaticItems()); - ASSERT_EQ(2, mset->getNumDynamicItems()); - -#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 - // Only C++11 because C++03 uses one entry per pool block which breaks ordering - ASSERT_EQ("a", *mset->getByIndex(0)); - ASSERT_EQ("b", *mset->getByIndex(1)); - ASSERT_FALSE(mset->getByIndex(3)); - ASSERT_FALSE(mset->getByIndex(1000)); -#endif -} - - TEST(Multiset, PrimitiveKey) { using uavcan::Multiset; From fdf5100985e465d0e9f8042dd55c1624181642f5 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 16 May 2015 22:46:33 +0300 Subject: [PATCH 037/143] Safer list traversing in Multiset<> and Map<> --- libuavcan/include/uavcan/util/map.hpp | 21 +++++++++++++++------ libuavcan/include/uavcan/util/multiset.hpp | 13 +++++++++---- 2 files changed, 24 insertions(+), 10 deletions(-) diff --git a/libuavcan/include/uavcan/util/map.hpp b/libuavcan/include/uavcan/util/map.hpp index 1f805fa0a2..ece759af0e 100644 --- a/libuavcan/include/uavcan/util/map.hpp +++ b/libuavcan/include/uavcan/util/map.hpp @@ -417,8 +417,10 @@ void MapBase::removeAllWhere(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; @@ -431,7 +433,8 @@ void MapBase::removeAllWhere(Predicate predicate) } } } - p = p->getNextListNode(); + + p = next_group; } if (num_removed > 0) @@ -461,8 +464,10 @@ const Key* MapBase::find(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; @@ -474,7 +479,8 @@ const Key* MapBase::find(Predicate predicate) const } } } - p = p->getNextListNode(); + + p = next_group; } return NULL; } @@ -505,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; @@ -519,7 +527,8 @@ typename MapBase::KVPair* MapBase::getByIndex(unsigned i index--; } } - p = p->getNextListNode(); + + p = next_group; } return NULL; diff --git a/libuavcan/include/uavcan/util/multiset.hpp b/libuavcan/include/uavcan/util/multiset.hpp index aae7d295f4..90e4bf3d6c 100644 --- a/libuavcan/include/uavcan/util/multiset.hpp +++ b/libuavcan/include/uavcan/util/multiset.hpp @@ -435,8 +435,10 @@ void Multiset::removeWhere(Predicate predicate, const Remov #endif Chunk* p = list_.get(); - while (p) + 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; @@ -459,7 +461,7 @@ void Multiset::removeWhere(Predicate predicate, const Remov } } - p = p->getNextListNode(); + p = next_chunk; } if (num_removed > 0) @@ -486,8 +488,10 @@ T* Multiset::find(Predicate predicate) #endif Chunk* p = list_.get(); - while (p) + 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()) @@ -498,7 +502,8 @@ T* Multiset::find(Predicate predicate) } } } - p = p->getNextListNode(); + + p = next_chunk; } return NULL; } From 3f9cad4f3b353cee381e09083c3c05ccd70c10f9 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 16 May 2015 23:17:26 +0300 Subject: [PATCH 038/143] Multiset: Simpler type handling in predicate adapter template --- libuavcan/include/uavcan/util/multiset.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libuavcan/include/uavcan/util/multiset.hpp b/libuavcan/include/uavcan/util/multiset.hpp index 90e4bf3d6c..c1a99d31ea 100644 --- a/libuavcan/include/uavcan/util/multiset.hpp +++ b/libuavcan/include/uavcan/util/multiset.hpp @@ -171,9 +171,9 @@ private: template struct OperatorToFalsePredicateAdapter : ::uavcan::Noncopyable { - const typename ParameterType::Type oper; + Operator oper; - OperatorToFalsePredicateAdapter(typename ParameterType::Type o) + OperatorToFalsePredicateAdapter(Operator o) : oper(o) { } From 7df9fb082076faaa206bfe66c9ee3b38829462ee Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 16 May 2015 23:17:54 +0300 Subject: [PATCH 039/143] NodeInfoRetriever - using Multiset instead of Map<> --- .../uavcan/protocol/node_info_retriever.hpp | 44 ++++++++++++------- 1 file changed, 28 insertions(+), 16 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/node_info_retriever.hpp b/libuavcan/include/uavcan/protocol/node_info_retriever.hpp index 29391a15a6..1d9d16322d 100644 --- a/libuavcan/include/uavcan/protocol/node_info_retriever.hpp +++ b/libuavcan/include/uavcan/protocol/node_info_retriever.hpp @@ -7,7 +7,7 @@ #include #include -#include +#include #include #include #include @@ -109,7 +109,7 @@ private: , node_info(arg_node_info) { } - bool operator()(INodeInfoListener* key, bool) + bool operator()(INodeInfoListener* key) { UAVCAN_ASSERT(key != NULL); key->handleNodeInfoRetrieved(node_id, node_info); @@ -128,7 +128,7 @@ private: , event(arg_event) { } - bool operator()(INodeInfoListener* key, bool) + bool operator()(INodeInfoListener* key) { UAVCAN_ASSERT(key != NULL); (key->*method)(event); @@ -141,7 +141,7 @@ private: */ Entry entries_[NodeID::Max]; // [1, NodeID::Max] - Map listeners_; // Only keys are used + Multiset listeners_; ServiceClient get_node_info_client_; @@ -253,7 +253,7 @@ private: } } - listeners_.removeAllWhere( + listeners_.forEach( GenericHandlerCaller(&INodeInfoListener::handleNodeStatusChange, event)); } @@ -271,7 +271,7 @@ private: entry.uptime_sec = msg.uptime_sec; entry.updated_since_last_attempt = true; - listeners_.removeAllWhere(GenericHandlerCaller&>( + listeners_.forEach(GenericHandlerCaller&>( &INodeInfoListener::handleNodeStatusMessage, msg)); } @@ -287,8 +287,8 @@ private: */ entry.uptime_sec = result.getResponse().status.uptime_sec; entry.request_needed = false; - listeners_.removeAllWhere(NodeInfoRetrievedHandlerCaller(result.getCallID().server_node_id, - result.getResponse())); + listeners_.forEach(NodeInfoRetrievedHandlerCaller(result.getCallID().server_node_id, + result.getResponse())); } else { @@ -298,8 +298,8 @@ private: if (entry.num_attempts_made >= num_attempts_) { entry.request_needed = false; - listeners_.removeAllWhere(GenericHandlerCaller( - &INodeInfoListener::handleNodeInfoUnavailable, result.getCallID().server_node_id)); + listeners_.forEach(GenericHandlerCaller(&INodeInfoListener::handleNodeInfoUnavailable, + result.getCallID().server_node_id)); } } } @@ -340,14 +340,20 @@ public: } /** - * Adds one listener to the set. + * 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) { - UAVCAN_ASSERT(listener != NULL); - bool* value = listeners_.insert(listener, true); - return (value == NULL) ? -ErrMemory : 0; + if (listener != NULL) + { + removeListener(listener); + return (NULL == listeners_.emplace(listener)) ? -ErrMemory : 0; + } + else + { + return -ErrInvalidParam; + } } /** @@ -356,8 +362,14 @@ public: */ void removeListener(INodeInfoListener* listener) { - UAVCAN_ASSERT(listener != NULL); - listeners_.remove(listener); + if (listener != NULL) + { + listeners_.removeAll(listener); + } + else + { + UAVCAN_ASSERT(0); + } } /** From 5e5540b8ce3020b7ef826e994aeb70c4e08148e8 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 16 May 2015 23:57:11 +0300 Subject: [PATCH 040/143] ServiceClient<>::hasPendingCallToServer() --- .../include/uavcan/node/service_client.hpp | 18 ++++++++++++++++++ libuavcan/test/node/service_client.cpp | 9 +++++++++ 2 files changed, 27 insertions(+) diff --git a/libuavcan/include/uavcan/node/service_client.hpp b/libuavcan/include/uavcan/node/service_client.hpp index 47035056a1..b5a84715fa 100644 --- a/libuavcan/include/uavcan/node/service_client.hpp +++ b/libuavcan/include/uavcan/node/service_client.hpp @@ -165,6 +165,13 @@ protected: 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) @@ -329,6 +336,11 @@ public: */ void cancelAllCalls(); + /** + * Checks whether there's currently a pending call addressed to the specified node ID. + */ + bool hasPendingCallToServer(NodeID server_node_id) const; + /** * Service response callback must be set prior service call. */ @@ -538,6 +550,12 @@ void ServiceClient::cancelAllCalls() SubscriberType::stop(); } +template +bool ServiceClient::hasPendingCallToServer(NodeID server_node_id) const +{ + return NULL != call_registry_.find(ServerSearchPredicate(server_node_id)); +} + } #endif // UAVCAN_NODE_SERVICE_CLIENT_HPP_INCLUDED diff --git a/libuavcan/test/node/service_client.cpp b/libuavcan/test/node/service_client.cpp index 4022de062e..8b24848a30 100644 --- a/libuavcan/test/node/service_client.cpp +++ b/libuavcan/test/node/service_client.cpp @@ -109,6 +109,8 @@ TEST(ServiceClient, Basic) ASSERT_EQ(0, client2.getNumPendingCalls()); ASSERT_EQ(0, client3.getNumPendingCalls()); + ASSERT_FALSE(client1.hasPendingCallToServer(1)); + client1.setCallback(handler.bind()); client2.setCallback(client1.getCallback()); client3.setCallback(client1.getCallback()); @@ -128,6 +130,11 @@ TEST(ServiceClient, Basic) ASSERT_LT(0, client3.call(99, request)); // Will timeout! ASSERT_LT(0, client3.call(1, request)); // OK - second request + ASSERT_TRUE(client1.hasPendingCallToServer(1)); + ASSERT_TRUE(client2.hasPendingCallToServer(1)); + ASSERT_TRUE(client3.hasPendingCallToServer(99)); + ASSERT_TRUE(client3.hasPendingCallToServer(1)); + std::cout << "!!! Spinning!" << std::endl; ASSERT_EQ(3, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Listening now! @@ -211,9 +218,11 @@ TEST(ServiceClient, Rejection) ASSERT_EQ(1, nodes.b.getDispatcher().getNumServiceResponseListeners()); ASSERT_TRUE(client1.hasPendingCalls()); + ASSERT_TRUE(client1.hasPendingCallToServer(1)); nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(200)); ASSERT_FALSE(client1.hasPendingCalls()); + ASSERT_FALSE(client1.hasPendingCallToServer(1)); ASSERT_EQ(0, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Timed out From 2123853cae81aaeb58745344266910093b6d943c Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 17 May 2015 00:01:49 +0300 Subject: [PATCH 041/143] Using concurrent calls in NodeInfoRetriever --- .../uavcan/protocol/node_info_retriever.hpp | 26 +++++++++++++++---- 1 file changed, 21 insertions(+), 5 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/node_info_retriever.hpp b/libuavcan/include/uavcan/protocol/node_info_retriever.hpp index 1d9d16322d..ceffa9bf28 100644 --- a/libuavcan/include/uavcan/protocol/node_info_retriever.hpp +++ b/libuavcan/include/uavcan/protocol/node_info_retriever.hpp @@ -69,7 +69,6 @@ class UAVCAN_EXPORT NodeInfoRetriever : NodeStatusMonitor { public: enum { MaxNumRequestAttempts = 254 }; - enum { DefaultNumRequestAttempts = 30 }; enum { UnlimitedRequestAttempts = 0 }; private: @@ -136,6 +135,9 @@ private: } }; + enum { NumStaticCalls = 4 }; + enum { DefaultNumRequestAttempts = 30 }; + /* * State */ @@ -143,11 +145,12 @@ private: Multiset listeners_; - ServiceClient get_node_info_client_; + ServiceClient get_node_info_client_; mutable uint8_t last_picked_node_; uint8_t num_attempts_; + uint8_t max_concurrent_requests_; /* * Methods @@ -185,7 +188,9 @@ private: (last_picked_node_ <= NodeID::Max)); const Entry& entry = getEntry(last_picked_node_); - if (entry.request_needed && entry.updated_since_last_attempt) + if (entry.request_needed && + 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_); @@ -196,9 +201,9 @@ private: virtual void handleTimerEvent(const TimerEvent&) { - if (get_node_info_client_.hasPendingCalls()) // If request is pending, this condition will fail every second time + if (get_node_info_client_.getNumPendingCalls() >= max_concurrent_requests_) { - return; // TODO FIXME Concurrent calls!! + return; } const NodeID next = pickNextNodeToQuery(); @@ -313,6 +318,7 @@ public: , get_node_info_client_(node) , last_picked_node_(1) , num_attempts_(DefaultNumRequestAttempts) + , max_concurrent_requests_(NumStaticCalls * 2) { } /** @@ -382,6 +388,16 @@ public: { num_attempts_ = min(static_cast(MaxNumRequestAttempts), num); } + + /** + * Number of concurrent requests limits the number of simultaneous service calls to different nodes. + * This value cannot be less than one. + */ + uint8_t getNumConcurrentRequests() const { return max_concurrent_requests_; } + void setNumConcurrentRequests(uint8_t num) + { + max_concurrent_requests_ = max(static_cast(1), num); + } }; } From 36dda9c0175f2eb4f9dd7ea772f01a2bf62baa67 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 17 May 2015 13:49:40 +0300 Subject: [PATCH 042/143] NodeInfoRetriever basic test --- .../uavcan/protocol/node_info_retriever.hpp | 15 ++++ .../test/protocol/node_info_retriever.cpp | 70 +++++++++++++++++-- 2 files changed, 81 insertions(+), 4 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/node_info_retriever.hpp b/libuavcan/include/uavcan/protocol/node_info_retriever.hpp index ceffa9bf28..4a10321e11 100644 --- a/libuavcan/include/uavcan/protocol/node_info_retriever.hpp +++ b/libuavcan/include/uavcan/protocol/node_info_retriever.hpp @@ -172,6 +172,8 @@ private: if (!TimerBase::isRunning()) { TimerBase::startPeriodic(getTimerPollInterval()); + UAVCAN_TRACE("NodeInfoRetriever", "Timer started, interval %ld ms", + static_cast(getTimerPollInterval().toMSec())); } } @@ -230,6 +232,7 @@ private: if (!requests_needed) { TimerBase::stop(); + UAVCAN_TRACE("NodeInfoRetriever", "Timer stopped"); } } } @@ -398,6 +401,18 @@ public: { max_concurrent_requests_ = max(static_cast(1), num); } + + /** + * 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); + } }; } diff --git a/libuavcan/test/protocol/node_info_retriever.cpp b/libuavcan/test/protocol/node_info_retriever.cpp index 04f2d346ac..2b8f92d5d9 100644 --- a/libuavcan/test/protocol/node_info_retriever.cpp +++ b/libuavcan/test/protocol/node_info_retriever.cpp @@ -54,7 +54,8 @@ struct NodeInfoListener : public uavcan::INodeInfoListener virtual void handleNodeStatusChange(const uavcan::NodeStatusMonitor::NodeStatusChangeEvent& event) { - (void)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++; } @@ -102,10 +103,16 @@ TEST(NodeInfoRetriever, Basic) ASSERT_LE(0, provider->startAndPublish()); + ASSERT_FALSE(retr.isRetrievingInProgress()); + ASSERT_EQ(0, retr.getNumPendingRequests()); + /* * Waiting for discovery */ - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1600)); + 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); @@ -120,6 +127,8 @@ TEST(NodeInfoRetriever, Basic) /* * Declaring a bunch of different nodes that don't support GetNodeInfo */ + ASSERT_FALSE(retr.isRetrievingInProgress()); + retr.setNumRequestAttempts(3); uavcan::TransferID tid; @@ -128,12 +137,65 @@ TEST(NodeInfoRetriever, Basic) 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(2100)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(110)); + ASSERT_EQ(1, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(110)); + ASSERT_EQ(2, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(110)); + 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); - // TODO finish the test when the logic is fixed + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(110)); + ASSERT_EQ(1, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(110)); + ASSERT_EQ(2, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(110)); + 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(110)); + ASSERT_EQ(1, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(110)); + ASSERT_EQ(2, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(110)); + 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(110)); + ASSERT_EQ(1, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(110)); + 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); } From 600c29a9539787ee4de555dd1cbeeeae1854a93f Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 17 May 2015 16:29:19 +0300 Subject: [PATCH 043/143] NodeInfoRetriever - docs, logical fixes, tests --- .../uavcan/protocol/node_info_retriever.hpp | 73 +++++++++----- .../test/protocol/node_info_retriever.cpp | 94 ++++++++++++++++--- 2 files changed, 135 insertions(+), 32 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/node_info_retriever.hpp b/libuavcan/include/uavcan/protocol/node_info_retriever.hpp index 4a10321e11..92a5e42471 100644 --- a/libuavcan/include/uavcan/protocol/node_info_retriever.hpp +++ b/libuavcan/include/uavcan/protocol/node_info_retriever.hpp @@ -61,8 +61,32 @@ public: /** * 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. - * Events from this class can be routed to many subscribers. + * 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] = foor(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 @@ -135,8 +159,9 @@ private: } }; - enum { NumStaticCalls = 4 }; - enum { DefaultNumRequestAttempts = 30 }; + enum { NumStaticCalls = 2 }; + enum { DefaultNumRequestAttempts = 16 }; + enum { DefaultTimerIntervalMSec = 40 }; ///< Read explanation in the class documentation /* * State @@ -147,16 +172,15 @@ private: ServiceClient get_node_info_client_; + MonotonicDuration request_interval_; + mutable uint8_t last_picked_node_; uint8_t num_attempts_; - uint8_t max_concurrent_requests_; /* * Methods */ - static MonotonicDuration getTimerPollInterval() { return MonotonicDuration::fromMSec(100); } - const Entry& getEntry(NodeID node_id) const { return const_cast(this)->getEntry(node_id); } Entry& getEntry(NodeID node_id) { @@ -171,9 +195,8 @@ private: { if (!TimerBase::isRunning()) { - TimerBase::startPeriodic(getTimerPollInterval()); - UAVCAN_TRACE("NodeInfoRetriever", "Timer started, interval %ld ms", - static_cast(getTimerPollInterval().toMSec())); + TimerBase::startPeriodic(request_interval_); + UAVCAN_TRACE("NodeInfoRetriever", "Timer started, interval %s sec", request_interval_.toString().c_str()); } } @@ -203,11 +226,6 @@ private: virtual void handleTimerEvent(const TimerEvent&) { - if (get_node_info_client_.getNumPendingCalls() >= max_concurrent_requests_) - { - return; - } - const NodeID next = pickNextNodeToQuery(); if (next.isUnicast()) { @@ -319,9 +337,9 @@ public: , TimerBase(node) , listeners_(node.getAllocator()) , get_node_info_client_(node) + , request_interval_(MonotonicDuration::fromMSec(DefaultTimerIntervalMSec)) , last_picked_node_(1) , num_attempts_(DefaultNumRequestAttempts) - , max_concurrent_requests_(NumStaticCalls * 2) { } /** @@ -381,6 +399,8 @@ public: } } + 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. @@ -393,13 +413,24 @@ public: } /** - * Number of concurrent requests limits the number of simultaneous service calls to different nodes. - * This value cannot be less than one. + * Request interval also implicitly defines the maximum number of concurrent requests. + * Read the class documentation for details. */ - uint8_t getNumConcurrentRequests() const { return max_concurrent_requests_; } - void setNumConcurrentRequests(uint8_t num) + MonotonicDuration getRequestInterval() const { return request_interval_; } + void setRequestInterval(const MonotonicDuration interval) { - max_concurrent_requests_ = max(static_cast(1), num); + if (interval.isPositive()) + { + request_interval_ = interval; + if (TimerBase::isRunning()) + { + TimerBase::startPeriodic(request_interval_); + } + } + else + { + UAVCAN_ASSERT(0); + } } /** diff --git a/libuavcan/test/protocol/node_info_retriever.cpp b/libuavcan/test/protocol/node_info_retriever.cpp index 2b8f92d5d9..71088ebbef 100644 --- a/libuavcan/test/protocol/node_info_retriever.cpp +++ b/libuavcan/test/protocol/node_info_retriever.cpp @@ -92,6 +92,9 @@ TEST(NodeInfoRetriever, Basic) 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; @@ -106,6 +109,8 @@ TEST(NodeInfoRetriever, Basic) ASSERT_FALSE(retr.isRetrievingInProgress()); ASSERT_EQ(0, retr.getNumPendingRequests()); + EXPECT_EQ(40, retr.getRequestInterval().toMSec()); // Default + /* * Waiting for discovery */ @@ -137,11 +142,11 @@ TEST(NodeInfoRetriever, Basic) 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(110)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(45)); ASSERT_EQ(1, retr.getNumPendingRequests()); - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(110)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); ASSERT_EQ(2, retr.getNumPendingRequests()); - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(110)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); ASSERT_EQ(3, retr.getNumPendingRequests()); nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000)); ASSERT_TRUE(retr.isRetrievingInProgress()); @@ -151,11 +156,11 @@ TEST(NodeInfoRetriever, Basic) 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(110)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(45)); ASSERT_EQ(1, retr.getNumPendingRequests()); - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(110)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); ASSERT_EQ(2, retr.getNumPendingRequests()); - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(110)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); ASSERT_EQ(3, retr.getNumPendingRequests()); nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000)); ASSERT_TRUE(retr.isRetrievingInProgress()); @@ -165,11 +170,11 @@ TEST(NodeInfoRetriever, Basic) 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(110)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(45)); ASSERT_EQ(1, retr.getNumPendingRequests()); - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(110)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); ASSERT_EQ(2, retr.getNumPendingRequests()); - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(110)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); ASSERT_EQ(3, retr.getNumPendingRequests()); nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000)); ASSERT_TRUE(retr.isRetrievingInProgress()); @@ -181,9 +186,9 @@ TEST(NodeInfoRetriever, Basic) tid.increment(); publishNodeStatus(nodes.can_a, uavcan::NodeID(12), 0, 11, tid); - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(110)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(45)); ASSERT_EQ(1, retr.getNumPendingRequests()); - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(110)); + 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()); @@ -199,3 +204,70 @@ TEST(NodeInfoRetriever, Basic) 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 = 13; // 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()); +} From c089f4d72bfb8639cf367bd2e77e1a3cb831f2cd Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 17 May 2015 16:34:04 +0300 Subject: [PATCH 044/143] Node info retriever - timer event optimization --- .../uavcan/protocol/node_info_retriever.hpp | 36 ++++++++++--------- 1 file changed, 19 insertions(+), 17 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/node_info_retriever.hpp b/libuavcan/include/uavcan/protocol/node_info_retriever.hpp index 92a5e42471..af81caa4b4 100644 --- a/libuavcan/include/uavcan/protocol/node_info_retriever.hpp +++ b/libuavcan/include/uavcan/protocol/node_info_retriever.hpp @@ -200,8 +200,10 @@ private: } } - NodeID pickNextNodeToQuery() const + 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_++; @@ -213,22 +215,31 @@ private: (last_picked_node_ <= NodeID::Max)); const Entry& entry = getEntry(last_picked_node_); - if (entry.request_needed && - entry.updated_since_last_attempt && - !get_node_info_client_.hasPendingCallToServer(last_picked_node_)) + + if (entry.request_needed) { - UAVCAN_TRACE("NodeInfoRetriever", "Next node to query: %d", int(last_picked_node_)); - return NodeID(last_picked_node_); + 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&) { - const NodeID next = pickNextNodeToQuery(); + 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) @@ -238,16 +249,7 @@ private: } else { - bool requests_needed = false; - for (uint8_t i = 1; i <= NodeID::Max; i++) - { - if (getEntry(i).request_needed) - { - requests_needed = true; - break; - } - } - if (!requests_needed) + if (!at_least_one_request_needed) { TimerBase::stop(); UAVCAN_TRACE("NodeInfoRetriever", "Timer stopped"); From e5fddfdb666bf9b93f9601e314926451869a6f48 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 17 May 2015 17:05:58 +0300 Subject: [PATCH 045/143] Node info retriever unit test fix --- .../test/protocol/node_info_retriever.cpp | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/libuavcan/test/protocol/node_info_retriever.cpp b/libuavcan/test/protocol/node_info_retriever.cpp index 71088ebbef..88fc4aace8 100644 --- a/libuavcan/test/protocol/node_info_retriever.cpp +++ b/libuavcan/test/protocol/node_info_retriever.cpp @@ -142,10 +142,10 @@ TEST(NodeInfoRetriever, Basic) 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(45)); - ASSERT_EQ(1, retr.getNumPendingRequests()); nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); - ASSERT_EQ(2, retr.getNumPendingRequests()); + 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)); @@ -156,10 +156,10 @@ TEST(NodeInfoRetriever, Basic) 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(45)); - ASSERT_EQ(1, retr.getNumPendingRequests()); nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); - ASSERT_EQ(2, retr.getNumPendingRequests()); + 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)); @@ -170,10 +170,10 @@ TEST(NodeInfoRetriever, Basic) 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(45)); - ASSERT_EQ(1, retr.getNumPendingRequests()); nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); - ASSERT_EQ(2, retr.getNumPendingRequests()); + 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)); @@ -186,7 +186,7 @@ TEST(NodeInfoRetriever, Basic) tid.increment(); publishNodeStatus(nodes.can_a, uavcan::NodeID(12), 0, 11, tid); - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(45)); + 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 @@ -235,7 +235,7 @@ TEST(NodeInfoRetriever, MaxConcurrentRequests) ASSERT_EQ(40, retr.getRequestInterval().toMSec()); - const unsigned MaxPendingRequests = 13; // See class docs + const unsigned MaxPendingRequests = 14; // See class docs const unsigned MinPendingRequestsAtFullLoad = 12; /* From cd41840f5919f2763c85f229aebe8faab7b44a87 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 17 May 2015 17:18:14 +0300 Subject: [PATCH 046/143] Multi-call client in RaftCore --- .../distributed/raft_core.hpp | 36 +++++++------------ 1 file changed, 12 insertions(+), 24 deletions(-) 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 89f84c2260..b77dfadf9e 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 @@ -138,9 +138,8 @@ class RaftCore : private TimerBase ServiceClient append_entries_client_; ServiceServer request_vote_srv_; - enum { NumRequestVoteClients = ClusterManager::MaxClusterSize - 1 }; - LazyConstructor > - request_vote_clients_[NumRequestVoteClients]; + enum { NumRequestVoteCalls = ClusterManager::MaxClusterSize - 1 }; + ServiceClient request_vote_client_; /* * Methods @@ -218,7 +217,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 < NumRequestVoteCalls; i++) { const NodeID node_id = cluster_.getRemoteServerNodeIDAtIndex(i); if (!node_id.isUnicast()) @@ -230,7 +229,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); @@ -325,10 +324,7 @@ 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]->cancelAllCalls(); // TODO FIXME Concurrent calls!! - } + request_vote_client_.cancelAllCalls(); append_entries_client_.cancelAllCalls(); /* @@ -729,12 +725,8 @@ public: , 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. @@ -790,17 +782,13 @@ public: &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)); + request_vote_client_.setRequestTimeout(update_interval_); startPeriodic(update_interval_); From 58ca7319dd12a85ebf6a72d2dabd0b2eba8d5365 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 18 May 2015 14:00:26 +0300 Subject: [PATCH 047/143] File server implementation with test --- dsdl/uavcan/protocol/file/Error.uavcan | 19 +- .../include/uavcan/protocol/file_server.hpp | 180 ++++++++++++++++++ libuavcan/test/protocol/file_server.cpp | 118 ++++++++++++ 3 files changed, 308 insertions(+), 9 deletions(-) create mode 100644 libuavcan/include/uavcan/protocol/file_server.hpp create mode 100644 libuavcan/test/protocol/file_server.cpp 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/include/uavcan/protocol/file_server.hpp b/libuavcan/include/uavcan/protocol/file_server.hpp new file mode 100644 index 0000000000..2339eeb6b0 --- /dev/null +++ b/libuavcan/include/uavcan/protocol/file_server.hpp @@ -0,0 +1,180 @@ +/* + * 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. + */ +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. + * 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. + * 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 (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; + } +}; + +} + +#endif // Include guard diff --git a/libuavcan/test/protocol/file_server.cpp b/libuavcan/test/protocol/file_server.cpp new file mode 100644 index 0000000000..641ad2f9d2 --- /dev/null +++ b/libuavcan/test/protocol/file_server.cpp @@ -0,0 +1,118 @@ +/* + * 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); + } +} From 51a2ce39c5b5a225dade7ce434278ba98c64765d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 18 May 2015 14:21:58 +0300 Subject: [PATCH 048/143] Full file server implementation --- .../include/uavcan/protocol/file_server.hpp | 84 +++++++++++++++++++ libuavcan/test/protocol/file_server.cpp | 24 ++++++ 2 files changed, 108 insertions(+) diff --git a/libuavcan/include/uavcan/protocol/file_server.hpp b/libuavcan/include/uavcan/protocol/file_server.hpp index 2339eeb6b0..d525b70156 100644 --- a/libuavcan/include/uavcan/protocol/file_server.hpp +++ b/libuavcan/include/uavcan/protocol/file_server.hpp @@ -175,6 +175,90 @@ public: } }; +/** + * 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/test/protocol/file_server.cpp b/libuavcan/test/protocol/file_server.cpp index 641ad2f9d2..f3591e79e3 100644 --- a/libuavcan/test/protocol/file_server.cpp +++ b/libuavcan/test/protocol/file_server.cpp @@ -116,3 +116,27 @@ TEST(BasicFileServer, Basic) 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 +} From e4886606f0966d6bec65d9ef7f93bd2b37cb6cb4 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 18 May 2015 14:31:10 +0300 Subject: [PATCH 049/143] Typo --- libuavcan/include/uavcan/protocol/node_info_retriever.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libuavcan/include/uavcan/protocol/node_info_retriever.hpp b/libuavcan/include/uavcan/protocol/node_info_retriever.hpp index af81caa4b4..5b27a9b68f 100644 --- a/libuavcan/include/uavcan/protocol/node_info_retriever.hpp +++ b/libuavcan/include/uavcan/protocol/node_info_retriever.hpp @@ -71,7 +71,7 @@ public: * 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] = foor(5000 [ms] bootloader timeout / 123 nodes) + * 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 From 46afa99b27a1e9b6ce0753d480408c1cd9d5e5ff Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 18 May 2015 15:09:52 +0300 Subject: [PATCH 050/143] Refactored POSIX tools --- CMakeLists.txt | 1 + .../{posix_tools => posix}/include.mk | 0 .../file_event_tracer.hpp | 43 +++++++++++-------- .../file_storage_backend.hpp | 40 +++++++---------- 4 files changed, 41 insertions(+), 43 deletions(-) rename libuavcan_drivers/{posix_tools => posix}/include.mk (100%) rename libuavcan_drivers/{posix_tools/include/posix_tools => posix/include/uavcan_posix/dynamic_node_id_server}/file_event_tracer.hpp (65%) rename libuavcan_drivers/{posix_tools/include/posix_tools => posix/include/uavcan_posix/dynamic_node_id_server}/file_storage_backend.hpp (88%) 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/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_tools/include/posix_tools/file_event_tracer.hpp b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp similarity index 65% rename from libuavcan_drivers/posix_tools/include/posix_tools/file_event_tracer.hpp rename to libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp index 719e34694d..60fe4448fb 100644 --- a/libuavcan_drivers/posix_tools/include/posix_tools/file_event_tracer.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp @@ -1,11 +1,17 @@ -/* - * Copyright (C) 2015 Pavel Kirienko - */ +/**************************************************************************** +* +* Copyright (c) 2015 PX4 Development Team. All rights reserved. +* Author: Pavel Kirienko +* David Sidrane +* +****************************************************************************/ -#pragma once +#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 namespace uavcan_posix { @@ -19,9 +25,7 @@ class FileEventTracer : public uavcan::dynamic_node_id_server::IEventTracer /** * Maximum length of full path to log file */ - - enum { MaxPathLength = 128, FormatBufferLength = 64 }; - + enum { MaxPathLength = 128 }; /** * This type is used for the path @@ -29,32 +33,32 @@ class FileEventTracer : public uavcan::dynamic_node_id_server::IEventTracer 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) { + using namespace std; + struct timespec ts; clock_gettime(CLOCK_REALTIME, &ts); + int fd = open(path_.c_str(), O_WRONLY | O_CREAT | O_APPEND); if (fd >= 0 ) { + const int FormatBufferLength = 64; char buffer[FormatBufferLength + 1]; - int n = snprintf(buffer, FormatBufferLength, "%d.%ld,%d,%lld\n", ts.tv_sec, ts.tv_nsec, code, argument); + 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 - * - */ +public: + FileEventTracer() { } + + /** + * Initializes the file based event tracer. + */ int init(const PathString & path) { using namespace std; @@ -73,8 +77,9 @@ public: } 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 88% 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..139004dccd 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,16 @@ * ****************************************************************************/ -#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 @@ -30,24 +31,16 @@ class FileStorageBackend : public uavcan::dynamic_node_id_server::IStorageBacken /** * Maximum length of full path including / and key max */ - enum { MaxPathLength = 128 }; - /** * This type is used for the path */ typedef uavcan::Array, uavcan::ArrayModeDynamic, MaxPathLength> PathString; - PathString base_path; -public: - - FileStorageBackend() { } - - virtual String get(const String& key) const { using namespace std; @@ -75,7 +68,6 @@ public: } } return value; - } virtual void set(const String& key, const String& value) @@ -91,16 +83,17 @@ public: } } - /** - * Initializes the File based back end storage by passing to a path to - * the directory where the key named files will be stored. - * This the return result 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; - * - */ +public: + FileStorageBackend() { } - int init(const PathString & path) + /** + * Initializes the file based backend storage by passing a path to + * the directory where the key named files will be stored. + * 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. + */ + int init(const PathString& path) { using namespace std; @@ -108,7 +101,6 @@ public: if (path.size() > 0) { - base_path = path.c_str(); if (base_path.back() == '/') @@ -132,10 +124,10 @@ public: } } return rv; - } - }; } } + +#endif // Include guard From 09a96061adb47ed1e6e81856ec4cf42e8e6a920e Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 18 May 2015 17:02:17 +0300 Subject: [PATCH 051/143] Fixed and improved file event tracer + POSIX test --- libuavcan_drivers/linux/CMakeLists.txt | 5 ++ libuavcan_drivers/linux/apps/test_posix.cpp | 50 +++++++++++++++++++ libuavcan_drivers/posix/CMakeLists.txt | 12 +++++ .../file_event_tracer.hpp | 22 +++++--- .../file_storage_backend.hpp | 1 + 5 files changed, 82 insertions(+), 8 deletions(-) create mode 100644 libuavcan_drivers/linux/apps/test_posix.cpp create mode 100644 libuavcan_drivers/posix/CMakeLists.txt diff --git a/libuavcan_drivers/linux/CMakeLists.txt b/libuavcan_drivers/linux/CMakeLists.txt index a88c7da13a..fd8ed5eef1 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,9 @@ 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}) + # # Tools # diff --git a/libuavcan_drivers/linux/apps/test_posix.cpp b/libuavcan_drivers/linux/apps/test_posix.cpp new file mode 100644 index 0000000000..be432e1f36 --- /dev/null +++ b/libuavcan_drivers/linux/apps/test_posix.cpp @@ -0,0 +1,50 @@ +/* + * 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())); + } + + return 0; + } + catch (const std::exception& ex) + { + std::cerr << "Exception: " << ex.what() << std::endl; + return 1; + } +} 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/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 index 60fe4448fb..3746592534 100644 --- 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 @@ -10,8 +10,10 @@ #define UAVCAN_POSIX_DYNAMIC_NODE_ID_SERVER_FILE_EVENT_TRACER_HPP_INCLUDED #include +#include #include #include +#include namespace uavcan_posix { @@ -27,6 +29,8 @@ class FileEventTracer : public uavcan::dynamic_node_id_server::IEventTracer */ enum { MaxPathLength = 128 }; + enum { FilePermissions = 438 }; ///< 0o666 + /** * This type is used for the path */ @@ -39,15 +43,17 @@ class FileEventTracer : public uavcan::dynamic_node_id_server::IEventTracer { using namespace std; - struct timespec ts; - clock_gettime(CLOCK_REALTIME, &ts); + 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); - if (fd >= 0 ) + int fd = open(path_.c_str(), O_WRONLY | O_CREAT | O_APPEND, FilePermissions); + if (fd >= 0) { - const int FormatBufferLength = 64; + const int FormatBufferLength = 63; char buffer[FormatBufferLength + 1]; - int n = snprintf(buffer, FormatBufferLength, "%d.%ld,%d,%lld\n", ts.tv_sec, ts.tv_nsec, code, argument); + 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)); write(fd, buffer, n); close(fd); } @@ -69,8 +75,8 @@ public: { rv = 0; path_ = path.c_str(); - int fd = open(path_.c_str(), O_RDWR | O_CREAT | O_TRUNC); - if ( fd >= 0) + int fd = open(path_.c_str(), O_RDWR | O_CREAT | O_TRUNC, FilePermissions); + if (fd >= 0) { close(fd); } diff --git a/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp index 139004dccd..20346e588c 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp @@ -15,6 +15,7 @@ #include #include #include +#include #include #include From 3c073ac9d421dff92480d2a26e2d106d9d5557a3 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 18 May 2015 17:08:49 +0300 Subject: [PATCH 052/143] Simple test for POSIX storage backend --- libuavcan_drivers/linux/apps/test_posix.cpp | 23 +++++++++++++++++++ .../file_storage_backend.hpp | 4 +++- 2 files changed, 26 insertions(+), 1 deletion(-) diff --git a/libuavcan_drivers/linux/apps/test_posix.cpp b/libuavcan_drivers/linux/apps/test_posix.cpp index be432e1f36..a59aa1c3ef 100644 --- a/libuavcan_drivers/linux/apps/test_posix.cpp +++ b/libuavcan_drivers/linux/apps/test_posix.cpp @@ -40,6 +40,29 @@ int main(int argc, const char** argv) 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) diff --git a/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp index 20346e588c..e0ccf7cd3d 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp @@ -34,6 +34,8 @@ class FileStorageBackend : public uavcan::dynamic_node_id_server::IStorageBacken */ enum { MaxPathLength = 128 }; + enum { FilePermissions = 438 }; ///< 0o666 + /** * This type is used for the path */ @@ -76,7 +78,7 @@ class FileStorageBackend : public uavcan::dynamic_node_id_server::IStorageBacken 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()); From 875c74d88e0c3fcf68e173498dafd422bc614031 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 18 May 2015 17:11:05 +0300 Subject: [PATCH 053/143] Removed useless constructors --- .../uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp | 2 -- .../dynamic_node_id_server/file_storage_backend.hpp | 2 -- 2 files changed, 4 deletions(-) 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 index 3746592534..c8e97244dd 100644 --- 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 @@ -60,8 +60,6 @@ class FileEventTracer : public uavcan::dynamic_node_id_server::IEventTracer } public: - FileEventTracer() { } - /** * Initializes the file based event tracer. */ diff --git a/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp index e0ccf7cd3d..e4ce8c8e23 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp @@ -87,8 +87,6 @@ class FileStorageBackend : public uavcan::dynamic_node_id_server::IStorageBacken } public: - FileStorageBackend() { } - /** * Initializes the file based backend storage by passing a path to * the directory where the key named files will be stored. From 5e458e918d7606ffa89dc79ee357391799b501a9 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 18 May 2015 22:29:09 +0300 Subject: [PATCH 054/143] MakeString<> helper template --- libuavcan/include/uavcan/marshal/array.hpp | 13 +++++++++++++ .../dynamic_node_id_server/storage_backend.hpp | 2 +- .../dynamic_node_id_server/file_event_tracer.hpp | 3 +-- .../dynamic_node_id_server/file_storage_backend.hpp | 3 +-- 4 files changed, 16 insertions(+), 5 deletions(-) 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/protocol/dynamic_node_id_server/storage_backend.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/storage_backend.hpp index 9e970dd671..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 @@ -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_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 index c8e97244dd..3398a54418 100644 --- 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 @@ -34,8 +34,7 @@ class FileEventTracer : public uavcan::dynamic_node_id_server::IEventTracer /** * This type is used for the path */ - typedef uavcan::Array, - uavcan::ArrayModeDynamic, MaxPathLength> PathString; + typedef uavcan::MakeString::Type PathString; PathString path_; diff --git a/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp index e4ce8c8e23..fdf173cc13 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp @@ -39,8 +39,7 @@ class FileStorageBackend : public uavcan::dynamic_node_id_server::IStorageBacken /** * This type is used for the path */ - typedef uavcan::Array, - uavcan::ArrayModeDynamic, MaxPathLength> PathString; + typedef uavcan::MakeString::Type PathString; PathString base_path; From 6b179d032bb146a6d5925ec2254cf9b082cb412f Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 19 May 2015 01:37:10 +0300 Subject: [PATCH 055/143] Firmware update trigger implementation. It is most likely broken, because I'm half asleep by now; proper tests will be added later --- .../protocol/firmware_update_trigger.hpp | 430 ++++++++++++++++++ .../test/protocol/firmware_update_trigger.cpp | 83 ++++ 2 files changed, 513 insertions(+) create mode 100644 libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp create mode 100644 libuavcan/test/protocol/firmware_update_trigger.cpp 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..9f08371c98 --- /dev/null +++ b/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp @@ -0,0 +1,430 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_FILE_SERVER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_FILE_SERVER_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 shouldSendFirmwareUpdateRequest(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 a request simply + * times out, nothing will be sent. + * + * 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. + * + * @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 shouldRetryFirmwareUpdateRequest(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. + * + * @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) = 0; + + 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 NodeIDSelectorPredicate + { + const FirmwareUpdateTrigger& owner; + + NodeIDSelectorPredicate(const FirmwareUpdateTrigger& arg_owner) + : owner(arg_owner) + { } + + bool operator()(const NodeID node_id, const FirmwareFilePath&) + { + return + (node_id > owner.last_queried_node_id_) && + !owner.begin_fw_update_client_.hasPendingCallToServer(node_id); + } + }; + + /* + * 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) { /* Not used */ } + + virtual void handleNodeInfoRetrieved(const NodeID node_id, const protocol::GetNodeInfo::Response& node_info) + { + FirmwareFilePath firmware_file_path; + const bool update_needed = checker_.shouldSendFirmwareUpdateRequest(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 + { + 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) + { + FirmwareFilePath* const value = pending_nodes_.access(node_id); + if (value != NULL) + { + *value = 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<> + const NodeID* found = pending_nodes_.find(NodeIDSelectorPredicate(*this)); + if (found == NULL) + { + // Resetting the round-robin selector and trying again + last_queried_node_id_ = 0; + found = pending_nodes_.find(NodeIDSelectorPredicate(*this)); + if (found == NULL) + { + return NodeID(); + } + } + + UAVCAN_ASSERT(found != NULL); + UAVCAN_ASSERT(found->get() >= last_queried_node_id_); + + last_queried_node_id_ = found->get(); + UAVCAN_ASSERT(NodeID(last_queried_node_id_).isUnicast()); + + UAVCAN_TRACE("FirmwareUpdateTrigger", "Next node ID to query: %d", int(last_queried_node_id_)); + 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; + } + + const bool confirmed = + result.getResponse().error == protocol::file::BeginFirmwareUpdate::Response::ERROR_OK || + result.getResponse().error == protocol::file::BeginFirmwareUpdate::Response::ERROR_IN_PROGRESS; + + if (confirmed) + { + pending_nodes_.remove(result.getCallID().server_node_id); + checker_.handleFirmwareUpdateConfirmation(result.getCallID().server_node_id, result.getResponse()); + } + else + { + FirmwareFilePath firmware_file_path; + const bool update_needed = + checker_.shouldRetryFirmwareUpdateRequest(result.getCallID().server_node_id, result.getResponse(), + firmware_file_path); + if (update_needed) + { + UAVCAN_TRACE("FirmwareUpdateTrigger", "Node ID %d requires retry; file path: %s", + int(result.getCallID().server_node_id.get()), firmware_file_path.c_str()); + trySetPendingNode(result.getCallID().server_node_id, firmware_file_path); + } + else + { + 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); + 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 class. 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 isTriggeringInProgress() const { return TimerBase::isRunning(); } +}; + +} + +#endif // Include guard diff --git a/libuavcan/test/protocol/firmware_update_trigger.cpp b/libuavcan/test/protocol/firmware_update_trigger.cpp new file mode 100644 index 0000000000..566a25fc07 --- /dev/null +++ b/libuavcan/test/protocol/firmware_update_trigger.cpp @@ -0,0 +1,83 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include +#include "helpers.hpp" + +using namespace uavcan::protocol::file; + +struct FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker +{ + virtual bool shouldSendFirmwareUpdateRequest(uavcan::NodeID node_id, + const uavcan::protocol::GetNodeInfo::Response& node_info, + FirmwareFilePath& out_firmware_file_path) + { + (void)node_id; + (void)node_info; + (void)out_firmware_file_path; + return false; + } + + virtual bool shouldRetryFirmwareUpdateRequest(uavcan::NodeID node_id, + const BeginFirmwareUpdate::Response& error_response, + FirmwareFilePath& out_firmware_file_path) + { + (void)node_id; + (void)error_response; + (void)out_firmware_file_path; + return false; + } + + virtual void handleFirmwareUpdateConfirmation(uavcan::NodeID node_id, + const BeginFirmwareUpdate::Response& response) + { + (void)node_id; + (void)response; + } +}; + +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()); + + /* + * Discovering one node + */ + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2000)); +} From 694d29ab47d0d943e69fee8aa3ca84e223a49c95 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 19 May 2015 02:10:22 +0300 Subject: [PATCH 056/143] Misleading comment that somehow survived refactoring --- libuavcan/include/uavcan/protocol/node_info_retriever.hpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/node_info_retriever.hpp b/libuavcan/include/uavcan/protocol/node_info_retriever.hpp index 5b27a9b68f..017102fa40 100644 --- a/libuavcan/include/uavcan/protocol/node_info_retriever.hpp +++ b/libuavcan/include/uavcan/protocol/node_info_retriever.hpp @@ -119,9 +119,6 @@ private: } }; - /* - * Callers are used with removeWhere() predicate. They don't actually remove anything. - */ struct NodeInfoRetrievedHandlerCaller { const NodeID node_id; From d7ae3f90c0a460bf07d5a1019abae30fe5a0167b Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 19 May 2015 02:13:50 +0300 Subject: [PATCH 057/143] Doc fix --- libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp b/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp index 9f08371c98..6ab44f329a 100644 --- a/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp +++ b/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp @@ -54,8 +54,8 @@ public: FirmwareFilePath& out_firmware_file_path) = 0; /** - * This method will be invoked when a node responds to the update request with an error. If a request simply - * times out, nothing will be sent. + * 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. * * 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. From 0d60595d7c9025bd3db714b79421427002252a99 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 19 May 2015 13:16:51 +0300 Subject: [PATCH 058/143] FirmwareUpdateTrigger - fixes and test --- .../protocol/firmware_update_trigger.hpp | 39 +++-- .../test/protocol/firmware_update_trigger.cpp | 145 ++++++++++++++++-- 2 files changed, 152 insertions(+), 32 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp b/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp index 6ab44f329a..9aaa65c97a 100644 --- a/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp +++ b/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp @@ -50,8 +50,8 @@ public: * @return True - the class will begin sending update requests. * False - the node will be ignored, no request will be sent. */ - virtual bool shouldSendFirmwareUpdateRequest(NodeID node_id, const protocol::GetNodeInfo::Response& node_info, - FirmwareFilePath& out_firmware_file_path) = 0; + 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 @@ -69,9 +69,9 @@ public: * @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 shouldRetryFirmwareUpdateRequest(NodeID node_id, - const protocol::file::BeginFirmwareUpdate::Response& error_response, - FirmwareFilePath& out_firmware_file_path) = 0; + 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. @@ -170,12 +170,16 @@ class FirmwareUpdateTrigger : public INodeInfoListener, /* * Methods of INodeInfoListener */ - virtual void handleNodeInfoUnavailable(NodeID) { /* Not used */ } + 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_.shouldSendFirmwareUpdateRequest(node_id, node_info, 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", @@ -205,10 +209,8 @@ class FirmwareUpdateTrigger : public INodeInfoListener, void trySetPendingNode(const NodeID node_id, const FirmwareFilePath& path) { - FirmwareFilePath* const value = pending_nodes_.access(node_id); - if (value != NULL) + if (NULL != pending_nodes_.insert(node_id, path)) { - *value = path; if (!TimerBase::isRunning()) { TimerBase::startPeriodic(request_interval_); @@ -268,8 +270,8 @@ class FirmwareUpdateTrigger : public INodeInfoListener, { FirmwareFilePath firmware_file_path; const bool update_needed = - checker_.shouldRetryFirmwareUpdateRequest(result.getCallID().server_node_id, result.getResponse(), - firmware_file_path); + checker_.shouldRetryFirmwareUpdate(result.getCallID().server_node_id, result.getResponse(), + firmware_file_path); if (update_needed) { UAVCAN_TRACE("FirmwareUpdateTrigger", "Node ID %d requires retry; file path: %s", @@ -301,7 +303,7 @@ class FirmwareUpdateTrigger : public INodeInfoListener, FirmwareFilePath* const path = pending_nodes_.access(node_id); if (path == NULL) { - UAVCAN_ASSERT(0); + UAVCAN_ASSERT(0); // pickNextNodeID() returned a node ID that is not present in the map return; } @@ -345,7 +347,7 @@ public: } /** - * Starts the class. Once started, it can't be stopped unless destroyed. + * 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. @@ -422,7 +424,14 @@ public: * This method is mostly needed for testing. * When triggering is not in progress, the class consumes zero CPU time. */ - bool isTriggeringInProgress() const { return TimerBase::isRunning(); } + bool isTimerRunning() const { return TimerBase::isRunning(); } + + unsigned getNumPendingNodes() const + { + const unsigned ret = pending_nodes_.getSize(); + UAVCAN_ASSERT((ret > 0) ? isTimerRunning() : true); + return ret; + } }; } diff --git a/libuavcan/test/protocol/firmware_update_trigger.cpp b/libuavcan/test/protocol/firmware_update_trigger.cpp index 566a25fc07..9b1910a701 100644 --- a/libuavcan/test/protocol/firmware_update_trigger.cpp +++ b/libuavcan/test/protocol/firmware_update_trigger.cpp @@ -11,34 +11,66 @@ using namespace uavcan::protocol::file; struct FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker { - virtual bool shouldSendFirmwareUpdateRequest(uavcan::NodeID node_id, - const uavcan::protocol::GetNodeInfo::Response& node_info, - FirmwareFilePath& out_firmware_file_path) + unsigned should_request_cnt; + unsigned should_retry_cnt; + unsigned confirmation_cnt; + + std::string firmware_path; + + bool should_retry; + std::string expected_node_name_to_update; + + BeginFirmwareUpdate::Response last_error_response; + + FirmwareVersionChecker() + : should_request_cnt(0) + , should_retry_cnt(0) + , confirmation_cnt(0) + , should_retry(false) + { } + + virtual bool shouldRequestFirmwareUpdate(uavcan::NodeID node_id, + const uavcan::protocol::GetNodeInfo::Response& node_info, + FirmwareFilePath& out_firmware_file_path) { - (void)node_id; - (void)node_info; - (void)out_firmware_file_path; - return false; + 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 shouldRetryFirmwareUpdateRequest(uavcan::NodeID node_id, - const BeginFirmwareUpdate::Response& error_response, - FirmwareFilePath& out_firmware_file_path) + virtual bool shouldRetryFirmwareUpdate(uavcan::NodeID node_id, + const BeginFirmwareUpdate::Response& error_response, + FirmwareFilePath& out_firmware_file_path) { - (void)node_id; - (void)error_response; - (void)out_firmware_file_path; - return false; + last_error_response = error_response; + std::cout << "RETRY? " << int(node_id.get()) << "\n" << error_response << std::endl; + should_retry_cnt++; + out_firmware_file_path = firmware_path.c_str(); + return should_retry; } virtual void handleFirmwareUpdateConfirmation(uavcan::NodeID node_id, const BeginFirmwareUpdate::Response& response) { - (void)node_id; - (void)response; + confirmation_cnt++; + std::cout << "CONFIRMED " << int(node_id.get()) << "\n" << response << std::endl; } }; +static uint8_t response_error_code = 0; + +static void beginFirmwareUpdateRequestCallback( + const uavcan::ReceivedDataStructure& req, + uavcan::ServiceResponseDataStructure& res) +{ + std::cout << "REQUEST\n" << req << std::endl; + + res.error = response_error_code; + res.optional_error_message = "foobar"; +} + + TEST(FirmwareUpdateTrigger, Basic) { uavcan::GlobalDataTypeRegistry::instance().reset(); @@ -76,8 +108,87 @@ TEST(FirmwareUpdateTrigger, Basic) ASSERT_LE(0, provider->startAndPublish()); + ASSERT_FALSE(trigger.isTimerRunning()); + ASSERT_EQ(0, trigger.getNumPendingNodes()); + /* - * Discovering one node + * 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.should_retry = true; + 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); + + ASSERT_LE(0, srv.start(beginFirmwareUpdateRequestCallback)); + + response_error_code = BeginFirmwareUpdate::Response::ERROR_UNKNOWN; + checker.should_retry = true; + + 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 + */ + response_error_code = BeginFirmwareUpdate::Response::ERROR_IN_PROGRESS; + checker.should_retry = false; + + 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()); } From 1047a2587292e1f7c81dc973b38ad167c5de650d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 19 May 2015 13:22:52 +0300 Subject: [PATCH 059/143] FirmwareUpdateTrigger - fixes and test --- libuavcan/test/protocol/firmware_update_trigger.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/libuavcan/test/protocol/firmware_update_trigger.cpp b/libuavcan/test/protocol/firmware_update_trigger.cpp index 9b1910a701..d37679c0c5 100644 --- a/libuavcan/test/protocol/firmware_update_trigger.cpp +++ b/libuavcan/test/protocol/firmware_update_trigger.cpp @@ -191,4 +191,10 @@ TEST(FirmwareUpdateTrigger, Basic) // 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); } From 228785b8f8921be705ed7d855bb7584602169f6d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 20 May 2015 00:12:06 +0300 Subject: [PATCH 060/143] libuavcan testing: TestNetwork<> helper --- libuavcan/test/node/test_node.hpp | 78 +++++++++++++++++-- libuavcan/test/node/test_node_test.cpp | 32 ++++++++ .../test/protocol/global_time_sync_master.cpp | 6 +- .../test/protocol/global_time_sync_slave.cpp | 2 +- 4 files changed, 106 insertions(+), 12 deletions(-) create mode 100644 libuavcan/test/node/test_node_test.cpp diff --git a/libuavcan/test/node/test_node.hpp b/libuavcan/test/node/test_node.hpp index 418159aa33..d36fcc8e2e 100644 --- a/libuavcan/test/node/test_node.hpp +++ b/libuavcan/test/node/test_node.hpp @@ -6,6 +6,8 @@ #include #include +#include +#include #include "../transport/can/can.hpp" @@ -43,21 +45,21 @@ struct TestNode : public uavcan::INode struct PairableCanDriver : public uavcan::ICanDriver, public uavcan::ICanIface { uavcan::ISystemClock& clock; - PairableCanDriver* other; + std::set others; std::queue read_queue; std::queue loopback_queue; uint64_t error_count; PairableCanDriver(uavcan::ISystemClock& clock) : clock(clock) - , other(NULL) , error_count(0) { } void linkTogether(PairableCanDriver* with) { - this->other = with; - with->other = this; + this->others.insert(with); + with->others.insert(this); + others.erase(this); } virtual uavcan::ICanIface* getIface(uavcan::uint8_t iface_index) @@ -73,7 +75,6 @@ struct PairableCanDriver : public uavcan::ICanDriver, public uavcan::ICanIface virtual uavcan::int16_t select(uavcan::CanSelectMasks& inout_masks, uavcan::MonotonicTime blocking_deadline) { - assert(other); if (inout_masks.read == 1) { inout_masks.read = (!read_queue.empty() || !loopback_queue.empty()) ? 1 : 0; @@ -91,8 +92,11 @@ struct PairableCanDriver : public uavcan::ICanDriver, public uavcan::ICanIface virtual uavcan::int16_t send(const uavcan::CanFrame& frame, uavcan::MonotonicTime, uavcan::CanIOFlags flags) { - assert(other); - other->read_queue.push(frame); + assert(!others.empty()); + for (std::set::iterator it = others.begin(); it != others.end(); ++it) + { + (*it)->read_queue.push(frame); + } if (flags & uavcan::CanIOFlagLoopback) { loopback_queue.push(frame); @@ -103,7 +107,6 @@ struct PairableCanDriver : public uavcan::ICanDriver, public uavcan::ICanIface virtual uavcan::int16_t receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic, uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags) { - assert(other); out_flags = 0; if (loopback_queue.empty()) { @@ -186,3 +189,62 @@ struct InterlinkedTestNodes typedef InterlinkedTestNodes InterlinkedTestNodesWithSysClock; typedef InterlinkedTestNodes InterlinkedTestNodesWithClockMock; + + +template +struct TestNetwork +{ + struct NodeEnvironment + { + SystemClockDriver clock; + PairableCanDriver can_driver; + TestNode node; + + NodeEnvironment(uavcan::NodeID node_id) + : can_driver(clock) + , node(can_driver, clock, node_id) + { } + }; + + std::auto_ptr nodes[NumNodes]; + + TestNetwork(uavcan::uint8_t first_node_id = 1) + { + for (uavcan::uint8_t i = 0; i < NumNodes; i++) + { + nodes[i].reset(new NodeEnvironment(first_node_id + i)); + } + + for (uavcan::uint8_t i = 0; i < NumNodes; i++) + { + for (uavcan::uint8_t k = 0; k < NumNodes; k++) + { + nodes[i]->linkTogether(nodes[k].get()); + } + } + + for (uavcan::uint8_t i = 0; i < NumNodes; i++) + { + assert(nodes[i]->others.size() == (NumNodes - 1)); + } + } + + int spinAll(uavcan::MonotonicDuration duration) + { + assert(!duration.isNegative()); + unsigned nspins = unsigned(duration.toMSec() / NumNodes); + nspins = nspins ? nspins : 1; + while (nspins --> 0) + { + for (uavcan::uint8_t i = 0; i < NumNodes; i++) + { + int ret = nodes[i]->spin(uavcan::MonotonicDuration::fromMSec(1)); + if (ret < 0) + { + return ret; + } + } + } + return 0; + } +}; diff --git a/libuavcan/test/node/test_node_test.cpp b/libuavcan/test/node/test_node_test.cpp new file mode 100644 index 0000000000..c5731640eb --- /dev/null +++ b/libuavcan/test/node/test_node_test.cpp @@ -0,0 +1,32 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include "test_node.hpp" + + +TEST(TestNode, TestNetwork) +{ + TestNetwork<4> nwk; + + uavcan::CanFrame frame; + for (int i = 0; i < 8; i++) + { + frame.data[i] = i; + } + frame.id = 1234U; + + ASSERT_EQ(1, nwk.nodes[0]->can_driver.send(frame, uavcan::MonotonicTime(), uavcan::CanIOFlags())); + + for (int i = 1; i < 4; i++) + { + uavcan::CanFrame rx; + uavcan::MonotonicTime ts_mono; + uavcan::UtcTime ts_utc; + uavcan::CanIOFlags flags = 0; + ASSERT_EQ(1, nwk.nodes[i]->can_driver.receive(rx, ts_mono, ts_utc, flags)); + + ASSERT_TRUE(rx == frame); + } +} diff --git a/libuavcan/test/protocol/global_time_sync_master.cpp b/libuavcan/test/protocol/global_time_sync_master.cpp index 404d649027..bed4d908dc 100644 --- a/libuavcan/test/protocol/global_time_sync_master.cpp +++ b/libuavcan/test/protocol/global_time_sync_master.cpp @@ -30,9 +30,9 @@ struct GlobalTimeSyncTestNetwork , master_low(120) , master_high(8) { - slave.can.other = &master_low.can; - master_low.can.other = &slave.can; - master_high.can.other = &slave.can; + slave.can.others.insert(&master_low.can); + master_low.can.others.insert(&slave.can); + master_high.can.others.insert(&slave.can); } void spinAll(uavcan::MonotonicDuration duration = uavcan::MonotonicDuration::fromMSec(9)) diff --git a/libuavcan/test/protocol/global_time_sync_slave.cpp b/libuavcan/test/protocol/global_time_sync_slave.cpp index 5bb2e91c0d..25c4e50975 100644 --- a/libuavcan/test/protocol/global_time_sync_slave.cpp +++ b/libuavcan/test/protocol/global_time_sync_slave.cpp @@ -109,7 +109,7 @@ TEST(GlobalTimeSyncSlave, Basic) master2_clock.monotonic_auto_advance = 1000; master2_clock.preserve_utc = true; PairableCanDriver master2_can(master2_clock); - master2_can.other = &nodes.can_a; + master2_can.others.insert(&nodes.can_a); TestNode master2_node(master2_can, master2_clock, 8); uavcan::Publisher gts_pub2(master2_node); From 4398cceb4cc230ebf9bae9c161fd17636d93b81f Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 20 May 2015 00:41:44 +0300 Subject: [PATCH 061/143] Fixing the previous commit --- libuavcan/test/node/test_node.hpp | 20 +++++++++++++++++--- libuavcan/test/node/test_node_test.cpp | 2 +- 2 files changed, 18 insertions(+), 4 deletions(-) diff --git a/libuavcan/test/node/test_node.hpp b/libuavcan/test/node/test_node.hpp index d36fcc8e2e..4329fc7c21 100644 --- a/libuavcan/test/node/test_node.hpp +++ b/libuavcan/test/node/test_node.hpp @@ -4,6 +4,11 @@ #pragma once +#if __GNUC__ +// We need auto_ptr for compatibility reasons +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#endif + #include #include #include @@ -212,20 +217,20 @@ struct TestNetwork { for (uavcan::uint8_t i = 0; i < NumNodes; i++) { - nodes[i].reset(new NodeEnvironment(first_node_id + 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]->linkTogether(nodes[k].get()); + nodes[i]->can_driver.linkTogether(&nodes[k]->can_driver); } } for (uavcan::uint8_t i = 0; i < NumNodes; i++) { - assert(nodes[i]->others.size() == (NumNodes - 1)); + assert(nodes[i]->can_driver.others.size() == (NumNodes - 1)); } } @@ -247,4 +252,13 @@ struct TestNetwork } 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 index c5731640eb..b40357be21 100644 --- a/libuavcan/test/node/test_node_test.cpp +++ b/libuavcan/test/node/test_node_test.cpp @@ -11,7 +11,7 @@ TEST(TestNode, TestNetwork) TestNetwork<4> nwk; uavcan::CanFrame frame; - for (int i = 0; i < 8; i++) + for (uint8_t i = 0; i < 8; i++) { frame.data[i] = i; } From 19cffa682f5632574f487d497c307be43734abe8 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 20 May 2015 01:20:42 +0300 Subject: [PATCH 062/143] TestNetwork<> fix --- libuavcan/test/node/test_node.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libuavcan/test/node/test_node.hpp b/libuavcan/test/node/test_node.hpp index 4329fc7c21..543f99a50d 100644 --- a/libuavcan/test/node/test_node.hpp +++ b/libuavcan/test/node/test_node.hpp @@ -243,7 +243,7 @@ struct TestNetwork { for (uavcan::uint8_t i = 0; i < NumNodes; i++) { - int ret = nodes[i]->spin(uavcan::MonotonicDuration::fromMSec(1)); + int ret = nodes[i]->node.spin(uavcan::MonotonicDuration::fromMSec(1)); if (ret < 0) { return ret; From ee25e3db45c5c133fe6725ef9781e9fe126ee805 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Tue, 19 May 2015 17:47:37 -1000 Subject: [PATCH 063/143] POSIX File Server Backend --- .../uavcan_posix/file_server_backend.hpp | 180 ++++++++++++++++++ 1 file changed, 180 insertions(+) create mode 100644 libuavcan_drivers/posix/include/uavcan_posix/file_server_backend.hpp diff --git a/libuavcan_drivers/posix/include/uavcan_posix/file_server_backend.hpp b/libuavcan_drivers/posix/include/uavcan_posix/file_server_backend.hpp new file mode 100644 index 0000000000..54bf531ac4 --- /dev/null +++ b/libuavcan_drivers/posix/include/uavcan_posix/file_server_backend.hpp @@ -0,0 +1,180 @@ +/**************************************************************************** +* +* Copyright (c) 2015 PX4 Development Team. All rights reserved. +* Author: Pavel Kirienko +* David Sidrane +* +****************************************************************************/ + +#ifndef UAVCAN_POSIX_FILE_SERVER_BACKEND_HPP_INCLUDED +#define UAVCAN_POSIX_FILE_SERVER_BACKEND_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace uavcan_posix +{ +/** + * This interface implements a POSIX compliant IFileServerBackend interface + */ +class FileSeverBackend : public uavcan::IFileServerBackend +{ + /** + * Maximum length of full path including / the file name + */ + enum { MaxPathLength = Path::MaxSize + 128 }; + + enum { FilePermissions = 438 }; ///< 0o666 + + /** + * This type is used for the path + */ + typedef uavcan::MakeString::Type PathString; + + PathString base_path; + +public: + /** + * + * Backend 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) + { + + enum { MaxBufferLength = 256 }; + + int rv = -uavcan::ErrInvalidParam; + if (path.size()) { + + rv = -uavcan::ErrFailure; + + PathString root_path = base_path.c_str(); + root_path += path; + + uint32_t size = 0; + int fd = open(path.c_str(), O_RDONLY); + if (fd >= 0) + { + uavcan::DataTypeSignatureCRC crc; + ssize_t len; + + uint8_t bytes[MaxBufferLength]; + + do { + + len = ::read(fd, bytes, MaxBufferLength); + + if (len < 0) { + return rv; + } + + if (len > 0) { + size += len; + crc.add(bytes, len); + } + + } while(len); + + out_crc64 = crc.get(); + out_size = size; + EntryType t; + t.flags = uavcan::protocol::file::EntryType::FLAG_FILE; + out_type = t; + rv = 0; + } + } + return rv; + } + + /** + * 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) + { + + int rv = -uavcan::ErrInvalidParam; + + if (path.size()) { + + rv = -uavcan::ErrFailure; + + PathString root_path = base_path.c_str(); + root_path += path; + + int fd = open(path.c_str(), O_RDONLY); + + if (fd >= 0) + { + if (::lseek(fd, offset, SEEK_SET) >= 0) { + + ssize_t len = ::read(fd, out_buffer, inout_size); + + if (len < 0) { + return rv; + } + inout_size = len; + rv = 0; + } + } + } + return rv; + } + + /** + * Initializes the file serrver based backend storage by passing a path to + * the directory where files will be stored. + */ + int init(const PathString& path) + { + using namespace std; + + int rv = -uavcan::ErrInvalidParam; + + if (path.size() > 0) + { + base_path = path.c_str(); + + if (base_path.back() == '/') + { + base_path.pop_back(); + } + + rv = 0; + struct stat sb; + if (stat(base_path.c_str(), &sb) != 0 || !S_ISDIR(sb.st_mode)) + { + rv = mkdir(base_path.c_str(), S_IRWXU | S_IRWXG | S_IRWXO); + } + if (rv >= 0 ) + { + base_path.push_back('/'); + if ((base_path.size() + Path::MaxSize) > MaxPathLength) + { + rv = -uavcan::ErrInvalidConfiguration; + } + } + } + return rv; + } +}; + +} + +#endif // Include guard From 5a0bccf7872fd2f7916e2b46990adb7fd34da8dd Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Tue, 19 May 2015 18:28:19 -1000 Subject: [PATCH 064/143] Update file_server_backend.hpp --- .../posix/include/uavcan_posix/file_server_backend.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/libuavcan_drivers/posix/include/uavcan_posix/file_server_backend.hpp b/libuavcan_drivers/posix/include/uavcan_posix/file_server_backend.hpp index 54bf531ac4..963982bb23 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/file_server_backend.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/file_server_backend.hpp @@ -87,7 +87,7 @@ public: } } while(len); - + close(fd); out_crc64 = crc.get(); out_size = size; EntryType t; @@ -125,6 +125,7 @@ public: if (::lseek(fd, offset, SEEK_SET) >= 0) { ssize_t len = ::read(fd, out_buffer, inout_size); + close(fd); if (len < 0) { return rv; From b8d3884eb68f63ea4a4e939b3a7ca70f9d33f546 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Tue, 19 May 2015 18:54:15 -1000 Subject: [PATCH 065/143] Ensured close in all paths. This still needed the EAGAIN logic added --- .../posix/include/uavcan_posix/file_server_backend.hpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/libuavcan_drivers/posix/include/uavcan_posix/file_server_backend.hpp b/libuavcan_drivers/posix/include/uavcan_posix/file_server_backend.hpp index 963982bb23..d6280e6bc3 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/file_server_backend.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/file_server_backend.hpp @@ -78,7 +78,7 @@ public: len = ::read(fd, bytes, MaxBufferLength); if (len < 0) { - return rv; + goto out_close; } if (len > 0) { @@ -87,7 +87,6 @@ public: } } while(len); - close(fd); out_crc64 = crc.get(); out_size = size; EntryType t; @@ -95,6 +94,9 @@ public: out_type = t; rv = 0; } + +out_close: + close(fd); } return rv; } From 1f47596688b38c8847736cc2a902f9bd69d67e6c Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 20 May 2015 14:53:16 +0300 Subject: [PATCH 066/143] FirmwareUpdateTrigger node selector fix, more testing --- .../protocol/firmware_update_trigger.hpp | 62 +++++-- .../test/protocol/firmware_update_trigger.cpp | 172 ++++++++++++++++-- 2 files changed, 195 insertions(+), 39 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp b/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp index 9aaa65c97a..d95a4b9656 100644 --- a/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp +++ b/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp @@ -134,19 +134,26 @@ class FirmwareUpdateTrigger : public INodeInfoListener, enum { DefaultRequestIntervalMs = 1000 }; ///< Shall not be less than default service response timeout. - struct NodeIDSelectorPredicate + struct NextNodeIDSearchPredicate : ::uavcan::Noncopyable { - const FirmwareUpdateTrigger& owner; + enum { DefaultOutput = 0xFFU }; - NodeIDSelectorPredicate(const FirmwareUpdateTrigger& arg_owner) + const FirmwareUpdateTrigger& owner; + uint8_t output; + + NextNodeIDSearchPredicate(const FirmwareUpdateTrigger& arg_owner) : owner(arg_owner) + , output(DefaultOutput) { } bool operator()(const NodeID node_id, const FirmwareFilePath&) { - return - (node_id > owner.last_queried_node_id_) && - !owner.begin_fw_update_client_.hasPendingCallToServer(node_id); + 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; } }; @@ -188,6 +195,7 @@ class FirmwareUpdateTrigger : public INodeInfoListener, } else { + UAVCAN_TRACE("FirmwareUpdateTrigger", "Node ID %d does not need update", int(node_id.get())); pending_nodes_.remove(node_id); } } @@ -226,25 +234,35 @@ class FirmwareUpdateTrigger : public INodeInfoListener, NodeID pickNextNodeID() const { // We can't do index search because indices are unstable in Map<> - const NodeID* found = pending_nodes_.find(NodeIDSelectorPredicate(*this)); - if (found == NULL) + // First try - from the current node up + NextNodeIDSearchPredicate s1(*this); + (void)pending_nodes_.find(s1); + + if (s1.output != NextNodeIDSearchPredicate::DefaultOutput) { - // Resetting the round-robin selector and trying again + 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; - found = pending_nodes_.find(NodeIDSelectorPredicate(*this)); - if (found == NULL) + + NextNodeIDSearchPredicate s2(*this); + (void)pending_nodes_.find(s2); + + if (s2.output != NextNodeIDSearchPredicate::DefaultOutput) { - return NodeID(); + last_queried_node_id_ = s2.output; } } - - UAVCAN_ASSERT(found != NULL); - UAVCAN_ASSERT(found->get() >= last_queried_node_id_); - - last_queried_node_id_ = found->get(); - UAVCAN_ASSERT(NodeID(last_queried_node_id_).isUnicast()); - - UAVCAN_TRACE("FirmwareUpdateTrigger", "Next node ID to query: %d", int(last_queried_node_id_)); + 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_; } @@ -263,6 +281,8 @@ class FirmwareUpdateTrigger : public INodeInfoListener, if (confirmed) { + UAVCAN_TRACE("FirmwareUpdateTrigger", "Node ID %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()); } @@ -280,6 +300,8 @@ class FirmwareUpdateTrigger : public INodeInfoListener, } else { + UAVCAN_TRACE("FirmwareUpdateTrigger", "Node ID %d does not need retry", + int(result.getCallID().server_node_id.get())); pending_nodes_.remove(result.getCallID().server_node_id); } } diff --git a/libuavcan/test/protocol/firmware_update_trigger.cpp b/libuavcan/test/protocol/firmware_update_trigger.cpp index d37679c0c5..c0ae718efe 100644 --- a/libuavcan/test/protocol/firmware_update_trigger.cpp +++ b/libuavcan/test/protocol/firmware_update_trigger.cpp @@ -17,7 +17,7 @@ struct FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker std::string firmware_path; - bool should_retry; + int retry_quota; std::string expected_node_name_to_update; BeginFirmwareUpdate::Response last_error_response; @@ -26,7 +26,7 @@ struct FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker : should_request_cnt(0) , should_retry_cnt(0) , confirmation_cnt(0) - , should_retry(false) + , retry_quota(0) { } virtual bool shouldRequestFirmwareUpdate(uavcan::NodeID node_id, @@ -47,7 +47,15 @@ struct FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker std::cout << "RETRY? " << int(node_id.get()) << "\n" << error_response << std::endl; should_retry_cnt++; out_firmware_file_path = firmware_path.c_str(); - return should_retry; + if (retry_quota > 0) + { + retry_quota--; + return true; + } + else + { + return false; + } } virtual void handleFirmwareUpdateConfirmation(uavcan::NodeID node_id, @@ -58,17 +66,27 @@ struct FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker } }; -static uint8_t response_error_code = 0; - -static void beginFirmwareUpdateRequestCallback( - const uavcan::ReceivedDataStructure& req, - uavcan::ServiceResponseDataStructure& res) +struct BeginFirmwareUpdateServer { - std::cout << "REQUEST\n" << req << std::endl; + uint8_t response_error_code; - res.error = response_error_code; - res.optional_error_message = "foobar"; -} + 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) @@ -117,7 +135,7 @@ TEST(FirmwareUpdateTrigger, Basic) */ checker.firmware_path = "firmware_path"; checker.expected_node_name_to_update = "Ivan"; - checker.should_retry = true; + checker.retry_quota = 1000; nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2000)); @@ -138,12 +156,13 @@ TEST(FirmwareUpdateTrigger, Basic) * Starting the firmware update server that returns an error * The checker will instruct the trigger to repeat */ - uavcan::ServiceServer srv(nodes.b); + uavcan::ServiceServer srv(nodes.b); + BeginFirmwareUpdateServer srv_impl; - ASSERT_LE(0, srv.start(beginFirmwareUpdateRequestCallback)); + ASSERT_LE(0, srv.start(srv_impl.makeCallback())); - response_error_code = BeginFirmwareUpdate::Response::ERROR_UNKNOWN; - checker.should_retry = true; + srv_impl.response_error_code = BeginFirmwareUpdate::Response::ERROR_UNKNOWN; + checker.retry_quota = 1000; nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1100)); @@ -158,8 +177,8 @@ TEST(FirmwareUpdateTrigger, Basic) /* * Trying again, this time with ERROR_IN_PROGRESS */ - response_error_code = BeginFirmwareUpdate::Response::ERROR_IN_PROGRESS; - checker.should_retry = false; + srv_impl.response_error_code = BeginFirmwareUpdate::Response::ERROR_IN_PROGRESS; + checker.retry_quota = 0; nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2100)); @@ -198,3 +217,118 @@ TEST(FirmwareUpdateTrigger, Basic) 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(4100)); // 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()); +} From bb412f3f93817c3b256382d32b4483338a0841cb Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 20 May 2015 15:13:31 +0300 Subject: [PATCH 067/143] FirmwareUpdateTrigger retry logic optimization --- .../protocol/firmware_update_trigger.hpp | 34 ++++++++++++------- .../test/protocol/firmware_update_trigger.cpp | 4 ++- 2 files changed, 24 insertions(+), 14 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp b/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp index d95a4b9656..7380be1d9d 100644 --- a/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp +++ b/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp @@ -56,6 +56,7 @@ public: /** * 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. @@ -64,7 +65,9 @@ public: * * @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. + * @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. @@ -75,6 +78,7 @@ public: /** * 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. * * @param node_id Node ID that confirmed the request. * @@ -275,32 +279,36 @@ class FirmwareUpdateTrigger : public INodeInfoListener, 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 ID %d confirmed the update request", + 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 { - FirmwareFilePath firmware_file_path; + 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(), - firmware_file_path); - if (update_needed) + checker_.shouldRetryFirmwareUpdate(result.getCallID().server_node_id, result.getResponse(), *old_path); + + if (!update_needed) { - UAVCAN_TRACE("FirmwareUpdateTrigger", "Node ID %d requires retry; file path: %s", - int(result.getCallID().server_node_id.get()), firmware_file_path.c_str()); - trySetPendingNode(result.getCallID().server_node_id, firmware_file_path); - } - else - { - UAVCAN_TRACE("FirmwareUpdateTrigger", "Node ID %d does not need retry", + UAVCAN_TRACE("FirmwareUpdateTrigger", "Node %d does not need retry", int(result.getCallID().server_node_id.get())); pending_nodes_.remove(result.getCallID().server_node_id); } diff --git a/libuavcan/test/protocol/firmware_update_trigger.cpp b/libuavcan/test/protocol/firmware_update_trigger.cpp index c0ae718efe..2e6f286160 100644 --- a/libuavcan/test/protocol/firmware_update_trigger.cpp +++ b/libuavcan/test/protocol/firmware_update_trigger.cpp @@ -46,7 +46,9 @@ struct FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker last_error_response = error_response; std::cout << "RETRY? " << int(node_id.get()) << "\n" << error_response << std::endl; should_retry_cnt++; - out_firmware_file_path = firmware_path.c_str(); + + EXPECT_STREQ(firmware_path.c_str(), out_firmware_file_path.c_str()); + if (retry_quota > 0) { retry_quota--; From 6abe343f04837676d6927a5701849a2f911cd885 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 20 May 2015 15:19:51 +0300 Subject: [PATCH 068/143] FirmwareUpdateTrigger handleFirmwareUpdateConfirmation() made optional --- .../include/uavcan/protocol/firmware_update_trigger.hpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp b/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp index 7380be1d9d..9bea80eaca 100644 --- a/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp +++ b/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp @@ -80,12 +80,18 @@ public: * 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) = 0; + const protocol::file::BeginFirmwareUpdate::Response& response) + { + (void)node_id; + (void)response; + } virtual ~IFirmwareVersionChecker() { } }; From 9f17dca012f01f2783989c99c2ecc49c697f6671 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 20 May 2015 02:57:20 -1000 Subject: [PATCH 069/143] Fixed guard --- libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp b/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp index 9bea80eaca..9c46090598 100644 --- a/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp +++ b/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp @@ -2,8 +2,8 @@ * Copyright (C) 2015 Pavel Kirienko */ -#ifndef UAVCAN_PROTOCOL_FILE_SERVER_HPP_INCLUDED -#define UAVCAN_PROTOCOL_FILE_SERVER_HPP_INCLUDED +#ifndef UAVCAN_PROTOCOL_UPDATE_TRIGGER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_UPDATE_TRIGGER_HPP_INCLUDED #include #include From 8b87990c1b4104479b51c7d082a9005ffaf18bb6 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 20 May 2015 02:58:33 -1000 Subject: [PATCH 070/143] Fixed guard --- libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp b/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp index 9c46090598..7003aef075 100644 --- a/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp +++ b/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp @@ -2,8 +2,8 @@ * Copyright (C) 2015 Pavel Kirienko */ -#ifndef UAVCAN_PROTOCOL_UPDATE_TRIGGER_HPP_INCLUDED -#define UAVCAN_PROTOCOL_UPDATE_TRIGGER_HPP_INCLUDED +#ifndef UAVCAN_PROTOCOL_FIRMWAREUPDATE_TRIGGER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_FIRMWARE_UPDATE_TRIGGER_HPP_INCLUDED #include #include From 5358c734efc7ff607db67aa65254eeb9c698dc90 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 20 May 2015 02:59:36 -1000 Subject: [PATCH 071/143] Fixed guard --- libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp b/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp index 7003aef075..f32e1598c7 100644 --- a/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp +++ b/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp @@ -2,7 +2,7 @@ * Copyright (C) 2015 Pavel Kirienko */ -#ifndef UAVCAN_PROTOCOL_FIRMWAREUPDATE_TRIGGER_HPP_INCLUDED +#ifndef UAVCAN_PROTOCOL_FIRMWARE_UPDATE_TRIGGER_HPP_INCLUDED #define UAVCAN_PROTOCOL_FIRMWARE_UPDATE_TRIGGER_HPP_INCLUDED #include From 489a27f70d41edcc29e60d4dc3e7cbd9452c72a0 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 20 May 2015 22:13:32 +0300 Subject: [PATCH 072/143] BasicFileServer::handleRead() error handling fix --- libuavcan/include/uavcan/protocol/file_server.hpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/libuavcan/include/uavcan/protocol/file_server.hpp b/libuavcan/include/uavcan/protocol/file_server.hpp index d525b70156..ef6008766c 100644 --- a/libuavcan/include/uavcan/protocol/file_server.hpp +++ b/libuavcan/include/uavcan/protocol/file_server.hpp @@ -136,6 +136,11 @@ class BasicFileServer 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); From 45942eef1fb7685491a9051c47f57bdadbb9297e Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 20 May 2015 22:15:19 +0300 Subject: [PATCH 073/143] Note on error codes --- libuavcan/include/uavcan/protocol/file_server.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/libuavcan/include/uavcan/protocol/file_server.hpp b/libuavcan/include/uavcan/protocol/file_server.hpp index ef6008766c..ba458cefad 100644 --- a/libuavcan/include/uavcan/protocol/file_server.hpp +++ b/libuavcan/include/uavcan/protocol/file_server.hpp @@ -20,6 +20,8 @@ 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 { From 823b14c1211b3cef27a1ec3ee3eb256437db2f9c Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 21 May 2015 01:28:41 +0300 Subject: [PATCH 074/143] POSIX dynamic ID storage backend: calling fsync() on set(), plus a minor style fix --- .../dynamic_node_id_server/file_storage_backend.hpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp index fdf173cc13..a2834c6cfb 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp @@ -53,9 +53,9 @@ class FileStorageBackend : public uavcan::dynamic_node_id_server::IStorageBacken 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++) @@ -80,8 +80,9 @@ class FileStorageBackend : public uavcan::dynamic_node_id_server::IStorageBacken 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()); + (void)fsync(fd); + (void)close(fd); } } From fa11a76143159f9934f69b877e0a5e17ee6a9990 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 20 May 2015 18:17:16 -1000 Subject: [PATCH 075/143] Refactoring paths --- .../protocol/firmware_update_trigger.hpp | 1 + .../basic_file_server_backend.hpp | 147 ++++++++++ .../uavcan_posix/file_server_backend.hpp | 183 ------------ .../include/uavcan_posix/firmware_common.hpp | 217 +++++++++++++++ .../uavcan_posix/firmware_version_checker.hpp | 262 ++++++++++++++++++ 5 files changed, 627 insertions(+), 183 deletions(-) create mode 100644 libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp delete mode 100644 libuavcan_drivers/posix/include/uavcan_posix/file_server_backend.hpp create mode 100644 libuavcan_drivers/posix/include/uavcan_posix/firmware_common.hpp create mode 100644 libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp diff --git a/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp b/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp index f32e1598c7..788536c07c 100644 --- a/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp +++ b/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp @@ -468,6 +468,7 @@ public: UAVCAN_ASSERT((ret > 0) ? isTimerRunning() : true); return ret; } + }; } 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..c28e0cd381 --- /dev/null +++ b/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp @@ -0,0 +1,147 @@ +/**************************************************************************** +* +* 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 "firmware_common.hpp" + +namespace uavcan_posix +{ +/** + * This interface implements a POSIX compliant IFileServerBackend interface + */ +class BasicFileSeverBackend : public uavcan::IFileServerBackend +{ + + enum { FilePermissions = 438 }; ///< 0o666 + + FirmwareCommon::BasePathString base_path; + +public: + /** + * + * Back-end for uavcan.protocol.file.GetInfo. + * Implementation of this method is required. + * On success the method must return zero. + */ + __attribute__((optimize("O0"))) + virtual int16_t getInfo(const Path& path, uint64_t& out_crc64, uint32_t& out_size, EntryType& out_type) + { + + enum { MaxBufferLength = 256 }; + + int rv = uavcan::protocol::file::Error::INVALID_VALUE; + + FirmwareCommon::PathString fw_full_path = FirmwareCommon::getFirmwareCachePath(FirmwareCommon::PathString( + base_path.c_str())); + + if (path.size() > 0 && (fw_full_path.size() + path.size()) <= FirmwareCommon::PathString::MaxSize) + { + + fw_full_path += path; + FirmwareCommon fw; + fw.getFileInfo(fw_full_path); + out_crc64 = fw.descriptor.image_crc; + out_size = fw.descriptor.image_size; + // todo Using fixed flag FLAG_READABLE until we add file permission checks to return actual value. + out_type.flags = uavcan::protocol::file::EntryType::FLAG_FILE | + uavcan::protocol::file::EntryType::FLAG_READABLE;; + rv = 0; + } + 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. + */ + __attribute__((optimize("O0"))) + 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; + + FirmwareCommon::PathString fw_full_path = FirmwareCommon::getFirmwareCachePath(FirmwareCommon::PathString( + base_path.c_str())); + + if (path.size() > 0 && (fw_full_path.size() + path.size()) <= FirmwareCommon::PathString::MaxSize) + { + + fw_full_path += path; + + int fd = open(fw_full_path.c_str(), O_RDONLY); + + if (fd < 0) + { + rv = errno; + } + else + { + + if (::lseek(fd, offset, SEEK_SET) < 0) + { + rv = errno; + } + else + { + //todo uses 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; + } + } + close(fd); + } + } + return rv; + } + + /** + * Initializes the file server based back-end storage by passing a path to + * the directory where files will be stored. + */ + __attribute__((optimize("O0"))) + int init(const FirmwareCommon::BasePathString& path) + { + using namespace std; + base_path = path; + int rv = FirmwareCommon::create_fw_paths(base_path); + return rv; + } +}; + +} + +#endif // Include guard diff --git a/libuavcan_drivers/posix/include/uavcan_posix/file_server_backend.hpp b/libuavcan_drivers/posix/include/uavcan_posix/file_server_backend.hpp deleted file mode 100644 index d6280e6bc3..0000000000 --- a/libuavcan_drivers/posix/include/uavcan_posix/file_server_backend.hpp +++ /dev/null @@ -1,183 +0,0 @@ -/**************************************************************************** -* -* Copyright (c) 2015 PX4 Development Team. All rights reserved. -* Author: Pavel Kirienko -* David Sidrane -* -****************************************************************************/ - -#ifndef UAVCAN_POSIX_FILE_SERVER_BACKEND_HPP_INCLUDED -#define UAVCAN_POSIX_FILE_SERVER_BACKEND_HPP_INCLUDED - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -namespace uavcan_posix -{ -/** - * This interface implements a POSIX compliant IFileServerBackend interface - */ -class FileSeverBackend : public uavcan::IFileServerBackend -{ - /** - * Maximum length of full path including / the file name - */ - enum { MaxPathLength = Path::MaxSize + 128 }; - - enum { FilePermissions = 438 }; ///< 0o666 - - /** - * This type is used for the path - */ - typedef uavcan::MakeString::Type PathString; - - PathString base_path; - -public: - /** - * - * Backend 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) - { - - enum { MaxBufferLength = 256 }; - - int rv = -uavcan::ErrInvalidParam; - if (path.size()) { - - rv = -uavcan::ErrFailure; - - PathString root_path = base_path.c_str(); - root_path += path; - - uint32_t size = 0; - int fd = open(path.c_str(), O_RDONLY); - if (fd >= 0) - { - uavcan::DataTypeSignatureCRC crc; - ssize_t len; - - uint8_t bytes[MaxBufferLength]; - - do { - - len = ::read(fd, bytes, MaxBufferLength); - - if (len < 0) { - goto out_close; - } - - if (len > 0) { - size += len; - crc.add(bytes, len); - } - - } while(len); - out_crc64 = crc.get(); - out_size = size; - EntryType t; - t.flags = uavcan::protocol::file::EntryType::FLAG_FILE; - out_type = t; - rv = 0; - } - -out_close: - close(fd); - } - return rv; - } - - /** - * 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) - { - - int rv = -uavcan::ErrInvalidParam; - - if (path.size()) { - - rv = -uavcan::ErrFailure; - - PathString root_path = base_path.c_str(); - root_path += path; - - int fd = open(path.c_str(), O_RDONLY); - - if (fd >= 0) - { - if (::lseek(fd, offset, SEEK_SET) >= 0) { - - ssize_t len = ::read(fd, out_buffer, inout_size); - close(fd); - - if (len < 0) { - return rv; - } - inout_size = len; - rv = 0; - } - } - } - return rv; - } - - /** - * Initializes the file serrver based backend storage by passing a path to - * the directory where files will be stored. - */ - int init(const PathString& path) - { - using namespace std; - - int rv = -uavcan::ErrInvalidParam; - - if (path.size() > 0) - { - base_path = path.c_str(); - - if (base_path.back() == '/') - { - base_path.pop_back(); - } - - rv = 0; - struct stat sb; - if (stat(base_path.c_str(), &sb) != 0 || !S_ISDIR(sb.st_mode)) - { - rv = mkdir(base_path.c_str(), S_IRWXU | S_IRWXG | S_IRWXO); - } - if (rv >= 0 ) - { - base_path.push_back('/'); - if ((base_path.size() + Path::MaxSize) > MaxPathLength) - { - rv = -uavcan::ErrInvalidConfiguration; - } - } - } - return rv; - } -}; - -} - -#endif // Include guard diff --git a/libuavcan_drivers/posix/include/uavcan_posix/firmware_common.hpp b/libuavcan_drivers/posix/include/uavcan_posix/firmware_common.hpp new file mode 100644 index 0000000000..6973b45f6c --- /dev/null +++ b/libuavcan_drivers/posix/include/uavcan_posix/firmware_common.hpp @@ -0,0 +1,217 @@ +/**************************************************************************** +* +* Copyright (c) 2015 PX4 Development Team. All rights reserved. +* Author: Pavel Kirienko +* David Sidrane +* +****************************************************************************/ + +#ifndef UAVCAN_POSIX_FIRMWARE_COMMON_HPP_INCLUDED +#define UAVCAN_POSIX_FIRMWARE_COMMON_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include + +#include + +namespace uavcan_posix +{ +/** + * Firmware version checking logic. + * Refer to @ref FirmwareUpdateTrigger for details. + */ +class FirmwareCommon +{ + /* The folder where the files will be copied and Read from */ + + static const char* get_cache_dir_() + { + return "c"; + } + +public: + + 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; + + static char getPathSeparator() { return static_cast(uavcan::protocol::file::Path::SEPARATOR); } + + typedef struct app_descriptor_t + { + uint8_t signature[sizeof(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]; + } app_descriptor_t; + + app_descriptor_t descriptor; + + /** + * This method Is used to get the app_descriptor_t from a firmware image + * + * @param path Complete path to the file + * + * @param add_separator Complete path with trailing separator + * + * + * @return 0 if the app_descriptor_t was found and out_descriptor + * has been updated. + * Otherwise -errno is returned + * -ENOENT Signature was not found. + */ + + static PathString getFirmwareCachePath(const PathString& p, bool add_separator = true) + { + PathString r; + r = p; + r.push_back(getPathSeparator()); + r += get_cache_dir_(); + if (add_separator) { + r.push_back(getPathSeparator()); + } + return r; + } + + + static BasePathString getFirmwareCachePath(const BasePathString& p, bool add_separator = true) + { + BasePathString r; + r = p; + r.push_back(getPathSeparator()); + r += get_cache_dir_(); + if (add_separator) { + r.push_back(getPathSeparator()); + } + return r; + } + + int getFileInfo(PathString& path) + { + enum { MaxChunk = (512 / sizeof(uint64_t)) }; + int rv = -ENOENT; + uint64_t chunk[MaxChunk]; + int fd = open(path.c_str(), O_RDONLY); + + if (fd >= 0) + { + app_descriptor_t *pdescriptor = 0; + + while(!pdescriptor) + { + 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 == sig.ull) + { + pdescriptor = (app_descriptor_t *)p; + descriptor = *pdescriptor; + rv = 0; + break; + } + } + while(p++ <= &chunk[MaxChunk - (sizeof(app_descriptor_t) / sizeof(chunk[0]))]); + } + out_close: + close(fd); + } + return rv; + } + + + /** + * Creates the Directories were the files will be stored + */ + + static int create_fw_paths(FirmwareCommon::BasePathString& path) + { + using namespace std; + + int rv = -uavcan::ErrInvalidParam; + + if (path.size() > 0) + { + if (path.back() == FirmwareCommon::getPathSeparator()) + { + path.pop_back(); + } + + rv = 0; + struct stat sb; + if (stat(path.c_str(), &sb) != 0 || !S_ISDIR(sb.st_mode)) + { + rv = mkdir(path.c_str(), S_IRWXU | S_IRWXG | S_IRWXO); + } + + PathString cache = getFirmwareCachePath(PathString(path.c_str()), false); + if (rv >= 0 && (stat(cache.c_str(), &sb) != 0 || !S_ISDIR(sb.st_mode))) + { + rv = mkdir(cache.c_str(), S_IRWXU | S_IRWXG | S_IRWXO); + } + + if (rv >= 0 ) + { + path.push_back(FirmwareCommon::getPathSeparator()); + if ((path.size() + uavcan::protocol::file::Path::FieldTypes::path::MaxSize) > + FirmwareCommon::MaxPathLength) + { + rv = -uavcan::ErrInvalidConfiguration; + } + } + } + return rv; + } + + +private: + +#define APP_DESCRIPTOR_SIGNATURE_ID 'A', 'P', 'D', 'e', 's', 'c' +#define APP_DESCRIPTOR_SIGNATURE_REV '0', '0' +#define APP_DESCRIPTOR_SIGNATURE APP_DESCRIPTOR_SIGNATURE_ID, APP_DESCRIPTOR_SIGNATURE_REV + + union + { + uint64_t ull; + char text[sizeof(uint64_t)]; + } sig = { + .text = {APP_DESCRIPTOR_SIGNATURE} + }; + +}; +} + +#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..69c71c3a39 --- /dev/null +++ b/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp @@ -0,0 +1,262 @@ +/**************************************************************************** +* +* 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 + +#include +#include "firmware_common.hpp" + +namespace uavcan_posix +{ +/** + * Firmware version checking logic. + * Refer to @ref FirmwareUpdateTrigger for details. + */ +class FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker +{ + enum { FilePermissions = 438 }; ///< 0o666 + + FirmwareCommon::BasePathString base_path; + FirmwareCommon::BasePathString cache_path; + +public: + + /** + * 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. + */ + __attribute__((optimize("O0"))) + virtual bool shouldRequestFirmwareUpdate(uavcan::NodeID node_id, const + uavcan::protocol::GetNodeInfo::Response& node_info, + FirmwareFilePath& out_firmware_file_path) + { + /* This is a work around for two issues. + * 1) FirmwareFilePath is 40 + * 2) Nuttx 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[FirmwareCommon::MaxBasePathLength + 1]; + int n = snprintf(fname_root, sizeof(fname_root), "%s%s/%d.%d", + base_path.c_str(), + node_info.name.c_str(), + node_info.hardware_version.major, + node_info.hardware_version.minor); + + if (n > 0) + { + FAR DIR *fwdir = opendir(fname_root); + + if (fwdir) + { + FAR struct dirent *pfile; + while((pfile = readdir(fwdir)) != NULL) + { + if (DIRENT_ISFILE(pfile->d_type)) + { + // Open any bin file in there. + if (strstr(pfile->d_name, ".bin")) + { + FirmwareCommon::PathString full_src_path = fname_root; + full_src_path.push_back(FirmwareCommon::getPathSeparator()); + full_src_path += pfile->d_name; + + FirmwareCommon::PathString full_dst_path = cache_path.c_str(); + full_dst_path += pfile->d_name; + + /* ease the burden on the user */ + int cr = copy_if_not(full_src_path.c_str(), full_dst_path.c_str()); + + /* We have a file, is it a valid image */ + + FirmwareCommon fw; + + if (cr == 0 && fw.getFileInfo(full_dst_path) == 0) + { + if ((node_info.software_version.major == 0 && + node_info.software_version.minor == 0) || + fw.descriptor.image_crc != + node_info.software_version.image_crc) + { + rv = true; + out_firmware_file_path = pfile->d_name; + } + break; + } + } + } + } + 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. + */ + __attribute__((optimize("O0"))) + virtual bool shouldRetryFirmwareUpdate(uavcan::NodeID node_id, + const uavcan::protocol::file::BeginFirmwareUpdate::Response& error_response, + FirmwareFilePath& out_firmware_file_path) + { + return true; + + } + + /** + * 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(uavcan::NodeID node_id, + const uavcan::protocol::file::BeginFirmwareUpdate::Response& response) + { + (void)node_id; + (void)response; + } + + FirmwareVersionChecker() { } + + virtual ~FirmwareVersionChecker() { } + + /** + * Initializes the file server based back-end storage by passing a path to + * the directory where files will be stored. + */ + __attribute__((optimize("O0"))) + int init(const char * path) + { + base_path = path; + cache_path = FirmwareCommon::getFirmwareCachePath(base_path); + if (base_path.back() != FirmwareCommon::getPathSeparator()) + { + base_path.push_back(FirmwareCommon::getPathSeparator()); + } + return 0; + } + +private: + + + __attribute__((optimize("O0"))) + int copy_if_not(const char *srcpath, const char * destpath) + { + /* Does the file exits */ + int rv = 0; + int dfd = open(destpath, O_RDONLY, 0); + + if (dfd >= 0) + { + /* Close it and exit 0 */ + 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); + close(sfd); + } + close(dfd); + } + } + return rv; + } +}; +} + +#endif // Include guard From e5ce6f74c693b1722c97490c7817693d2a1c9e30 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 21 May 2015 17:16:20 +0300 Subject: [PATCH 076/143] POSIX file event tracer visibility fix --- .../uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp | 1 + 1 file changed, 1 insertion(+) 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 index 3398a54418..863d7465a1 100644 --- 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 @@ -38,6 +38,7 @@ class FileEventTracer : public uavcan::dynamic_node_id_server::IEventTracer PathString path_; +protected: virtual void onEvent(uavcan::dynamic_node_id_server::TraceCode code, uavcan::int64_t argument) { using namespace std; From 64b14db1f59b8ac151dacfc153e96f9831ba5607 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Thu, 21 May 2015 07:25:53 -1000 Subject: [PATCH 077/143] More string refactoring --- .../include/uavcan/protocol/file_server.hpp | 7 + .../basic_file_server_backend.hpp | 37 +--- .../file_event_tracer.hpp | 1 + .../file_storage_backend.hpp | 9 +- .../include/uavcan_posix/firmware_common.hpp | 117 +----------- .../include/uavcan_posix/firmware_path.hpp | 171 ++++++++++++++++++ .../uavcan_posix/firmware_version_checker.hpp | 47 ++--- 7 files changed, 222 insertions(+), 167 deletions(-) create mode 100644 libuavcan_drivers/posix/include/uavcan_posix/firmware_path.hpp diff --git a/libuavcan/include/uavcan/protocol/file_server.hpp b/libuavcan/include/uavcan/protocol/file_server.hpp index d525b70156..ba458cefad 100644 --- a/libuavcan/include/uavcan/protocol/file_server.hpp +++ b/libuavcan/include/uavcan/protocol/file_server.hpp @@ -20,6 +20,8 @@ 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 { @@ -136,6 +138,11 @@ class BasicFileServer 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); 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 index c28e0cd381..c88bf8b731 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp @@ -36,8 +36,6 @@ class BasicFileSeverBackend : public uavcan::IFileServerBackend enum { FilePermissions = 438 }; ///< 0o666 - FirmwareCommon::BasePathString base_path; - public: /** * @@ -49,19 +47,13 @@ public: virtual int16_t getInfo(const Path& path, uint64_t& out_crc64, uint32_t& out_size, EntryType& out_type) { - enum { MaxBufferLength = 256 }; - int rv = uavcan::protocol::file::Error::INVALID_VALUE; - FirmwareCommon::PathString fw_full_path = FirmwareCommon::getFirmwareCachePath(FirmwareCommon::PathString( - base_path.c_str())); - - if (path.size() > 0 && (fw_full_path.size() + path.size()) <= FirmwareCommon::PathString::MaxSize) + if (path.size() > 0) { - fw_full_path += path; FirmwareCommon fw; - fw.getFileInfo(fw_full_path); + fw.getFileInfo(path.c_str()); out_crc64 = fw.descriptor.image_crc; out_size = fw.descriptor.image_size; // todo Using fixed flag FLAG_READABLE until we add file permission checks to return actual value. @@ -85,15 +77,11 @@ public: int rv = uavcan::protocol::file::Error::INVALID_VALUE; - FirmwareCommon::PathString fw_full_path = FirmwareCommon::getFirmwareCachePath(FirmwareCommon::PathString( - base_path.c_str())); - - if (path.size() > 0 && (fw_full_path.size() + path.size()) <= FirmwareCommon::PathString::MaxSize) + if (path.size() > 0) { - fw_full_path += path; - int fd = open(fw_full_path.c_str(), O_RDONLY); + int fd = open(path.c_str(), O_RDONLY); if (fd < 0) { @@ -128,18 +116,11 @@ public: return rv; } - /** - * Initializes the file server based back-end storage by passing a path to - * the directory where files will be stored. - */ - __attribute__((optimize("O0"))) - int init(const FirmwareCommon::BasePathString& path) - { - using namespace std; - base_path = path; - int rv = FirmwareCommon::create_fw_paths(base_path); - return rv; - } + BasicFileSeverBackend() { } + + + + }; } 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 index 3398a54418..863d7465a1 100644 --- 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 @@ -38,6 +38,7 @@ class FileEventTracer : public uavcan::dynamic_node_id_server::IEventTracer PathString path_; +protected: virtual void onEvent(uavcan::dynamic_node_id_server::TraceCode code, uavcan::int64_t argument) { using namespace std; diff --git a/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp index fdf173cc13..a2834c6cfb 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp @@ -53,9 +53,9 @@ class FileStorageBackend : public uavcan::dynamic_node_id_server::IStorageBacken 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++) @@ -80,8 +80,9 @@ class FileStorageBackend : public uavcan::dynamic_node_id_server::IStorageBacken 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()); + (void)fsync(fd); + (void)close(fd); } } diff --git a/libuavcan_drivers/posix/include/uavcan_posix/firmware_common.hpp b/libuavcan_drivers/posix/include/uavcan_posix/firmware_common.hpp index 6973b45f6c..33ebba88b8 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/firmware_common.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/firmware_common.hpp @@ -17,43 +17,17 @@ #include #include +#include "firmware_path.hpp" namespace uavcan_posix { /** - * Firmware version checking logic. - * Refer to @ref FirmwareUpdateTrigger for details. + * Firmware file validation logic. */ class FirmwareCommon { - /* The folder where the files will be copied and Read from */ - - static const char* get_cache_dir_() - { - return "c"; - } - public: - 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; - - static char getPathSeparator() { return static_cast(uavcan::protocol::file::Path::SEPARATOR); } - typedef struct app_descriptor_t { uint8_t signature[sizeof(uint64_t)]; @@ -67,51 +41,12 @@ public: app_descriptor_t descriptor; - /** - * This method Is used to get the app_descriptor_t from a firmware image - * - * @param path Complete path to the file - * - * @param add_separator Complete path with trailing separator - * - * - * @return 0 if the app_descriptor_t was found and out_descriptor - * has been updated. - * Otherwise -errno is returned - * -ENOENT Signature was not found. - */ - - static PathString getFirmwareCachePath(const PathString& p, bool add_separator = true) - { - PathString r; - r = p; - r.push_back(getPathSeparator()); - r += get_cache_dir_(); - if (add_separator) { - r.push_back(getPathSeparator()); - } - return r; - } - - - static BasePathString getFirmwareCachePath(const BasePathString& p, bool add_separator = true) - { - BasePathString r; - r = p; - r.push_back(getPathSeparator()); - r += get_cache_dir_(); - if (add_separator) { - r.push_back(getPathSeparator()); - } - return r; - } - - int getFileInfo(PathString& path) + int getFileInfo(const char *path) { enum { MaxChunk = (512 / sizeof(uint64_t)) }; int rv = -ENOENT; uint64_t chunk[MaxChunk]; - int fd = open(path.c_str(), O_RDONLY); + int fd = open(path, O_RDONLY); if (fd >= 0) { @@ -153,50 +88,6 @@ public: } - /** - * Creates the Directories were the files will be stored - */ - - static int create_fw_paths(FirmwareCommon::BasePathString& path) - { - using namespace std; - - int rv = -uavcan::ErrInvalidParam; - - if (path.size() > 0) - { - if (path.back() == FirmwareCommon::getPathSeparator()) - { - path.pop_back(); - } - - rv = 0; - struct stat sb; - if (stat(path.c_str(), &sb) != 0 || !S_ISDIR(sb.st_mode)) - { - rv = mkdir(path.c_str(), S_IRWXU | S_IRWXG | S_IRWXO); - } - - PathString cache = getFirmwareCachePath(PathString(path.c_str()), false); - if (rv >= 0 && (stat(cache.c_str(), &sb) != 0 || !S_ISDIR(sb.st_mode))) - { - rv = mkdir(cache.c_str(), S_IRWXU | S_IRWXG | S_IRWXO); - } - - if (rv >= 0 ) - { - path.push_back(FirmwareCommon::getPathSeparator()); - if ((path.size() + uavcan::protocol::file::Path::FieldTypes::path::MaxSize) > - FirmwareCommon::MaxPathLength) - { - rv = -uavcan::ErrInvalidConfiguration; - } - } - } - return rv; - } - - private: #define APP_DESCRIPTOR_SIGNATURE_ID 'A', 'P', 'D', 'e', 's', 'c' diff --git a/libuavcan_drivers/posix/include/uavcan_posix/firmware_path.hpp b/libuavcan_drivers/posix/include/uavcan_posix/firmware_path.hpp new file mode 100644 index 0000000000..31bb0baab4 --- /dev/null +++ b/libuavcan_drivers/posix/include/uavcan_posix/firmware_path.hpp @@ -0,0 +1,171 @@ +/**************************************************************************** +* +* Copyright (c) 2015 PX4 Development Team. All rights reserved. +* Author: Pavel Kirienko +* David Sidrane +* +****************************************************************************/ + +#ifndef UAVCAN_POSIX_FIRMWARE_PATH_HPP_INCLUDED +#define UAVCAN_POSIX_FIRMWARE_PATH_HPP_INCLUDED + +#include + +#include + +namespace uavcan_posix +{ +/** + * Firmware Path Management. + */ +class FirmwarePath +{ + /* The folder where the files will be copied and Read from */ + + static const char* get_cache_dir_() + { + return "c"; + } + +public: + + 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 }; + + BasePathString base_path_; + BasePathString cache_path_; + + /** + * This type is used internally for the full path to file + */ + typedef uavcan::MakeString::Type PathString; + + static char getPathSeparator() { return static_cast(uavcan::protocol::file::Path::SEPARATOR); } + + + BasePathString& getFirmwareBasePath() + { + return base_path_; + } + + void setFirmwareBasePath(const char * path) + { + base_path_ = path; + } + + BasePathString& getFirmwareCachePath() + { + return cache_path_; + } + + void setFirmwareCachePath(const char *path) + { + cache_path_ = path; + + } + + + 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(); + } + + } + + /** + * 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 cashed here. + */ + + int CreateFwPaths(const char *base_path) + { + using namespace std; + int rv = -uavcan::ErrInvalidParam; + + if (base_path) + { + + int len = strlen(base_path); + + if (len > 0 && len < base_path_.MaxSize) + { + + setFirmwareBasePath(base_path); + removeSlash(getFirmwareBasePath()); + const char *path = getFirmwareBasePath().c_str(); + + setFirmwareCachePath(path); + addSlash(cache_path_); + cache_path_ += get_cache_dir_(); + + 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(getFirmwareBasePath()); + addSlash(getFirmwareCachePath()); + + + if (rv >= 0 ) + { + if ((getFirmwareCachePath().size() + uavcan::protocol::file::Path::FieldTypes::path::MaxSize) > + MaxPathLength) + { + rv = -uavcan::ErrInvalidConfiguration; + } + } + } + } + return rv; + } + + int init(const char *path) + { + return CreateFwPaths(path); + } + +}; + +} + +#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 index 69c71c3a39..3066c07a18 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp @@ -18,6 +18,7 @@ #include #include + #include "firmware_common.hpp" namespace uavcan_posix @@ -30,8 +31,7 @@ class FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker { enum { FilePermissions = 438 }; ///< 0o666 - FirmwareCommon::BasePathString base_path; - FirmwareCommon::BasePathString cache_path; + FirmwarePath *paths_; public: @@ -56,7 +56,7 @@ public: { /* This is a work around for two issues. * 1) FirmwareFilePath is 40 - * 2) Nuttx is using 32 for max file names. + * 2) OK using is using 32 for max file names. * * So for the file: * org.pixhawk.px4cannode-v1-0.1.59efc137.uavcan.bin @@ -72,17 +72,21 @@ public: bool rv = false; - char fname_root[FirmwareCommon::MaxBasePathLength + 1]; + char fname_root[FirmwarePath::MaxBasePathLength + 1]; int n = snprintf(fname_root, sizeof(fname_root), "%s%s/%d.%d", - base_path.c_str(), + paths_->getFirmwareBasePath().c_str(), node_info.name.c_str(), node_info.hardware_version.major, node_info.hardware_version.minor); - if (n > 0) + if (n > 0 && n < (int) sizeof(fname_root) - 2) { + FAR DIR *fwdir = opendir(fname_root); + fname_root[n++] = FirmwarePath::getPathSeparator(); + fname_root[n++] = '\0'; + if (fwdir) { FAR struct dirent *pfile; @@ -93,11 +97,10 @@ public: // Open any bin file in there. if (strstr(pfile->d_name, ".bin")) { - FirmwareCommon::PathString full_src_path = fname_root; - full_src_path.push_back(FirmwareCommon::getPathSeparator()); + FirmwarePath::PathString full_src_path = fname_root; full_src_path += pfile->d_name; - FirmwareCommon::PathString full_dst_path = cache_path.c_str(); + FirmwarePath::PathString full_dst_path = paths_->getFirmwareCachePath().c_str(); full_dst_path += pfile->d_name; /* ease the burden on the user */ @@ -107,9 +110,10 @@ public: FirmwareCommon fw; - if (cr == 0 && fw.getFileInfo(full_dst_path) == 0) + if (cr == 0 && fw.getFileInfo(full_dst_path.c_str()) == 0) { - if ((node_info.software_version.major == 0 && + if (node_info.software_version.image_crc == 0 || + (node_info.software_version.major == 0 && node_info.software_version.minor == 0) || fw.descriptor.image_crc != node_info.software_version.image_crc) @@ -173,26 +177,25 @@ public: (void)response; } - FirmwareVersionChecker() { } + FirmwareVersionChecker() : + paths_(0) + { } virtual ~FirmwareVersionChecker() { } /** - * Initializes the file server based back-end storage by passing a path to - * the directory where files will be stored. + * Initializes the Firmware File back-end storage by passing a paths object + * that maintains the directory where files will be stored. */ - __attribute__((optimize("O0"))) - int init(const char * path) + + int init(FirmwarePath& paths) { - base_path = path; - cache_path = FirmwareCommon::getFirmwareCachePath(base_path); - if (base_path.back() != FirmwareCommon::getPathSeparator()) - { - base_path.push_back(FirmwareCommon::getPathSeparator()); - } + paths_ = &paths; return 0; } + const char * getFirmwarePath() { return paths_->getFirmwareCachePath().c_str(); } + private: From 5f4adbf1a3a40632d5d471e9acccd058220de0a5 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 23 May 2015 00:38:17 +0300 Subject: [PATCH 078/143] dynamic_node_id_server::distributed::StateReport structure --- .../distributed/raft_core.hpp | 25 +++++--- .../distributed/server.hpp | 62 +++++++++++++++++++ .../node_discoverer.hpp | 6 ++ 3 files changed, 86 insertions(+), 7 deletions(-) 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 b77dfadf9e..ab26635bbe 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 @@ -74,6 +74,15 @@ public: */ class RaftCore : private TimerBase { +public: + enum ServerState + { + ServerStateFollower, + ServerStateCandidate, + ServerStateLeader + }; + +private: typedef MethodBinder&, ServiceResponseDataStructure&)> AppendEntriesCallback; @@ -88,13 +97,6 @@ class RaftCore : private TimerBase typedef MethodBinder&)> RequestVoteResponseCallback; - enum ServerState - { - ServerStateFollower, - ServerStateCandidate, - ServerStateLeader - }; - struct PendingAppendEntriesFields { Log::Index prev_log_index; @@ -895,6 +897,15 @@ 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_; } + bool isInActiveMode() const { return active_mode_; } + ServerState getServerState() const { return server_state_; } }; } 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 2fd6e2158a..940529e358 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 @@ -295,6 +295,68 @@ 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; + bool is_active; + + Log::Index last_log_index; + Log::Index commit_index; + + Term last_log_term; + Term current_term; + + NodeID voted_for; + + MonotonicTime last_activity_timestamp; + + 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()) + , is_active (s.getRaftCore().isInActiveMode()) + , 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()) + , 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; + } + } }; } 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 03b4145759..d8042074a2 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 @@ -361,6 +361,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()); } }; } From 66dc702a7e2c66a6edf6634ad4ff70542e0c8f0f Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 23 May 2015 03:26:52 +0300 Subject: [PATCH 079/143] New Linux tool: uavcan_dynamic_node_id_server (mostly complete) --- libuavcan_drivers/linux/CMakeLists.txt | 4 + .../apps/uavcan_dynamic_node_id_server.cpp | 546 ++++++++++++++++++ 2 files changed, 550 insertions(+) create mode 100644 libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp diff --git a/libuavcan_drivers/linux/CMakeLists.txt b/libuavcan_drivers/linux/CMakeLists.txt index fd8ed5eef1..408fff617c 100644 --- a/libuavcan_drivers/linux/CMakeLists.txt +++ b/libuavcan_drivers/linux/CMakeLists.txt @@ -69,7 +69,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/uavcan_dynamic_node_id_server.cpp b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp new file mode 100644 index 0000000000..5883fb2a82 --- /dev/null +++ b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp @@ -0,0 +1,546 @@ +/* + * 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 +{ + +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 machine_id = uavcan_linux::MachineIDReader().read(); + + uavcan::protocol::HardwareVersion hwver; + std::copy(machine_id.begin(), machine_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())); + } + + return node; +} + + +class EventTracer : public uavcan_posix::dynamic_node_id_server::FileEventTracer +{ +public: + struct RecentEvent + { + const uavcan::MonotonicDuration time_since_startup; + const uavcan::dynamic_node_id_server::TraceCode code; + const std::int64_t argument; + + RecentEvent(uavcan::MonotonicDuration arg_time_since_startup, + uavcan::dynamic_node_id_server::TraceCode arg_code, + std::int64_t arg_argument) + : time_since_startup(arg_time_since_startup) + , code(arg_code) + , argument(arg_argument) + { } + + uavcan::MakeString<81>::Type toString() const // Heapless return + { + const double ts = time_since_startup.toUSec() / 1e6; + decltype(toString()) out; + out.resize(out.capacity()); + (void)std::snprintf(reinterpret_cast(out.begin()), out.size() - 1U, + "%-11.1f %-28s % -20lld %016llx", + ts, + getEventName(code), + static_cast(argument), + static_cast(argument)); + return out; + } + + static const char* getTableHeader() + { + // Matches the string format above + return "Rel. time 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); + + const auto ts = clock_.getMonotonic(); + const auto time_since_startup = ts - started_at_; + + last_events_.emplace_front(time_since_startup, code, argument); + if (last_events_.size() > num_last_events_) + { + last_events_.pop_back(); + } + + event_counters_[code].hit(ts); + } + +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::sort(pairs.begin(), pairs.end(), [](const Pair& a, const Pair& b) { + return a.second.count > b.second.count; + }); + + return pairs; +} + + +void redraw(const uavcan_linux::NodePtr& node, + const uavcan::MonotonicTime timestamp, + const EventTracer& event_tracer, + const uavcan::dynamic_node_id_server::DistributedServer& server) +{ + /* + * 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] = { }; + + 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)); + } + next_relevant_event_index++; + + 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) + { + 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) + { + char buf[21]; + std::snprintf(buf, sizeof(buf) - 1U, "%lld", local_state_value); + render_top_str(local_state_name, buf); + }; + + const auto raft_state_to_string = [](uavcan::dynamic_node_id_server::distributed::RaftCore::ServerState s) + { + using uavcan::dynamic_node_id_server::distributed::RaftCore; + 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; + }; + + /* + * Rendering the data to the CLI + */ + std::printf("\x1b\x5b\x48\x1b\x5b\x32\x4a"); // Clear screen and move caret to the upper-left corner + + // Local state and relevant event counters - two columns + std::printf(" Local state | Event counters\n"); + + render_top_int("Node ID", node->getNodeID().get()); + render_top_str("State", raft_state_to_string(report.state)); + render_top_str("Mode", report.is_active ? "Active" : "Passive"); + render_top_int("Last log index", report.last_log_index); + render_top_int("Last log term", report.last_log_term); + render_top_int("Commit index", report.commit_index); + render_top_int("Current term", report.current_term); + render_top_int("Voted for", report.voted_for.get()); + render_top_str("Since activity", duration_to_string(time_since_last_activity).c_str()); + render_top_int("Unknown nodes", report.num_unknown_nodes); + + // 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) + { + std::printf("%-17s", name); + for (std::uint8_t i = 0; i < 4; i++) + { + if (i < (report.cluster_size - 1)) + { + 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(); + }; + + render_followers_state("Node ID", [&](std::uint8_t i) + { + const auto nid = report.followers[i].node_id; + return nid.isValid() ? nid.get() : -1; + }); + render_followers_state("Next index", [&](std::uint8_t i) { return report.followers[i].next_index; }); + render_followers_state("Match index", [&](std::uint8_t i) { return report.followers[i].match_index; }); + + // Empty line before the next block + std::printf(" "); + render_next_event_counter(); + + assert(next_relevant_event_index == NumRelevantEvents); // Ensuring that all events can be printed + + // Separator + std::printf("--------------------------------------+----------------------------------------\n"); + + // Event log + std::printf("%s", EventTracer::RecentEvent::getTableHeader()); // NO NEWLINE + const int num_events_to_render = static_cast(num_rows) - + static_cast(next_relevant_event_index) - + static_cast(NumRowsWithoutEvents); + for (int i = 0; + i < num_events_to_render && i < static_cast(event_tracer.getNumEvents()); + i++) + { + // NEWLINE IS PREPENDED + std::printf("\n%s", event_tracer.getEventByIndex(i).toString().c_str()); + } + // Note that the last line does not have trailing newline. This allows to use all available rows. + + 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 + */ + const unsigned num_last_events_to_keep = 100; + + EventTracer event_tracer(num_last_events_to_keep); + 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); + + ENFORCE(0 <= server.init(node->getNodeStatusProvider().getHardwareVersion().unique_id, cluster_size)); + + /* + * Spinning the node + */ + uavcan::MonotonicTime last_redraw_at; + + while (true) + { + const int res = node->spin(uavcan::MonotonicDuration::fromMSec(100)); + 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 + { + const 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 + */ + (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; + } +} From 65db68a5146a3d67f4bec0581f04bb36d8fe7a82 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 23 May 2015 20:13:23 +0300 Subject: [PATCH 080/143] uavcan_dynamic_node_id_server - a couple of fixes --- .../apps/uavcan_dynamic_node_id_server.cpp | 22 ++++++++++--------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp index 5883fb2a82..6a07bccf0b 100644 --- a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp +++ b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp @@ -27,6 +27,9 @@ 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); @@ -133,6 +136,8 @@ private: { uavcan_posix::dynamic_node_id_server::FileEventTracer::onEvent(code, argument); + had_events_ = true; + const auto ts = clock_.getMonotonic(); const auto time_since_startup = ts - started_at_; @@ -197,7 +202,7 @@ collectRelevantEvents(const EventTracer& event_tracer, const unsigned num_events pairs.resize(std::min(num_events, unsigned(pairs.size()))); // Sorting so that the most frequent events are on top of the list - std::sort(pairs.begin(), pairs.end(), [](const Pair& a, const Pair& b) { + std::stable_sort(pairs.begin(), pairs.end(), [](const Pair& a, const Pair& b) { return a.second.count > b.second.count; }); @@ -352,18 +357,17 @@ void redraw(const uavcan_linux::NodePtr& node, std::printf("--------------------------------------+----------------------------------------\n"); // Event log - std::printf("%s", EventTracer::RecentEvent::getTableHeader()); // NO NEWLINE + 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); + 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++) { - // NEWLINE IS PREPENDED - std::printf("\n%s", event_tracer.getEventByIndex(i).toString().c_str()); + std::printf("%s\n", event_tracer.getEventByIndex(i).toString().c_str()); } - // Note that the last line does not have trailing newline. This allows to use all available rows. std::fflush(stdout); } @@ -377,9 +381,7 @@ void runForever(const uavcan_linux::NodePtr& node, /* * Event tracer */ - const unsigned num_last_events_to_keep = 100; - - EventTracer event_tracer(num_last_events_to_keep); + EventTracer event_tracer(MaxNumLastEvents); ENFORCE(0 <= event_tracer.init(event_log_file.c_str())); /* @@ -403,7 +405,7 @@ void runForever(const uavcan_linux::NodePtr& node, while (true) { - const int res = node->spin(uavcan::MonotonicDuration::fromMSec(100)); + const int res = node->spin(uavcan::MonotonicDuration::fromMSec(MinUpdateInterval)); if (res < 0) { std::cerr << "Spin error: " << res << std::endl; From bf0fd63bfed5a54e8de039e06e89e35cc6620301 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 23 May 2015 20:54:12 +0300 Subject: [PATCH 081/143] uavcan_dynamic_node_id_server - simple output coloring --- .../apps/uavcan_dynamic_node_id_server.cpp | 120 +++++++++++++++--- 1 file changed, 100 insertions(+), 20 deletions(-) diff --git a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp index 6a07bccf0b..96e5583dc2 100644 --- a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp +++ b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp @@ -209,12 +209,40 @@ collectRelevantEvents(const EventTracer& event_tracer, const unsigned num_events return pairs; } +enum class CLIColor : unsigned +{ + Red = 31, + Green = 32, + Yellow = 33, + Blue = 34, + Magenta = 35, + Cyan = 36, + Default = 39 +}; + +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 */ @@ -254,22 +282,24 @@ void redraw(const uavcan_linux::NodePtr& node, 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) + const auto render_top_str = [&](const char* local_state_name, const char* local_state_value, CLIColor color) { - std::printf("%-20s %-16s", local_state_name, local_state_value); + { + 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) + 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); + render_top_str(local_state_name, buf, color); }; const auto raft_state_to_string = [](uavcan::dynamic_node_id_server::distributed::RaftCore::ServerState s) { - using uavcan::dynamic_node_id_server::distributed::RaftCore; switch (s) { case RaftCore::ServerStateFollower: return "Follower"; @@ -286,6 +316,11 @@ void redraw(const uavcan_linux::NodePtr& node, return str; }; + const auto colorize_if = [](bool condition, CLIColor color) + { + return condition ? color : CLIColor::Default; + }; + /* * Rendering the data to the CLI */ @@ -294,16 +329,45 @@ void redraw(const uavcan_linux::NodePtr& node, // Local state and relevant event counters - two columns std::printf(" Local state | Event counters\n"); - render_top_int("Node ID", node->getNodeID().get()); - render_top_str("State", raft_state_to_string(report.state)); - render_top_str("Mode", report.is_active ? "Active" : "Passive"); - render_top_int("Last log index", report.last_log_index); - render_top_int("Last log term", report.last_log_term); - render_top_int("Commit index", report.commit_index); - render_top_int("Current term", report.current_term); - render_top_int("Voted for", report.voted_for.get()); - render_top_str("Since activity", duration_to_string(time_since_last_activity).c_str()); - render_top_int("Unknown nodes", report.num_unknown_nodes); + render_top_int("Node ID", + node->getNodeID().get(), + CLIColor::Default); + + render_top_str("State", + raft_state_to_string(report.state), + colorize_if(report.state == RaftCore::ServerStateCandidate, CLIColor::Magenta)); + + render_top_str("Mode", + report.is_active ? "Active" : "Passive", + colorize_if(report.is_active, CLIColor::Magenta)); + + 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_int("Unknown nodes", + report.num_unknown_nodes, + colorize_if(report.num_unknown_nodes != 0, CLIColor::Magenta)); // Empty line before the next block std::printf(" "); @@ -313,14 +377,16 @@ void redraw(const uavcan_linux::NodePtr& node, std::printf(" Followers "); render_next_event_counter(); - const auto render_followers_state = - [&](const char* name, const std::function value_getter) + 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) { @@ -339,13 +405,27 @@ void redraw(const uavcan_linux::NodePtr& node, render_next_event_counter(); }; + const auto follower_color_getter = [&](std::uint8_t i) + { + if (!report.followers[i].node_id.isValid()) { return CLIColor::Red; } + if (report.followers[i].match_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; - }); - render_followers_state("Next index", [&](std::uint8_t i) { return report.followers[i].next_index; }); - render_followers_state("Match index", [&](std::uint8_t i) { return report.followers[i].match_index; }); + }, + 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); // Empty line before the next block std::printf(" "); From a6618d9be0e45ee9d4ea4e5099d46fe6349b7b31 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 23 May 2015 21:30:10 +0300 Subject: [PATCH 082/143] uavcan_dynamic_node_id_server - colored events --- .../linux/apps/uavcan_dynamic_node_id_server.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp index 96e5583dc2..e45c57dd75 100644 --- a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp +++ b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp @@ -217,9 +217,12 @@ enum class CLIColor : unsigned Blue = 34, Magenta = 35, Cyan = 36, + White = 37, Default = 39 }; +CLIColor getColorHash(unsigned value) { return CLIColor(31 + value % 7); } + class CLIColorizer { const CLIColor color_; @@ -269,6 +272,7 @@ void redraw(const uavcan_linux::NodePtr& node, { const char* event_name = ""; char event_count_str[10] = { }; + CLIColor event_color = CLIColor::Default; if (next_relevant_event_index < relevant_events.size()) { @@ -276,10 +280,13 @@ void redraw(const uavcan_linux::NodePtr& node, 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(" | %-29s %-9s\n", event_name, event_count_str); + 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) @@ -446,7 +453,9 @@ void redraw(const uavcan_linux::NodePtr& node, i < num_events_to_render && i < static_cast(event_tracer.getNumEvents()); i++) { - std::printf("%s\n", event_tracer.getEventByIndex(i).toString().c_str()); + 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); From 9b5074051fb989b4dc905ccbc4648da5eb3294da Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 23 May 2015 21:32:00 +0300 Subject: [PATCH 083/143] uavcan_dynamic_node_id_server - setting status OK --- libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp index e45c57dd75..8f778d7ba8 100644 --- a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp +++ b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp @@ -58,6 +58,8 @@ uavcan_linux::NodePtr initNode(const std::vector& ifaces, uavcan::N throw std::runtime_error("Network conflict with node " + std::to_string(check_result.conflicting_node.get())); } + node->setStatusOk(); + return node; } From aaa3c225c4c11287c94a8491b7b2fec04a14d6d9 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 23 May 2015 21:35:12 +0300 Subject: [PATCH 084/143] uavcan_dynamic_node_id_server - posfixing the storage path with self node ID --- .../linux/apps/uavcan_dynamic_node_id_server.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp index 8f778d7ba8..c24426def8 100644 --- a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp +++ b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp @@ -603,7 +603,7 @@ int main(int argc, const char** argv) { try { - const auto options = parseOptions(argc, argv); + auto options = parseOptions(argc, argv); std::cout << "Self node ID: " << int(options.node_id.get()) << "\n" "Cluster size: " << int(options.cluster_size) << "\n" @@ -619,6 +619,8 @@ int main(int argc, const char** argv) /* * 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"; From 0348b22b1e6d2bf6b87f5cff55934a9e28b2e68d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 23 May 2015 21:44:29 +0300 Subject: [PATCH 085/143] distributed allocation server - StateReport fix --- .../dynamic_node_id_server/distributed/server.hpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) 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 940529e358..22b3a4b9be 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 @@ -356,6 +356,18 @@ struct StateReport { 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); + } + } } }; From ce4ae983a3d1bea9e6c6dc66737add4a1aa7d886 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 23 May 2015 22:46:16 +0300 Subject: [PATCH 086/143] Event logs uses local time --- .../apps/uavcan_dynamic_node_id_server.cpp | 32 +++++++++++++------ 1 file changed, 22 insertions(+), 10 deletions(-) diff --git a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp index c24426def8..4c23536f4b 100644 --- a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp +++ b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp @@ -70,25 +70,36 @@ 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::dynamic_node_id_server::TraceCode arg_code, - std::int64_t arg_argument) + 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 { - const double ts = time_since_startup.toUSec() / 1e6; + char timerbuf[11] = { }; + { + const std::time_t rawtime = utc_timestamp.toUSec() * 1e-6; + const auto tm = localtime(&rawtime); + std::strftime(timerbuf, sizeof(timerbuf) - 1U, "%H:%M:%S.", tm); + timerbuf[9] = '0' + (utc_timestamp.toMSec() % 1000) / 100; + timerbuf[10] = '\0'; + } + decltype(toString()) out; out.resize(out.capacity()); (void)std::snprintf(reinterpret_cast(out.begin()), out.size() - 1U, - "%-11.1f %-28s % -20lld %016llx", - ts, + "%-10s %-28s % -20lld %016llx", + timerbuf, getEventName(code), static_cast(argument), static_cast(argument)); @@ -98,7 +109,7 @@ public: static const char* getTableHeader() { // Matches the string format above - return "Rel. time Event name Argument (dec) Argument (hex)"; + return "Timestamp Event name Argument (dec) Argument (hex)"; } }; @@ -140,16 +151,17 @@ private: had_events_ = true; - const auto ts = clock_.getMonotonic(); - const auto time_since_startup = ts - started_at_; + 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, code, argument); + 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); + event_counters_[code].hit(ts_m); } public: From ed96e9f0fdbf34f28d6943348e8ff8b2ece549c7 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 23 May 2015 23:21:03 +0300 Subject: [PATCH 087/143] uavcan_dynamic_node_id_server - Fixed output coloring --- libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp index 4c23536f4b..fb275f68d0 100644 --- a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp +++ b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp @@ -428,6 +428,7 @@ void redraw(const uavcan_linux::NodePtr& node, 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) { return CLIColor::Magenta; } return CLIColor::Default; From 132ab39c49cc67c7f5575f4c3b21a09e6fbd1bb9 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 23 May 2015 23:25:08 +0300 Subject: [PATCH 088/143] uavcan_dynamic_node_id_server - Highliting Leader state in green --- .../linux/apps/uavcan_dynamic_node_id_server.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp index fb275f68d0..b71bb9dba9 100644 --- a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp +++ b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp @@ -356,7 +356,9 @@ void redraw(const uavcan_linux::NodePtr& node, render_top_str("State", raft_state_to_string(report.state), - colorize_if(report.state == RaftCore::ServerStateCandidate, CLIColor::Magenta)); + (report.state == RaftCore::ServerStateCandidate) ? CLIColor::Magenta : + (report.state == RaftCore::ServerStateLeader) ? CLIColor::Green : + CLIColor::Default); render_top_str("Mode", report.is_active ? "Active" : "Passive", From 0bb767c42f0bd6812aa9a3a4aa46537eca6a3526 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 May 2015 13:34:05 +0300 Subject: [PATCH 089/143] uavcan_dynamic_node_id_server - improved CLI rendering --- libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp index b71bb9dba9..7ccb690af1 100644 --- a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp +++ b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp @@ -345,7 +345,8 @@ void redraw(const uavcan_linux::NodePtr& node, /* * Rendering the data to the CLI */ - std::printf("\x1b\x5b\x48\x1b\x5b\x32\x4a"); // Clear screen and move caret to the upper-left corner + 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"); From 5361d7bbeb146b1bce30a9d7a2d434ca5dc01a97 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 May 2015 13:36:39 +0300 Subject: [PATCH 090/143] uavcan_status_monitor - improved CLI rendering --- libuavcan_drivers/linux/apps/uavcan_status_monitor.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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++) From 7cac4cd4fa221b3479c8ee72d551d51e651e3c9f Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 May 2015 14:23:04 +0300 Subject: [PATCH 091/143] uavcan_dynamic_node_id_server - cleaner time formatting --- .../linux/apps/uavcan_dynamic_node_id_server.cpp | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp index 7ccb690af1..a922230892 100644 --- a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp +++ b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp @@ -86,20 +86,19 @@ public: uavcan::MakeString<81>::Type toString() const // Heapless return { - char timerbuf[11] = { }; + char timebuf[11] = { }; { const std::time_t rawtime = utc_timestamp.toUSec() * 1e-6; const auto tm = localtime(&rawtime); - std::strftime(timerbuf, sizeof(timerbuf) - 1U, "%H:%M:%S.", tm); - timerbuf[9] = '0' + (utc_timestamp.toMSec() % 1000) / 100; - timerbuf[10] = '\0'; + 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, - "%-10s %-28s % -20lld %016llx", - timerbuf, + "%-11s %-28s %-20lld %016llx", + timebuf, getEventName(code), static_cast(argument), static_cast(argument)); @@ -109,7 +108,7 @@ public: static const char* getTableHeader() { // Matches the string format above - return "Timestamp Event name Argument (dec) Argument (hex)"; + return "Timestamp Event name Argument (dec) Argument (hex)"; } }; From 9e9ade0055eaf65c59119e324b2faa2302576b6e Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 May 2015 14:25:56 +0300 Subject: [PATCH 092/143] Node ID allocation server - allocation response TX timeout set to DEFAULT_REQUEST_PERIOD_MS --- .../dynamic_node_id_server/allocation_request_manager.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp index 8c63132adc..cd0f44832e 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp @@ -251,6 +251,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) From 4f64e2378e25106812801f66db616d0ba84d0cf4 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 May 2015 15:41:51 +0300 Subject: [PATCH 093/143] RaftCore - runtime assertions --- .../distributed/raft_core.hpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) 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 ab26635bbe..1d0d676d74 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 @@ -545,6 +545,14 @@ private: if (!result.isSuccessful()) { + /* + * This callback WILL NEVER be invoked by timeout, because: + * - Any pending request will be cancelled on the next update of the Leader. + * - When the state switches (i.e. the node is not Leader anymore), all pending calls will be cancelled. + * Also note that 'pending_append_entries_fields_' invalidates after every update of the Leader, so + * if there were timeout callbacks, they would be using outdated state. + */ + UAVCAN_ASSERT(0); return; } @@ -650,6 +658,12 @@ private: if (!result.isSuccessful()) { + /* + * This callback WILL NEVER be invoked by timeout, because all pending calls will be cancelled on + * state switch, which ALWAYS happens 100ms after elections (either to Follower or to Leader, depending + * on the number of votes collected). + */ + UAVCAN_ASSERT(0); return; } From 8729d6a2d652403de9226dc67978eda8a7a2722b Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 May 2015 15:59:32 +0300 Subject: [PATCH 094/143] ServiceClient<>::getCallIDByIndex() --- libuavcan/include/uavcan/node/service_client.hpp | 13 +++++++++++++ libuavcan/test/node/service_client.cpp | 6 ++++++ 2 files changed, 19 insertions(+) diff --git a/libuavcan/include/uavcan/node/service_client.hpp b/libuavcan/include/uavcan/node/service_client.hpp index b5a84715fa..b8467c4694 100644 --- a/libuavcan/include/uavcan/node/service_client.hpp +++ b/libuavcan/include/uavcan/node/service_client.hpp @@ -341,6 +341,12 @@ public: */ 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. */ @@ -556,6 +562,13 @@ bool ServiceClient::hasPendingCallToServe 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/test/node/service_client.cpp b/libuavcan/test/node/service_client.cpp index 8b24848a30..82d99a66ac 100644 --- a/libuavcan/test/node/service_client.cpp +++ b/libuavcan/test/node/service_client.cpp @@ -147,6 +147,9 @@ TEST(ServiceClient, Basic) ASSERT_EQ(1, client2.getNumPendingCalls()); ASSERT_EQ(2, client3.getNumPendingCalls()); + ASSERT_EQ(uavcan::NodeID(1), client2.getCallIDByIndex(0).server_node_id); + ASSERT_EQ(uavcan::NodeID(), client2.getCallIDByIndex(1).server_node_id); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(20)); std::cout << "!!! Spin finished!" << std::endl; @@ -216,6 +219,9 @@ TEST(ServiceClient, Rejection) ASSERT_LT(0, client1.call(1, request)); + ASSERT_EQ(uavcan::NodeID(1), client1.getCallIDByIndex(0).server_node_id); + ASSERT_EQ(uavcan::NodeID(), client1.getCallIDByIndex(1).server_node_id); + ASSERT_EQ(1, nodes.b.getDispatcher().getNumServiceResponseListeners()); ASSERT_TRUE(client1.hasPendingCalls()); ASSERT_TRUE(client1.hasPendingCallToServer(1)); From 546be2b89b7ebb74f55573c2b1f316b059d5e958 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 May 2015 16:26:14 +0300 Subject: [PATCH 095/143] Fixed RaftCore. The logic is even more complicated. --- .../distributed/raft_core.hpp | 52 +++++++++++++++---- 1 file changed, 43 insertions(+), 9 deletions(-) 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 1d0d676d74..aaaa563381 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 @@ -64,13 +64,13 @@ public: * - 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) + * - log is not fully replicated or there are uncommitted entries * * Deactivation: * - switch to follower state * - persistent state update error * - only if leader: - * - all log entries are fully replicated + * - all log entries are fully replicated and committed */ class RaftCore : private TimerBase { @@ -373,25 +373,59 @@ private: if (commit_index_ == persistent_state_.getLog().getLastIndex()) { - // All local entries are committed - bool commit_index_fully_replicated = true; + /* + * All local entries are committed. + * Deciding if it is safe to go into passive mode now. + * + * We can go into passive mode if the log is known to be fully replicated and all entries are committed. + * The high-level conditions above are guaranteed to be met if all of the following lower-level conditions + * are met: + * - All local entries are committed (already checked here). + * - Match index on all nodes equals local commit index. + * - Next index on all nodes is strictly greater than the local commit index. + * + * The following code checks if the last two conditions are met. + */ + bool match_index_equals_commit_index = true; + bool next_index_greater_than_commit_index = true; for (uint8_t i = 0; i < cluster_.getNumKnownServers(); i++) { - const Log::Index match_index = cluster_.getServerMatchIndex(cluster_.getRemoteServerNodeIDAtIndex(i)); + const NodeID server_node_id = cluster_.getRemoteServerNodeIDAtIndex(i); + + const Log::Index match_index = cluster_.getServerMatchIndex(server_node_id); if (match_index != commit_index_) { - commit_index_fully_replicated = false; + match_index_equals_commit_index = false; + break; + } + + const Log::Index next_index = cluster_.getServerNextIndex(server_node_id); + if (next_index <= commit_index_) + { + next_index_greater_than_commit_index = 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 + /* + * Now we know whether the log is replicated and whether all entries are committed, so we can make + * the decision. Remember that since we ended up in this branch, it is already known that all local + * log entries are committed. + */ + const bool all_done = + match_index_equals_commit_index && + next_index_greater_than_commit_index && + cluster_.isClusterDiscovered(); + + setActiveMode(!all_done); } else { - // Not all local entries are committed + /* + * Not all local entries are committed. + * Deciding if it is safe to increment commit index. + */ setActiveMode(true); uint8_t num_nodes_with_next_log_entry_available = 1; // Local node From e5f3a96476562c6ccc1b3bb757b266a1f2685947 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 May 2015 16:32:52 +0300 Subject: [PATCH 096/143] RaftCore implementation fix --- .../dynamic_node_id_server/distributed/raft_core.hpp | 5 +++++ 1 file changed, 5 insertions(+) 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 aaaa563381..a7abd0bcc4 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 @@ -247,6 +247,11 @@ private: setActiveMode(false); // Haha } + if (append_entries_client_.hasPendingCalls()) + { + append_entries_client_.cancelAllCalls(); // Refer to the response callback to learn why + } + if (active_mode_ || (next_server_index_ > 0)) { const NodeID node_id = cluster_.getRemoteServerNodeIDAtIndex(next_server_index_); From 1234494e7711137755be9d738654352d2ad57497 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 May 2015 16:53:58 +0300 Subject: [PATCH 097/143] uavcan_dynamic_node_id_server will not run if stdout is not a tty --- .../linux/apps/uavcan_dynamic_node_id_server.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp index a922230892..6545ed2d30 100644 --- a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp +++ b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp @@ -618,6 +618,12 @@ int main(int argc, const char** argv) { try { + 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" From 11161e7b1f4c3749e1d5965c480606177262975d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 May 2015 17:18:14 +0300 Subject: [PATCH 098/143] NodeDiscoverer logic fix --- .../dynamic_node_id_server/node_discoverer.hpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) 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 d8042074a2..ef7f901749 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 @@ -267,11 +267,6 @@ class NodeDiscoverer : TimerBase 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()) { @@ -280,6 +275,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", From 73273ab06dcce4898a401d2a0d0b865447f24080 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 May 2015 17:21:26 +0300 Subject: [PATCH 099/143] Optimized NodeDiscoverer - picking any node to query, without any preference --- .../node_discoverer.hpp | 31 ++----------------- 1 file changed, 3 insertions(+), 28 deletions(-) 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 ef7f901749..827551250c 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 @@ -86,27 +86,6 @@ class NodeDiscoverer : TimerBase typedef Map NodeMap; - class HighestUptimeSearcher : ::uavcan::Noncopyable - { - uint32_t highest_uptime_sec_; - NodeID node_id_; - - public: - HighestUptimeSearcher() : highest_uptime_sec_(0) { } - - bool operator()(const NodeID& key, const NodeData& value) - { - if (value.last_seen_uptime >= highest_uptime_sec_) - { - highest_uptime_sec_ = value.last_seen_uptime; - node_id_ = key; - } - return false; - } - - NodeID getNodeWithHighestUptime() const { return node_id_; } - }; - /** * When this number of attempts has been made, the discoverer will give up and assume that the node * does not implement this service. @@ -144,13 +123,9 @@ class NodeDiscoverer : TimerBase NodeID pickNextNodeToQuery() const { - HighestUptimeSearcher searcher; - - const NodeID* const out = node_map_.find(searcher); - (void)out; - UAVCAN_ASSERT(out == NULL); - - return searcher.getNodeWithHighestUptime(); + // 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) From a2104f0bba6296269789309da0d4529b8c68c043 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 May 2015 17:53:23 +0300 Subject: [PATCH 100/143] Fixed Raft timings --- .../dynamic_node_id/server/220.AppendEntries.uavcan | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) 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..77c32edf59 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,17 @@ # Please refer to the specification for details. # -uint16 DEFAULT_REQUEST_TIMEOUT_MS = 100 +uint16 DEFAULT_REQUEST_TIMEOUT_MS = 200 -uint16 DEFAULT_BASE_ELECTION_TIMEOUT_MS = 1000 # request timeout * max cluster size * 2 requests +uint16 DEFAULT_BASE_ELECTION_TIMEOUT_MS = 1600 # request timeout * (max cluster size - 1) * 2 requests uint32 term uint32 prev_log_term uint8 prev_log_index uint8 leader_commit -Entry[<=5] entries +# Full replication: 127 requests * 0.2 sec interval * 4 followers * 2 trips of next_index value ~ 3.5 min +Entry[<=1] entries --- From 2952608ffaca86c41b8052f16282ba7b1f005a12 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 May 2015 18:20:57 +0300 Subject: [PATCH 101/143] RaftCore: new event code --- .../protocol/dynamic_node_id_server/distributed/raft_core.hpp | 1 + .../include/uavcan/protocol/dynamic_node_id_server/event.hpp | 4 ++-- 2 files changed, 3 insertions(+), 2 deletions(-) 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 a7abd0bcc4..97a92f70aa 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 @@ -612,6 +612,7 @@ private: else { cluster_.decrementServerNextIndex(result.getCallID().server_node_id); + trace(TraceRaftAppendEntriesRespUnsucfl, result.getCallID().server_node_id.get()); } } 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 a6b452aaa0..1a75ef7657 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp @@ -52,7 +52,7 @@ enum UAVCAN_EXPORT TraceCode // 25 TraceRaftAppendEntriesCallFailure, // error code (may be negated) TraceRaftElectionComplete, // number of votes collected - Trace1, + TraceRaftAppendEntriesRespUnsucfl, // node ID of the client Trace2, Trace3, // 30 @@ -127,7 +127,7 @@ public: "RaftNewEntryCommitted", "RaftAppendEntriesCallFailure", "RaftElectionComplete", - "", + "RaftAppendEntriesRespUnsucfl", "", "", "AllocationFollowupResponse", From fc173aca445a7a62c5c2ac1028737230815781f0 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 May 2015 18:32:27 +0300 Subject: [PATCH 102/143] uavcan_dynamic_node_id_server fixed coloring --- .../linux/apps/uavcan_dynamic_node_id_server.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp index 6545ed2d30..b1be286fc4 100644 --- a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp +++ b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp @@ -432,7 +432,11 @@ void redraw(const uavcan_linux::NodePtr& node, { 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) { return CLIColor::Magenta; } + 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; }; From 59dd6d090532141040367c25646621d2dc9c78f0 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 May 2015 20:06:13 +0300 Subject: [PATCH 103/143] Raft fix --- .../protocol/dynamic_node_id_server/distributed/raft_core.hpp | 3 +++ 1 file changed, 3 insertions(+) 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 97a92f70aa..3066d8da88 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 @@ -642,6 +642,8 @@ private: */ 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) { @@ -679,6 +681,7 @@ private: 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()); From c323d8e724a24a66a591ca550adb7421c3e42b42 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 May 2015 20:12:33 +0300 Subject: [PATCH 104/143] Raft - ignoring Allocation activity if it is a response --- .../dynamic_node_id_server/distributed/raft_core.hpp | 2 +- .../protocol/dynamic_node_id_server/distributed/server.hpp | 7 +++++-- 2 files changed, 6 insertions(+), 3 deletions(-) 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 3066d8da88..4f3b0a9d05 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 @@ -61,7 +61,7 @@ public: * Active state switch logic: * Activation (this is default state): * - vote request - * - allocation activity detected + * - allocation request at any stage * - only if leader: * - discovery activity detected * - log is not fully replicated or there are uncommitted entries 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 22b3a4b9be..03c4ede47c 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 @@ -135,9 +135,12 @@ class UAVCAN_EXPORT Server : IAllocationRequestHandler } } - virtual void handleAllocationActivityDetection(const ReceivedDataStructure&) + virtual void handleAllocationActivityDetection(const ReceivedDataStructure& msg) { - raft_core_.forceActiveMode(); + if (msg.isAnonymousTransfer()) + { + raft_core_.forceActiveMode(); + } } /* From b7f7defd85435e2857bbc8aa52f700d53c879c5d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 May 2015 20:19:56 +0300 Subject: [PATCH 105/143] Raft implementation fix --- .../dynamic_node_id_server/distributed/cluster_manager.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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..6e0a8e038c 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 @@ -91,7 +91,7 @@ private: void addServer(NodeID node_id) { - UAVCAN_ASSERT((num_known_servers_ + 1) < (MaxClusterSize - 2)); + UAVCAN_ASSERT((num_known_servers_ + 1) < MaxClusterSize); if (!isKnownServer(node_id) && node_id.isUnicast()) { tracer_.onEvent(TraceRaftNewServerDiscovered, node_id.get()); From eb6102a9d49831492312910d690bcdc65bbb80e1 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 May 2015 21:09:56 +0300 Subject: [PATCH 106/143] Raft - minor timing fix, no changes to the logic --- .../dynamic_node_id_server/distributed/raft_core.hpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) 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 4f3b0a9d05..a1600a9e16 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 @@ -156,10 +156,7 @@ private: 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); - + const MonotonicDuration activity_timeout = base_activity_timeout_ + update_interval_ * multiplier; return getNode().getMonotonicTime() > (last_activity_timestamp_ + activity_timeout); } @@ -798,7 +795,7 @@ public: /* * Initializing state variables */ - last_activity_timestamp_ = getNode().getMonotonicTime(); + last_activity_timestamp_ = getNode().getMonotonicTime() + update_interval_; active_mode_ = true; server_state_ = ServerStateFollower; next_server_index_ = 0; From 8c777938926c3fd45f2bb2acfe3265ce7f5356c1 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 May 2015 21:14:59 +0300 Subject: [PATCH 107/143] Raft logic fix: auto-discovery on AE request --- .../distributed/cluster_manager.hpp | 35 ++++++++++--------- .../distributed/raft_core.hpp | 13 +++++-- 2 files changed, 29 insertions(+), 19 deletions(-) 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 6e0a8e038c..f237b19436 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 @@ -89,22 +89,6 @@ private: return NULL; } - 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); - } - } - virtual void handleTimerEvent(const TimerEvent&) { UAVCAN_ASSERT(num_known_servers_ < cluster_size_); @@ -301,6 +285,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. */ 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 a1600a9e16..8abdb8919e 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 @@ -457,9 +457,16 @@ private: { 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 From 702c96a1923ab1b3d4a0e5f7a1f9d0a8f30f1ba5 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 May 2015 21:21:36 +0300 Subject: [PATCH 108/143] Node<>::getInternalFailureCount() --- libuavcan/include/uavcan/node/node.hpp | 5 +++++ 1 file changed, 5 insertions(+) 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. From 78a380062c7fa23a229db0102cc4f5564ed1a685 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 May 2015 21:25:38 +0300 Subject: [PATCH 109/143] uavcan_dynamic_node_id_server - printing the number of internal failures --- .../linux/apps/uavcan_dynamic_node_id_server.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp index b1be286fc4..a46c398801 100644 --- a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp +++ b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp @@ -392,6 +392,10 @@ void redraw(const uavcan_linux::NodePtr& node, 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(); @@ -455,10 +459,6 @@ void redraw(const uavcan_linux::NodePtr& node, [&](std::uint8_t i) { return report.followers[i].match_index; }, follower_color_getter); - // Empty line before the next block - std::printf(" "); - render_next_event_counter(); - assert(next_relevant_event_index == NumRelevantEvents); // Ensuring that all events can be printed // Separator From 17c4b975acf5a0d70181c7a763dcca5c9d33f981 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 May 2015 23:13:10 +0300 Subject: [PATCH 110/143] Test fix --- .../protocol/dynamic_node_id_server/distributed/server.cpp | 2 +- .../protocol/dynamic_node_id_server/node_discoverer.cpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) 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..eac00afa31 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp @@ -166,7 +166,7 @@ TEST(dynamic_node_id_server_Server, Basic) /* * Fire */ - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(4000)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(6000)); ASSERT_TRUE(client.isAllocationComplete()); ASSERT_EQ(PreferredNodeID, client.getAllocatedNodeID()); 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 7d1d692976..304070bf3f 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp @@ -226,7 +226,7 @@ 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(1650)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1600)); ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound)); ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart)); @@ -244,7 +244,7 @@ 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(1650)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1600)); ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound)); ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart)); @@ -258,7 +258,7 @@ TEST(dynamic_node_id_server_NodeDiscoverer, RestartAndMaxAttempts) /* * Waiting for timeout */ - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1650)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1600)); ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound)); ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart)); From 258da95d12dc8efafd93f3eba15677d0382e3c16 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 26 May 2015 00:15:18 +0300 Subject: [PATCH 111/143] RaftCore::checkInvariants() --- .../distributed/raft_core.hpp | 29 +++++++++++++++++++ 1 file changed, 29 insertions(+) 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 8abdb8919e..18ba5f8c52 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 @@ -151,6 +151,28 @@ private: INode& getNode() { return append_entries_srv_.getNode(); } const INode& getNode() const { return append_entries_srv_.getNode(); } + void checkInvariants() const + { + // Commit index + UAVCAN_ASSERT(commit_index_ <= persistent_state_.getLog().getLastIndex()); + + // Term + UAVCAN_ASSERT(persistent_state_.getLog().getEntryAtIndex(persistent_state_.getLog().getLastIndex()) != NULL); + UAVCAN_ASSERT(persistent_state_.getLog().getEntryAtIndex(persistent_state_.getLog().getLastIndex())->term + <= persistent_state_.getCurrentTerm()); + + // Elections + UAVCAN_ASSERT(server_state_ != ServerStateCandidate || !request_vote_client_.hasPendingCalls() || + persistent_state_.getVotedFor() == getNode().getNodeID()); + UAVCAN_ASSERT(num_votes_received_in_this_campaign_ <= cluster_.getClusterSize()); + + // Transport + UAVCAN_ASSERT(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(); } bool isActivityTimedOut() const @@ -455,6 +477,8 @@ private: void handleAppendEntriesRequest(const ReceivedDataStructure& request, ServiceResponseDataStructure& response) { + checkInvariants(); + if (!cluster_.isKnownServer(request.getSrcNodeID())) { if (cluster_.isClusterDiscovered()) @@ -585,6 +609,7 @@ private: void handleAppendEntriesResponse(const ServiceCallResult& result) { UAVCAN_ASSERT(server_state_ == ServerStateLeader); // When state switches, all requests must be cancelled + checkInvariants(); if (!result.isSuccessful()) { @@ -627,6 +652,7 @@ private: void handleRequestVoteRequest(const ReceivedDataStructure& request, ServiceResponseDataStructure& response) { + checkInvariants(); trace(TraceRaftVoteRequestReceived, request.getSrcNodeID().get()); if (!cluster_.isKnownServer(request.getSrcNodeID())) @@ -702,6 +728,7 @@ private: void handleRequestVoteResponse(const ServiceCallResult& result) { UAVCAN_ASSERT(server_state_ == ServerStateCandidate); // When state switches, all requests must be cancelled + checkInvariants(); if (!result.isSuccessful()) { @@ -733,6 +760,8 @@ private: virtual void handleTimerEvent(const TimerEvent&) { + checkInvariants(); + if (cluster_.hadDiscoveryActivity() && isLeader()) { setActiveMode(true); From e289a1e09c3777ce1ae12b9aa1b6b0097bd162d7 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 26 May 2015 01:22:26 +0300 Subject: [PATCH 112/143] uavcan_linux::makeApplicationID() --- .../apps/uavcan_dynamic_node_id_server.cpp | 16 ++++++-- .../include/uavcan_linux/system_utils.hpp | 41 +++++++++++++++++++ 2 files changed, 53 insertions(+), 4 deletions(-) diff --git a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp index a46c398801..3fe3dac64c 100644 --- a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp +++ b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp @@ -39,10 +39,10 @@ uavcan_linux::NodePtr initNode(const std::vector& ifaces, uavcan::N node->getLogger().setLevel(uavcan::protocol::debug::LogLevel::DEBUG); { - const auto machine_id = uavcan_linux::MachineIDReader().read(); + const auto app_id = uavcan_linux::makeApplicationID(uavcan_linux::MachineIDReader().read(), name, nid.get()); uavcan::protocol::HardwareVersion hwver; - std::copy(machine_id.begin(), machine_id.end(), hwver.unique_id.begin()); + std::copy(app_id.begin(), app_id.end(), hwver.unique_id.begin()); std::cout << hwver << std::endl; node->setHardwareVersion(hwver); @@ -498,7 +498,6 @@ void runForever(const uavcan_linux::NodePtr& node, * Storage backend */ uavcan_posix::dynamic_node_id_server::FileStorageBackend storage_backend; - ENFORCE(0 <= storage_backend.init(persistent_storage_path.c_str())); /* @@ -506,7 +505,16 @@ void runForever(const uavcan_linux::NodePtr& node, */ uavcan::dynamic_node_id_server::DistributedServer server(*node, storage_backend, event_tracer); - ENFORCE(0 <= server.init(node->getNodeStatusProvider().getHardwareVersion().unique_id, cluster_size)); + 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 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; +} } From d89a8dcbcc7b2c4e6677308232485045cce74158 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 26 May 2015 01:50:24 +0300 Subject: [PATCH 113/143] Linux test app - Dynamic node ID client --- libuavcan_drivers/linux/CMakeLists.txt | 3 + .../apps/test_dynamic_node_id_client.cpp | 121 ++++++++++++++++++ 2 files changed, 124 insertions(+) create mode 100644 libuavcan_drivers/linux/apps/test_dynamic_node_id_client.cpp diff --git a/libuavcan_drivers/linux/CMakeLists.txt b/libuavcan_drivers/linux/CMakeLists.txt index 408fff617c..b33af01b8e 100644 --- a/libuavcan_drivers/linux/CMakeLists.txt +++ b/libuavcan_drivers/linux/CMakeLists.txt @@ -60,6 +60,9 @@ target_link_libraries(test_system_utils ${UAVCAN_LIB} rt ${CMAKE_THREAD_LIBS_INI 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 # 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; + } +} From 97b35cd09e553e097be68a37d00c2e767157aa4f Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 26 May 2015 01:57:07 +0300 Subject: [PATCH 114/143] NodeIDSelector fix --- .../uavcan/protocol/dynamic_node_id_server/node_id_selector.hpp | 1 - 1 file changed, 1 deletion(-) 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) From 388c023168404da0bdce36e06a0d471d6794fb56 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 26 May 2015 02:11:02 +0300 Subject: [PATCH 115/143] uavcan_dynamic_node_id_server - longer log --- libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp index 3fe3dac64c..4b2d13f1de 100644 --- a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp +++ b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp @@ -27,7 +27,7 @@ namespace { -constexpr int MaxNumLastEvents = 30; +constexpr int MaxNumLastEvents = 50; constexpr int MinUpdateInterval = 100; uavcan_linux::NodePtr initNode(const std::vector& ifaces, uavcan::NodeID nid, const std::string& name) From 2231b0063743aab06b09aa4888db42d444284baf Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 26 May 2015 20:04:59 +0300 Subject: [PATCH 116/143] Raft active state extension removed --- .../server/220.AppendEntries.uavcan | 17 +- .../distributed/cluster_manager.hpp | 18 -- .../distributed/raft_core.hpp | 200 +++++------------- .../distributed/server.hpp | 11 +- .../distributed/server.cpp | 19 +- .../apps/uavcan_dynamic_node_id_server.cpp | 10 +- 6 files changed, 87 insertions(+), 188 deletions(-) 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 77c32edf59..e3009286d2 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,25 @@ # Please refer to the specification for details. # -uint16 DEFAULT_REQUEST_TIMEOUT_MS = 200 - -uint16 DEFAULT_BASE_ELECTION_TIMEOUT_MS = 1600 # request timeout * (max cluster size - 1) * 2 requests +# +# Given min election timeout and cluster size, the maximum request interval can be derived as follows: +# max request interval = (min election timeout) / 2 requests / (cluster size - 1) +# Obviously, request interval can be lower than that if needed, but not higher. +# +# Real timeout is randomized in the range (MIN, MAX]. +# +uint16 MIN_ELECTION_TIMEOUT_MS = 4000 +uint16 MAX_ELECTION_TIMEOUT_MS = 6000 uint32 term uint32 prev_log_term uint8 prev_log_index uint8 leader_commit -# Full replication: 127 requests * 0.2 sec interval * 4 followers * 2 trips of next_index value ~ 3.5 min +# +# Worst-case replication time can be computed as: +# worst replication time = (127 log entries) * (2 trips of next_index) * (cluster size - 1) * (request interval) +# Entry[<=1] entries --- 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 f237b19436..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(); } @@ -151,8 +149,6 @@ private: return; } - had_discovery_activity_ = true; - /* * Updating the set of known servers */ @@ -204,7 +200,6 @@ public: , discovery_pub_(node) , cluster_size_(0) , num_known_servers_(0) - , had_discovery_activity_(false) { } /** @@ -417,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/raft_core.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp index 18ba5f8c52..5b776553c8 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,26 +52,14 @@ 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 request at any stage - * - only if leader: - * - discovery activity detected - * - log is not fully replicated or there are uncommitted entries - * - * Deactivation: - * - switch to follower state - * - persistent state update error - * - only if leader: - * - all log entries are fully replicated and committed */ class RaftCore : private TimerBase { @@ -111,8 +100,7 @@ private: /* * 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_; @@ -125,7 +113,7 @@ private: 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 @@ -139,9 +127,7 @@ private: ServiceServer append_entries_srv_; ServiceClient append_entries_client_; ServiceServer request_vote_srv_; - - enum { NumRequestVoteCalls = ClusterManager::MaxClusterSize - 1 }; - ServiceClient request_vote_client_; + ServiceClient request_vote_client_; /* * Methods @@ -167,19 +153,33 @@ private: 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(); } + void registerActivity() + { + last_activity_timestamp_ = getNode().getMonotonicTime(); + + static const int32_t randomization_range_msec = + AppendEntries::Request::MAX_ELECTION_TIMEOUT_MS - AppendEntries::Request::MIN_ELECTION_TIMEOUT_MS; + + const int32_t random_msec = (std::rand() % randomization_range_msec) + 1; + + randomized_activity_timeout_ = + MonotonicDuration::fromMSec(AppendEntries::Request::MIN_ELECTION_TIMEOUT_MS + random_msec); + + UAVCAN_ASSERT(randomized_activity_timeout_.toMSec() > AppendEntries::Request::MIN_ELECTION_TIMEOUT_MS); + UAVCAN_ASSERT(randomized_activity_timeout_.toMSec() <= AppendEntries::Request::MAX_ELECTION_TIMEOUT_MS); + } bool isActivityTimedOut() const { - const int multiplier = static_cast(getNode().getNodeID().get()) - 1; - const MonotonicDuration activity_timeout = base_activity_timeout_ + update_interval_ * multiplier; - return getNode().getMonotonicTime() > (last_activity_timestamp_ + activity_timeout); + return getNode().getMonotonicTime() > (last_activity_timestamp_ + randomized_activity_timeout_); } void handlePersistentStateUpdateError(int error) @@ -187,13 +187,12 @@ private: 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(); @@ -202,8 +201,6 @@ private: void updateCandidate() { - UAVCAN_ASSERT(active_mode_); - if (num_votes_received_in_this_campaign_ > 0) { trace(TraceRaftElectionComplete, num_votes_received_in_this_campaign_); @@ -238,7 +235,7 @@ private: req.last_log_term = persistent_state_.getLog().getEntryAtIndex(req.last_log_index)->term; req.term = persistent_state_.getCurrentTerm(); - for (uint8_t i = 0; i < NumRequestVoteCalls; i++) + for (uint8_t i = 0; i < MaxNumFollowers; i++) { const NodeID node_id = cluster_.getRemoteServerNodeIDAtIndex(i); if (!node_id.isUnicast()) @@ -261,17 +258,12 @@ private: void updateLeader() { - if (cluster_.getClusterSize() == 1) - { - setActiveMode(false); // Haha - } - if (append_entries_client_.hasPendingCalls()) { 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()); @@ -364,18 +356,6 @@ private: } } - 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); @@ -386,7 +366,6 @@ private: } registerActivity(); // Deferring future elections switchState(ServerStateFollower); - setActiveMode(false); } void propagateCommitIndex() @@ -395,63 +374,12 @@ private: UAVCAN_ASSERT(server_state_ == ServerStateLeader); UAVCAN_ASSERT(commit_index_ <= persistent_state_.getLog().getLastIndex()); - if (commit_index_ == persistent_state_.getLog().getLastIndex()) - { - /* - * All local entries are committed. - * Deciding if it is safe to go into passive mode now. - * - * We can go into passive mode if the log is known to be fully replicated and all entries are committed. - * The high-level conditions above are guaranteed to be met if all of the following lower-level conditions - * are met: - * - All local entries are committed (already checked here). - * - Match index on all nodes equals local commit index. - * - Next index on all nodes is strictly greater than the local commit index. - * - * The following code checks if the last two conditions are met. - */ - bool match_index_equals_commit_index = true; - bool next_index_greater_than_commit_index = true; - - for (uint8_t i = 0; i < cluster_.getNumKnownServers(); i++) - { - const NodeID server_node_id = cluster_.getRemoteServerNodeIDAtIndex(i); - - const Log::Index match_index = cluster_.getServerMatchIndex(server_node_id); - if (match_index != commit_index_) - { - match_index_equals_commit_index = false; - break; - } - - const Log::Index next_index = cluster_.getServerNextIndex(server_node_id); - if (next_index <= commit_index_) - { - next_index_greater_than_commit_index = false; - break; - } - } - - /* - * Now we know whether the log is replicated and whether all entries are committed, so we can make - * the decision. Remember that since we ended up in this branch, it is already known that all local - * log entries are committed. - */ - const bool all_done = - match_index_equals_commit_index && - next_index_greater_than_commit_index && - cluster_.isClusterDiscovered(); - - setActiveMode(!all_done); - } - else + if (commit_index_ < persistent_state_.getLog().getLastIndex()) { /* * Not all local entries are committed. * Deciding if it is safe to increment commit index. */ - setActiveMode(true); - uint8_t num_nodes_with_next_log_entry_available = 1; // Local node for (uint8_t i = 0; i < cluster_.getNumKnownServers(); i++) { @@ -536,7 +464,6 @@ private: registerActivity(); switchState(ServerStateFollower); - setActiveMode(false); /* * Step 2 @@ -613,14 +540,6 @@ private: if (!result.isSuccessful()) { - /* - * This callback WILL NEVER be invoked by timeout, because: - * - Any pending request will be cancelled on the next update of the Leader. - * - When the state switches (i.e. the node is not Leader anymore), all pending calls will be cancelled. - * Also note that 'pending_append_entries_fields_' invalidates after every update of the Leader, so - * if there were timeout callbacks, they would be using outdated state. - */ - UAVCAN_ASSERT(0); return; } @@ -664,8 +583,6 @@ private: 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. @@ -732,12 +649,6 @@ private: if (!result.isSuccessful()) { - /* - * This callback WILL NEVER be invoked by timeout, because all pending calls will be cancelled on - * state switch, which ALWAYS happens 100ms after elections (either to Follower or to Leader, depending - * on the number of votes collected). - */ - UAVCAN_ASSERT(0); return; } @@ -762,11 +673,6 @@ private: { checkInvariants(); - if (cluster_.hadDiscoveryActivity() && isLeader()) - { - setActiveMode(true); - } - switch (server_state_) { case ServerStateFollower: @@ -796,21 +702,15 @@ 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::MAX_ELECTION_TIMEOUT_MS)) , server_state_(ServerStateFollower) , next_server_index_(0) , num_votes_received_in_this_campaign_(0) @@ -831,13 +731,13 @@ public: /* * Initializing state variables */ - last_activity_timestamp_ = getNode().getMonotonicTime() + update_interval_; - active_mode_ = true; server_state_ = ServerStateFollower; next_server_index_ = 0; num_votes_received_in_this_campaign_ = 0; commit_index_ = 0; + registerActivity(); + /* * Initializing internals */ @@ -872,7 +772,6 @@ public: } append_entries_client_.setCallback(AppendEntriesResponseCallback(this, &RaftCore::handleAppendEntriesResponse)); - append_entries_client_.setRequestTimeout(update_interval_); res = request_vote_client_.init(); if (res < 0) @@ -880,24 +779,34 @@ public: return res; } request_vote_client_.setCallback(RequestVoteResponseCallback(this, &RaftCore::handleRequestVoteResponse)); - request_vote_client_.setRequestTimeout(update_interval_); - 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::MIN_ELECTION_TIMEOUT_MS / + 2 / max(static_cast(3), 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. */ @@ -994,8 +903,9 @@ public: const PersistentState& getPersistentState() const { return persistent_state_; } const ClusterManager& getClusterManager() const { return cluster_; } MonotonicTime getLastActivityTimestamp() const { return last_activity_timestamp_; } - bool isInActiveMode() const { return active_mode_; } 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 03c4ede47c..31b709d441 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 @@ -135,12 +135,9 @@ class UAVCAN_EXPORT Server : IAllocationRequestHandler } } - virtual void handleAllocationActivityDetection(const ReceivedDataStructure& msg) + virtual void handleAllocationActivityDetection(const ReceivedDataStructure&) { - if (msg.isAnonymousTransfer()) - { - raft_core_.forceActiveMode(); - } + // TODO: remove this method } /* @@ -315,7 +312,6 @@ struct StateReport uint8_t cluster_size; RaftCore::ServerState state; - bool is_active; Log::Index last_log_index; Log::Index commit_index; @@ -326,6 +322,7 @@ struct StateReport NodeID voted_for; MonotonicTime last_activity_timestamp; + MonotonicDuration randomized_timeout; uint8_t num_unknown_nodes; @@ -344,13 +341,13 @@ struct StateReport StateReport(const Server& s) : cluster_size (s.getRaftCore().getClusterManager().getClusterSize()) , state (s.getRaftCore().getServerState()) - , is_active (s.getRaftCore().isInActiveMode()) , 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); 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 eac00afa31..cd4a67d393 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(6000)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(9000)); ASSERT_TRUE(client.isAllocationComplete()); ASSERT_EQ(PreferredNodeID, client.getAllocatedNodeID()); diff --git a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp index 4b2d13f1de..03372ec396 100644 --- a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp +++ b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp @@ -360,10 +360,6 @@ void redraw(const uavcan_linux::NodePtr& node, (report.state == RaftCore::ServerStateLeader) ? CLIColor::Green : CLIColor::Default); - render_top_str("Mode", - report.is_active ? "Active" : "Passive", - colorize_if(report.is_active, CLIColor::Magenta)); - render_top_int("Last log index", report.last_log_index, CLIColor::Default); @@ -388,6 +384,10 @@ void redraw(const uavcan_linux::NodePtr& node, 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)); @@ -630,6 +630,8 @@ 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; From e60a76d5621a6cbf01de736aad4d5463ca707941 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 26 May 2015 20:08:38 +0300 Subject: [PATCH 117/143] Test timing fix --- libuavcan/test/protocol/firmware_update_trigger.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libuavcan/test/protocol/firmware_update_trigger.cpp b/libuavcan/test/protocol/firmware_update_trigger.cpp index 2e6f286160..08ef9c4fd8 100644 --- a/libuavcan/test/protocol/firmware_update_trigger.cpp +++ b/libuavcan/test/protocol/firmware_update_trigger.cpp @@ -318,7 +318,7 @@ TEST(FirmwareUpdateTrigger, MultiNode) * This also checks correctness of the round-robin selector */ checker.retry_quota = 2; - nodes.spinAll(uavcan::MonotonicDuration::fromMSec(4100)); // Two will retry, one drop, one confirm + nodes.spinAll(uavcan::MonotonicDuration::fromMSec(4200)); // Two will retry, one drop, one confirm ASSERT_TRUE(trigger.isTimerRunning()); ASSERT_EQ(0, trigger.getNumPendingNodes()); // All removed From a97762ae212f5519cf9eaec9712afeaf4f6bd968 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 26 May 2015 20:13:13 +0300 Subject: [PATCH 118/143] Dead code removal --- .../dynamic_node_id_server/allocation_request_manager.hpp | 6 ------ .../protocol/dynamic_node_id_server/distributed/server.hpp | 5 ----- .../dynamic_node_id_server/allocation_request_manager.cpp | 6 ------ 3 files changed, 17 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp index cd0f44832e..2c0aa98670 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp @@ -35,11 +35,6 @@ public: */ virtual void handleAllocationRequest(const UniqueID& unique_id, NodeID preferred_node_id) = 0; - /** - * This method will be invoked when there's an Allocation message detected on the bus. - */ - virtual void handleAllocationActivityDetection(const ReceivedDataStructure& 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()) { 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 31b709d441..5fd35bb50a 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 @@ -135,11 +135,6 @@ class UAVCAN_EXPORT Server : IAllocationRequestHandler } } - virtual void handleAllocationActivityDetection(const ReceivedDataStructure&) - { - // TODO: remove this method - } - /* * Methods of INodeDiscoveryHandler */ 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()) From 9faf8470e6e5cdd430ba68bc5e00aa23786e90ef Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 26 May 2015 20:18:07 +0300 Subject: [PATCH 119/143] Fixed Raft definitions --- .../server/220.AppendEntries.uavcan | 4 ++-- .../distributed/raft_core.hpp | 17 +++++++++-------- .../distributed/server.cpp | 2 +- .../apps/uavcan_dynamic_node_id_server.cpp | 2 +- 4 files changed, 13 insertions(+), 12 deletions(-) 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 e3009286d2..329bfff8e6 100644 --- a/dsdl/uavcan/protocol/dynamic_node_id/server/220.AppendEntries.uavcan +++ b/dsdl/uavcan/protocol/dynamic_node_id/server/220.AppendEntries.uavcan @@ -10,8 +10,8 @@ # # Real timeout is randomized in the range (MIN, MAX]. # -uint16 MIN_ELECTION_TIMEOUT_MS = 4000 -uint16 MAX_ELECTION_TIMEOUT_MS = 6000 +uint16 DEFAULT_MIN_ELECTION_TIMEOUT_MS = 4000 +uint16 DEFAULT_MAX_ELECTION_TIMEOUT_MS = 6000 uint32 term uint32 prev_log_term 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 5b776553c8..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 @@ -165,16 +165,16 @@ private: { last_activity_timestamp_ = getNode().getMonotonicTime(); - static const int32_t randomization_range_msec = - AppendEntries::Request::MAX_ELECTION_TIMEOUT_MS - AppendEntries::Request::MIN_ELECTION_TIMEOUT_MS; + 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::MIN_ELECTION_TIMEOUT_MS + random_msec); + MonotonicDuration::fromMSec(AppendEntries::Request::DEFAULT_MIN_ELECTION_TIMEOUT_MS + random_msec); - UAVCAN_ASSERT(randomized_activity_timeout_.toMSec() > AppendEntries::Request::MIN_ELECTION_TIMEOUT_MS); - UAVCAN_ASSERT(randomized_activity_timeout_.toMSec() <= AppendEntries::Request::MAX_ELECTION_TIMEOUT_MS); + 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 @@ -710,7 +710,8 @@ public: , cluster_(node, storage, persistent_state_.getLog(), tracer) , commit_index_(0) // Per Raft paper, commitIndex must be initialized to zero , last_activity_timestamp_(node.getMonotonicTime()) - , randomized_activity_timeout_(MonotonicDuration::fromMSec(AppendEntries::Request::MAX_ELECTION_TIMEOUT_MS)) + , 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) @@ -787,8 +788,8 @@ public: const uint8_t num_followers = static_cast(cluster_.getClusterSize() - 1); const MonotonicDuration update_interval = - MonotonicDuration::fromMSec(AppendEntries::Request::MIN_ELECTION_TIMEOUT_MS / - 2 / max(static_cast(3), num_followers)); + 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())); 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 cd4a67d393..59a4178ebd 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp @@ -165,7 +165,7 @@ TEST(dynamic_node_id_server_Server, Basic) /* * Fire */ - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(9000)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(15000)); ASSERT_TRUE(client.isAllocationComplete()); ASSERT_EQ(PreferredNodeID, client.getAllocatedNodeID()); diff --git a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp index 03372ec396..39634e313a 100644 --- a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp +++ b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp @@ -27,7 +27,7 @@ namespace { -constexpr int MaxNumLastEvents = 50; +constexpr int MaxNumLastEvents = 30; constexpr int MinUpdateInterval = 100; uavcan_linux::NodePtr initNode(const std::vector& ifaces, uavcan::NodeID nid, const std::string& name) From aafdf81f29735283ad6c904932618ae2cb7e4dc2 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 26 May 2015 20:25:29 +0300 Subject: [PATCH 120/143] Removed unused event code --- .../include/uavcan/protocol/dynamic_node_id_server/event.hpp | 4 ++-- libuavcan/test/protocol/dynamic_node_id_server/event.cpp | 1 - 2 files changed, 2 insertions(+), 3 deletions(-) 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 1a75ef7657..d447e01ac6 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp @@ -38,7 +38,7 @@ enum UAVCAN_EXPORT 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 @@ -115,7 +115,7 @@ public: "RaftBadClusterSizeReceived", "RaftCoreInited", "RaftStateSwitch", - "RaftActiveSwitch", + "", "RaftNewLogEntry", "RaftRequestIgnored", "RaftVoteRequestReceived", 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)); From 6a34d19cc5136ce193b733971e07738549d5651a Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 26 May 2015 20:36:41 +0300 Subject: [PATCH 121/143] Style fixes in uavcan_posix/dynamic_node_id_server --- .../dynamic_node_id_server/file_event_tracer.hpp | 6 +++--- .../dynamic_node_id_server/file_storage_backend.hpp | 3 ++- 2 files changed, 5 insertions(+), 4 deletions(-) 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 index 863d7465a1..45a6d11c12 100644 --- 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 @@ -54,8 +54,8 @@ protected: 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)); - write(fd, buffer, n); - close(fd); + (void)write(fd, buffer, n); // TODO FIXME Write loop + (void)close(fd); } } @@ -76,7 +76,7 @@ public: int fd = open(path_.c_str(), O_RDWR | O_CREAT | O_TRUNC, FilePermissions); if (fd >= 0) { - close(fd); + (void)close(fd); } } return rv; diff --git a/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp index a2834c6cfb..76c515a8cb 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp @@ -43,6 +43,7 @@ class FileStorageBackend : public uavcan::dynamic_node_id_server::IStorageBacken PathString base_path; +protected: virtual String get(const String& key) const { using namespace std; @@ -80,7 +81,7 @@ class FileStorageBackend : public uavcan::dynamic_node_id_server::IStorageBacken int fd = open(path.c_str(), O_WRONLY | O_CREAT | O_TRUNC, FilePermissions); if (fd >= 0) { - (void)write(fd, value.c_str(), value.size()); + (void)write(fd, value.c_str(), value.size()); // TODO FIXME Write loop (void)fsync(fd); (void)close(fd); } From 53317eb90249f4f3df6501f68761e1c4c99e86c3 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 26 May 2015 20:37:58 +0300 Subject: [PATCH 122/143] BasicFileSeverBackend style fixes --- .../basic_file_server_backend.hpp | 34 +++++-------------- 1 file changed, 9 insertions(+), 25 deletions(-) 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 index c88bf8b731..7a02ee11e1 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp @@ -33,32 +33,28 @@ namespace uavcan_posix */ class BasicFileSeverBackend : public uavcan::IFileServerBackend { - enum { FilePermissions = 438 }; ///< 0o666 -public: +protected: /** - * * Back-end for uavcan.protocol.file.GetInfo. * Implementation of this method is required. * On success the method must return zero. */ - __attribute__((optimize("O0"))) 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; if (path.size() > 0) { - FirmwareCommon fw; fw.getFileInfo(path.c_str()); out_crc64 = fw.descriptor.image_crc; out_size = fw.descriptor.image_size; - // todo Using fixed flag FLAG_READABLE until we add file permission checks to return actual value. + // 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 out_type.flags = uavcan::protocol::file::EntryType::FLAG_FILE | - uavcan::protocol::file::EntryType::FLAG_READABLE;; + uavcan::protocol::file::EntryType::FLAG_READABLE; rv = 0; } return rv; @@ -71,16 +67,12 @@ public: * if the end of file is reached. * On success the method must return zero. */ - __attribute__((optimize("O0"))) 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) @@ -89,38 +81,30 @@ public: } else { - if (::lseek(fd, offset, SEEK_SET) < 0) { rv = errno; } else { - //todo uses a read at offset to fill on EAGAIN - ssize_t len = ::read(fd, out_buffer, inout_size); + // TODO use a read at offset to fill on EAGAIN + ssize_t len = ::read(fd, out_buffer, inout_size); - if (len < 0) + if (len < 0) { rv = errno; } else { - - inout_size = len; + inout_size = len; rv = 0; } } - close(fd); + (void)close(fd); } } return rv; } - - BasicFileSeverBackend() { } - - - - }; } From 85f498bbe0c01137246975fa91a12e38e70a22a5 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 26 May 2015 21:05:35 +0300 Subject: [PATCH 123/143] FirmwareVersionChecker formatting fix --- .../uavcan_posix/firmware_version_checker.hpp | 328 ++++++++---------- 1 file changed, 151 insertions(+), 177 deletions(-) diff --git a/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp b/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp index 3066c07a18..c53b6e2c08 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp @@ -10,10 +10,9 @@ #define UAVCAN_POSIX_FIRMWARE_VERSION_CHECKER_HPP_INCLUDED #include -#include #include #include -#include +#include #include #include @@ -31,191 +30,24 @@ class FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker { enum { FilePermissions = 438 }; ///< 0o666 - FirmwarePath *paths_; + FirmwarePath* paths_; -public: - - /** - * 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. - */ - __attribute__((optimize("O0"))) - virtual bool shouldRequestFirmwareUpdate(uavcan::NodeID node_id, const - uavcan::protocol::GetNodeInfo::Response& node_info, - FirmwareFilePath& out_firmware_file_path) + int copyIfNot(const char* srcpath, const char* destpath) { - /* 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[FirmwarePath::MaxBasePathLength + 1]; - int n = snprintf(fname_root, sizeof(fname_root), "%s%s/%d.%d", - paths_->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) - { - - FAR DIR *fwdir = opendir(fname_root); - - fname_root[n++] = FirmwarePath::getPathSeparator(); - fname_root[n++] = '\0'; - - if (fwdir) - { - FAR struct dirent *pfile; - while((pfile = readdir(fwdir)) != NULL) - { - if (DIRENT_ISFILE(pfile->d_type)) - { - // Open any bin file in there. - if (strstr(pfile->d_name, ".bin")) - { - FirmwarePath::PathString full_src_path = fname_root; - full_src_path += pfile->d_name; - - FirmwarePath::PathString full_dst_path = paths_->getFirmwareCachePath().c_str(); - full_dst_path += pfile->d_name; - - /* ease the burden on the user */ - int cr = copy_if_not(full_src_path.c_str(), full_dst_path.c_str()); - - /* We have a file, is it a valid image */ - - FirmwareCommon fw; - - if (cr == 0 && fw.getFileInfo(full_dst_path.c_str()) == 0) - { - if (node_info.software_version.image_crc == 0 || - (node_info.software_version.major == 0 && - node_info.software_version.minor == 0) || - fw.descriptor.image_crc != - node_info.software_version.image_crc) - { - rv = true; - out_firmware_file_path = pfile->d_name; - } - break; - } - } - } - } - 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. - */ - __attribute__((optimize("O0"))) - virtual bool shouldRetryFirmwareUpdate(uavcan::NodeID node_id, - const uavcan::protocol::file::BeginFirmwareUpdate::Response& error_response, - FirmwareFilePath& out_firmware_file_path) - { - return true; - - } - - /** - * 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(uavcan::NodeID node_id, - const uavcan::protocol::file::BeginFirmwareUpdate::Response& response) - { - (void)node_id; - (void)response; - } - - FirmwareVersionChecker() : - paths_(0) - { } - - virtual ~FirmwareVersionChecker() { } - - /** - * Initializes the Firmware File back-end storage by passing a paths object - * that maintains the directory where files will be stored. - */ - - int init(FirmwarePath& paths) - { - paths_ = &paths; - return 0; - } - - const char * getFirmwarePath() { return paths_->getFirmwareCachePath().c_str(); } - -private: - - - __attribute__((optimize("O0"))) - int copy_if_not(const char *srcpath, const char * destpath) - { - /* Does the file exits */ + // Does the file exist int rv = 0; int dfd = open(destpath, O_RDONLY, 0); if (dfd >= 0) { - /* Close it and exit 0 */ - close(dfd); + // Close it and exit 0 + (void)close(dfd); } else { uint8_t buffer[512]; - dfd = open(destpath, O_WRONLY | O_CREAT , FilePermissions); + dfd = open(destpath, O_WRONLY | O_CREAT, FilePermissions); if (dfd < 0) { rv = -errno; @@ -252,14 +84,156 @@ private: } } while (rv == 0 && size); - close(sfd); + + (void)close(sfd); } - close(dfd); + (void)close(dfd); } } 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) + { + /* 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[FirmwarePath::MaxBasePathLength + 1]; + int n = snprintf(fname_root, sizeof(fname_root), "%s%s/%d.%d", + paths_->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++] = FirmwarePath::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) + { + FirmwarePath::PathString full_src_path = fname_root; + full_src_path += pfile->d_name; + + FirmwarePath::PathString full_dst_path = paths_->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 + FirmwareCommon fw; + + if (cr == 0 && fw.getFileInfo(full_dst_path.c_str()) == 0) + { + if (node_info.software_version.image_crc == 0 || + (node_info.software_version.major == 0 && node_info.software_version.minor == 0) || + fw.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: + FirmwareVersionChecker() + : paths_(NULL) + { } + + /** + * Initializes the Firmware File back-end storage by passing a paths object + * that maintains the directory where files will be stored. + */ + int init(FirmwarePath& paths) + { + paths_ = &paths; + return 0; + } + + const char* getFirmwarePath() const + { + return paths_->getFirmwareCachePath().c_str(); + } }; + } #endif // Include guard From adbf0595850cb7ca86b6967473352a06d2098190 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 26 May 2015 21:27:33 +0300 Subject: [PATCH 124/143] Fixes and notes in FirmwarePath --- .../include/uavcan_posix/firmware_path.hpp | 87 ++++++++----------- 1 file changed, 35 insertions(+), 52 deletions(-) diff --git a/libuavcan_drivers/posix/include/uavcan_posix/firmware_path.hpp b/libuavcan_drivers/posix/include/uavcan_posix/firmware_path.hpp index 31bb0baab4..a59ce1ec0e 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/firmware_path.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/firmware_path.hpp @@ -17,18 +17,11 @@ namespace uavcan_posix { /** * Firmware Path Management. + * FIXME Seems like the only purpose of this class is to initialize some directory. It probably should be replaced to + * a member function inside the firmware version checker class. */ class FirmwarePath { - /* The folder where the files will be copied and Read from */ - - static const char* get_cache_dir_() - { - return "c"; - } - -public: - enum { MaxBasePathLength = 128 }; /** @@ -39,7 +32,7 @@ public: /** * Maximum length of full path including / the file name */ - enum { MaxPathLength = uavcan::protocol::file::Path::FieldTypes::path::MaxSize + MaxBasePathLength }; + enum { MaxPathLength = uavcan::protocol::file::Path::FieldTypes::path::MaxSize + MaxBasePathLength }; BasePathString base_path_; BasePathString cache_path_; @@ -49,34 +42,18 @@ public: */ typedef uavcan::MakeString::Type PathString; - static char getPathSeparator() { return static_cast(uavcan::protocol::file::Path::SEPARATOR); } + /** + * The folder where the files will be copied and read from + */ + static const char* getCacheDir() { return "c"; } - - BasePathString& getFirmwareBasePath() + static char getPathSeparator() { - return base_path_; + return static_cast(uavcan::protocol::file::Path::SEPARATOR); } - void setFirmwareBasePath(const char * path) - { - base_path_ = path; - } - - BasePathString& getFirmwareCachePath() - { - return cache_path_; - } - - void setFirmwareCachePath(const char *path) - { - cache_path_ = path; - - } - - static void addSlash(BasePathString& path) { - if (path.back() != getPathSeparator()) { path.push_back(getPathSeparator()); @@ -85,48 +62,40 @@ public: static void removeSlash(BasePathString& path) { - if (path.back() == getPathSeparator()) { path.pop_back(); } - } /** * Creates the Directories were the files will be stored - */ - - /* This is directory structure is in support of a workaround + * + * 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 cashed here. + * +-c <----------- Files are cached here. */ - - int CreateFwPaths(const char *base_path) + int createFwPaths(const char* base_path) { using namespace std; int rv = -uavcan::ErrInvalidParam; if (base_path) { - - int len = strlen(base_path); + const int len = strlen(base_path); if (len > 0 && len < base_path_.MaxSize) { - setFirmwareBasePath(base_path); removeSlash(getFirmwareBasePath()); - const char *path = getFirmwareBasePath().c_str(); + const char* path = getFirmwareBasePath().c_str(); setFirmwareCachePath(path); addSlash(cache_path_); - cache_path_ += get_cache_dir_(); + cache_path_ += getCacheDir(); rv = 0; struct stat sb; @@ -137,7 +106,7 @@ public: path = getFirmwareCachePath().c_str(); - if (rv == 0 && (stat(path, &sb) != 0 || !S_ISDIR(sb.st_mode))) + if (rv == 0 && (stat(path, &sb) != 0 || !S_ISDIR(sb.st_mode))) { rv = mkdir(path, S_IRWXU | S_IRWXG | S_IRWXO); } @@ -145,8 +114,7 @@ public: addSlash(getFirmwareBasePath()); addSlash(getFirmwareCachePath()); - - if (rv >= 0 ) + if (rv >= 0) { if ((getFirmwareCachePath().size() + uavcan::protocol::file::Path::FieldTypes::path::MaxSize) > MaxPathLength) @@ -159,11 +127,26 @@ public: return rv; } - int init(const char *path) + void setFirmwareBasePath(const char* path) { - return CreateFwPaths(path); + base_path_ = path; } + void setFirmwareCachePath(const char* path) + { + cache_path_ = path; + } + +public: + const BasePathString& getFirmwareBasePath() const { return base_path_; } + + const BasePathString& getFirmwareCachePath() const { return cache_path_; } + + /// TODO Rename. Init is a bad name, as it not initializes the object, but modifies the FS. + int init(const char* path) + { + return createFwPaths(path); + } }; } From 3a07bde393c0815fabaa62a468415d2b11064ea7 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 26 May 2015 21:38:14 +0300 Subject: [PATCH 125/143] Fixes and notes in FirmwareCommon --- .../include/uavcan_posix/firmware_common.hpp | 63 +++++++++---------- 1 file changed, 30 insertions(+), 33 deletions(-) diff --git a/libuavcan_drivers/posix/include/uavcan_posix/firmware_common.hpp b/libuavcan_drivers/posix/include/uavcan_posix/firmware_common.hpp index 33ebba88b8..bee2ff4190 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/firmware_common.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/firmware_common.hpp @@ -10,10 +10,9 @@ #define UAVCAN_POSIX_FIRMWARE_COMMON_HPP_INCLUDED #include -#include #include #include -#include +#include #include #include @@ -23,36 +22,48 @@ namespace uavcan_posix { /** * Firmware file validation logic. + * TODO Rename - FirmwareCommon is a bad name as it doesn't reflect the purpose of this class. + * TODO Returning value via member variable is not a proper way of doing it. Probably the whole class should + * be replaced with a static function. */ class FirmwareCommon { -public: - - typedef struct app_descriptor_t + static uavcan::uint64_t getAppDescriptorSignature() { - uint8_t signature[sizeof(uint64_t)]; + uavcan::uint64_t ull = 0; + std::memcpy(&ull, "APDesc00", 8); + return ull; + } + +public: + 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]; - } app_descriptor_t; + }; - app_descriptor_t descriptor; + AppDescriptor descriptor; - int getFileInfo(const char *path) + int getFileInfo(const char* path) { - enum { MaxChunk = (512 / sizeof(uint64_t)) }; + using namespace std; + + const unsigned MaxChunk = 512 / sizeof(uint64_t); + int rv = -ENOENT; uint64_t chunk[MaxChunk]; int fd = open(path, O_RDONLY); if (fd >= 0) { - app_descriptor_t *pdescriptor = 0; + AppDescriptor* pdescriptor = NULL; - while(!pdescriptor) + while (pdescriptor == NULL) { int len = read(fd, chunk, sizeof(chunk)); @@ -67,42 +78,28 @@ public: goto out_close; } - uint64_t *p = &chunk[0]; + uint64_t* p = &chunk[0]; do { - if (*p == sig.ull) + if (*p == getAppDescriptorSignature()) { - pdescriptor = (app_descriptor_t *)p; + pdescriptor = (AppDescriptor*) p; descriptor = *pdescriptor; rv = 0; break; } } - while(p++ <= &chunk[MaxChunk - (sizeof(app_descriptor_t) / sizeof(chunk[0]))]); + while (p++ <= &chunk[MaxChunk - (sizeof(AppDescriptor) / sizeof(chunk[0]))]); } + out_close: - close(fd); + (void)close(fd); } return rv; } - - -private: - -#define APP_DESCRIPTOR_SIGNATURE_ID 'A', 'P', 'D', 'e', 's', 'c' -#define APP_DESCRIPTOR_SIGNATURE_REV '0', '0' -#define APP_DESCRIPTOR_SIGNATURE APP_DESCRIPTOR_SIGNATURE_ID, APP_DESCRIPTOR_SIGNATURE_REV - - union - { - uint64_t ull; - char text[sizeof(uint64_t)]; - } sig = { - .text = {APP_DESCRIPTOR_SIGNATURE} - }; - }; + } #endif // Include guard From fcca97d71c7036f01a34f2cfbf53bf8ffacd7f8c Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 26 May 2015 21:53:26 +0300 Subject: [PATCH 126/143] FirmwareCommon signature fix --- .../posix/include/uavcan_posix/firmware_common.hpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/libuavcan_drivers/posix/include/uavcan_posix/firmware_common.hpp b/libuavcan_drivers/posix/include/uavcan_posix/firmware_common.hpp index bee2ff4190..31a139349f 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/firmware_common.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/firmware_common.hpp @@ -55,6 +55,8 @@ public: const unsigned MaxChunk = 512 / sizeof(uint64_t); + const uint64_t signature = getAppDescriptorSignature(); + int rv = -ENOENT; uint64_t chunk[MaxChunk]; int fd = open(path, O_RDONLY); @@ -82,7 +84,7 @@ public: do { - if (*p == getAppDescriptorSignature()) + if (*p == signature) { pdescriptor = (AppDescriptor*) p; descriptor = *pdescriptor; From 28802a38ddcb4e2213b4bac978ba81e956e9fb78 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Tue, 26 May 2015 14:00:49 -1000 Subject: [PATCH 127/143] removed firmware common, made GetInfo part of the firmware checke class fixed firmware path so that code compiles --- .../basic_file_server_backend.hpp | 90 ++++++++++++--- .../include/uavcan_posix/firmware_common.hpp | 107 ------------------ .../include/uavcan_posix/firmware_path.hpp | 67 +++++------ .../uavcan_posix/firmware_version_checker.hpp | 81 ++++++++++++- 4 files changed, 184 insertions(+), 161 deletions(-) delete mode 100644 libuavcan_drivers/posix/include/uavcan_posix/firmware_common.hpp 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 index 7a02ee11e1..c71a177bb9 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp @@ -23,8 +23,7 @@ #include #include #include - -#include "firmware_common.hpp" +#include namespace uavcan_posix { @@ -33,29 +32,74 @@ namespace uavcan_posix */ class BasicFileSeverBackend : public uavcan::IFileServerBackend { + enum { FilePermissions = 438 }; ///< 0o666 -protected: +public: /** + * * 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; + uavcan::DataTypeSignatureCRC crc; if (path.size() > 0) { - FirmwareCommon fw; - fw.getFileInfo(path.c_str()); - out_crc64 = fw.descriptor.image_crc; - out_size = fw.descriptor.image_size; - // 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 - out_type.flags = uavcan::protocol::file::EntryType::FLAG_FILE | - uavcan::protocol::file::EntryType::FLAG_READABLE; - rv = 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; } @@ -67,12 +111,16 @@ protected: * 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) @@ -81,30 +129,38 @@ protected: } 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); + //todo uses a read at offset to fill on EAGAIN + ssize_t len = ::read(fd, out_buffer, inout_size); - if (len < 0) + if (len < 0) { rv = errno; } else { - inout_size = len; + + inout_size = len; rv = 0; } } - (void)close(fd); + close(fd); } } return rv; } + + BasicFileSeverBackend() { } + + + + }; } diff --git a/libuavcan_drivers/posix/include/uavcan_posix/firmware_common.hpp b/libuavcan_drivers/posix/include/uavcan_posix/firmware_common.hpp deleted file mode 100644 index 31a139349f..0000000000 --- a/libuavcan_drivers/posix/include/uavcan_posix/firmware_common.hpp +++ /dev/null @@ -1,107 +0,0 @@ -/**************************************************************************** -* -* Copyright (c) 2015 PX4 Development Team. All rights reserved. -* Author: Pavel Kirienko -* David Sidrane -* -****************************************************************************/ - -#ifndef UAVCAN_POSIX_FIRMWARE_COMMON_HPP_INCLUDED -#define UAVCAN_POSIX_FIRMWARE_COMMON_HPP_INCLUDED - -#include -#include -#include -#include -#include - -#include -#include "firmware_path.hpp" - -namespace uavcan_posix -{ -/** - * Firmware file validation logic. - * TODO Rename - FirmwareCommon is a bad name as it doesn't reflect the purpose of this class. - * TODO Returning value via member variable is not a proper way of doing it. Probably the whole class should - * be replaced with a static function. - */ -class FirmwareCommon -{ - static uavcan::uint64_t getAppDescriptorSignature() - { - uavcan::uint64_t ull = 0; - std::memcpy(&ull, "APDesc00", 8); - return ull; - } - -public: - 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]; - }; - - AppDescriptor descriptor; - - int getFileInfo(const char* path) - { - using namespace std; - - const unsigned MaxChunk = 512 / sizeof(uint64_t); - - const uint64_t signature = getAppDescriptorSignature(); - - 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; - } -}; - -} - -#endif // Include guard diff --git a/libuavcan_drivers/posix/include/uavcan_posix/firmware_path.hpp b/libuavcan_drivers/posix/include/uavcan_posix/firmware_path.hpp index a59ce1ec0e..4777a11a36 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/firmware_path.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/firmware_path.hpp @@ -17,11 +17,14 @@ namespace uavcan_posix { /** * Firmware Path Management. - * FIXME Seems like the only purpose of this class is to initialize some directory. It probably should be replaced to - * a member function inside the firmware version checker class. + * This class manages the sole copies of the cache path and the base path of the fw + * This prevent having to have large stack allocations to manipulate the 2 paths. + * It ensures that the paths have the proper slash as endings for concatenation. + * It also provides the creation of the folders and encapsulates the paths to */ class FirmwarePath { +public: enum { MaxBasePathLength = 128 }; /** @@ -34,24 +37,28 @@ class FirmwarePath */ enum { MaxPathLength = uavcan::protocol::file::Path::FieldTypes::path::MaxSize + MaxBasePathLength }; - BasePathString base_path_; - BasePathString cache_path_; + static char getPathSeparator() + { + return static_cast(uavcan::protocol::file::Path::SEPARATOR); + } /** * This type is used internally for the full path to file */ typedef uavcan::MakeString::Type PathString; + + +private: + + BasePathString base_path_; + BasePathString cache_path_; + /** * The folder where the files will be copied and read from */ static const char* getCacheDir() { return "c"; } - static char getPathSeparator() - { - return static_cast(uavcan::protocol::file::Path::SEPARATOR); - } - static void addSlash(BasePathString& path) { if (path.back() != getPathSeparator()) @@ -68,6 +75,21 @@ class FirmwarePath } } + void setFirmwareBasePath(const char* path) + { + base_path_ = path; + } + + void setFirmwareCachePath(const char* path) + { + cache_path_ = path; + } + +public: + const BasePathString& getFirmwareBasePath() const { return base_path_; } + + const BasePathString& getFirmwareCachePath() const { return cache_path_; } + /** * Creates the Directories were the files will be stored * @@ -90,7 +112,7 @@ class FirmwarePath if (len > 0 && len < base_path_.MaxSize) { setFirmwareBasePath(base_path); - removeSlash(getFirmwareBasePath()); + removeSlash(base_path_); const char* path = getFirmwareBasePath().c_str(); setFirmwareCachePath(path); @@ -111,8 +133,8 @@ class FirmwarePath rv = mkdir(path, S_IRWXU | S_IRWXG | S_IRWXO); } - addSlash(getFirmwareBasePath()); - addSlash(getFirmwareCachePath()); + addSlash(base_path_); + addSlash(cache_path_); if (rv >= 0) { @@ -126,27 +148,6 @@ class FirmwarePath } return rv; } - - void setFirmwareBasePath(const char* path) - { - base_path_ = path; - } - - void setFirmwareCachePath(const char* path) - { - cache_path_ = path; - } - -public: - const BasePathString& getFirmwareBasePath() const { return base_path_; } - - const BasePathString& getFirmwareCachePath() const { return cache_path_; } - - /// TODO Rename. Init is a bad name, as it not initializes the object, but modifies the FS. - int init(const char* path) - { - return createFwPaths(path); - } }; } diff --git a/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp b/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp index c53b6e2c08..beeb9ce8c1 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp @@ -17,8 +17,8 @@ #include #include +#include "firmware_path.hpp" -#include "firmware_common.hpp" namespace uavcan_posix { @@ -32,6 +32,18 @@ class FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker FirmwarePath* paths_; + 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]; + }; + + int copyIfNot(const char* srcpath, const char* destpath) { // Does the file exist @@ -93,6 +105,61 @@ class FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker return rv; } + __attribute__((optimize("O0"))) + 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; + } + protected: /** * This method will be invoked when the class obtains a response to GetNodeInfo request. @@ -108,6 +175,7 @@ protected: * @return True - the class will begin sending update requests. * False - the node will be ignored, no request will be sent. */ + __attribute__((optimize("O0"))) virtual bool shouldRequestFirmwareUpdate(uavcan::NodeID node_id, const uavcan::protocol::GetNodeInfo::Response& node_info, FirmwareFilePath& out_firmware_file_path) @@ -164,13 +232,18 @@ protected: int cr = copyIfNot(full_src_path.c_str(), full_dst_path.c_str()); // We have a file, is it a valid image - FirmwareCommon fw; + AppDescriptor descriptor; - if (cr == 0 && fw.getFileInfo(full_dst_path.c_str()) == 0) + 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) || - fw.descriptor.image_crc != node_info.software_version.image_crc) + descriptor.image_crc != node_info.software_version.image_crc) { rv = true; out_firmware_file_path = pfile->d_name; From 75b8831f21f5c789ef49f4d98bb381da9df57614 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Tue, 26 May 2015 14:21:16 -1000 Subject: [PATCH 128/143] Removed perfunction optimization setting --- .../posix/include/uavcan_posix/firmware_version_checker.hpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp b/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp index beeb9ce8c1..cfeaa03ec5 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp @@ -105,7 +105,6 @@ class FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker return rv; } - __attribute__((optimize("O0"))) static int getFileInfo(const char* path, AppDescriptor & descriptor) { using namespace std; @@ -175,7 +174,7 @@ protected: * @return True - the class will begin sending update requests. * False - the node will be ignored, no request will be sent. */ - __attribute__((optimize("O0"))) + virtual bool shouldRequestFirmwareUpdate(uavcan::NodeID node_id, const uavcan::protocol::GetNodeInfo::Response& node_info, FirmwareFilePath& out_firmware_file_path) From 89c8ed0cf152c8b77aeb95690276b2fbe0bdd4c5 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 27 May 2015 11:06:23 +0300 Subject: [PATCH 129/143] Docs for dsdl/uavcan/protocol/dynamic_node_id/server/220.AppendEntries.uavcan --- .../server/220.AppendEntries.uavcan | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) 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 329bfff8e6..30a966eb78 100644 --- a/dsdl/uavcan/protocol/dynamic_node_id/server/220.AppendEntries.uavcan +++ b/dsdl/uavcan/protocol/dynamic_node_id/server/220.AppendEntries.uavcan @@ -4,11 +4,14 @@ # # -# Given min election timeout and cluster size, the maximum request interval can be derived as follows: -# max request interval = (min election timeout) / 2 requests / (cluster size - 1) -# Obviously, request interval can be lower than that if needed, but not higher. +# 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]. +# 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 @@ -19,8 +22,8 @@ uint8 prev_log_index uint8 leader_commit # -# Worst-case replication time can be computed as: -# worst replication time = (127 log entries) * (2 trips of next_index) * (cluster size - 1) * (request interval) +# 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 From 74298b1860ecee06773551bf5d300734772ea5fc Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 27 May 2015 11:53:12 +0300 Subject: [PATCH 130/143] spinOnce(), fixes #31 --- .../include/uavcan/node/abstract_node.hpp | 12 +++++++ libuavcan/include/uavcan/node/scheduler.hpp | 19 +++++++++++ .../include/uavcan/transport/dispatcher.hpp | 8 +++++ libuavcan/src/node/uc_scheduler.cpp | 26 ++++++++++++-- libuavcan/src/transport/uc_dispatcher.cpp | 34 +++++++++++++++++++ libuavcan/test/transport/dispatcher.cpp | 4 +-- 6 files changed, 99 insertions(+), 4 deletions(-) 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/scheduler.hpp b/libuavcan/include/uavcan/node/scheduler.hpp index 0d1d82553c..01e58eb877 100644 --- a/libuavcan/include/uavcan/node/scheduler.hpp +++ b/libuavcan/include/uavcan/node/scheduler.hpp @@ -77,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); @@ -91,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/transport/dispatcher.hpp b/libuavcan/include/uavcan/transport/dispatcher.hpp index a671f4ac85..0405d904fd 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 untill all available frames are processed. + */ + int spinOnce(); + /** * Refer to CanIOManager::send() for the parameter description */ 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/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/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); From 75e2bed3c2ba5b63d2fd95cd93d6c459fabc60f5 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 27 May 2015 11:56:18 +0300 Subject: [PATCH 131/143] Typo --- libuavcan/include/uavcan/transport/dispatcher.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libuavcan/include/uavcan/transport/dispatcher.hpp b/libuavcan/include/uavcan/transport/dispatcher.hpp index 0405d904fd..d179b32193 100644 --- a/libuavcan/include/uavcan/transport/dispatcher.hpp +++ b/libuavcan/include/uavcan/transport/dispatcher.hpp @@ -125,7 +125,7 @@ public: int spin(MonotonicTime deadline); /** - * This version does not return untill all available frames are processed. + * This version does not return until all available frames are processed. */ int spinOnce(); From 82c24967e77b5cd65bec360bd50d0ef3c9d1bc7f Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 27 May 2015 12:55:49 +0300 Subject: [PATCH 132/143] Minor fixes in Raft server --- .../protocol/dynamic_node_id_server/distributed/log.hpp | 8 -------- .../distributed/persistent_state.hpp | 8 ++++++++ .../dynamic_node_id_server/distributed/server.hpp | 1 - 3 files changed, 8 insertions(+), 9 deletions(-) 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/server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp index 5fd35bb50a..98ed66eec5 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 From 638de081150ef4f333accae5577057ae3d5cd682 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 27 May 2015 14:52:41 +0300 Subject: [PATCH 133/143] CentralizedServer - storage implementation and test --- .../centralized/storage.hpp | 241 ++++++++++++++++++ .../centralized/storage.cpp | 115 +++++++++ 2 files changed, 356 insertions(+) create mode 100644 libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/storage.hpp create mode 100644 libuavcan/test/protocol/dynamic_node_id_server/centralized/storage.cpp 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..280e077f38 --- /dev/null +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/storage.hpp @@ -0,0 +1,241 @@ +/* + * 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; + + 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; + } + + 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/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..b2d4e83819 --- /dev/null +++ b/libuavcan/test/protocol/dynamic_node_id_server/centralized/storage.cpp @@ -0,0 +1,115 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include "../../helpers.hpp" +#include "../memory_storage_backend.hpp" + + +TEST(dynamic_node_id_server_centralized_Storage, Basic) +{ + 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); + } +} + + From a78c01593401287c64bfdf26c6f9ff163cf05228 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 27 May 2015 15:16:17 +0300 Subject: [PATCH 134/143] Distributed server tracing fix --- .../protocol/dynamic_node_id_server/distributed/server.hpp | 3 +++ 1 file changed, 3 insertions(+) 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 98ed66eec5..314fc40334 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 @@ -64,6 +64,7 @@ class UAVCAN_EXPORT Server : IAllocationRequestHandler * States */ INode& node_; + IEventTracer& tracer_; RaftCore raft_core_; AllocationRequestManager allocation_request_manager_; NodeDiscoverer node_discoverer_; @@ -229,6 +230,7 @@ class UAVCAN_EXPORT Server : IAllocationRequestHandler const int res = allocation_request_manager_.broadcastAllocationResponse(entry.unique_id, entry.node_id); if (res < 0) { + tracer_.onEvent(TraceError, res); node_.registerInternalFailure("Dynamic allocation final broadcast"); } } @@ -238,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) From abe2401e38a9a33ec44c0c02652c0ed11bdef9c8 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 27 May 2015 15:24:55 +0300 Subject: [PATCH 135/143] Distributed server logging fix --- .../protocol/dynamic_node_id_server/distributed/server.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 314fc40334..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 @@ -231,7 +231,7 @@ class UAVCAN_EXPORT Server : IAllocationRequestHandler if (res < 0) { tracer_.onEvent(TraceError, res); - node_.registerInternalFailure("Dynamic allocation final broadcast"); + node_.registerInternalFailure("Dynamic allocation response"); } } From bdc0a327b702a31e01663511a3df9e16d14a686d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 27 May 2015 16:10:14 +0300 Subject: [PATCH 136/143] centralized::Storage - new test --- .../centralized/storage.hpp | 14 ++++- .../centralized/storage.cpp | 61 ++++++++++++++++++- 2 files changed, 73 insertions(+), 2 deletions(-) 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 index 280e077f38..99e04eb183 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/storage.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/storage.hpp @@ -25,13 +25,20 @@ class Storage public: typedef uint8_t Size; - enum { Capacity = NodeID::Max }; + enum { Capacity = NodeID::Max - 1 }; 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 && @@ -171,6 +178,11 @@ public: return -ErrLogic; } + if (!node_id.isUnicast()) + { + return -ErrInvalidParam; + } + Entry entry; entry.node_id = node_id; entry.unique_id = unique_id; diff --git a/libuavcan/test/protocol/dynamic_node_id_server/centralized/storage.cpp b/libuavcan/test/protocol/dynamic_node_id_server/centralized/storage.cpp index b2d4e83819..615dc73f11 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/centralized/storage.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_server/centralized/storage.cpp @@ -8,7 +8,7 @@ #include "../memory_storage_backend.hpp" -TEST(dynamic_node_id_server_centralized_Storage, Basic) +TEST(dynamic_node_id_server_centralized_Storage, Initialization) { using namespace uavcan::dynamic_node_id_server::centralized; @@ -113,3 +113,62 @@ TEST(dynamic_node_id_server_centralized_Storage, Basic) } +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(entry.node_id.get() + 1U); + entry.unique_id[0] = uint8_t(entry.unique_id[0] + 1U); + } + + ASSERT_GT(0, stor.add(entry.node_id, entry.unique_id)); // Failing because full + + storage.print(); +} From 5930bf3ed415255e4658c94a8e5da39597329909 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 27 May 2015 16:10:38 +0300 Subject: [PATCH 137/143] CentralizedServer --- .../dynamic_node_id_server/centralized.hpp | 20 ++ .../centralized/server.hpp | 208 ++++++++++++++++++ .../centralized/server.cpp | 77 +++++++ 3 files changed, 305 insertions(+) create mode 100644 libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized.hpp create mode 100644 libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/server.hpp create mode 100644 libuavcan/test/protocol/dynamic_node_id_server/centralized/server.cpp 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/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..c1da6f09d1 --- /dev/null +++ b/libuavcan/test/protocol/dynamic_node_id_server/centralized/server.cpp @@ -0,0 +1,77 @@ +/* + * 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; +} From 5205d13f37af06005b2023568eca7106d7276205 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 27 May 2015 16:28:31 +0300 Subject: [PATCH 138/143] centralized::Storage fix --- .../protocol/dynamic_node_id_server/centralized/storage.hpp | 2 +- .../protocol/dynamic_node_id_server/centralized/storage.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) 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 index 99e04eb183..c03957d14f 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/storage.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/storage.hpp @@ -25,7 +25,7 @@ class Storage public: typedef uint8_t Size; - enum { Capacity = NodeID::Max - 1 }; + enum { Capacity = NodeID::Max }; struct Entry { diff --git a/libuavcan/test/protocol/dynamic_node_id_server/centralized/storage.cpp b/libuavcan/test/protocol/dynamic_node_id_server/centralized/storage.cpp index 615dc73f11..51ada6d104 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/centralized/storage.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_server/centralized/storage.cpp @@ -164,11 +164,11 @@ TEST(dynamic_node_id_server_centralized_Storage, Basic) { ASSERT_LE(0, stor.add(entry.node_id, entry.unique_id)); - entry.node_id = uint8_t(entry.node_id.get() + 1U); + 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(entry.node_id, entry.unique_id)); // Failing because full + ASSERT_GT(0, stor.add(123, entry.unique_id)); // Failing because full storage.print(); } From be50b94cba0f73962fb6988ec16736a08f7a7634 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 27 May 2015 03:53:54 -1000 Subject: [PATCH 139/143] Revert of format changes --- .../basic_file_server_backend.hpp | 26 +++++-------------- 1 file changed, 6 insertions(+), 20 deletions(-) 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 index c71a177bb9..2ad056e7be 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp @@ -32,23 +32,18 @@ namespace uavcan_posix */ class BasicFileSeverBackend : public uavcan::IFileServerBackend { - enum { FilePermissions = 438 }; ///< 0o666 -public: +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; uavcan::DataTypeSignatureCRC crc; - if (path.size() > 0) { using namespace std; @@ -114,13 +109,10 @@ public: 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) @@ -129,38 +121,32 @@ public: } else { - if (::lseek(fd, offset, SEEK_SET) < 0) { rv = errno; } else { - //todo uses a read at offset to fill on EAGAIN - ssize_t len = ::read(fd, out_buffer, inout_size); + // TODO use a read at offset to fill on EAGAIN + ssize_t len = ::read(fd, out_buffer, inout_size); - if (len < 0) + if (len < 0) { rv = errno; } else { - inout_size = len; + inout_size = len; rv = 0; } } - close(fd); + (void)close(fd); } } return rv; } - BasicFileSeverBackend() { } - - - - }; } From 436cfcefa96f336eee89030224ff469acf621352 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 27 May 2015 04:32:50 -1000 Subject: [PATCH 140/143] Use FileCRC typedef --- .../posix/include/uavcan_posix/basic_file_server_backend.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 index 2ad056e7be..32226c3b38 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp @@ -43,7 +43,7 @@ protected: 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; - uavcan::DataTypeSignatureCRC crc; + FileCRC crc; if (path.size() > 0) { using namespace std; From f2fe415e55b31c0ca0e94793bbea3c4b37e40c92 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 27 May 2015 06:07:47 -1000 Subject: [PATCH 141/143] Code consolidation --- .../include/uavcan_posix/firmware_path.hpp | 155 ---------------- .../uavcan_posix/firmware_version_checker.hpp | 175 ++++++++++++++---- 2 files changed, 144 insertions(+), 186 deletions(-) delete mode 100644 libuavcan_drivers/posix/include/uavcan_posix/firmware_path.hpp diff --git a/libuavcan_drivers/posix/include/uavcan_posix/firmware_path.hpp b/libuavcan_drivers/posix/include/uavcan_posix/firmware_path.hpp deleted file mode 100644 index 4777a11a36..0000000000 --- a/libuavcan_drivers/posix/include/uavcan_posix/firmware_path.hpp +++ /dev/null @@ -1,155 +0,0 @@ -/**************************************************************************** -* -* Copyright (c) 2015 PX4 Development Team. All rights reserved. -* Author: Pavel Kirienko -* David Sidrane -* -****************************************************************************/ - -#ifndef UAVCAN_POSIX_FIRMWARE_PATH_HPP_INCLUDED -#define UAVCAN_POSIX_FIRMWARE_PATH_HPP_INCLUDED - -#include - -#include - -namespace uavcan_posix -{ -/** - * Firmware Path Management. - * This class manages the sole copies of the cache path and the base path of the fw - * This prevent having to have large stack allocations to manipulate the 2 paths. - * It ensures that the paths have the proper slash as endings for concatenation. - * It also provides the creation of the folders and encapsulates the paths to - */ -class FirmwarePath -{ -public: - 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 }; - - static char getPathSeparator() - { - return static_cast(uavcan::protocol::file::Path::SEPARATOR); - } - - /** - * This type is used internally for the full path to file - */ - typedef uavcan::MakeString::Type PathString; - - - -private: - - 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; - } - -public: - const BasePathString& getFirmwareBasePath() const { return base_path_; } - - const BasePathString& getFirmwareCachePath() const { return cache_path_; } - - /** - * 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; - } -}; - -} - -#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 index cfeaa03ec5..d38bf1a1f2 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp @@ -17,8 +17,10 @@ #include #include -#include "firmware_path.hpp" +#if !defined(DIRENT_ISFILE) && defined(DT_REG) +# define DIRENT_ISFILE(dtype) ((dtype) == DT_REG) +#endif namespace uavcan_posix { @@ -30,19 +32,57 @@ class FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker { enum { FilePermissions = 438 }; ///< 0o666 - FirmwarePath* paths_; + enum { MaxBasePathLength = 128 }; - struct AppDescriptor + /** + * 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) { - 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]; - }; + 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) { @@ -105,6 +145,18 @@ class FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker 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; @@ -159,6 +211,78 @@ class FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker 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. @@ -179,6 +303,8 @@ protected: 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. @@ -196,9 +322,9 @@ protected: */ bool rv = false; - char fname_root[FirmwarePath::MaxBasePathLength + 1]; + char fname_root[MaxBasePathLength + 1]; int n = snprintf(fname_root, sizeof(fname_root), "%s%s/%d.%d", - paths_->getFirmwareBasePath().c_str(), + getFirmwareBasePath().c_str(), node_info.name.c_str(), node_info.hardware_version.major, node_info.hardware_version.minor); @@ -207,7 +333,7 @@ protected: { DIR* const fwdir = opendir(fname_root); - fname_root[n++] = FirmwarePath::getPathSeparator(); + fname_root[n++] = getPathSeparator(); fname_root[n++] = '\0'; if (fwdir != NULL) @@ -221,10 +347,10 @@ protected: // Open any bin file in there. if (strstr(pfile->d_name, ".bin") != NULL) { - FirmwarePath::PathString full_src_path = fname_root; + PathString full_src_path = fname_root; full_src_path += pfile->d_name; - FirmwarePath::PathString full_dst_path = paths_->getFirmwareCachePath().c_str(); + PathString full_dst_path = getFirmwareCachePath().c_str(); full_dst_path += pfile->d_name; // ease the burden on the user @@ -233,7 +359,7 @@ protected: // We have a file, is it a valid image AppDescriptor descriptor; - std::memset(&descriptor,0, sizeof(descriptor)); + std::memset(&descriptor, 0, sizeof(descriptor)); if (cr == 0 && getFileInfo(full_dst_path.c_str(), descriptor) == 0) { @@ -286,23 +412,10 @@ protected: } public: - FirmwareVersionChecker() - : paths_(NULL) - { } - - /** - * Initializes the Firmware File back-end storage by passing a paths object - * that maintains the directory where files will be stored. - */ - int init(FirmwarePath& paths) - { - paths_ = &paths; - return 0; - } const char* getFirmwarePath() const { - return paths_->getFirmwareCachePath().c_str(); + return getFirmwareCachePath().c_str(); } }; From 34bcfb21df0097839b42cf265d7861956f955deb Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 28 May 2015 11:57:00 +0300 Subject: [PATCH 142/143] Improved docs for IFileServerBackend --- libuavcan/include/uavcan/protocol/file_server.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/libuavcan/include/uavcan/protocol/file_server.hpp b/libuavcan/include/uavcan/protocol/file_server.hpp index ba458cefad..1e5fed04a2 100644 --- a/libuavcan/include/uavcan/protocol/file_server.hpp +++ b/libuavcan/include/uavcan/protocol/file_server.hpp @@ -47,6 +47,7 @@ public: /** * 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. */ @@ -90,6 +91,7 @@ public: /** * 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. */ From 954ab2491a93456380ee955e691cf4cf3aad5079 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 28 May 2015 12:50:12 +0300 Subject: [PATCH 143/143] Fruitless attempt to optimize memory use --- .../dynamic_node_id_server/node_discoverer.hpp | 2 +- .../dynamic_node_id_server/centralized/server.cpp | 1 + .../dynamic_node_id_server/node_discoverer.cpp | 13 +++++++++++++ 3 files changed, 15 insertions(+), 1 deletion(-) 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 827551250c..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 @@ -106,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 diff --git a/libuavcan/test/protocol/dynamic_node_id_server/centralized/server.cpp b/libuavcan/test/protocol/dynamic_node_id_server/centralized/server.cpp index c1da6f09d1..5e83b19d05 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/centralized/server.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_server/centralized/server.cpp @@ -74,4 +74,5 @@ 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/node_discoverer.cpp b/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp index 304070bf3f..7313bf0b0a 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp @@ -275,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; +}