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

Using global RX object buffer

Browse Source
This commit is contained in:
Pavel Kirienko
2015-05-29 16:47:02 +03:00
parent d4e49d518a
commit d20f8e7356
7 changed files with 152 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libuavcan/include/uavcan/node/generic_subscriber.hpp
+98 -32
View File
@@ -40,7 +40,6 @@ class UAVCAN_EXPORT ReceivedDataStructure : public DataType_
{
if (_transfer_ == NULL)
{
UAVCAN_ASSERT(0);
return Ret();
}
return (_transfer_->*Fun)();
@@ -162,23 +161,71 @@ class UAVCAN_EXPORT GenericSubscriber : public GenericSubscriberBase
{ }
};
struct ReceivedDataStructureSpec : public ReceivedDataStructure<DataStruct>
{
using ReceivedDataStructure<DataStruct>::setTransfer;
};
LazyConstructor<TransferForwarder> forwarder_;
ReceivedDataStructureSpec message_;
int checkInit();
bool decodeTransfer(IncomingTransfer& transfer);
void handleIncomingTransfer(IncomingTransfer& transfer);
int genericStart(bool (Dispatcher::*registration_method)(TransferListenerBase*));
/*
* Received object and its allocators
*/
struct ReceivedDataStructureSpec : public ReceivedDataStructure<DataStruct>
{
using ReceivedDataStructure<DataStruct>::setTransfer;
};
class StructBufferStatic
{
ReceivedDataStructureSpec object_;
public:
explicit StructBufferStatic(GenericSubscriber&) { }
ReceivedDataStructureSpec* getObjectPtr() { return &object_; }
};
class StructBufferShared
{
ReceivedDataStructureSpec* object_;
public:
explicit StructBufferShared(GenericSubscriber& owner)
: object_(NULL)
{
IMarshalBuffer* const buf =
owner.getNode().getMarshalBufferProvider().getBuffer(sizeof(ReceivedDataStructureSpec));
if (buf != NULL && buf->getSize() >= sizeof(ReceivedDataStructureSpec))
{
object_ = new (buf->getDataPtr()) ReceivedDataStructureSpec;
}
}
/*
* Destructor MUST NOT be invoked
* TODO explain why
*/
ReceivedDataStructureSpec* getObjectPtr() { return object_; }
};
protected:
/**
* This value defines the threshold for stack allocation. See the typedef below.
*/
enum { MaxObjectSizeForStackAllocation = 80 };
/**
* This type resolves to either type of buffer, depending on the object size:
* - if the object is small, it will be allocated on the stack (StructBufferStatic);
* - if the object is large, it will be allocated in the shared buffer (StructBufferShared).
*/
typedef typename Select<(sizeof(DataStruct) > MaxObjectSizeForStackAllocation),
StructBufferShared,
StructBufferStatic>::Result ReceivedDataStructureBuffer;
explicit GenericSubscriber(INode& node)
: GenericSubscriberBase(node)
{ }
@@ -227,15 +274,6 @@ protected:
}
TransferListenerType* getTransferListener() { return forwarder_; }
/**
* Returns a reference to the temporary storage for decoded received messages.
* Reference to this storage is used as a parameter for subscription callbacks.
* This storage is guaranteed to stay intact after the last message was decoded, i.e.
* the application can use it to access the last received message object.
*/
ReceivedDataStructure<DataStruct>& getReceivedStructStorage() { return message_; }
const ReceivedDataStructure<DataStruct>& getReceivedStructStorage() const { return message_; }
};
// ----------------------------------------------------------------------------
@@ -250,48 +288,76 @@ int GenericSubscriber<DataSpec, DataStruct, TransferListenerType>::checkInit()
{
return 0;
}
/*
* Making sure that the buffer is large enough
* This check MUST be performed BEFORE initialization, otherwise we may encounter some terrible runtime failures.
*/
if (ReceivedDataStructureBuffer(*this).getObjectPtr() == NULL)
{
return -ErrBufferTooSmall;
}
/*
* Initializing the transfer forwarder
*/
GlobalDataTypeRegistry::instance().freeze();
const DataTypeDescriptor* const descr =
GlobalDataTypeRegistry::instance().find(DataTypeKind(DataSpec::DataTypeKind), DataSpec::getDataTypeFullName());
if (!descr)
if (descr == NULL)
{
UAVCAN_TRACE("GenericSubscriber", "Type [%s] is not registered", DataSpec::getDataTypeFullName());
return -ErrUnknownDataType;
}
forwarder_.template construct<SelfType&, const DataTypeDescriptor&, IPoolAllocator&>
(*this, *descr, node_.getAllocator());
return 0;
}
template <typename DataSpec, typename DataStruct, typename TransferListenerType>
bool GenericSubscriber<DataSpec, DataStruct, TransferListenerType>::decodeTransfer(IncomingTransfer& transfer)
void GenericSubscriber<DataSpec, DataStruct, TransferListenerType>::handleIncomingTransfer(IncomingTransfer& transfer)
{
/*
* Initializing the temporary RX structure
*/
ReceivedDataStructureBuffer rx_struct_buffer(*this);
if (rx_struct_buffer.getObjectPtr() == NULL)
{
UAVCAN_ASSERT(0); // The init method should have caught this error.
failure_count_++;
node_.getDispatcher().getTransferPerfCounter().addError();
return;
}
rx_struct_buffer.getObjectPtr()->setTransfer(&transfer);
/*
* Decoding into the temporary storage
*/
BitStream bitstream(transfer);
ScalarCodec codec(bitstream);
message_.setTransfer(&transfer);
const int decode_res = DataStruct::decode(*rx_struct_buffer.getObjectPtr(), codec);
const int decode_res = DataStruct::decode(message_, codec);
// We don't need the data anymore, the memory can be reused from the callback:
transfer.release();
if (decode_res <= 0)
{
UAVCAN_TRACE("GenericSubscriber", "Unable to decode the message [%i] [%s]",
decode_res, DataSpec::getDataTypeFullName());
failure_count_++;
node_.getDispatcher().getTransferPerfCounter().addError();
return false;
return;
}
return true;
}
template <typename DataSpec, typename DataStruct, typename TransferListenerType>
void GenericSubscriber<DataSpec, DataStruct, TransferListenerType>::handleIncomingTransfer(IncomingTransfer& transfer)
{
if (decodeTransfer(transfer))
{
handleReceivedDataStruct(message_);
}
/*
* Invoking the callback
*/
handleReceivedDataStruct(*rx_struct_buffer.getObjectPtr());
}
template <typename DataSpec, typename DataStruct, typename TransferListenerType>
libuavcan/include/uavcan/node/marshal_buffer.hpp
+4 -2
View File
@@ -17,9 +17,11 @@ namespace uavcan
class UAVCAN_EXPORT IMarshalBuffer : public ITransferBuffer
{
public:
virtual const uint8_t* getDataPtr() const = 0;
virtual uint8_t* getDataPtr() = 0;
virtual unsigned getMaxWritePos() const = 0;
virtual unsigned getSize() const = 0;
const uint8_t* getDataPtr() const { return const_cast<IMarshalBuffer*>(this)->getDataPtr(); }
};
/**
@@ -62,7 +64,7 @@ class UAVCAN_EXPORT MarshalBufferProvider : public IMarshalBufferProvider
return buf_.write(offset, data, len);
}
virtual const uint8_t* getDataPtr() const { return buf_.getRawPtr(); }
virtual uint8_t* getDataPtr() { return buf_.getRawPtr(); }
virtual unsigned getMaxWritePos() const { return buf_.getMaxWritePos(); }
libuavcan/include/uavcan/node/service_client.hpp
+10 -2
View File
@@ -61,7 +61,7 @@ struct ServiceCallID
* Note that application ALWAYS gets this result, even when it times out or fails because of some other reason.
*/
template <typename DataType>
class UAVCAN_EXPORT ServiceCallResult
class UAVCAN_EXPORT ServiceCallResult : Noncopyable
{
public:
typedef ReceivedDataStructure<typename DataType::Response> ResponseFieldType;
@@ -257,8 +257,16 @@ private:
UAVCAN_TRACE("ServiceClient::TimeoutCallbackCaller", "Timeout from nid=%d, tid=%d, dtname=%s",
int(state.getCallID().server_node_id.get()), int(state.getCallID().transfer_id.get()),
DataType::getDataTypeFullName());
typename SubscriberType::ReceivedDataStructureBuffer rx_struct_buffer(owner);
if (rx_struct_buffer.getObjectPtr() == NULL)
{
handleFatalError("RX obj buf");
}
ServiceCallResultType result(ServiceCallResultType::ErrorTimeout, state.getCallID(),
owner.getReceivedStructStorage()); // Mutable!
*rx_struct_buffer.getObjectPtr()); // Mutable!
owner.invokeCallback(result);
}
}