From 77184fc062703e2b8490ec765d917083ba30ea2b Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 8 Mar 2014 23:01:05 +0400 Subject: [PATCH] Writing top-level logic - publisher --- libuavcan/include/uavcan/marshal_buffer.hpp | 68 +++++++++++ libuavcan/include/uavcan/publisher.hpp | 118 ++++++++++++++++++++ libuavcan/test/publisher.cpp | 109 ++++++++++++++++++ 3 files changed, 295 insertions(+) create mode 100644 libuavcan/include/uavcan/marshal_buffer.hpp create mode 100644 libuavcan/include/uavcan/publisher.hpp create mode 100644 libuavcan/test/publisher.cpp diff --git a/libuavcan/include/uavcan/marshal_buffer.hpp b/libuavcan/include/uavcan/marshal_buffer.hpp new file mode 100644 index 0000000000..0e82eb77d2 --- /dev/null +++ b/libuavcan/include/uavcan/marshal_buffer.hpp @@ -0,0 +1,68 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include +#include + +namespace uavcan +{ + +class IMarshalBuffer : public ITransferBuffer +{ +public: + virtual const uint8_t* getDataPtr() const = 0; + virtual unsigned int getDataLength() const = 0; +}; + + +class IMarshalBufferProvider +{ +public: + virtual ~IMarshalBufferProvider() { } + virtual IMarshalBuffer* getBuffer(unsigned int size) = 0; +}; + + +template +class MarshalBufferProvider : public IMarshalBufferProvider +{ + class Buffer : public IMarshalBuffer + { + StaticTransferBuffer buf_; + + int read(unsigned int offset, uint8_t* data, unsigned int len) const + { + return buf_.read(offset, data, len); + } + + int write(unsigned int offset, const uint8_t* data, unsigned int len) + { + return buf_.write(offset, data, len); + } + + const uint8_t* getDataPtr() const { return buf_.getRawPtr(); } + + unsigned int getDataLength() const { return buf_.getMaxWritePos(); } + + public: + void reset() { buf_.reset(); } + }; + + Buffer buffer_; + +public: + enum { MaxSize = MaxSize_ }; + + IMarshalBuffer* getBuffer(unsigned int size) + { + if (size > MaxSize) + return NULL; + buffer_.reset(); + return &buffer_; + } +}; + +} diff --git a/libuavcan/include/uavcan/publisher.hpp b/libuavcan/include/uavcan/publisher.hpp new file mode 100644 index 0000000000..ad8ed8fcd6 --- /dev/null +++ b/libuavcan/include/uavcan/publisher.hpp @@ -0,0 +1,118 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ + +template +class Publisher +{ +public: + typedef DataType_ DataType; + +private: + enum { MinTxTimeoutUsec = 200 }; + + const uint64_t max_transfer_interval_; // TODO: memory usage can be reduced + uint64_t tx_timeout_; + Scheduler& scheduler_; + IMarshalBufferProvider& buffer_provider_; + LazyConstructor sender_; + + bool checkInit() + { + if (sender_) + return true; + + GlobalDataTypeRegistry::instance().freeze(); + + const DataTypeDescriptor* const descr = + GlobalDataTypeRegistry::instance().find(DataTypeKindMessage, DataType::getDataTypeFullName()); + if (!descr) + { + UAVCAN_TRACE("Publisher", "Type [%s] is not registered", DataType::getDataTypeFullName()); + return false; + } + sender_.construct + (scheduler_.getDispatcher(), *descr, CanTxQueue::Volatile, max_transfer_interval_); + return true; + } + + uint64_t getTxDeadline() const { return scheduler_.getMonotonicTimestamp() + tx_timeout_; } + + IMarshalBuffer* getBuffer() + { + const int size = (DataType::MaxBitLen + 7) / 8; + return buffer_provider_.getBuffer(size); + } + + int genericSend(const DataType& message, TransferType transfer_type, NodeID dst_node_id, + uint64_t monotonic_blocking_deadline) + { + if (!checkInit()) + return -1; + + IMarshalBuffer* const buf = getBuffer(); + if (!buf) + return -1; + + BitStream bitstream(*buf); + ScalarCodec codec(bitstream); + const int encode_res = DataType::encode(message, codec); + if (encode_res <= 0) + { + assert(0); // Impossible, internal error + return -1; + } + + return sender_->send(buf->getDataPtr(), buf->getDataLength(), getTxDeadline(), + monotonic_blocking_deadline, transfer_type, dst_node_id); + } + +public: + Publisher(Scheduler& scheduler, IMarshalBufferProvider& buffer_provider, uint64_t tx_timeout_usec, + uint64_t max_transfer_interval = TransferSender::DefaultMaxTransferInterval) + : max_transfer_interval_(max_transfer_interval) + , tx_timeout_(tx_timeout_usec) + , scheduler_(scheduler) + , buffer_provider_(buffer_provider) + { + setTxTimeout(tx_timeout_usec); + StaticAssert::check(); + } + + int broadcast(const DataType& message, uint64_t monotonic_blocking_deadline = 0) + { + return genericSend(message, TransferTypeMessageBroadcast, NodeID::Broadcast, monotonic_blocking_deadline); + } + + int unicast(const DataType& message, NodeID dst_node_id, uint64_t monotonic_blocking_deadline = 0) + { + if (!dst_node_id.isUnicast()) + { + assert(0); + return -1; + } + return genericSend(message, TransferTypeMessageUnicast, dst_node_id, monotonic_blocking_deadline); + } + + uint64_t getTxTimeout() const { return tx_timeout_; } + void setTxTimeout(uint64_t usec) + { + tx_timeout_ = std::max(usec, uint64_t(MinTxTimeoutUsec)); + } +}; + +} diff --git a/libuavcan/test/publisher.cpp b/libuavcan/test/publisher.cpp new file mode 100644 index 0000000000..64da45dbf3 --- /dev/null +++ b/libuavcan/test/publisher.cpp @@ -0,0 +1,109 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include "common.hpp" +#include "transport/can/iface_mock.hpp" + + +TEST(Publisher, Basic) +{ + uavcan::PoolAllocator pool; + uavcan::PoolManager<1> poolmgr; + poolmgr.addPool(&pool); + + SystemClockMock clock_mock(100); + CanDriverMock can_driver(2, clock_mock); + + uavcan::OutgoingTransferRegistry<8> out_trans_reg(poolmgr); + + uavcan::Scheduler sch(can_driver, poolmgr, clock_mock, out_trans_reg, uavcan::NodeID(1)); + + uavcan::MarshalBufferProvider<> buffer_provider; + + uavcan::Publisher publisher(sch, buffer_provider, 10000); + + ASSERT_FALSE(uavcan::GlobalDataTypeRegistry::instance().isFrozen()); + + /* + * Message layout: + * uint8 seq + * uint8 sysid + * uint8 compid + * uint8 msgid + * uint8[<256] payload + */ + uavcan::mavlink::Message msg; + msg.seq = 0x42; + msg.sysid = 0x72; + msg.compid = 0x08; + msg.msgid = 0xa5; + msg.payload = "Msg"; + + static const uint8_t expected_transfer_payload[] = {0x42, 0x72, 0x08, 0xa5, 'M', 's', 'g'}; + + /* + * Broadcast + */ + { + ASSERT_LT(0, publisher.broadcast(msg)); + + // uint_fast16_t data_type_id, TransferType transfer_type, NodeID src_node_id, NodeID dst_node_id, + // uint_fast8_t frame_index, TransferID transfer_id, bool last_frame = false + uavcan::Frame expected_frame(uavcan::mavlink::Message::DefaultDataTypeID, uavcan::TransferTypeMessageBroadcast, + sch.getDispatcher().getSelfNodeID(), uavcan::NodeID::Broadcast, 0, 0, true); + expected_frame.setPayload(expected_transfer_payload, 7); + + uavcan::CanFrame expected_can_frame; + ASSERT_TRUE(expected_frame.compile(expected_can_frame)); + + ASSERT_TRUE(can_driver.ifaces[0].matchAndPopTx(expected_can_frame, 10000 + 100)); + ASSERT_TRUE(can_driver.ifaces[1].matchAndPopTx(expected_can_frame, 10000 + 100)); + ASSERT_TRUE(can_driver.ifaces[0].tx.empty()); + ASSERT_TRUE(can_driver.ifaces[1].tx.empty()); + + // Second shot - checking the transfer ID + ASSERT_LT(0, publisher.broadcast(msg)); + + expected_frame = uavcan::Frame(uavcan::mavlink::Message::DefaultDataTypeID, uavcan::TransferTypeMessageBroadcast, + sch.getDispatcher().getSelfNodeID(), uavcan::NodeID::Broadcast, 0, 1, true); + expected_frame.setPayload(expected_transfer_payload, 7); + ASSERT_TRUE(expected_frame.compile(expected_can_frame)); + + ASSERT_TRUE(can_driver.ifaces[0].matchAndPopTx(expected_can_frame, 10000 + 100)); + ASSERT_TRUE(can_driver.ifaces[1].matchAndPopTx(expected_can_frame, 10000 + 100)); + ASSERT_TRUE(can_driver.ifaces[0].tx.empty()); + ASSERT_TRUE(can_driver.ifaces[1].tx.empty()); + } + + clock_mock.advance(1000); + + /* + * Unicast + */ + { + ASSERT_LT(0, publisher.unicast(msg, 0x44)); + + // uint_fast16_t data_type_id, TransferType transfer_type, NodeID src_node_id, NodeID dst_node_id, + // uint_fast8_t frame_index, TransferID transfer_id, bool last_frame = false + uavcan::Frame expected_frame(uavcan::mavlink::Message::DefaultDataTypeID, uavcan::TransferTypeMessageUnicast, + sch.getDispatcher().getSelfNodeID(), uavcan::NodeID(0x44), 0, 0, true); + expected_frame.setPayload(expected_transfer_payload, 7); + + uavcan::CanFrame expected_can_frame; + ASSERT_TRUE(expected_frame.compile(expected_can_frame)); + + ASSERT_TRUE(can_driver.ifaces[0].matchAndPopTx(expected_can_frame, 10000 + 100 + 1000)); + ASSERT_TRUE(can_driver.ifaces[1].matchAndPopTx(expected_can_frame, 10000 + 100 + 1000)); + ASSERT_TRUE(can_driver.ifaces[0].tx.empty()); + ASSERT_TRUE(can_driver.ifaces[1].tx.empty()); + } + + /* + * Misc + */ + ASSERT_TRUE(uavcan::GlobalDataTypeRegistry::instance().isFrozen()); +}