mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-21 05:23:33 +08:00
Reorganized directory structure - directory 'internal' pulled up one level. No logical changes.
This commit is contained in:
Pavel Kirienko
@@ -0,0 +1,138 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <uavcan/node/scheduler.hpp>
|
||||
#include <uavcan/data_type.hpp>
|
||||
#include <uavcan/node/marshal_buffer.hpp>
|
||||
#include <uavcan/node/global_data_type_registry.hpp>
|
||||
#include <uavcan/util/lazy_constructor.hpp>
|
||||
#include <uavcan/debug.hpp>
|
||||
#include <uavcan/transport/transfer_sender.hpp>
|
||||
#include <uavcan/marshal/scalar_codec.hpp>
|
||||
#include <uavcan/marshal/types.hpp>
|
||||
|
||||
namespace uavcan
|
||||
{
|
||||
|
||||
/**
|
||||
* Generic publisher, suitable for messages and services.
|
||||
* DataSpec - data type specification class
|
||||
* DataStruct - instantiable class
|
||||
*/
|
||||
template <typename DataSpec, typename DataStruct>
|
||||
class GenericPublisher
|
||||
{
|
||||
enum { Qos = (DataTypeKind(DataSpec::DataTypeKind) == DataTypeKindMessage) ?
|
||||
CanTxQueue::Volatile : CanTxQueue::Persistent };
|
||||
|
||||
const MonotonicDuration max_transfer_interval_; // TODO: memory usage can be reduced
|
||||
MonotonicDuration tx_timeout_;
|
||||
Scheduler& scheduler_;
|
||||
IMarshalBufferProvider& buffer_provider_;
|
||||
LazyConstructor<TransferSender> sender_;
|
||||
|
||||
bool checkInit()
|
||||
{
|
||||
if (sender_)
|
||||
return true;
|
||||
|
||||
GlobalDataTypeRegistry::instance().freeze();
|
||||
|
||||
const DataTypeDescriptor* const descr =
|
||||
GlobalDataTypeRegistry::instance().find(DataTypeKind(DataSpec::DataTypeKind),
|
||||
DataSpec::getDataTypeFullName());
|
||||
if (!descr)
|
||||
{
|
||||
UAVCAN_TRACE("GenericPublisher", "Type [%s] is not registered", DataSpec::getDataTypeFullName());
|
||||
return false;
|
||||
}
|
||||
sender_.template construct<Dispatcher&, const DataTypeDescriptor&, CanTxQueue::Qos, MonotonicDuration>
|
||||
(scheduler_.getDispatcher(), *descr, CanTxQueue::Qos(Qos), max_transfer_interval_);
|
||||
return true;
|
||||
}
|
||||
|
||||
MonotonicTime getTxDeadline() const { return scheduler_.getMonotonicTimestamp() + tx_timeout_; }
|
||||
|
||||
IMarshalBuffer* getBuffer()
|
||||
{
|
||||
return buffer_provider_.getBuffer(BitLenToByteLen<DataStruct::MaxBitLen>::Result);
|
||||
}
|
||||
|
||||
int genericPublish(const DataStruct& message, TransferType transfer_type, NodeID dst_node_id,
|
||||
TransferID* tid, MonotonicTime blocking_deadline)
|
||||
{
|
||||
if (!checkInit())
|
||||
return -1;
|
||||
|
||||
IMarshalBuffer* const buf = getBuffer();
|
||||
if (!buf)
|
||||
return -1;
|
||||
|
||||
{
|
||||
BitStream bitstream(*buf);
|
||||
ScalarCodec codec(bitstream);
|
||||
const int encode_res = DataStruct::encode(message, codec);
|
||||
if (encode_res <= 0)
|
||||
{
|
||||
assert(0); // Impossible, internal error
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
if (tid)
|
||||
{
|
||||
return sender_->send(buf->getDataPtr(), buf->getDataLength(), getTxDeadline(),
|
||||
blocking_deadline, transfer_type, dst_node_id, *tid);
|
||||
}
|
||||
else
|
||||
{
|
||||
return sender_->send(buf->getDataPtr(), buf->getDataLength(), getTxDeadline(),
|
||||
blocking_deadline, transfer_type, dst_node_id);
|
||||
}
|
||||
}
|
||||
|
||||
public:
|
||||
GenericPublisher(Scheduler& scheduler, IMarshalBufferProvider& buffer_provider, MonotonicDuration tx_timeout,
|
||||
MonotonicDuration max_transfer_interval = TransferSender::getDefaultMaxTransferInterval())
|
||||
: max_transfer_interval_(max_transfer_interval)
|
||||
, tx_timeout_(tx_timeout)
|
||||
, scheduler_(scheduler)
|
||||
, buffer_provider_(buffer_provider)
|
||||
{
|
||||
setTxTimeout(tx_timeout);
|
||||
#if UAVCAN_DEBUG
|
||||
assert(getTxTimeout() == tx_timeout); // Making sure default values are OK
|
||||
#endif
|
||||
}
|
||||
|
||||
~GenericPublisher() { }
|
||||
|
||||
int publish(const DataStruct& message, TransferType transfer_type, NodeID dst_node_id,
|
||||
MonotonicTime blocking_deadline = MonotonicTime())
|
||||
{
|
||||
return genericPublish(message, transfer_type, dst_node_id, NULL, blocking_deadline);
|
||||
}
|
||||
|
||||
int publish(const DataStruct& message, TransferType transfer_type, NodeID dst_node_id, TransferID tid,
|
||||
MonotonicTime blocking_deadline = MonotonicTime())
|
||||
{
|
||||
return genericPublish(message, transfer_type, dst_node_id, &tid, blocking_deadline);
|
||||
}
|
||||
|
||||
static MonotonicDuration getMinTxTimeout() { return MonotonicDuration::fromUSec(200); }
|
||||
static MonotonicDuration getMaxTxTimeout() { return MonotonicDuration::fromMSec(60000); }
|
||||
|
||||
MonotonicDuration getTxTimeout() const { return tx_timeout_; }
|
||||
void setTxTimeout(MonotonicDuration tx_timeout)
|
||||
{
|
||||
tx_timeout = std::max(tx_timeout, getMinTxTimeout());
|
||||
tx_timeout = std::min(tx_timeout, getMaxTxTimeout());
|
||||
tx_timeout_ = tx_timeout;
|
||||
}
|
||||
|
||||
Scheduler& getScheduler() const { return scheduler_; }
|
||||
};
|
||||
|
||||
}
|
||||
@@ -0,0 +1,228 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <uavcan/node/scheduler.hpp>
|
||||
#include <uavcan/data_type.hpp>
|
||||
#include <uavcan/node/global_data_type_registry.hpp>
|
||||
#include <uavcan/util/compile_time.hpp>
|
||||
#include <uavcan/util/lazy_constructor.hpp>
|
||||
#include <uavcan/debug.hpp>
|
||||
#include <uavcan/transport/transfer_listener.hpp>
|
||||
#include <uavcan/marshal/scalar_codec.hpp>
|
||||
#include <uavcan/marshal/types.hpp>
|
||||
|
||||
namespace uavcan
|
||||
{
|
||||
|
||||
template <typename DataType_>
|
||||
class ReceivedDataStructure : public DataType_
|
||||
{
|
||||
const IncomingTransfer* transfer_;
|
||||
|
||||
template <typename Ret, Ret (IncomingTransfer::*Fun)() const>
|
||||
Ret safeget() const
|
||||
{
|
||||
if (!transfer_)
|
||||
{
|
||||
assert(0);
|
||||
return Ret();
|
||||
}
|
||||
return (transfer_->*Fun)();
|
||||
}
|
||||
|
||||
protected:
|
||||
ReceivedDataStructure() : transfer_(NULL) { }
|
||||
|
||||
void setTransfer(const IncomingTransfer* transfer)
|
||||
{
|
||||
assert(transfer);
|
||||
transfer_ = transfer;
|
||||
}
|
||||
|
||||
public:
|
||||
typedef DataType_ DataType;
|
||||
|
||||
MonotonicTime getMonotonicTimestamp() const
|
||||
{
|
||||
return safeget<MonotonicTime, &IncomingTransfer::getMonotonicTimestamp>();
|
||||
}
|
||||
UtcTime getUtcTimestamp() const { return safeget<UtcTime, &IncomingTransfer::getUtcTimestamp>(); }
|
||||
TransferType getTransferType() const { return safeget<TransferType, &IncomingTransfer::getTransferType>(); }
|
||||
TransferID getTransferID() const { return safeget<TransferID, &IncomingTransfer::getTransferID>(); }
|
||||
NodeID getSrcNodeID() const { return safeget<NodeID, &IncomingTransfer::getSrcNodeID>(); }
|
||||
};
|
||||
|
||||
template <typename Stream, typename DataType>
|
||||
static Stream& operator<<(Stream& s, const ReceivedDataStructure<DataType>& rds)
|
||||
{
|
||||
s << "# Received struct ts_m=" << rds.getMonotonicTimestamp()
|
||||
<< " ts_utc=" << rds.getUtcTimestamp()
|
||||
<< " snid=" << int(rds.getSrcNodeID().get()) << "\n";
|
||||
s << static_cast<const DataType&>(rds);
|
||||
return s;
|
||||
}
|
||||
|
||||
|
||||
template <typename DataStruct_, unsigned int NumStaticReceivers_, unsigned int NumStaticBufs_>
|
||||
class TransferListenerInstantiationHelper
|
||||
{
|
||||
enum { DataTypeMaxByteLen = BitLenToByteLen<DataStruct_::MaxBitLen>::Result };
|
||||
enum { NeedsBuffer = int(DataTypeMaxByteLen) > int(MaxSingleFrameTransferPayloadLen) };
|
||||
enum { BufferSize = NeedsBuffer ? DataTypeMaxByteLen : 0 };
|
||||
enum { NumStaticBufs = NeedsBuffer ? (NumStaticBufs_ ? NumStaticBufs_ : 1) : 0 };
|
||||
|
||||
public:
|
||||
// TODO: support for zero static bufs
|
||||
typedef TransferListener<BufferSize, NumStaticBufs, NumStaticReceivers_ ? NumStaticReceivers_ : 1> Type;
|
||||
};
|
||||
|
||||
|
||||
template <typename DataSpec, typename DataStruct, typename TransferListenerType>
|
||||
class GenericSubscriber : Noncopyable
|
||||
{
|
||||
typedef GenericSubscriber<DataSpec, DataStruct, TransferListenerType> SelfType;
|
||||
|
||||
// We need to break the inheritance chain here to implement lazy initialization
|
||||
class TransferForwarder : public TransferListenerType
|
||||
{
|
||||
SelfType& obj_;
|
||||
|
||||
void handleIncomingTransfer(IncomingTransfer& transfer)
|
||||
{
|
||||
obj_.handleIncomingTransfer(transfer);
|
||||
}
|
||||
|
||||
public:
|
||||
TransferForwarder(SelfType& obj, const DataTypeDescriptor& data_type, IAllocator& allocator)
|
||||
: TransferListenerType(data_type, allocator)
|
||||
, obj_(obj)
|
||||
{ }
|
||||
};
|
||||
|
||||
struct ReceivedDataStructureSpec : public ReceivedDataStructure<DataStruct>
|
||||
{
|
||||
using ReceivedDataStructure<DataStruct>::setTransfer;
|
||||
};
|
||||
|
||||
Scheduler& scheduler_;
|
||||
IAllocator& allocator_;
|
||||
LazyConstructor<TransferForwarder> forwarder_;
|
||||
ReceivedDataStructureSpec message_;
|
||||
uint32_t failure_count_;
|
||||
|
||||
bool checkInit()
|
||||
{
|
||||
if (forwarder_)
|
||||
return true;
|
||||
|
||||
GlobalDataTypeRegistry::instance().freeze();
|
||||
|
||||
const DataTypeDescriptor* const descr =
|
||||
GlobalDataTypeRegistry::instance().find(DataTypeKind(DataSpec::DataTypeKind),
|
||||
DataSpec::getDataTypeFullName());
|
||||
if (!descr)
|
||||
{
|
||||
UAVCAN_TRACE("GenericSubscriber", "Type [%s] is not registered", DataSpec::getDataTypeFullName());
|
||||
return false;
|
||||
}
|
||||
forwarder_.template construct<SelfType&, const DataTypeDescriptor&, IAllocator&>(*this, *descr, allocator_);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool decodeTransfer(IncomingTransfer& transfer)
|
||||
{
|
||||
BitStream bitstream(transfer);
|
||||
ScalarCodec codec(bitstream);
|
||||
|
||||
message_.setTransfer(&transfer);
|
||||
|
||||
const int decode_res = DataStruct::decode(message_, codec);
|
||||
// We don't need the data anymore, the memory can be reused from the callback:
|
||||
transfer.release();
|
||||
if (decode_res <= 0)
|
||||
{
|
||||
UAVCAN_TRACE("GenericSubscriber", "Unable to decode the message [%i] [%s]",
|
||||
decode_res, DataSpec::getDataTypeFullName());
|
||||
failure_count_++;
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void handleIncomingTransfer(IncomingTransfer& transfer)
|
||||
{
|
||||
if (decodeTransfer(transfer))
|
||||
{
|
||||
handleReceivedDataStruct(message_);
|
||||
}
|
||||
}
|
||||
|
||||
int genericStart(bool(Dispatcher::*registration_method)(TransferListenerBase* listener))
|
||||
{
|
||||
stop();
|
||||
|
||||
if (!checkInit())
|
||||
{
|
||||
UAVCAN_TRACE("GenericSubscriber", "Initialization failure [%s]", DataSpec::getDataTypeFullName());
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (!(scheduler_.getDispatcher().*registration_method)(forwarder_))
|
||||
{
|
||||
UAVCAN_TRACE("GenericSubscriber", "Failed to register transfer listener [%s]",
|
||||
DataSpec::getDataTypeFullName());
|
||||
return -1;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
protected:
|
||||
GenericSubscriber(Scheduler& scheduler, IAllocator& allocator)
|
||||
: scheduler_(scheduler)
|
||||
, allocator_(allocator)
|
||||
, failure_count_(0)
|
||||
{ }
|
||||
|
||||
virtual ~GenericSubscriber() { stop(); }
|
||||
|
||||
virtual void handleReceivedDataStruct(ReceivedDataStructure<DataStruct>&) = 0;
|
||||
|
||||
int startAsMessageListener()
|
||||
{
|
||||
return genericStart(&Dispatcher::registerMessageListener);
|
||||
}
|
||||
|
||||
int startAsServiceRequestListener()
|
||||
{
|
||||
return genericStart(&Dispatcher::registerServiceRequestListener);
|
||||
}
|
||||
|
||||
int startAsServiceResponseListener()
|
||||
{
|
||||
return genericStart(&Dispatcher::registerServiceResponseListener);
|
||||
}
|
||||
|
||||
void stop()
|
||||
{
|
||||
if (forwarder_)
|
||||
{
|
||||
scheduler_.getDispatcher().unregisterMessageListener(forwarder_);
|
||||
scheduler_.getDispatcher().unregisterServiceRequestListener(forwarder_);
|
||||
scheduler_.getDispatcher().unregisterServiceResponseListener(forwarder_);
|
||||
}
|
||||
}
|
||||
|
||||
uint32_t getFailureCount() const { return failure_count_; }
|
||||
|
||||
TransferListenerType* getTransferListener() { return forwarder_; }
|
||||
|
||||
ReceivedDataStructure<DataStruct>& getReceivedStructStorage() { return message_; }
|
||||
|
||||
public:
|
||||
Scheduler& getScheduler() const { return scheduler_; }
|
||||
};
|
||||
|
||||
}
|
||||
@@ -0,0 +1,138 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <cassert>
|
||||
#include <bitset>
|
||||
#include <stdint.h>
|
||||
#include <algorithm>
|
||||
#include <uavcan/data_type.hpp>
|
||||
#include <uavcan/util/compile_time.hpp>
|
||||
#include <uavcan/fatal_error.hpp>
|
||||
#include <uavcan/linked_list.hpp>
|
||||
#if UAVCAN_DEBUG
|
||||
#include <uavcan/debug.hpp>
|
||||
#endif
|
||||
|
||||
namespace uavcan
|
||||
{
|
||||
|
||||
typedef std::bitset<DataTypeID::Max + 1> DataTypeIDMask;
|
||||
|
||||
class GlobalDataTypeRegistry : Noncopyable
|
||||
{
|
||||
struct Entry : public LinkedListNode<Entry>
|
||||
{
|
||||
DataTypeDescriptor descriptor;
|
||||
|
||||
Entry() { }
|
||||
|
||||
Entry(DataTypeKind kind, DataTypeID id, const DataTypeSignature& signature, const char* name)
|
||||
: descriptor(kind, id, signature, name)
|
||||
{ }
|
||||
};
|
||||
|
||||
struct EntryInsertionComparator
|
||||
{
|
||||
const DataTypeID id;
|
||||
EntryInsertionComparator(Entry* dtd) : id(dtd->descriptor.getID()) { }
|
||||
bool operator()(const Entry* entry) const
|
||||
{
|
||||
assert(entry);
|
||||
return entry->descriptor.getID() > id;
|
||||
}
|
||||
};
|
||||
|
||||
public:
|
||||
enum RegistResult
|
||||
{
|
||||
RegistResultOk,
|
||||
RegistResultCollision,
|
||||
RegistResultInvalidParams,
|
||||
RegistResultFrozen
|
||||
};
|
||||
|
||||
private:
|
||||
typedef LinkedListRoot<Entry> List;
|
||||
List msgs_;
|
||||
List srvs_;
|
||||
bool frozen_;
|
||||
|
||||
GlobalDataTypeRegistry() : frozen_(false) { }
|
||||
|
||||
const List* selectList(DataTypeKind kind) const;
|
||||
List* selectList(DataTypeKind kind);
|
||||
|
||||
RegistResult remove(Entry* dtd);
|
||||
RegistResult registImpl(Entry* dtd);
|
||||
|
||||
public:
|
||||
static GlobalDataTypeRegistry& instance();
|
||||
|
||||
/// Last call wins
|
||||
template <typename Type>
|
||||
RegistResult regist(DataTypeID id)
|
||||
{
|
||||
if (isFrozen())
|
||||
return RegistResultFrozen;
|
||||
|
||||
static Entry entry;
|
||||
{
|
||||
const RegistResult remove_res = remove(&entry);
|
||||
if (remove_res != RegistResultOk)
|
||||
return remove_res;
|
||||
}
|
||||
entry = Entry(DataTypeKind(Type::DataTypeKind), id, Type::getDataTypeSignature(), Type::getDataTypeFullName());
|
||||
{
|
||||
const RegistResult remove_res = remove(&entry);
|
||||
if (remove_res != RegistResultOk)
|
||||
return remove_res;
|
||||
}
|
||||
return registImpl(&entry);
|
||||
}
|
||||
|
||||
/// Further calls to regist<>() will fail
|
||||
void freeze();
|
||||
bool isFrozen() const { return frozen_; }
|
||||
|
||||
const DataTypeDescriptor* find(DataTypeKind kind, const char* name) const;
|
||||
|
||||
DataTypeSignature computeAggregateSignature(DataTypeKind kind, DataTypeIDMask& inout_id_mask) const;
|
||||
|
||||
void getDataTypeIDMask(DataTypeKind kind, DataTypeIDMask& mask) const;
|
||||
|
||||
unsigned int getNumMessageTypes() const { return msgs_.getLength(); }
|
||||
unsigned int getNumServiceTypes() const { return srvs_.getLength(); }
|
||||
|
||||
#if UAVCAN_DEBUG
|
||||
/// Required for unit testing
|
||||
void reset()
|
||||
{
|
||||
UAVCAN_TRACE("GlobalDataTypeRegistry", "Reset; was frozen: %i, num msgs: %u, num srvs: %u",
|
||||
int(frozen_), getNumMessageTypes(), getNumServiceTypes());
|
||||
frozen_ = false;
|
||||
while (msgs_.get())
|
||||
msgs_.remove(msgs_.get());
|
||||
while (srvs_.get())
|
||||
srvs_.remove(srvs_.get());
|
||||
}
|
||||
#endif
|
||||
};
|
||||
|
||||
|
||||
template <typename Type>
|
||||
struct DefaultDataTypeRegistrator
|
||||
{
|
||||
DefaultDataTypeRegistrator()
|
||||
{
|
||||
const GlobalDataTypeRegistry::RegistResult res =
|
||||
GlobalDataTypeRegistry::instance().regist<Type>(Type::DefaultDataTypeID);
|
||||
|
||||
if (res != GlobalDataTypeRegistry::RegistResultOk)
|
||||
handleFatalError("Type registration failed");
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
@@ -0,0 +1,68 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <uavcan/transport/transfer.hpp>
|
||||
#include <uavcan/transport/transfer_buffer.hpp>
|
||||
|
||||
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 <unsigned int MaxSize_ = MaxTransferPayloadLen>
|
||||
class MarshalBufferProvider : public IMarshalBufferProvider
|
||||
{
|
||||
class Buffer : public IMarshalBuffer
|
||||
{
|
||||
StaticTransferBuffer<MaxSize_> 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_;
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
@@ -0,0 +1,55 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <uavcan/node/generic_publisher.hpp>
|
||||
|
||||
namespace uavcan
|
||||
{
|
||||
|
||||
template <typename DataType_>
|
||||
class Publisher : protected GenericPublisher<DataType_, DataType_>
|
||||
{
|
||||
typedef GenericPublisher<DataType_, DataType_> BaseType;
|
||||
|
||||
public:
|
||||
typedef DataType_ DataType;
|
||||
|
||||
Publisher(Scheduler& scheduler, IMarshalBufferProvider& buffer_provider,
|
||||
MonotonicDuration tx_timeout = getDefaultTxTimeout(),
|
||||
MonotonicDuration max_transfer_interval = TransferSender::getDefaultMaxTransferInterval())
|
||||
: BaseType(scheduler, buffer_provider, tx_timeout, max_transfer_interval)
|
||||
{
|
||||
#if UAVCAN_DEBUG
|
||||
assert(getTxTimeout() == tx_timeout); // Making sure default values are OK
|
||||
#endif
|
||||
StaticAssert<DataTypeKind(DataType::DataTypeKind) == DataTypeKindMessage>::check();
|
||||
}
|
||||
|
||||
int broadcast(const DataType& message)
|
||||
{
|
||||
return publish(message, TransferTypeMessageBroadcast, NodeID::Broadcast);
|
||||
}
|
||||
|
||||
int unicast(const DataType& message, NodeID dst_node_id)
|
||||
{
|
||||
if (!dst_node_id.isUnicast())
|
||||
{
|
||||
assert(0);
|
||||
return -1;
|
||||
}
|
||||
return publish(message, TransferTypeMessageUnicast, dst_node_id);
|
||||
}
|
||||
|
||||
static MonotonicDuration getDefaultTxTimeout() { return MonotonicDuration::fromMSec(5); } // 5 ms --> 200 Hz max
|
||||
|
||||
using BaseType::getMinTxTimeout;
|
||||
using BaseType::getMaxTxTimeout;
|
||||
using BaseType::getTxTimeout;
|
||||
using BaseType::setTxTimeout;
|
||||
using BaseType::getScheduler;
|
||||
};
|
||||
|
||||
}
|
||||
@@ -0,0 +1,112 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <uavcan/linked_list.hpp>
|
||||
#include <uavcan/transport/dispatcher.hpp>
|
||||
|
||||
namespace uavcan
|
||||
{
|
||||
|
||||
class Scheduler;
|
||||
|
||||
class DeadlineHandler : public LinkedListNode<DeadlineHandler>, Noncopyable
|
||||
{
|
||||
MonotonicTime deadline_;
|
||||
|
||||
protected:
|
||||
Scheduler& scheduler_;
|
||||
|
||||
explicit DeadlineHandler(Scheduler& scheduler)
|
||||
: scheduler_(scheduler)
|
||||
{ }
|
||||
|
||||
virtual ~DeadlineHandler() { stop(); }
|
||||
|
||||
public:
|
||||
virtual void handleDeadline(MonotonicTime current_timestamp) = 0;
|
||||
|
||||
void startWithDeadline(MonotonicTime deadline);
|
||||
void startWithDelay(MonotonicDuration delay);
|
||||
|
||||
void stop();
|
||||
|
||||
bool isRunning() const;
|
||||
|
||||
MonotonicTime getDeadline() const { return deadline_; }
|
||||
Scheduler& getScheduler() const { return scheduler_; }
|
||||
};
|
||||
|
||||
|
||||
class DeadlineScheduler : Noncopyable
|
||||
{
|
||||
LinkedListRoot<DeadlineHandler> handlers_; // Ordered by deadline, lowest first
|
||||
|
||||
public:
|
||||
void add(DeadlineHandler* mdh);
|
||||
void remove(DeadlineHandler* mdh);
|
||||
bool doesExist(const DeadlineHandler* mdh) const;
|
||||
unsigned int getNumHandlers() const { return handlers_.getLength(); }
|
||||
|
||||
MonotonicTime pollAndGetMonotonicTimestamp(ISystemClock& sysclock);
|
||||
MonotonicTime getEarliestDeadline() const;
|
||||
};
|
||||
|
||||
|
||||
class Scheduler : Noncopyable
|
||||
{
|
||||
enum { DefaultDeadlineResolutionMs = 5 };
|
||||
enum { MinDeadlineResolutionMs = 1 };
|
||||
enum { MaxDeadlineResolutionMs = 100 };
|
||||
|
||||
enum { DefaultCleanupPeriodMs = 1000 };
|
||||
enum { MinCleanupPeriodMs = 10 };
|
||||
enum { MaxCleanupPeriodMs = 10000 };
|
||||
|
||||
DeadlineScheduler deadline_scheduler_;
|
||||
Dispatcher dispatcher_;
|
||||
MonotonicTime prev_cleanup_ts_;
|
||||
MonotonicDuration deadline_resolution_;
|
||||
MonotonicDuration cleanup_period_;
|
||||
|
||||
MonotonicTime computeDispatcherSpinDeadline(MonotonicTime spin_deadline) const;
|
||||
void pollCleanup(MonotonicTime mono_ts, uint32_t num_frames_processed_with_last_spin);
|
||||
|
||||
public:
|
||||
Scheduler(ICanDriver& can_driver, IAllocator& allocator, ISystemClock& sysclock, IOutgoingTransferRegistry& otr,
|
||||
NodeID self_node_id)
|
||||
: dispatcher_(can_driver, allocator, sysclock, otr, self_node_id)
|
||||
, prev_cleanup_ts_(sysclock.getMonotonic())
|
||||
, deadline_resolution_(MonotonicDuration::fromMSec(DefaultDeadlineResolutionMs))
|
||||
, cleanup_period_(MonotonicDuration::fromMSec(DefaultCleanupPeriodMs))
|
||||
{ }
|
||||
|
||||
int spin(MonotonicTime deadline);
|
||||
|
||||
DeadlineScheduler& getDeadlineScheduler() { return deadline_scheduler_; }
|
||||
Dispatcher& getDispatcher() { return dispatcher_; }
|
||||
|
||||
ISystemClock& getSystemClock() { return dispatcher_.getSystemClock(); }
|
||||
MonotonicTime getMonotonicTimestamp() const { return dispatcher_.getSystemClock().getMonotonic(); }
|
||||
UtcTime getUtcTimestamp() const { return dispatcher_.getSystemClock().getUtc(); }
|
||||
|
||||
MonotonicDuration getDeadlineResolution() const { return deadline_resolution_; }
|
||||
void setDeadlineResolution(MonotonicDuration res)
|
||||
{
|
||||
res = std::min(res, MonotonicDuration::fromMSec(MaxDeadlineResolutionMs));
|
||||
res = std::max(res, MonotonicDuration::fromMSec(MinDeadlineResolutionMs));
|
||||
deadline_resolution_ = res;
|
||||
}
|
||||
|
||||
MonotonicDuration getCleanupPeriod() const { return cleanup_period_; }
|
||||
void setCleanupPeriod(MonotonicDuration period)
|
||||
{
|
||||
period = std::min(period, MonotonicDuration::fromMSec(MaxCleanupPeriodMs));
|
||||
period = std::max(period, MonotonicDuration::fromMSec(MinCleanupPeriodMs));
|
||||
cleanup_period_ = period;
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
@@ -0,0 +1,268 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#include <uavcan/node/generic_publisher.hpp>
|
||||
#include <uavcan/node/generic_subscriber.hpp>
|
||||
|
||||
namespace uavcan
|
||||
{
|
||||
|
||||
template <typename ServiceDataType>
|
||||
class ServiceResponseTransferListenerInstantiationHelper
|
||||
{
|
||||
enum { DataTypeMaxByteLen = BitLenToByteLen<ServiceDataType::Response::MaxBitLen>::Result };
|
||||
public:
|
||||
typedef ServiceResponseTransferListener<DataTypeMaxByteLen> Type;
|
||||
};
|
||||
|
||||
|
||||
template <typename DataType>
|
||||
struct ServiceCallResult
|
||||
{
|
||||
typedef ReceivedDataStructure<typename DataType::Response> ResponseFieldType;
|
||||
|
||||
enum Status { Success, ErrorTimeout };
|
||||
|
||||
const Status status;
|
||||
NodeID server_node_id;
|
||||
ResponseFieldType& response; ///< Either response contents or unspecified response structure
|
||||
|
||||
ServiceCallResult(Status status, NodeID server_node_id, ResponseFieldType& response)
|
||||
: status(status)
|
||||
, server_node_id(server_node_id)
|
||||
, response(response)
|
||||
{
|
||||
assert(server_node_id.isUnicast());
|
||||
assert(status == Success || status == ErrorTimeout);
|
||||
}
|
||||
|
||||
bool isSuccessful() const { return status == Success; }
|
||||
};
|
||||
|
||||
template <typename Stream, typename DataType>
|
||||
static Stream& operator<<(Stream& s, const ServiceCallResult<DataType>& scr)
|
||||
{
|
||||
s << "# Service call result [" << DataType::getDataTypeFullName() << "] "
|
||||
<< (scr.isSuccessful() ? "OK" : "FAILURE")
|
||||
<< " server_node_id=" << int(scr.server_node_id.get()) << "\n";
|
||||
if (scr.isSuccessful())
|
||||
s << scr.response;
|
||||
else
|
||||
s << "# (no data)";
|
||||
return s;
|
||||
}
|
||||
|
||||
|
||||
template <typename DataType_, typename Callback = void(*)(const ServiceCallResult<DataType_>&)>
|
||||
class ServiceClient :
|
||||
public GenericSubscriber<DataType_, typename DataType_::Response,
|
||||
typename ServiceResponseTransferListenerInstantiationHelper<DataType_>::Type >,
|
||||
public DeadlineHandler
|
||||
{
|
||||
public:
|
||||
typedef DataType_ DataType;
|
||||
typedef typename DataType::Request RequestType;
|
||||
typedef typename DataType::Response ResponseType;
|
||||
typedef ServiceCallResult<DataType> ServiceCallResultType;
|
||||
|
||||
private:
|
||||
typedef ServiceClient<DataType, Callback> SelfType;
|
||||
typedef GenericPublisher<DataType, RequestType> PublisherType;
|
||||
typedef typename ServiceResponseTransferListenerInstantiationHelper<DataType>::Type TransferListenerType;
|
||||
typedef GenericSubscriber<DataType, ResponseType, TransferListenerType> SubscriberType;
|
||||
|
||||
PublisherType publisher_;
|
||||
IAllocator& allocator_;
|
||||
Callback callback_;
|
||||
MonotonicDuration request_timeout_;
|
||||
bool pending_;
|
||||
|
||||
bool isCallbackValid() const { return try_implicit_cast<bool>(callback_, true); }
|
||||
|
||||
void invokeCallback(ServiceCallResultType& result)
|
||||
{
|
||||
if (isCallbackValid())
|
||||
callback_(result);
|
||||
else
|
||||
handleFatalError("Invalid caller callback");
|
||||
}
|
||||
|
||||
void handleReceivedDataStruct(ReceivedDataStructure<ResponseType>& response)
|
||||
{
|
||||
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
|
||||
{
|
||||
assert(0);
|
||||
cancel();
|
||||
}
|
||||
}
|
||||
|
||||
void handleDeadline(MonotonicTime)
|
||||
{
|
||||
const TransferListenerType* const listener = SubscriberType::getTransferListener();
|
||||
if (listener)
|
||||
{
|
||||
const typename TransferListenerType::ExpectedResponseParams erp = listener->getExpectedResponseParams();
|
||||
ReceivedDataStructure<ResponseType>& 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
|
||||
{
|
||||
assert(0);
|
||||
cancel();
|
||||
}
|
||||
}
|
||||
|
||||
public:
|
||||
ServiceClient(Scheduler& scheduler, IAllocator& allocator, IMarshalBufferProvider& buffer_provider,
|
||||
const Callback& callback = Callback())
|
||||
: SubscriberType(scheduler, allocator)
|
||||
, DeadlineHandler(scheduler)
|
||||
, publisher_(scheduler, buffer_provider, getDefaultRequestTimeout())
|
||||
, allocator_(allocator)
|
||||
, callback_(callback)
|
||||
, request_timeout_(getDefaultRequestTimeout())
|
||||
, pending_(false)
|
||||
{
|
||||
setRequestTimeout(getDefaultRequestTimeout());
|
||||
#if UAVCAN_DEBUG
|
||||
assert(getRequestTimeout() == getDefaultRequestTimeout()); // Making sure default values are OK
|
||||
#endif
|
||||
}
|
||||
|
||||
virtual ~ServiceClient() { cancel(); }
|
||||
|
||||
int call(NodeID server_node_id, const RequestType& request) // TODO: Refactor, move into a non-template subclass!
|
||||
{
|
||||
/*
|
||||
* Cancelling the pending request
|
||||
*/
|
||||
cancel();
|
||||
if (!isCallbackValid())
|
||||
{
|
||||
UAVCAN_TRACE("ServiceClient", "Invalid callback");
|
||||
return -1;
|
||||
}
|
||||
pending_ = true;
|
||||
|
||||
/*
|
||||
* Determining the Data Type ID
|
||||
*/
|
||||
GlobalDataTypeRegistry::instance().freeze();
|
||||
const DataTypeDescriptor* const descr =
|
||||
GlobalDataTypeRegistry::instance().find(DataTypeKindService, DataType::getDataTypeFullName());
|
||||
if (!descr)
|
||||
{
|
||||
UAVCAN_TRACE("ServiceClient", "Type [%s] is not registered", DataType::getDataTypeFullName());
|
||||
cancel();
|
||||
return -1;
|
||||
}
|
||||
|
||||
/*
|
||||
* Determining the Transfer ID
|
||||
*/
|
||||
const OutgoingTransferRegistryKey otr_key(descr->getID(), TransferTypeServiceRequest, server_node_id);
|
||||
const MonotonicTime otr_deadline =
|
||||
SubscriberType::getScheduler().getMonotonicTimestamp() + TransferSender::getDefaultMaxTransferInterval();
|
||||
TransferID* const otr_tid = SubscriberType::getScheduler().getDispatcher().getOutgoingTransferRegistry()
|
||||
.accessOrCreate(otr_key, otr_deadline);
|
||||
if (!otr_tid)
|
||||
{
|
||||
UAVCAN_TRACE("ServiceClient", "OTR access failure, dtd=%s", descr->toString().c_str());
|
||||
cancel();
|
||||
return -1;
|
||||
}
|
||||
const TransferID transfer_id = *otr_tid;
|
||||
otr_tid->increment();
|
||||
|
||||
/*
|
||||
* Starting the subscriber
|
||||
*/
|
||||
const int subscriber_res = SubscriberType::startAsServiceResponseListener();
|
||||
if (subscriber_res <= 0)
|
||||
{
|
||||
UAVCAN_TRACE("ServiceClient", "Failed to start the subscriber, error: %i", subscriber_res);
|
||||
cancel();
|
||||
return subscriber_res;
|
||||
}
|
||||
|
||||
/*
|
||||
* Configuring the listener so it will accept only the matching response
|
||||
*/
|
||||
TransferListenerType* const tl = SubscriberType::getTransferListener();
|
||||
if (!tl)
|
||||
{
|
||||
assert(0); // Must have been created
|
||||
cancel();
|
||||
return -1;
|
||||
}
|
||||
const typename TransferListenerType::ExpectedResponseParams erp(server_node_id, transfer_id);
|
||||
tl->setExpectedResponseParams(erp);
|
||||
|
||||
/*
|
||||
* Registering the deadline handler
|
||||
*/
|
||||
DeadlineHandler::startWithDelay(request_timeout_);
|
||||
|
||||
/*
|
||||
* Publishing the request
|
||||
*/
|
||||
const int publisher_res = publisher_.publish(request, TransferTypeServiceRequest, server_node_id, transfer_id);
|
||||
if (!publisher_res)
|
||||
{
|
||||
cancel();
|
||||
}
|
||||
return publisher_res;
|
||||
}
|
||||
|
||||
void cancel()
|
||||
{
|
||||
pending_ = false;
|
||||
SubscriberType::stop();
|
||||
DeadlineHandler::stop();
|
||||
TransferListenerType* const tl = SubscriberType::getTransferListener();
|
||||
if (tl)
|
||||
tl->stopAcceptingAnything();
|
||||
}
|
||||
|
||||
bool isPending() const { return pending_; }
|
||||
|
||||
const Callback& getCallback() const { return callback_; }
|
||||
void setCallback(const Callback& cb) { callback_ = cb; }
|
||||
|
||||
uint32_t getResponseFailureCount() const { return SubscriberType::getFailureCount(); }
|
||||
|
||||
/*
|
||||
* Request timeouts
|
||||
* There is no such config as TX timeout - TX timeouts are configured automagically according to request timeouts
|
||||
*/
|
||||
static MonotonicDuration getDefaultRequestTimeout() { return MonotonicDuration::fromMSec(1000); }
|
||||
static MonotonicDuration getMinRequestTimeout() { return MonotonicDuration::fromMSec(10); }
|
||||
static MonotonicDuration getMaxRequestTimeout() { return MonotonicDuration::fromMSec(60000); }
|
||||
|
||||
MonotonicDuration getRequestTimeout() const { return request_timeout_; }
|
||||
void setRequestTimeout(MonotonicDuration timeout)
|
||||
{
|
||||
timeout = std::max(timeout, getMinRequestTimeout());
|
||||
timeout = std::min(timeout, getMaxRequestTimeout());
|
||||
|
||||
publisher_.setTxTimeout(timeout);
|
||||
request_timeout_ = std::max(timeout, publisher_.getTxTimeout()); // No less than TX timeout
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
@@ -0,0 +1,98 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <uavcan/node/generic_publisher.hpp>
|
||||
#include <uavcan/node/generic_subscriber.hpp>
|
||||
|
||||
namespace uavcan
|
||||
{
|
||||
|
||||
template <typename DataType_,
|
||||
typename Callback = void(*)(const ReceivedDataStructure<typename DataType_::Request>&,
|
||||
typename DataType_::Response&),
|
||||
unsigned int NumStaticReceivers = 2,
|
||||
unsigned int NumStaticBufs = 1>
|
||||
class ServiceServer : public GenericSubscriber<DataType_, typename DataType_::Request,
|
||||
typename TransferListenerInstantiationHelper<typename DataType_::Request,
|
||||
NumStaticReceivers,
|
||||
NumStaticBufs>::Type>
|
||||
{
|
||||
public:
|
||||
typedef DataType_ DataType;
|
||||
typedef typename DataType::Request RequestType;
|
||||
typedef typename DataType::Response ResponseType;
|
||||
|
||||
private:
|
||||
typedef typename TransferListenerInstantiationHelper<RequestType, NumStaticReceivers, NumStaticBufs>::Type
|
||||
TransferListenerType;
|
||||
typedef GenericSubscriber<DataType, RequestType, TransferListenerType> SubscriberType;
|
||||
typedef GenericPublisher<DataType, ResponseType> PublisherType;
|
||||
|
||||
PublisherType publisher_;
|
||||
Callback callback_;
|
||||
uint32_t response_failure_count_;
|
||||
ResponseType response_;
|
||||
|
||||
void handleReceivedDataStruct(ReceivedDataStructure<RequestType>& request)
|
||||
{
|
||||
assert(request.getTransferType() == TransferTypeServiceRequest);
|
||||
if (try_implicit_cast<bool>(callback_, true))
|
||||
{
|
||||
response_ = ResponseType(); // The application needs newly initialized structure
|
||||
callback_(request, response_);
|
||||
}
|
||||
else
|
||||
handleFatalError("Invalid service server callback");
|
||||
|
||||
const int res = publisher_.publish(response_, TransferTypeServiceResponse, request.getSrcNodeID(),
|
||||
request.getTransferID());
|
||||
if (res <= 0)
|
||||
{
|
||||
UAVCAN_TRACE("ServiceServer", "Response publication failure: %i", res);
|
||||
response_failure_count_++;
|
||||
}
|
||||
}
|
||||
|
||||
public:
|
||||
ServiceServer(Scheduler& scheduler, IAllocator& allocator, IMarshalBufferProvider& buffer_provider)
|
||||
: SubscriberType(scheduler, allocator)
|
||||
, publisher_(scheduler, buffer_provider, getDefaultTxTimeout())
|
||||
, callback_()
|
||||
, response_failure_count_(0)
|
||||
{
|
||||
assert(getTxTimeout() == getDefaultTxTimeout()); // Making sure it is valid
|
||||
|
||||
StaticAssert<DataTypeKind(DataType::DataTypeKind) == DataTypeKindService>::check();
|
||||
}
|
||||
|
||||
int start(const Callback& callback)
|
||||
{
|
||||
stop();
|
||||
|
||||
if (!try_implicit_cast<bool>(callback, true))
|
||||
{
|
||||
UAVCAN_TRACE("ServiceServer", "Invalid callback");
|
||||
return -1;
|
||||
}
|
||||
callback_ = callback;
|
||||
|
||||
return SubscriberType::startAsServiceRequestListener();
|
||||
}
|
||||
|
||||
using SubscriberType::stop;
|
||||
|
||||
static MonotonicDuration getDefaultTxTimeout() { return MonotonicDuration::fromMSec(1000); }
|
||||
static MonotonicDuration getMinTxTimeout() { return PublisherType::getMinTxTimeout(); }
|
||||
static MonotonicDuration getMaxTxTimeout() { return PublisherType::getMaxTxTimeout(); }
|
||||
|
||||
MonotonicDuration getTxTimeout() const { return publisher_.getTxTimeout(); }
|
||||
void setTxTimeout(MonotonicDuration tx_timeout) { publisher_.setTxTimeout(tx_timeout); }
|
||||
|
||||
uint32_t getRequestFailureCount() const { return SubscriberType::getFailureCount(); }
|
||||
uint32_t getResponseFailureCount() const { return response_failure_count_; }
|
||||
};
|
||||
|
||||
}
|
||||
@@ -0,0 +1,64 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <cassert>
|
||||
#include <uavcan/node/generic_subscriber.hpp>
|
||||
|
||||
namespace uavcan
|
||||
{
|
||||
|
||||
template <typename DataType_,
|
||||
typename Callback = void(*)(const ReceivedDataStructure<DataType_>&),
|
||||
unsigned int NumStaticReceivers = 2,
|
||||
unsigned int NumStaticBufs = 1>
|
||||
class Subscriber : public GenericSubscriber<DataType_, DataType_,
|
||||
typename TransferListenerInstantiationHelper<DataType_,
|
||||
NumStaticReceivers,
|
||||
NumStaticBufs>::Type>
|
||||
{
|
||||
typedef typename TransferListenerInstantiationHelper<DataType_, NumStaticReceivers, NumStaticBufs>::Type
|
||||
TransferListenerType;
|
||||
typedef GenericSubscriber<DataType_, DataType_, TransferListenerType> BaseType;
|
||||
|
||||
Callback callback_;
|
||||
|
||||
void handleReceivedDataStruct(ReceivedDataStructure<DataType_>& msg)
|
||||
{
|
||||
if (try_implicit_cast<bool>(callback_, true))
|
||||
callback_(msg);
|
||||
else
|
||||
handleFatalError("Invalid subscriber callback");
|
||||
}
|
||||
|
||||
public:
|
||||
typedef DataType_ DataType;
|
||||
|
||||
Subscriber(Scheduler& scheduler, IAllocator& allocator)
|
||||
: BaseType(scheduler, allocator)
|
||||
, callback_()
|
||||
{
|
||||
StaticAssert<DataTypeKind(DataType::DataTypeKind) == DataTypeKindMessage>::check();
|
||||
}
|
||||
|
||||
int start(const Callback& callback)
|
||||
{
|
||||
stop();
|
||||
|
||||
if (!try_implicit_cast<bool>(callback, true))
|
||||
{
|
||||
UAVCAN_TRACE("Subscriber", "Invalid callback");
|
||||
return -1;
|
||||
}
|
||||
callback_ = callback;
|
||||
|
||||
return BaseType::startAsMessageListener();
|
||||
}
|
||||
|
||||
using BaseType::stop;
|
||||
using BaseType::getFailureCount;
|
||||
};
|
||||
|
||||
}
|
||||
@@ -0,0 +1,85 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
#include <uavcan/node/scheduler.hpp>
|
||||
#include <uavcan/util/compile_time.hpp>
|
||||
#include <uavcan/linked_list.hpp>
|
||||
#include <uavcan/fatal_error.hpp>
|
||||
|
||||
namespace uavcan
|
||||
{
|
||||
|
||||
class Timer;
|
||||
|
||||
struct TimerEvent
|
||||
{
|
||||
MonotonicTime scheduled_time;
|
||||
MonotonicTime real_time;
|
||||
|
||||
TimerEvent(MonotonicTime scheduled_time, MonotonicTime real_time)
|
||||
: scheduled_time(scheduled_time)
|
||||
, real_time(real_time)
|
||||
{ }
|
||||
};
|
||||
|
||||
|
||||
class Timer : private DeadlineHandler
|
||||
{
|
||||
MonotonicDuration period_;
|
||||
|
||||
void handleDeadline(MonotonicTime current_timestamp);
|
||||
|
||||
public:
|
||||
using DeadlineHandler::stop;
|
||||
using DeadlineHandler::isRunning;
|
||||
using DeadlineHandler::getDeadline;
|
||||
using DeadlineHandler::getScheduler;
|
||||
|
||||
explicit Timer(Scheduler& scheduler)
|
||||
: DeadlineHandler(scheduler)
|
||||
, period_(MonotonicDuration::getInfinite())
|
||||
{ }
|
||||
|
||||
void startOneShotWithDeadline(MonotonicTime deadline);
|
||||
void startOneShotWithDelay(MonotonicDuration delay);
|
||||
void startPeriodic(MonotonicDuration period);
|
||||
|
||||
MonotonicDuration getPeriod() const { return period_; }
|
||||
|
||||
virtual void handleTimerEvent(const TimerEvent& event) = 0;
|
||||
};
|
||||
|
||||
|
||||
template <typename Callback>
|
||||
class TimerEventForwarder : public Timer
|
||||
{
|
||||
Callback callback_;
|
||||
|
||||
public:
|
||||
explicit TimerEventForwarder(Scheduler& node)
|
||||
: Timer(node)
|
||||
, callback_()
|
||||
{ }
|
||||
|
||||
TimerEventForwarder(Scheduler& node, Callback callback)
|
||||
: Timer(node)
|
||||
, callback_(callback)
|
||||
{ }
|
||||
|
||||
const Callback& getCallback() const { return callback_; }
|
||||
void setCallback(const Callback& callback) { callback_ = callback; }
|
||||
|
||||
void handleTimerEvent(const TimerEvent& event)
|
||||
{
|
||||
if (try_implicit_cast<bool>(callback_, true))
|
||||
callback_(event);
|
||||
else
|
||||
handleFatalError("Invalid timer callback");
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
Reference in New Issue
Block a user