mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-11-06 07:31:36 +08:00
Superrefactoring - all time values were replaced with new safer classes from uavcan/time.hpp; generated types were moved away from anonymous namespaces because this makes it impossible to use a type from different compilation units. Some less vital fixes are to follow - see the next few commits
This commit is contained in:
@@ -52,7 +52,7 @@ if (GTEST_FOUND)
|
|||||||
${CMAKE_SOURCE_DIR}/../dsdl/uavcan # Input
|
${CMAKE_SOURCE_DIR}/../dsdl/uavcan # Input
|
||||||
-Otest/dsdlc_output # Output
|
-Otest/dsdlc_output # Output
|
||||||
WORKING_DIRECTORY ${CMAKE_SOURCE_DIR})
|
WORKING_DIRECTORY ${CMAKE_SOURCE_DIR})
|
||||||
add_dependencies(libuavcan_test dsdlc)
|
add_dependencies(uavcan dsdlc)
|
||||||
include_directories(${CMAKE_SOURCE_DIR}/test/dsdlc_output)
|
include_directories(${CMAKE_SOURCE_DIR}/test/dsdlc_output)
|
||||||
|
|
||||||
# Tests run automatically upon successful build
|
# Tests run automatically upon successful build
|
||||||
|
|||||||
@@ -8,6 +8,21 @@
|
|||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Forward declaration
|
||||||
|
*/
|
||||||
|
% for nsc in t.cpp_namespace_components:
|
||||||
|
namespace ${nsc}
|
||||||
|
{
|
||||||
|
% endfor
|
||||||
|
|
||||||
|
struct ${t.cpp_type_name};
|
||||||
|
typedef ${t.cpp_type_name} ${t.short_name};
|
||||||
|
|
||||||
|
% for nsc in t.cpp_namespace_components:
|
||||||
|
}
|
||||||
|
% endfor
|
||||||
|
|
||||||
#include <uavcan/data_type.hpp>
|
#include <uavcan/data_type.hpp>
|
||||||
#include <uavcan/global_data_type_registry.hpp>
|
#include <uavcan/global_data_type_registry.hpp>
|
||||||
#include <uavcan/internal/marshal/types.hpp>
|
#include <uavcan/internal/marshal/types.hpp>
|
||||||
@@ -37,8 +52,6 @@ ${line}
|
|||||||
namespace ${nsc}
|
namespace ${nsc}
|
||||||
{
|
{
|
||||||
% endfor
|
% endfor
|
||||||
namespace
|
|
||||||
{
|
|
||||||
|
|
||||||
#if UAVCAN_PACK_STRUCTS
|
#if UAVCAN_PACK_STRUCTS
|
||||||
UAVCAN_PACKED_BEGIN
|
UAVCAN_PACKED_BEGIN
|
||||||
@@ -214,14 +227,16 @@ UAVCAN_PACKED_END
|
|||||||
#endif
|
#endif
|
||||||
|
|
||||||
% if t.has_default_dtid:
|
% if t.has_default_dtid:
|
||||||
|
namespace
|
||||||
|
{
|
||||||
|
|
||||||
const ::uavcan::DefaultDataTypeRegistrator< ${t.cpp_type_name} > _uavcan_gdtr_registrator_${t.cpp_type_name};
|
const ::uavcan::DefaultDataTypeRegistrator< ${t.cpp_type_name} > _uavcan_gdtr_registrator_${t.cpp_type_name};
|
||||||
|
|
||||||
|
}
|
||||||
% else:
|
% else:
|
||||||
// No default registration
|
// No default registration
|
||||||
% endif
|
% endif
|
||||||
|
|
||||||
typedef ${t.cpp_type_name} ${t.short_name};
|
|
||||||
|
|
||||||
} // Anonymous namespace
|
|
||||||
% for nsc in t.cpp_namespace_components:
|
% for nsc in t.cpp_namespace_components:
|
||||||
} // Namespace ${nsc}
|
} // Namespace ${nsc}
|
||||||
% endfor
|
% endfor
|
||||||
@@ -268,8 +283,6 @@ ${define_yaml_streamer(t.cpp_full_type_name, t.fields)}
|
|||||||
namespace ${nsc}
|
namespace ${nsc}
|
||||||
{
|
{
|
||||||
% endfor
|
% endfor
|
||||||
namespace
|
|
||||||
{
|
|
||||||
|
|
||||||
<%def name="define_streaming_operator(type_name)">
|
<%def name="define_streaming_operator(type_name)">
|
||||||
template <typename Stream>
|
template <typename Stream>
|
||||||
@@ -286,7 +299,6 @@ ${define_streaming_operator(t.cpp_full_type_name + '::Response')}
|
|||||||
${define_streaming_operator(t.cpp_full_type_name)}
|
${define_streaming_operator(t.cpp_full_type_name)}
|
||||||
% endif
|
% endif
|
||||||
|
|
||||||
}
|
|
||||||
% for nsc in t.cpp_namespace_components:
|
% for nsc in t.cpp_namespace_components:
|
||||||
}
|
}
|
||||||
% endfor
|
% endfor
|
||||||
|
|||||||
@@ -161,7 +161,7 @@ def generate_one_type(t):
|
|||||||
c.cpp_value = c.string_value
|
c.cpp_value = c.string_value
|
||||||
else:
|
else:
|
||||||
int(c.string_value) # Making sure that this is a valid integer literal
|
int(c.string_value) # Making sure that this is a valid integer literal
|
||||||
c.cpp_use_enum = c.value >= 0 and c.type.bitlen <= MAX_BITLEN_FOR_ENUM
|
c.cpp_use_enum = c.value >= 0 and c.value.bit_length() <= MAX_BITLEN_FOR_ENUM
|
||||||
c.cpp_value = c.string_value
|
c.cpp_value = c.string_value
|
||||||
if c.type.kind == c.type.KIND_UNSIGNED_INT:
|
if c.type.kind == c.type.KIND_UNSIGNED_INT:
|
||||||
c.cpp_value += 'U'
|
c.cpp_value += 'U'
|
||||||
|
|||||||
@@ -10,6 +10,7 @@
|
|||||||
#include <algorithm>
|
#include <algorithm>
|
||||||
#include <string>
|
#include <string>
|
||||||
#include <uavcan/internal/impl_constants.hpp>
|
#include <uavcan/internal/impl_constants.hpp>
|
||||||
|
#include <uavcan/system_clock.hpp>
|
||||||
|
|
||||||
namespace uavcan
|
namespace uavcan
|
||||||
{
|
{
|
||||||
@@ -87,10 +88,10 @@ public:
|
|||||||
|
|
||||||
/**
|
/**
|
||||||
* Non-blocking transmission.
|
* Non-blocking transmission.
|
||||||
* If the frame wasn't transmitted upon TX timeout expiration, the driver should discard it.
|
* If the frame wasn't transmitted upon TX deadline, the driver should discard it.
|
||||||
* @return 1 = one frame transmitted, 0 = TX buffer full, negative for error.
|
* @return 1 = one frame transmitted, 0 = TX buffer full, negative for error.
|
||||||
*/
|
*/
|
||||||
virtual int send(const CanFrame& frame, uint64_t tx_timeout_usec) = 0;
|
virtual int send(const CanFrame& frame, MonotonicTime tx_deadline) = 0;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Non-blocking reception.
|
* Non-blocking reception.
|
||||||
@@ -100,11 +101,11 @@ public:
|
|||||||
* UTC timestamp is optional, if available it will be used for precise time synchronization;
|
* UTC timestamp is optional, if available it will be used for precise time synchronization;
|
||||||
* must be set to zero if not available.
|
* must be set to zero if not available.
|
||||||
* Refer to @ref ISystemClock to learn more about timestamps.
|
* Refer to @ref ISystemClock to learn more about timestamps.
|
||||||
* @param [out] out_ts_monotonic_usec Monotonic timestamp, usec, mandatory.
|
* @param [out] out_ts_monotonic Monotonic timestamp, mandatory.
|
||||||
* @param [out] out_ts_utc_usec UTC timestamp, usec, optional, zero if unknown.
|
* @param [out] out_ts_utc UTC timestamp, optional, zero if unknown.
|
||||||
* @return 1 = one frame received, 0 = RX buffer empty, negative for error.
|
* @return 1 = one frame received, 0 = RX buffer empty, negative for error.
|
||||||
*/
|
*/
|
||||||
virtual int receive(CanFrame& out_frame, uint64_t& out_ts_monotonic_usec, uint64_t& out_ts_utc_usec) = 0;
|
virtual int receive(CanFrame& out_frame, MonotonicTime& out_ts_monotonic, UtcTime& out_ts_utc) = 0;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Configure the hardware CAN filters. @ref CanFilterConfig.
|
* Configure the hardware CAN filters. @ref CanFilterConfig.
|
||||||
@@ -142,15 +143,15 @@ public:
|
|||||||
virtual int getNumIfaces() const = 0;
|
virtual int getNumIfaces() const = 0;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Block until the blocking timeout expires, or one of the specified interfaces becomes available for read or write.
|
* Block until the deadline, or one of the specified interfaces becomes available for read or write.
|
||||||
* Iface masks will be modified by the driver to indicate which exactly interfaces are available for IO.
|
* Iface masks will be modified by the driver to indicate which exactly interfaces are available for IO.
|
||||||
* Bit position in the masks defines interface index.
|
* Bit position in the masks defines interface index.
|
||||||
* @param [in,out] inout_write_iface_mask Mask indicating which interfaces are needed/available to write.
|
* @param [in,out] inout_write_iface_mask Mask indicating which interfaces are needed/available to write.
|
||||||
* @param [in,out] inout_read_iface_mask Same as above for reading.
|
* @param [in,out] inout_read_iface_mask Same as above for reading.
|
||||||
* @param [in] timeout_usec Zero means non-blocking operation.
|
* @param [in] blocking_deadline Zero means non-blocking operation.
|
||||||
* @return Positive number of ready interfaces or negative error code.
|
* @return Positive number of ready interfaces or negative error code.
|
||||||
*/
|
*/
|
||||||
virtual int select(int& inout_write_iface_mask, int& inout_read_iface_mask, uint64_t timeout_usec) = 0;
|
virtual int select(int& inout_write_iface_mask, int& inout_read_iface_mask, MonotonicTime blocking_deadline) = 0;
|
||||||
};
|
};
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -8,12 +8,13 @@
|
|||||||
#include <cstring>
|
#include <cstring>
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
#include <algorithm>
|
#include <algorithm>
|
||||||
#include <uavcan/internal/transport/crc.hpp>
|
|
||||||
#include <uavcan/internal/transport/transfer.hpp>
|
#include <uavcan/internal/transport/transfer.hpp>
|
||||||
|
|
||||||
namespace uavcan
|
namespace uavcan
|
||||||
{
|
{
|
||||||
|
|
||||||
|
class TransferCRC;
|
||||||
|
|
||||||
enum DataTypeKind
|
enum DataTypeKind
|
||||||
{
|
{
|
||||||
DataTypeKindService,
|
DataTypeKindService,
|
||||||
|
|||||||
@@ -25,16 +25,11 @@ namespace uavcan
|
|||||||
template <typename DataSpec, typename DataStruct>
|
template <typename DataSpec, typename DataStruct>
|
||||||
class GenericPublisher
|
class GenericPublisher
|
||||||
{
|
{
|
||||||
public:
|
|
||||||
enum { DefaultTxTimeoutUsec = 2500 }; // 2500 ms --> 400Hz max
|
|
||||||
enum { MinTxTimeoutUsec = 200 };
|
|
||||||
|
|
||||||
private:
|
|
||||||
enum { Qos = (DataTypeKind(DataSpec::DataTypeKind) == DataTypeKindMessage) ?
|
enum { Qos = (DataTypeKind(DataSpec::DataTypeKind) == DataTypeKindMessage) ?
|
||||||
CanTxQueue::Volatile : CanTxQueue::Persistent };
|
CanTxQueue::Volatile : CanTxQueue::Persistent };
|
||||||
|
|
||||||
const uint64_t max_transfer_interval_; // TODO: memory usage can be reduced
|
const MonotonicDuration max_transfer_interval_; // TODO: memory usage can be reduced
|
||||||
uint64_t tx_timeout_;
|
MonotonicDuration tx_timeout_;
|
||||||
Scheduler& scheduler_;
|
Scheduler& scheduler_;
|
||||||
IMarshalBufferProvider& buffer_provider_;
|
IMarshalBufferProvider& buffer_provider_;
|
||||||
LazyConstructor<TransferSender> sender_;
|
LazyConstructor<TransferSender> sender_;
|
||||||
@@ -54,12 +49,12 @@ private:
|
|||||||
UAVCAN_TRACE("GenericPublisher", "Type [%s] is not registered", DataSpec::getDataTypeFullName());
|
UAVCAN_TRACE("GenericPublisher", "Type [%s] is not registered", DataSpec::getDataTypeFullName());
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
sender_.template construct<Dispatcher&, const DataTypeDescriptor&, CanTxQueue::Qos, uint64_t>
|
sender_.template construct<Dispatcher&, const DataTypeDescriptor&, CanTxQueue::Qos, MonotonicDuration>
|
||||||
(scheduler_.getDispatcher(), *descr, CanTxQueue::Qos(Qos), max_transfer_interval_);
|
(scheduler_.getDispatcher(), *descr, CanTxQueue::Qos(Qos), max_transfer_interval_);
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
uint64_t getTxDeadline() const { return scheduler_.getMonotonicTimestamp() + tx_timeout_; }
|
MonotonicTime getTxDeadline() const { return scheduler_.getMonotonicTimestamp() + tx_timeout_; }
|
||||||
|
|
||||||
IMarshalBuffer* getBuffer()
|
IMarshalBuffer* getBuffer()
|
||||||
{
|
{
|
||||||
@@ -67,7 +62,7 @@ private:
|
|||||||
}
|
}
|
||||||
|
|
||||||
int genericPublish(const DataStruct& message, TransferType transfer_type, NodeID dst_node_id,
|
int genericPublish(const DataStruct& message, TransferType transfer_type, NodeID dst_node_id,
|
||||||
TransferID* tid, uint64_t monotonic_blocking_deadline)
|
TransferID* tid, MonotonicTime blocking_deadline)
|
||||||
{
|
{
|
||||||
if (!checkInit())
|
if (!checkInit())
|
||||||
return -1;
|
return -1;
|
||||||
@@ -89,20 +84,20 @@ private:
|
|||||||
if (tid)
|
if (tid)
|
||||||
{
|
{
|
||||||
return sender_->send(buf->getDataPtr(), buf->getDataLength(), getTxDeadline(),
|
return sender_->send(buf->getDataPtr(), buf->getDataLength(), getTxDeadline(),
|
||||||
monotonic_blocking_deadline, transfer_type, dst_node_id, *tid);
|
blocking_deadline, transfer_type, dst_node_id, *tid);
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
return sender_->send(buf->getDataPtr(), buf->getDataLength(), getTxDeadline(),
|
return sender_->send(buf->getDataPtr(), buf->getDataLength(), getTxDeadline(),
|
||||||
monotonic_blocking_deadline, transfer_type, dst_node_id);
|
blocking_deadline, transfer_type, dst_node_id);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
GenericPublisher(Scheduler& scheduler, IMarshalBufferProvider& buffer_provider,
|
GenericPublisher(Scheduler& scheduler, IMarshalBufferProvider& buffer_provider,
|
||||||
uint64_t max_transfer_interval = TransferSender::DefaultMaxTransferInterval)
|
MonotonicDuration max_transfer_interval = TransferSender::getDefaultMaxTransferInterval())
|
||||||
: max_transfer_interval_(max_transfer_interval)
|
: max_transfer_interval_(max_transfer_interval)
|
||||||
, tx_timeout_(DefaultTxTimeoutUsec)
|
, tx_timeout_(getDefaultTxTimeout())
|
||||||
, scheduler_(scheduler)
|
, scheduler_(scheduler)
|
||||||
, buffer_provider_(buffer_provider)
|
, buffer_provider_(buffer_provider)
|
||||||
{ }
|
{ }
|
||||||
@@ -110,22 +105,25 @@ protected:
|
|||||||
~GenericPublisher() { }
|
~GenericPublisher() { }
|
||||||
|
|
||||||
int publish(const DataStruct& message, TransferType transfer_type, NodeID dst_node_id,
|
int publish(const DataStruct& message, TransferType transfer_type, NodeID dst_node_id,
|
||||||
uint64_t monotonic_blocking_deadline = 0)
|
MonotonicTime blocking_deadline = MonotonicTime())
|
||||||
{
|
{
|
||||||
return genericPublish(message, transfer_type, dst_node_id, NULL, monotonic_blocking_deadline);
|
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,
|
int publish(const DataStruct& message, TransferType transfer_type, NodeID dst_node_id, TransferID tid,
|
||||||
uint64_t monotonic_blocking_deadline = 0)
|
MonotonicTime blocking_deadline = MonotonicTime())
|
||||||
{
|
{
|
||||||
return genericPublish(message, transfer_type, dst_node_id, &tid, monotonic_blocking_deadline);
|
return genericPublish(message, transfer_type, dst_node_id, &tid, blocking_deadline);
|
||||||
}
|
}
|
||||||
|
|
||||||
public:
|
public:
|
||||||
uint64_t getTxTimeout() const { return tx_timeout_; }
|
static MonotonicDuration getDefaultTxTimeout() { return MonotonicDuration::fromUSec(2500); }// 2500ms --> 400Hz max
|
||||||
void setTxTimeout(uint64_t usec)
|
static MonotonicDuration getMinTxTimeout() { return MonotonicDuration::fromUSec(200); }
|
||||||
|
|
||||||
|
MonotonicDuration getTxTimeout() const { return tx_timeout_; }
|
||||||
|
void setTxTimeout(MonotonicDuration tx_timeout)
|
||||||
{
|
{
|
||||||
tx_timeout_ = std::max(usec, uint64_t(MinTxTimeoutUsec));
|
tx_timeout_ = std::max(tx_timeout, getMinTxTimeout());
|
||||||
}
|
}
|
||||||
|
|
||||||
Scheduler& getScheduler() const { return scheduler_; }
|
Scheduler& getScheduler() const { return scheduler_; }
|
||||||
|
|||||||
@@ -45,8 +45,11 @@ protected:
|
|||||||
public:
|
public:
|
||||||
typedef DataType_ DataType;
|
typedef DataType_ DataType;
|
||||||
|
|
||||||
uint64_t getMonotonicTimestamp() const { return safeget<uint64_t, &IncomingTransfer::getMonotonicTimestamp>(); }
|
MonotonicTime getMonotonicTimestamp() const
|
||||||
uint64_t getUtcTimestamp() const { return safeget<uint64_t, &IncomingTransfer::getUtcTimestamp>(); }
|
{
|
||||||
|
return safeget<MonotonicTime, &IncomingTransfer::getMonotonicTimestamp>();
|
||||||
|
}
|
||||||
|
UtcTime getUtcTimestamp() const { return safeget<UtcTime, &IncomingTransfer::getUtcTimestamp>(); }
|
||||||
TransferType getTransferType() const { return safeget<TransferType, &IncomingTransfer::getTransferType>(); }
|
TransferType getTransferType() const { return safeget<TransferType, &IncomingTransfer::getTransferType>(); }
|
||||||
TransferID getTransferID() const { return safeget<TransferID, &IncomingTransfer::getTransferID>(); }
|
TransferID getTransferID() const { return safeget<TransferID, &IncomingTransfer::getTransferID>(); }
|
||||||
NodeID getSrcNodeID() const { return safeget<NodeID, &IncomingTransfer::getSrcNodeID>(); }
|
NodeID getSrcNodeID() const { return safeget<NodeID, &IncomingTransfer::getSrcNodeID>(); }
|
||||||
|
|||||||
@@ -14,29 +14,28 @@ class Scheduler;
|
|||||||
|
|
||||||
class MonotonicDeadlineHandler : public LinkedListNode<MonotonicDeadlineHandler>, Noncopyable
|
class MonotonicDeadlineHandler : public LinkedListNode<MonotonicDeadlineHandler>, Noncopyable
|
||||||
{
|
{
|
||||||
uint64_t monotonic_deadline_;
|
MonotonicTime deadline_;
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
Scheduler& scheduler_;
|
Scheduler& scheduler_;
|
||||||
|
|
||||||
explicit MonotonicDeadlineHandler(Scheduler& scheduler)
|
explicit MonotonicDeadlineHandler(Scheduler& scheduler)
|
||||||
: monotonic_deadline_(0)
|
: scheduler_(scheduler)
|
||||||
, scheduler_(scheduler)
|
|
||||||
{ }
|
{ }
|
||||||
|
|
||||||
virtual ~MonotonicDeadlineHandler() { stop(); }
|
virtual ~MonotonicDeadlineHandler() { stop(); }
|
||||||
|
|
||||||
public:
|
public:
|
||||||
virtual void handleMonotonicDeadline(uint64_t monotonic_timestamp) = 0;
|
virtual void handleDeadline(MonotonicTime current_timestamp) = 0;
|
||||||
|
|
||||||
void startWithDeadline(uint64_t monotonic_deadline);
|
void startWithDeadline(MonotonicTime deadline);
|
||||||
void startWithDelay(uint64_t delay_usec);
|
void startWithDelay(MonotonicDuration delay);
|
||||||
|
|
||||||
void stop();
|
void stop();
|
||||||
|
|
||||||
bool isRunning() const;
|
bool isRunning() const;
|
||||||
|
|
||||||
uint64_t getMonotonicDeadline() const { return monotonic_deadline_; }
|
MonotonicTime getDeadline() const { return deadline_; }
|
||||||
Scheduler& getScheduler() const { return scheduler_; }
|
Scheduler& getScheduler() const { return scheduler_; }
|
||||||
};
|
};
|
||||||
|
|
||||||
@@ -51,8 +50,8 @@ public:
|
|||||||
bool doesExist(const MonotonicDeadlineHandler* mdh) const;
|
bool doesExist(const MonotonicDeadlineHandler* mdh) const;
|
||||||
unsigned int getNumHandlers() const { return handlers_.getLength(); }
|
unsigned int getNumHandlers() const { return handlers_.getLength(); }
|
||||||
|
|
||||||
uint64_t pollAndGetMonotonicTimestamp(ISystemClock& sysclock);
|
MonotonicTime pollAndGetMonotonicTimestamp(ISystemClock& sysclock);
|
||||||
uint64_t getEarliestDeadline() const;
|
MonotonicTime getEarliestDeadline() const;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
@@ -68,45 +67,45 @@ class Scheduler : Noncopyable
|
|||||||
|
|
||||||
MonotonicDeadlineScheduler deadline_scheduler_;
|
MonotonicDeadlineScheduler deadline_scheduler_;
|
||||||
Dispatcher dispatcher_;
|
Dispatcher dispatcher_;
|
||||||
uint64_t prev_cleanup_ts_;
|
MonotonicTime prev_cleanup_ts_;
|
||||||
uint64_t monotonic_deadline_resolution_;
|
MonotonicDuration deadline_resolution_;
|
||||||
uint64_t cleanup_period_;
|
MonotonicDuration cleanup_period_;
|
||||||
|
|
||||||
uint64_t computeDispatcherSpinDeadline(uint64_t spin_deadline) const;
|
MonotonicTime computeDispatcherSpinDeadline(MonotonicTime spin_deadline) const;
|
||||||
void pollCleanup(uint64_t mono_ts, uint32_t num_frames_processed_with_last_spin);
|
void pollCleanup(MonotonicTime mono_ts, uint32_t num_frames_processed_with_last_spin);
|
||||||
|
|
||||||
public:
|
public:
|
||||||
Scheduler(ICanDriver& can_driver, IAllocator& allocator, ISystemClock& sysclock, IOutgoingTransferRegistry& otr,
|
Scheduler(ICanDriver& can_driver, IAllocator& allocator, ISystemClock& sysclock, IOutgoingTransferRegistry& otr,
|
||||||
NodeID self_node_id)
|
NodeID self_node_id)
|
||||||
: dispatcher_(can_driver, allocator, sysclock, otr, self_node_id)
|
: dispatcher_(can_driver, allocator, sysclock, otr, self_node_id)
|
||||||
, prev_cleanup_ts_(sysclock.getMonotonicMicroseconds())
|
, prev_cleanup_ts_(sysclock.getMonotonic())
|
||||||
, monotonic_deadline_resolution_(DefaultMonotonicDeadlineResolutionMs * 1000)
|
, deadline_resolution_(MonotonicDuration::fromMSec(DefaultMonotonicDeadlineResolutionMs))
|
||||||
, cleanup_period_(DefaultCleanupPeriodMs * 1000)
|
, cleanup_period_(MonotonicDuration::fromMSec(DefaultCleanupPeriodMs))
|
||||||
{ }
|
{ }
|
||||||
|
|
||||||
int spin(uint64_t monotonic_deadline);
|
int spin(MonotonicTime deadline);
|
||||||
|
|
||||||
MonotonicDeadlineScheduler& getMonotonicDeadlineScheduler() { return deadline_scheduler_; }
|
MonotonicDeadlineScheduler& getMonotonicDeadlineScheduler() { return deadline_scheduler_; }
|
||||||
Dispatcher& getDispatcher() { return dispatcher_; }
|
Dispatcher& getDispatcher() { return dispatcher_; }
|
||||||
|
|
||||||
ISystemClock& getSystemClock() { return dispatcher_.getSystemClock(); }
|
ISystemClock& getSystemClock() { return dispatcher_.getSystemClock(); }
|
||||||
uint64_t getMonotonicTimestamp() const { return dispatcher_.getSystemClock().getMonotonicMicroseconds(); }
|
MonotonicTime getMonotonicTimestamp() const { return dispatcher_.getSystemClock().getMonotonic(); }
|
||||||
uint64_t getUtcTimestamp() const { return dispatcher_.getSystemClock().getUtcMicroseconds(); }
|
UtcTime getUtcTimestamp() const { return dispatcher_.getSystemClock().getUtc(); }
|
||||||
|
|
||||||
uint64_t getMonotonicDeadlineResolution() const { return monotonic_deadline_resolution_; }
|
MonotonicDuration getMonotonicDeadlineResolution() const { return deadline_resolution_; }
|
||||||
void setMonotonicDeadlineResolution(uint64_t res_usec)
|
void setMonotonicDeadlineResolution(MonotonicDuration res)
|
||||||
{
|
{
|
||||||
res_usec = std::min(res_usec, MaxMonotonicDeadlineResolutionMs * uint64_t(1000));
|
res = std::min(res, MonotonicDuration::fromMSec(MaxMonotonicDeadlineResolutionMs));
|
||||||
res_usec = std::max(res_usec, MinMonotonicDeadlineResolutionMs * uint64_t(1000));
|
res = std::max(res, MonotonicDuration::fromMSec(MinMonotonicDeadlineResolutionMs));
|
||||||
monotonic_deadline_resolution_ = res_usec;
|
deadline_resolution_ = res;
|
||||||
}
|
}
|
||||||
|
|
||||||
uint64_t getCleanupPeriod() const { return cleanup_period_; }
|
MonotonicDuration getCleanupPeriod() const { return cleanup_period_; }
|
||||||
void setCleanupPeriod(uint64_t period_usec)
|
void setCleanupPeriod(MonotonicDuration period)
|
||||||
{
|
{
|
||||||
period_usec = std::min(period_usec, MaxCleanupPeriodMs * uint64_t(1000));
|
period = std::min(period, MonotonicDuration::fromMSec(MaxCleanupPeriodMs));
|
||||||
period_usec = std::max(period_usec, MinCleanupPeriodMs * uint64_t(1000));
|
period = std::max(period, MonotonicDuration::fromMSec(MinCleanupPeriodMs));
|
||||||
cleanup_period_ = period_usec;
|
cleanup_period_ = period;
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|||||||
@@ -13,20 +13,19 @@
|
|||||||
#include <uavcan/util/compile_time.hpp>
|
#include <uavcan/util/compile_time.hpp>
|
||||||
#include <uavcan/can_driver.hpp>
|
#include <uavcan/can_driver.hpp>
|
||||||
#include <uavcan/system_clock.hpp>
|
#include <uavcan/system_clock.hpp>
|
||||||
|
#include <uavcan/time.hpp>
|
||||||
|
|
||||||
namespace uavcan
|
namespace uavcan
|
||||||
{
|
{
|
||||||
|
|
||||||
struct CanRxFrame : public CanFrame
|
struct CanRxFrame : public CanFrame
|
||||||
{
|
{
|
||||||
uint64_t ts_monotonic;
|
MonotonicTime ts_mono;
|
||||||
uint64_t ts_utc;
|
UtcTime ts_utc;
|
||||||
uint8_t iface_index;
|
uint8_t iface_index;
|
||||||
|
|
||||||
CanRxFrame()
|
CanRxFrame()
|
||||||
: ts_monotonic(0)
|
: iface_index(0)
|
||||||
, ts_utc(0)
|
|
||||||
, iface_index(0)
|
|
||||||
{ }
|
{ }
|
||||||
|
|
||||||
std::string toString(StringRepresentation mode = StrTight) const;
|
std::string toString(StringRepresentation mode = StrTight) const;
|
||||||
@@ -40,12 +39,12 @@ public:
|
|||||||
|
|
||||||
struct Entry : public LinkedListNode<Entry> // Not required to be packed - fits the block in any case
|
struct Entry : public LinkedListNode<Entry> // Not required to be packed - fits the block in any case
|
||||||
{
|
{
|
||||||
uint64_t monotonic_deadline;
|
MonotonicTime deadline;
|
||||||
CanFrame frame;
|
CanFrame frame;
|
||||||
uint8_t qos;
|
uint8_t qos;
|
||||||
|
|
||||||
Entry(const CanFrame& frame, uint64_t monotonic_deadline, Qos qos)
|
Entry(const CanFrame& frame, MonotonicTime deadline, Qos qos)
|
||||||
: monotonic_deadline(monotonic_deadline)
|
: deadline(deadline)
|
||||||
, frame(frame)
|
, frame(frame)
|
||||||
, qos(uint8_t(qos))
|
, qos(uint8_t(qos))
|
||||||
{
|
{
|
||||||
@@ -55,7 +54,7 @@ public:
|
|||||||
|
|
||||||
static void destroy(Entry*& obj, IAllocator& allocator);
|
static void destroy(Entry*& obj, IAllocator& allocator);
|
||||||
|
|
||||||
bool isExpired(uint64_t monotonic_timestamp) const { return monotonic_timestamp > monotonic_deadline; }
|
bool isExpired(MonotonicTime timestamp) const { return timestamp > deadline; }
|
||||||
|
|
||||||
bool qosHigherThan(const CanFrame& rhs_frame, Qos rhs_qos) const;
|
bool qosHigherThan(const CanFrame& rhs_frame, Qos rhs_qos) const;
|
||||||
bool qosLowerThan(const CanFrame& rhs_frame, Qos rhs_qos) const;
|
bool qosLowerThan(const CanFrame& rhs_frame, Qos rhs_qos) const;
|
||||||
@@ -100,7 +99,7 @@ public:
|
|||||||
|
|
||||||
~CanTxQueue();
|
~CanTxQueue();
|
||||||
|
|
||||||
void push(const CanFrame& frame, uint64_t monotonic_tx_deadline, Qos qos);
|
void push(const CanFrame& frame, MonotonicTime tx_deadline, Qos qos);
|
||||||
|
|
||||||
Entry* peek(); // Modifier
|
Entry* peek(); // Modifier
|
||||||
void remove(Entry*& entry);
|
void remove(Entry*& entry);
|
||||||
@@ -128,10 +127,9 @@ private:
|
|||||||
CanIOManager(CanIOManager&);
|
CanIOManager(CanIOManager&);
|
||||||
CanIOManager& operator=(CanIOManager&);
|
CanIOManager& operator=(CanIOManager&);
|
||||||
|
|
||||||
int sendToIface(int iface_index, const CanFrame& frame, uint64_t monotonic_tx_deadline);
|
int sendToIface(int iface_index, const CanFrame& frame, MonotonicTime tx_deadline);
|
||||||
int sendFromTxQueue(int iface_index);
|
int sendFromTxQueue(int iface_index);
|
||||||
int makePendingTxMask() const;
|
int makePendingTxMask() const;
|
||||||
uint64_t getTimeUntilMonotonicDeadline(uint64_t monotonic_deadline) const;
|
|
||||||
|
|
||||||
public:
|
public:
|
||||||
CanIOManager(ICanDriver& driver, IAllocator& allocator, ISystemClock& sysclock)
|
CanIOManager(ICanDriver& driver, IAllocator& allocator, ISystemClock& sysclock)
|
||||||
@@ -157,9 +155,9 @@ public:
|
|||||||
* 1+ - sent/received
|
* 1+ - sent/received
|
||||||
* negative - failure
|
* negative - failure
|
||||||
*/
|
*/
|
||||||
int send(const CanFrame& frame, uint64_t monotonic_tx_deadline, uint64_t monotonic_blocking_deadline,
|
int send(const CanFrame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline,
|
||||||
int iface_mask, CanTxQueue::Qos qos);
|
int iface_mask, CanTxQueue::Qos qos);
|
||||||
int receive(CanRxFrame& frame, uint64_t monotonic_blocking_deadline);
|
int receive(CanRxFrame& frame, MonotonicTime blocking_deadline);
|
||||||
};
|
};
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -41,7 +41,7 @@ class Dispatcher : Noncopyable
|
|||||||
|
|
||||||
bool add(TransferListenerBase* listener, Mode mode);
|
bool add(TransferListenerBase* listener, Mode mode);
|
||||||
void remove(TransferListenerBase* listener);
|
void remove(TransferListenerBase* listener);
|
||||||
void cleanup(uint64_t ts_monotonic);
|
void cleanup(MonotonicTime ts);
|
||||||
void handleFrame(const RxFrame& frame);
|
void handleFrame(const RxFrame& frame);
|
||||||
|
|
||||||
int getNumEntries() const { return list_.getLength(); }
|
int getNumEntries() const { return list_.getLength(); }
|
||||||
@@ -64,15 +64,14 @@ public:
|
|||||||
, self_node_id_(self_node_id)
|
, self_node_id_(self_node_id)
|
||||||
{ }
|
{ }
|
||||||
|
|
||||||
int spin(uint64_t monotonic_deadline);
|
int spin(MonotonicTime deadline);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Refer to CanIOManager::send() for the parameter description
|
* Refer to CanIOManager::send() for the parameter description
|
||||||
*/
|
*/
|
||||||
int send(const Frame& frame, uint64_t monotonic_tx_deadline, uint64_t monotonic_blocking_deadline,
|
int send(const Frame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline, CanTxQueue::Qos qos);
|
||||||
CanTxQueue::Qos qos);
|
|
||||||
|
|
||||||
void cleanup(uint64_t ts_monotonic);
|
void cleanup(MonotonicTime ts);
|
||||||
|
|
||||||
bool registerMessageListener(TransferListenerBase* listener);
|
bool registerMessageListener(TransferListenerBase* listener);
|
||||||
bool registerServiceRequestListener(TransferListenerBase* listener);
|
bool registerServiceRequestListener(TransferListenerBase* listener);
|
||||||
|
|||||||
@@ -54,8 +54,8 @@ class IOutgoingTransferRegistry
|
|||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
virtual ~IOutgoingTransferRegistry() { }
|
virtual ~IOutgoingTransferRegistry() { }
|
||||||
virtual TransferID* accessOrCreate(const OutgoingTransferRegistryKey& key, uint64_t new_monotonic_deadline) = 0;
|
virtual TransferID* accessOrCreate(const OutgoingTransferRegistryKey& key, MonotonicTime new_deadline) = 0;
|
||||||
virtual void cleanup(uint64_t monotonic_deadline) = 0;
|
virtual void cleanup(MonotonicTime deadline) = 0;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
@@ -65,26 +65,25 @@ class OutgoingTransferRegistry : public IOutgoingTransferRegistry, Noncopyable
|
|||||||
UAVCAN_PACKED_BEGIN
|
UAVCAN_PACKED_BEGIN
|
||||||
struct Value
|
struct Value
|
||||||
{
|
{
|
||||||
uint64_t monotonic_deadline;
|
MonotonicTime deadline;
|
||||||
TransferID tid;
|
TransferID tid;
|
||||||
Value() : monotonic_deadline(0) { }
|
|
||||||
};
|
};
|
||||||
UAVCAN_PACKED_END
|
UAVCAN_PACKED_END
|
||||||
|
|
||||||
class DeadlineExpiredPredicate
|
class DeadlineExpiredPredicate
|
||||||
{
|
{
|
||||||
const uint64_t ts_monotonic_;
|
const MonotonicTime ts_;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
DeadlineExpiredPredicate(uint64_t ts_monotonic)
|
DeadlineExpiredPredicate(MonotonicTime ts)
|
||||||
: ts_monotonic_(ts_monotonic)
|
: ts_(ts)
|
||||||
{ }
|
{ }
|
||||||
|
|
||||||
bool operator()(const OutgoingTransferRegistryKey& key, const Value& value) const
|
bool operator()(const OutgoingTransferRegistryKey& key, const Value& value) const
|
||||||
{
|
{
|
||||||
(void)key;
|
(void)key;
|
||||||
assert(value.monotonic_deadline > 0);
|
assert(!value.deadline.isZero());
|
||||||
return value.monotonic_deadline <= ts_monotonic_;
|
return value.deadline <= ts_;
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
@@ -95,21 +94,21 @@ public:
|
|||||||
: map_(allocator)
|
: map_(allocator)
|
||||||
{ }
|
{ }
|
||||||
|
|
||||||
TransferID* accessOrCreate(const OutgoingTransferRegistryKey& key, uint64_t new_monotonic_deadline)
|
TransferID* accessOrCreate(const OutgoingTransferRegistryKey& key, MonotonicTime new_deadline)
|
||||||
{
|
{
|
||||||
assert(new_monotonic_deadline > 0);
|
assert(!new_deadline.isZero());
|
||||||
Value* p = map_.access(key);
|
Value* p = map_.access(key);
|
||||||
if (p == NULL)
|
if (p == NULL)
|
||||||
p = map_.insert(key, Value());
|
p = map_.insert(key, Value());
|
||||||
if (p == NULL)
|
if (p == NULL)
|
||||||
return NULL;
|
return NULL;
|
||||||
p->monotonic_deadline = new_monotonic_deadline;
|
p->deadline = new_deadline;
|
||||||
return &p->tid;
|
return &p->tid;
|
||||||
}
|
}
|
||||||
|
|
||||||
void cleanup(uint64_t ts_monotonic)
|
void cleanup(MonotonicTime ts)
|
||||||
{
|
{
|
||||||
map_.removeWhere(DeadlineExpiredPredicate(ts_monotonic));
|
map_.removeWhere(DeadlineExpiredPredicate(ts));
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|||||||
@@ -7,11 +7,13 @@
|
|||||||
#include <cassert>
|
#include <cassert>
|
||||||
#include <algorithm>
|
#include <algorithm>
|
||||||
#include <string>
|
#include <string>
|
||||||
#include <uavcan/internal/transport/can_io.hpp>
|
#include <uavcan/can_driver.hpp>
|
||||||
|
|
||||||
namespace uavcan
|
namespace uavcan
|
||||||
{
|
{
|
||||||
|
|
||||||
|
struct CanRxFrame;
|
||||||
|
|
||||||
enum { MaxTransferPayloadLen = 439 }; ///< According to the standard
|
enum { MaxTransferPayloadLen = 439 }; ///< According to the standard
|
||||||
|
|
||||||
enum { MaxSingleFrameTransferPayloadLen = 7 };
|
enum { MaxSingleFrameTransferPayloadLen = 7 };
|
||||||
@@ -175,37 +177,27 @@ public:
|
|||||||
|
|
||||||
class RxFrame : public Frame
|
class RxFrame : public Frame
|
||||||
{
|
{
|
||||||
uint64_t ts_monotonic_;
|
MonotonicTime ts_mono_;
|
||||||
uint64_t ts_utc_;
|
UtcTime ts_utc_;
|
||||||
uint8_t iface_index_;
|
uint8_t iface_index_;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
RxFrame()
|
RxFrame()
|
||||||
: ts_monotonic_(0)
|
: iface_index_(0)
|
||||||
, ts_utc_(0)
|
|
||||||
, iface_index_(0)
|
|
||||||
{ }
|
{ }
|
||||||
|
|
||||||
RxFrame(const Frame& frame, uint64_t ts_monotonic, uint64_t ts_utc, uint8_t iface_index)
|
RxFrame(const Frame& frame, MonotonicTime ts_mono, UtcTime ts_utc, uint8_t iface_index)
|
||||||
: ts_monotonic_(ts_monotonic)
|
: ts_mono_(ts_mono)
|
||||||
, ts_utc_(ts_utc)
|
, ts_utc_(ts_utc)
|
||||||
, iface_index_(iface_index)
|
, iface_index_(iface_index)
|
||||||
{
|
{
|
||||||
*static_cast<Frame*>(this) = frame;
|
*static_cast<Frame*>(this) = frame;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool parse(const CanRxFrame& can_frame)
|
bool parse(const CanRxFrame& can_frame);
|
||||||
{
|
|
||||||
if (!Frame::parse(can_frame))
|
|
||||||
return false;
|
|
||||||
ts_monotonic_ = can_frame.ts_monotonic;
|
|
||||||
ts_utc_ = can_frame.ts_utc;
|
|
||||||
iface_index_ = can_frame.iface_index;
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
uint64_t getMonotonicTimestamp() const { return ts_monotonic_; }
|
MonotonicTime getMonotonicTimestamp() const { return ts_mono_; }
|
||||||
uint64_t getUtcTimestamp() const { return ts_utc_; }
|
UtcTime getUtcTimestamp() const { return ts_utc_; }
|
||||||
|
|
||||||
uint8_t getIfaceIndex() const { return iface_index_; }
|
uint8_t getIfaceIndex() const { return iface_index_; }
|
||||||
|
|
||||||
|
|||||||
@@ -9,6 +9,7 @@
|
|||||||
#include <limits>
|
#include <limits>
|
||||||
#include <uavcan/internal/transport/transfer.hpp>
|
#include <uavcan/internal/transport/transfer.hpp>
|
||||||
#include <uavcan/internal/linked_list.hpp>
|
#include <uavcan/internal/linked_list.hpp>
|
||||||
|
#include <uavcan/internal/dynamic_memory.hpp>
|
||||||
#include <uavcan/internal/impl_constants.hpp>
|
#include <uavcan/internal/impl_constants.hpp>
|
||||||
#include <uavcan/internal/debug.hpp>
|
#include <uavcan/internal/debug.hpp>
|
||||||
|
|
||||||
|
|||||||
@@ -21,8 +21,8 @@ namespace uavcan
|
|||||||
*/
|
*/
|
||||||
class IncomingTransfer : public ITransferBuffer
|
class IncomingTransfer : public ITransferBuffer
|
||||||
{
|
{
|
||||||
uint64_t ts_monotonic_;
|
MonotonicTime ts_mono_;
|
||||||
uint64_t ts_utc_;
|
UtcTime ts_utc_;
|
||||||
TransferType transfer_type_;
|
TransferType transfer_type_;
|
||||||
TransferID transfer_id_;
|
TransferID transfer_id_;
|
||||||
NodeID src_node_id_;
|
NodeID src_node_id_;
|
||||||
@@ -31,9 +31,9 @@ class IncomingTransfer : public ITransferBuffer
|
|||||||
int write(unsigned int offset, const uint8_t* data, unsigned int len);
|
int write(unsigned int offset, const uint8_t* data, unsigned int len);
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
IncomingTransfer(uint64_t ts_monotonic, uint64_t ts_utc, TransferType transfer_type,
|
IncomingTransfer(MonotonicTime ts_mono, UtcTime ts_utc, TransferType transfer_type,
|
||||||
TransferID transfer_id, NodeID source_node_id)
|
TransferID transfer_id, NodeID source_node_id)
|
||||||
: ts_monotonic_(ts_monotonic)
|
: ts_mono_(ts_mono)
|
||||||
, ts_utc_(ts_utc)
|
, ts_utc_(ts_utc)
|
||||||
, transfer_type_(transfer_type)
|
, transfer_type_(transfer_type)
|
||||||
, transfer_id_(transfer_id)
|
, transfer_id_(transfer_id)
|
||||||
@@ -46,11 +46,11 @@ public:
|
|||||||
*/
|
*/
|
||||||
virtual void release() { }
|
virtual void release() { }
|
||||||
|
|
||||||
uint64_t getMonotonicTimestamp() const { return ts_monotonic_; }
|
MonotonicTime getMonotonicTimestamp() const { return ts_mono_; }
|
||||||
uint64_t getUtcTimestamp() const { return ts_utc_; }
|
UtcTime getUtcTimestamp() const { return ts_utc_; }
|
||||||
TransferType getTransferType() const { return transfer_type_; }
|
TransferType getTransferType() const { return transfer_type_; }
|
||||||
TransferID getTransferID() const { return transfer_id_; }
|
TransferID getTransferID() const { return transfer_id_; }
|
||||||
NodeID getSrcNodeID() const { return src_node_id_; }
|
NodeID getSrcNodeID() const { return src_node_id_; }
|
||||||
};
|
};
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@@ -72,7 +72,7 @@ class MultiFrameIncomingTransfer : public IncomingTransfer, Noncopyable
|
|||||||
{
|
{
|
||||||
TransferBufferAccessor& buf_acc_;
|
TransferBufferAccessor& buf_acc_;
|
||||||
public:
|
public:
|
||||||
MultiFrameIncomingTransfer(uint64_t ts_monotonic, uint64_t ts_utc, const RxFrame& last_frame,
|
MultiFrameIncomingTransfer(MonotonicTime ts_mono, UtcTime ts_utc, const RxFrame& last_frame,
|
||||||
TransferBufferAccessor& tba);
|
TransferBufferAccessor& tba);
|
||||||
int read(unsigned int offset, uint8_t* data, unsigned int len) const;
|
int read(unsigned int offset, uint8_t* data, unsigned int len) const;
|
||||||
void release() { buf_acc_.remove(); }
|
void release() { buf_acc_.remove(); }
|
||||||
@@ -104,7 +104,7 @@ public:
|
|||||||
const DataTypeDescriptor& getDataTypeDescriptor() const { return data_type_; }
|
const DataTypeDescriptor& getDataTypeDescriptor() const { return data_type_; }
|
||||||
|
|
||||||
virtual void handleFrame(const RxFrame& frame) = 0;
|
virtual void handleFrame(const RxFrame& frame) = 0;
|
||||||
virtual void cleanup(uint64_t ts_monotonic) = 0;
|
virtual void cleanup(MonotonicTime ts) = 0;
|
||||||
};
|
};
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@@ -141,18 +141,18 @@ class TransferListener : public TransferListenerBase, Noncopyable
|
|||||||
|
|
||||||
class TimedOutReceiverPredicate
|
class TimedOutReceiverPredicate
|
||||||
{
|
{
|
||||||
const uint64_t ts_monotonic_;
|
const MonotonicTime ts_;
|
||||||
BufferManager& bufmgr_;
|
BufferManager& bufmgr_;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
TimedOutReceiverPredicate(uint64_t ts_monotonic, BufferManager& bufmgr)
|
TimedOutReceiverPredicate(MonotonicTime ts, BufferManager& bufmgr)
|
||||||
: ts_monotonic_(ts_monotonic)
|
: ts_(ts)
|
||||||
, bufmgr_(bufmgr)
|
, bufmgr_(bufmgr)
|
||||||
{ }
|
{ }
|
||||||
|
|
||||||
bool operator()(const TransferBufferManagerKey& key, const TransferReceiver& value) const
|
bool operator()(const TransferBufferManagerKey& key, const TransferReceiver& value) const
|
||||||
{
|
{
|
||||||
if (value.isTimedOut(ts_monotonic_))
|
if (value.isTimedOut(ts_))
|
||||||
{
|
{
|
||||||
UAVCAN_TRACE("TransferListener", "Timed out receiver: %s", key.toString().c_str());
|
UAVCAN_TRACE("TransferListener", "Timed out receiver: %s", key.toString().c_str());
|
||||||
/*
|
/*
|
||||||
@@ -168,9 +168,9 @@ class TransferListener : public TransferListenerBase, Noncopyable
|
|||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
void cleanup(uint64_t ts_monotonic)
|
void cleanup(MonotonicTime ts)
|
||||||
{
|
{
|
||||||
receivers_.removeWhere(TimedOutReceiverPredicate(ts_monotonic, bufmgr_));
|
receivers_.removeWhere(TimedOutReceiverPredicate(ts, bufmgr_));
|
||||||
assert(receivers_.isEmpty() ? bufmgr_.isEmpty() : 1);
|
assert(receivers_.isEmpty() ? bufmgr_.isEmpty() : 1);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -17,18 +17,25 @@ class TransferReceiver
|
|||||||
public:
|
public:
|
||||||
enum ResultCode { ResultNotComplete, ResultComplete, ResultSingleFrame };
|
enum ResultCode { ResultNotComplete, ResultComplete, ResultSingleFrame };
|
||||||
|
|
||||||
static const uint32_t DefaultTransferInterval = 500 * 1000UL;
|
static const uint32_t DefaultTransferIntervalUSec = 500 * 1000UL;
|
||||||
static const uint32_t MinTransferInterval = 1 * 1000UL;
|
static const uint32_t MinTransferIntervalUSec = 1 * 1000UL;
|
||||||
static const uint32_t MaxTransferInterval = 10 * 1000 * 1000UL;
|
static const uint32_t MaxTransferIntervalUSec = 10 * 1000 * 1000UL;
|
||||||
|
|
||||||
|
static MonotonicDuration getDefaultTransferInterval()
|
||||||
|
{
|
||||||
|
return MonotonicDuration::fromUSec(DefaultTransferIntervalUSec);
|
||||||
|
}
|
||||||
|
static MonotonicDuration getMinTransferInterval() { return MonotonicDuration::fromUSec(MinTransferIntervalUSec); }
|
||||||
|
static MonotonicDuration getMaxTransferInterval() { return MonotonicDuration::fromUSec(MaxTransferIntervalUSec); }
|
||||||
|
|
||||||
private:
|
private:
|
||||||
enum TidRelation { TidSame, TidRepeat, TidFuture };
|
enum TidRelation { TidSame, TidRepeat, TidFuture };
|
||||||
enum { IfaceIndexNotSet = 0xFF };
|
enum { IfaceIndexNotSet = 0xFF };
|
||||||
|
|
||||||
uint64_t prev_transfer_ts_monotonic_;
|
MonotonicTime prev_transfer_ts_;
|
||||||
uint64_t this_transfer_ts_monotonic_;
|
MonotonicTime this_transfer_ts_;
|
||||||
uint64_t first_frame_ts_utc_;
|
UtcTime first_frame_ts_;
|
||||||
uint32_t transfer_interval_;
|
uint32_t transfer_interval_usec_;
|
||||||
uint16_t this_transfer_crc_;
|
uint16_t this_transfer_crc_;
|
||||||
uint16_t buffer_write_pos_;
|
uint16_t buffer_write_pos_;
|
||||||
TransferID tid_;
|
TransferID tid_;
|
||||||
@@ -48,26 +55,23 @@ private:
|
|||||||
|
|
||||||
public:
|
public:
|
||||||
TransferReceiver()
|
TransferReceiver()
|
||||||
: prev_transfer_ts_monotonic_(0)
|
: transfer_interval_usec_(DefaultTransferIntervalUSec)
|
||||||
, this_transfer_ts_monotonic_(0)
|
|
||||||
, first_frame_ts_utc_(0)
|
|
||||||
, transfer_interval_(DefaultTransferInterval)
|
|
||||||
, this_transfer_crc_(0)
|
, this_transfer_crc_(0)
|
||||||
, buffer_write_pos_(0)
|
, buffer_write_pos_(0)
|
||||||
, iface_index_(IfaceIndexNotSet)
|
, iface_index_(IfaceIndexNotSet)
|
||||||
, next_frame_index_(0)
|
, next_frame_index_(0)
|
||||||
{ }
|
{ }
|
||||||
|
|
||||||
bool isTimedOut(uint64_t ts_monotonic) const;
|
bool isTimedOut(MonotonicTime current_ts) const;
|
||||||
|
|
||||||
ResultCode addFrame(const RxFrame& frame, TransferBufferAccessor& tba);
|
ResultCode addFrame(const RxFrame& frame, TransferBufferAccessor& tba);
|
||||||
|
|
||||||
uint64_t getLastTransferTimestampMonotonic() const { return prev_transfer_ts_monotonic_; }
|
MonotonicTime getLastTransferTimestampMonotonic() const { return prev_transfer_ts_; }
|
||||||
uint64_t getLastTransferTimestampUtc() const { return first_frame_ts_utc_; }
|
UtcTime getLastTransferTimestampUtc() const { return first_frame_ts_; }
|
||||||
|
|
||||||
uint16_t getLastTransferCrc() const { return this_transfer_crc_; }
|
uint16_t getLastTransferCrc() const { return this_transfer_crc_; }
|
||||||
|
|
||||||
uint32_t getInterval() const { return transfer_interval_; }
|
MonotonicDuration getInterval() const { return MonotonicDuration::fromUSec(transfer_interval_usec_); }
|
||||||
};
|
};
|
||||||
UAVCAN_PACKED_END
|
UAVCAN_PACKED_END
|
||||||
|
|
||||||
|
|||||||
@@ -16,7 +16,7 @@ namespace uavcan
|
|||||||
|
|
||||||
class TransferSender
|
class TransferSender
|
||||||
{
|
{
|
||||||
const uint64_t max_transfer_interval_;
|
const MonotonicDuration max_transfer_interval_;
|
||||||
const DataTypeDescriptor& data_type_;
|
const DataTypeDescriptor& data_type_;
|
||||||
const CanTxQueue::Qos qos_;
|
const CanTxQueue::Qos qos_;
|
||||||
const TransferCRC crc_base_;
|
const TransferCRC crc_base_;
|
||||||
@@ -24,10 +24,13 @@ class TransferSender
|
|||||||
Dispatcher& dispatcher_;
|
Dispatcher& dispatcher_;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
static const uint64_t DefaultMaxTransferInterval = 60 * 1000 * 1000;
|
static MonotonicDuration getDefaultMaxTransferInterval()
|
||||||
|
{
|
||||||
|
return MonotonicDuration::fromMSec(60 * 1000);
|
||||||
|
}
|
||||||
|
|
||||||
TransferSender(Dispatcher& dispatcher, const DataTypeDescriptor& data_type, CanTxQueue::Qos qos,
|
TransferSender(Dispatcher& dispatcher, const DataTypeDescriptor& data_type, CanTxQueue::Qos qos,
|
||||||
uint64_t max_transfer_interval = DefaultMaxTransferInterval)
|
MonotonicDuration max_transfer_interval = getDefaultMaxTransferInterval())
|
||||||
: max_transfer_interval_(max_transfer_interval)
|
: max_transfer_interval_(max_transfer_interval)
|
||||||
, data_type_(data_type)
|
, data_type_(data_type)
|
||||||
, qos_(qos)
|
, qos_(qos)
|
||||||
@@ -39,16 +42,16 @@ public:
|
|||||||
* Send with explicit Transfer ID.
|
* Send with explicit Transfer ID.
|
||||||
* Should be used only for service responses, where response TID should match request TID.
|
* Should be used only for service responses, where response TID should match request TID.
|
||||||
*/
|
*/
|
||||||
int send(const uint8_t* payload, int payload_len, uint64_t monotonic_tx_deadline,
|
int send(const uint8_t* payload, int payload_len, MonotonicTime tx_deadline,
|
||||||
uint64_t monotonic_blocking_deadline, TransferType transfer_type, NodeID dst_node_id,
|
MonotonicTime blocking_deadline, TransferType transfer_type, NodeID dst_node_id,
|
||||||
TransferID tid);
|
TransferID tid);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Send with automatic Transfer ID.
|
* Send with automatic Transfer ID.
|
||||||
* TID is managed by OutgoingTransferRegistry.
|
* TID is managed by OutgoingTransferRegistry.
|
||||||
*/
|
*/
|
||||||
int send(const uint8_t* payload, int payload_len, uint64_t monotonic_tx_deadline,
|
int send(const uint8_t* payload, int payload_len, MonotonicTime tx_deadline,
|
||||||
uint64_t monotonic_blocking_deadline, TransferType transfer_type, NodeID dst_node_id);
|
MonotonicTime blocking_deadline, TransferType transfer_type, NodeID dst_node_id);
|
||||||
};
|
};
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -18,11 +18,11 @@ public:
|
|||||||
typedef DataType_ DataType;
|
typedef DataType_ DataType;
|
||||||
|
|
||||||
Publisher(Scheduler& scheduler, IMarshalBufferProvider& buffer_provider,
|
Publisher(Scheduler& scheduler, IMarshalBufferProvider& buffer_provider,
|
||||||
uint64_t tx_timeout_usec = BaseType::DefaultTxTimeoutUsec,
|
MonotonicDuration tx_timeout = BaseType::getDefaultTxTimeout(),
|
||||||
uint64_t max_transfer_interval = TransferSender::DefaultMaxTransferInterval)
|
MonotonicDuration max_transfer_interval = TransferSender::getDefaultMaxTransferInterval())
|
||||||
: BaseType(scheduler, buffer_provider, max_transfer_interval)
|
: BaseType(scheduler, buffer_provider, max_transfer_interval)
|
||||||
{
|
{
|
||||||
BaseType::setTxTimeout(tx_timeout_usec);
|
BaseType::setTxTimeout(tx_timeout);
|
||||||
StaticAssert<DataTypeKind(DataType::DataTypeKind) == DataTypeKindMessage>::check();
|
StaticAssert<DataTypeKind(DataType::DataTypeKind) == DataTypeKindMessage>::check();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -6,9 +6,12 @@
|
|||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
|
#include <uavcan/internal/impl_constants.hpp>
|
||||||
|
#include <uavcan/time.hpp>
|
||||||
|
|
||||||
namespace uavcan
|
namespace uavcan
|
||||||
{
|
{
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* System clock interface - monotonic and UTC.
|
* System clock interface - monotonic and UTC.
|
||||||
* Note that this library uses microseconds for all time related values (timestamps, deadlines, delays),
|
* Note that this library uses microseconds for all time related values (timestamps, deadlines, delays),
|
||||||
@@ -21,19 +24,19 @@ public:
|
|||||||
virtual ~ISystemClock() { }
|
virtual ~ISystemClock() { }
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Monototic system clock in microseconds.
|
* Monototic system clock.
|
||||||
* This shall never jump during UTC timestamp adjustments; the base time is irrelevant.
|
* This shall never jump during UTC timestamp adjustments; the base time is irrelevant.
|
||||||
* On POSIX systems use clock_gettime() with CLOCK_MONOTONIC.
|
* On POSIX systems use clock_gettime() with CLOCK_MONOTONIC.
|
||||||
*/
|
*/
|
||||||
virtual uint64_t getMonotonicMicroseconds() const = 0;
|
virtual MonotonicTime getMonotonic() const = 0;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* UTC clock in microseconds.
|
* UTC clock.
|
||||||
* This can jump when the UTC timestamp is being adjusted.
|
* This can jump when the UTC timestamp is being adjusted.
|
||||||
* Return 0 if the UTC time is not available yet (e.g. the device just started up with no battery clock).
|
* Return 0 if the UTC time is not available yet (e.g. the device just started up with no battery clock).
|
||||||
* On POSIX systems use gettimeofday().
|
* On POSIX systems use gettimeofday().
|
||||||
*/
|
*/
|
||||||
virtual uint64_t getUtcMicroseconds() const = 0;
|
virtual UtcTime getUtc() const = 0;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Set the UTC system clock.
|
* Set the UTC system clock.
|
||||||
@@ -41,7 +44,7 @@ public:
|
|||||||
* @param [in] offset Current UTC time error. More precise than just timestamp, use it if possible.
|
* @param [in] offset Current UTC time error. More precise than just timestamp, use it if possible.
|
||||||
* For POSIX refer to adjtime(), settimeofday().
|
* For POSIX refer to adjtime(), settimeofday().
|
||||||
*/
|
*/
|
||||||
virtual void adjustUtcMicroseconds(uint64_t new_timestamp_usec, int64_t offset_usec) = 0;
|
virtual void adjustUtc(UtcTime new_time, UtcDuration offset) = 0;
|
||||||
};
|
};
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -10,7 +10,15 @@
|
|||||||
#include <sstream>
|
#include <sstream>
|
||||||
#include <cstdio>
|
#include <cstdio>
|
||||||
#include <uavcan/util/compile_time.hpp>
|
#include <uavcan/util/compile_time.hpp>
|
||||||
#include <uavcan/Timestamp.hpp>
|
|
||||||
|
// TODO: Fix inclusion loops!
|
||||||
|
namespace uavcan
|
||||||
|
{
|
||||||
|
|
||||||
|
struct Timestamp_;
|
||||||
|
typedef Timestamp_ Timestamp;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
namespace uavcan
|
namespace uavcan
|
||||||
{
|
{
|
||||||
@@ -27,6 +35,8 @@ public:
|
|||||||
StaticAssert<(sizeof(D) == 8)>::check();
|
StaticAssert<(sizeof(D) == 8)>::check();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
static D getInfinite() { return fromUSec(std::numeric_limits<int64_t>::max()); }
|
||||||
|
|
||||||
static D fromUSec(int64_t us)
|
static D fromUSec(int64_t us)
|
||||||
{
|
{
|
||||||
D d;
|
D d;
|
||||||
@@ -95,6 +105,8 @@ public:
|
|||||||
StaticAssert<(sizeof(D) == 8)>::check();
|
StaticAssert<(sizeof(D) == 8)>::check();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
static T getMax() { return fromUSec(std::numeric_limits<uint64_t>::max()); }
|
||||||
|
|
||||||
static T fromUSec(uint64_t us)
|
static T fromUSec(uint64_t us)
|
||||||
{
|
{
|
||||||
T d;
|
T d;
|
||||||
@@ -120,15 +132,15 @@ public:
|
|||||||
{
|
{
|
||||||
if (r.isNegative())
|
if (r.isNegative())
|
||||||
{
|
{
|
||||||
if (uint64_t(r.getAbs().usec_) > usec_)
|
if (uint64_t(r.getAbs().toUSec()) > usec_)
|
||||||
return fromUSec(0);
|
return fromUSec(0);
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
if (uint64_t(usec_ + r.usec_) < usec_)
|
if (uint64_t(usec_ + r.toUSec()) < usec_)
|
||||||
return fromUSec(std::numeric_limits<uint64_t>::max());
|
return fromUSec(std::numeric_limits<uint64_t>::max());
|
||||||
}
|
}
|
||||||
return fromUSec(usec_ + r.usec_);
|
return fromUSec(usec_ + r.toUSec());
|
||||||
}
|
}
|
||||||
|
|
||||||
T operator-(const D& r) const
|
T operator-(const D& r) const
|
||||||
@@ -154,36 +166,25 @@ public:
|
|||||||
std::string toString() const;
|
std::string toString() const;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Monotonic
|
||||||
|
*/
|
||||||
class MonotonicDuration : public DurationBase<MonotonicDuration> { };
|
class MonotonicDuration : public DurationBase<MonotonicDuration> { };
|
||||||
|
|
||||||
class MonotonicTime : public TimeBase<MonotonicTime, MonotonicDuration> { };
|
class MonotonicTime : public TimeBase<MonotonicTime, MonotonicDuration> { };
|
||||||
|
|
||||||
|
/*
|
||||||
|
* UTC
|
||||||
|
*/
|
||||||
class UtcDuration : public DurationBase<UtcDuration> { };
|
class UtcDuration : public DurationBase<UtcDuration> { };
|
||||||
|
|
||||||
class UtcTime : public TimeBase<UtcTime, UtcDuration>
|
class UtcTime : public TimeBase<UtcTime, UtcDuration>
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
UtcTime() { }
|
UtcTime() { }
|
||||||
|
UtcTime(const Timestamp& ts); // Implicit
|
||||||
UtcTime(const Timestamp& ts) // Implicit
|
UtcTime& operator=(const Timestamp& ts);
|
||||||
{
|
operator Timestamp() const;
|
||||||
operator=(ts);
|
|
||||||
}
|
|
||||||
|
|
||||||
UtcTime& operator=(const Timestamp& ts)
|
|
||||||
{
|
|
||||||
*this = fromUSec(ts.husec * Timestamp::USEC_PER_LSB);
|
|
||||||
return *this;
|
|
||||||
}
|
|
||||||
|
|
||||||
operator Timestamp() const
|
|
||||||
{
|
|
||||||
Timestamp ts;
|
|
||||||
ts.husec = toUSec() / Timestamp::USEC_PER_LSB;
|
|
||||||
return ts;
|
|
||||||
}
|
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@@ -17,13 +17,13 @@ class Timer;
|
|||||||
|
|
||||||
struct TimerEvent
|
struct TimerEvent
|
||||||
{
|
{
|
||||||
uint64_t scheduled_monotonic_deadline;
|
MonotonicTime scheduled_deadline;
|
||||||
uint64_t monotonic_timestamp;
|
MonotonicTime current_timestamp;
|
||||||
Timer* timer;
|
Timer* timer;
|
||||||
|
|
||||||
TimerEvent(Timer* timer, uint64_t scheduled_monotonic_deadline, uint64_t monotonic_timestamp)
|
TimerEvent(Timer* timer, MonotonicTime scheduled_deadline, MonotonicTime current_timestamp)
|
||||||
: scheduled_monotonic_deadline(scheduled_monotonic_deadline)
|
: scheduled_deadline(scheduled_deadline)
|
||||||
, monotonic_timestamp(monotonic_timestamp)
|
, current_timestamp(current_timestamp)
|
||||||
, timer(timer)
|
, timer(timer)
|
||||||
{ }
|
{ }
|
||||||
};
|
};
|
||||||
@@ -31,28 +31,26 @@ struct TimerEvent
|
|||||||
|
|
||||||
class Timer : private MonotonicDeadlineHandler
|
class Timer : private MonotonicDeadlineHandler
|
||||||
{
|
{
|
||||||
uint64_t period_;
|
MonotonicDuration period_;
|
||||||
|
|
||||||
void handleMonotonicDeadline(uint64_t monotonic_timestamp);
|
void handleDeadline(MonotonicTime current_timestamp);
|
||||||
|
|
||||||
public:
|
public:
|
||||||
static const uint64_t InfinitePeriod = 0xFFFFFFFFFFFFFFFFUL;
|
|
||||||
|
|
||||||
using MonotonicDeadlineHandler::stop;
|
using MonotonicDeadlineHandler::stop;
|
||||||
using MonotonicDeadlineHandler::isRunning;
|
using MonotonicDeadlineHandler::isRunning;
|
||||||
using MonotonicDeadlineHandler::getMonotonicDeadline;
|
using MonotonicDeadlineHandler::getDeadline;
|
||||||
using MonotonicDeadlineHandler::getScheduler;
|
using MonotonicDeadlineHandler::getScheduler;
|
||||||
|
|
||||||
explicit Timer(Scheduler& scheduler)
|
explicit Timer(Scheduler& scheduler)
|
||||||
: MonotonicDeadlineHandler(scheduler)
|
: MonotonicDeadlineHandler(scheduler)
|
||||||
, period_(InfinitePeriod)
|
, period_(MonotonicDuration::getInfinite())
|
||||||
{ }
|
{ }
|
||||||
|
|
||||||
void startOneShotWithDeadline(uint64_t monotonic_deadline);
|
void startOneShotWithDeadline(MonotonicTime deadline);
|
||||||
void startOneShotWithDelay(uint64_t delay_usec);
|
void startOneShotWithDelay(MonotonicDuration delay);
|
||||||
void startPeriodic(uint64_t period_usec);
|
void startPeriodic(MonotonicDuration period);
|
||||||
|
|
||||||
uint64_t getPeriod() const { return period_; }
|
MonotonicDuration getPeriod() const { return period_; }
|
||||||
|
|
||||||
virtual void handleTimerEvent(const TimerEvent& event) = 0;
|
virtual void handleTimerEvent(const TimerEvent& event) = 0;
|
||||||
};
|
};
|
||||||
|
|||||||
@@ -7,6 +7,7 @@
|
|||||||
#include <iomanip>
|
#include <iomanip>
|
||||||
#include <cassert>
|
#include <cassert>
|
||||||
#include <uavcan/data_type.hpp>
|
#include <uavcan/data_type.hpp>
|
||||||
|
#include <uavcan/internal/transport/crc.hpp>
|
||||||
|
|
||||||
namespace uavcan
|
namespace uavcan
|
||||||
{
|
{
|
||||||
|
|||||||
@@ -3,7 +3,6 @@
|
|||||||
*/
|
*/
|
||||||
|
|
||||||
#include <cassert>
|
#include <cassert>
|
||||||
#include <limits>
|
|
||||||
#include <uavcan/internal/node/scheduler.hpp>
|
#include <uavcan/internal/node/scheduler.hpp>
|
||||||
#include <uavcan/internal/debug.hpp>
|
#include <uavcan/internal/debug.hpp>
|
||||||
|
|
||||||
@@ -12,17 +11,17 @@ namespace uavcan
|
|||||||
/*
|
/*
|
||||||
* MonotonicDeadlineHandler
|
* MonotonicDeadlineHandler
|
||||||
*/
|
*/
|
||||||
void MonotonicDeadlineHandler::startWithDeadline(uint64_t monotonic_deadline)
|
void MonotonicDeadlineHandler::startWithDeadline(MonotonicTime deadline)
|
||||||
{
|
{
|
||||||
assert(monotonic_deadline > 0);
|
assert(!deadline.isZero());
|
||||||
stop();
|
stop();
|
||||||
monotonic_deadline_ = monotonic_deadline;
|
deadline_ = deadline;
|
||||||
scheduler_.getMonotonicDeadlineScheduler().add(this);
|
scheduler_.getMonotonicDeadlineScheduler().add(this);
|
||||||
}
|
}
|
||||||
|
|
||||||
void MonotonicDeadlineHandler::startWithDelay(uint64_t delay_usec)
|
void MonotonicDeadlineHandler::startWithDelay(MonotonicDuration delay)
|
||||||
{
|
{
|
||||||
startWithDeadline(scheduler_.getMonotonicTimestamp() + delay_usec);
|
startWithDeadline(scheduler_.getMonotonicTimestamp() + delay);
|
||||||
}
|
}
|
||||||
|
|
||||||
void MonotonicDeadlineHandler::stop()
|
void MonotonicDeadlineHandler::stop()
|
||||||
@@ -40,18 +39,18 @@ bool MonotonicDeadlineHandler::isRunning() const
|
|||||||
*/
|
*/
|
||||||
struct MonotonicDeadlineHandlerInsertionComparator
|
struct MonotonicDeadlineHandlerInsertionComparator
|
||||||
{
|
{
|
||||||
const uint64_t ts;
|
const MonotonicTime ts;
|
||||||
MonotonicDeadlineHandlerInsertionComparator(uint64_t ts) : ts(ts) { }
|
MonotonicDeadlineHandlerInsertionComparator(MonotonicTime ts) : ts(ts) { }
|
||||||
bool operator()(const MonotonicDeadlineHandler* t) const
|
bool operator()(const MonotonicDeadlineHandler* t) const
|
||||||
{
|
{
|
||||||
return t->getMonotonicDeadline() > ts;
|
return t->getDeadline() > ts;
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
void MonotonicDeadlineScheduler::add(MonotonicDeadlineHandler* mdh)
|
void MonotonicDeadlineScheduler::add(MonotonicDeadlineHandler* mdh)
|
||||||
{
|
{
|
||||||
assert(mdh);
|
assert(mdh);
|
||||||
handlers_.insertBefore(mdh, MonotonicDeadlineHandlerInsertionComparator(mdh->getMonotonicDeadline()));
|
handlers_.insertBefore(mdh, MonotonicDeadlineHandlerInsertionComparator(mdh->getDeadline()));
|
||||||
}
|
}
|
||||||
|
|
||||||
void MonotonicDeadlineScheduler::remove(MonotonicDeadlineHandler* mdh)
|
void MonotonicDeadlineScheduler::remove(MonotonicDeadlineHandler* mdh)
|
||||||
@@ -65,14 +64,14 @@ bool MonotonicDeadlineScheduler::doesExist(const MonotonicDeadlineHandler* mdh)
|
|||||||
assert(mdh);
|
assert(mdh);
|
||||||
const MonotonicDeadlineHandler* p = handlers_.get();
|
const MonotonicDeadlineHandler* p = handlers_.get();
|
||||||
#if UAVCAN_DEBUG
|
#if UAVCAN_DEBUG
|
||||||
uint64_t prev_deadline = 0;
|
MonotonicTime prev_deadline;
|
||||||
#endif
|
#endif
|
||||||
while (p)
|
while (p)
|
||||||
{
|
{
|
||||||
#if UAVCAN_DEBUG
|
#if UAVCAN_DEBUG
|
||||||
if (prev_deadline > p->getMonotonicDeadline()) // Self check
|
if (prev_deadline > p->getDeadline()) // Self check
|
||||||
std::abort();
|
std::abort();
|
||||||
prev_deadline = p->getMonotonicDeadline();
|
prev_deadline = p->getDeadline();
|
||||||
#endif
|
#endif
|
||||||
if (p == mdh)
|
if (p == mdh)
|
||||||
return true;
|
return true;
|
||||||
@@ -81,56 +80,56 @@ bool MonotonicDeadlineScheduler::doesExist(const MonotonicDeadlineHandler* mdh)
|
|||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
uint64_t MonotonicDeadlineScheduler::pollAndGetMonotonicTimestamp(ISystemClock& sysclock)
|
MonotonicTime MonotonicDeadlineScheduler::pollAndGetMonotonicTimestamp(ISystemClock& sysclock)
|
||||||
{
|
{
|
||||||
while (true)
|
while (true)
|
||||||
{
|
{
|
||||||
MonotonicDeadlineHandler* const mdh = handlers_.get();
|
MonotonicDeadlineHandler* const mdh = handlers_.get();
|
||||||
if (!mdh)
|
if (!mdh)
|
||||||
return sysclock.getMonotonicMicroseconds();
|
return sysclock.getMonotonic();
|
||||||
#if UAVCAN_DEBUG
|
#if UAVCAN_DEBUG
|
||||||
if (mdh->getNextListNode()) // Order check
|
if (mdh->getNextListNode()) // Order check
|
||||||
assert(mdh->getMonotonicDeadline() <= mdh->getNextListNode()->getMonotonicDeadline());
|
assert(mdh->getDeadline() <= mdh->getNextListNode()->getDeadline());
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
const uint64_t ts = sysclock.getMonotonicMicroseconds();
|
const MonotonicTime ts = sysclock.getMonotonic();
|
||||||
if (ts < mdh->getMonotonicDeadline())
|
if (ts < mdh->getDeadline())
|
||||||
return ts;
|
return ts;
|
||||||
|
|
||||||
handlers_.remove(mdh);
|
handlers_.remove(mdh);
|
||||||
mdh->handleMonotonicDeadline(ts); // This handler can be re-registered immediately
|
mdh->handleDeadline(ts); // This handler can be re-registered immediately
|
||||||
}
|
}
|
||||||
assert(0);
|
assert(0);
|
||||||
return 0;
|
return MonotonicTime();
|
||||||
}
|
}
|
||||||
|
|
||||||
uint64_t MonotonicDeadlineScheduler::getEarliestDeadline() const
|
MonotonicTime MonotonicDeadlineScheduler::getEarliestDeadline() const
|
||||||
{
|
{
|
||||||
const MonotonicDeadlineHandler* const mdh = handlers_.get();
|
const MonotonicDeadlineHandler* const mdh = handlers_.get();
|
||||||
if (mdh)
|
if (mdh)
|
||||||
return mdh->getMonotonicDeadline();
|
return mdh->getDeadline();
|
||||||
return std::numeric_limits<uint64_t>::max();
|
return MonotonicTime::getMax();
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Scheduler
|
* Scheduler
|
||||||
*/
|
*/
|
||||||
uint64_t Scheduler::computeDispatcherSpinDeadline(uint64_t spin_deadline) const
|
MonotonicTime Scheduler::computeDispatcherSpinDeadline(MonotonicTime spin_deadline) const
|
||||||
{
|
{
|
||||||
const uint64_t earliest = std::min(deadline_scheduler_.getEarliestDeadline(), spin_deadline);
|
const MonotonicTime earliest = std::min(deadline_scheduler_.getEarliestDeadline(), spin_deadline);
|
||||||
const uint64_t ts = getMonotonicTimestamp();
|
const MonotonicTime ts = getMonotonicTimestamp();
|
||||||
if (earliest > ts)
|
if (earliest > ts)
|
||||||
{
|
{
|
||||||
if (ts - earliest > monotonic_deadline_resolution_)
|
if (ts - earliest > deadline_resolution_)
|
||||||
return ts + monotonic_deadline_resolution_;
|
return ts + deadline_resolution_;
|
||||||
}
|
}
|
||||||
return earliest;
|
return earliest;
|
||||||
}
|
}
|
||||||
|
|
||||||
void Scheduler::pollCleanup(uint64_t mono_ts, uint32_t num_frames_processed_with_last_spin)
|
void Scheduler::pollCleanup(MonotonicTime mono_ts, uint32_t num_frames_processed_with_last_spin)
|
||||||
{
|
{
|
||||||
// cleanup will be performed less frequently if the stack handles more frames per second
|
// cleanup will be performed less frequently if the stack handles more frames per second
|
||||||
const uint64_t deadline = prev_cleanup_ts_ + cleanup_period_ * (num_frames_processed_with_last_spin + 1);
|
const MonotonicTime deadline = prev_cleanup_ts_ + cleanup_period_ * (num_frames_processed_with_last_spin + 1);
|
||||||
if (mono_ts > deadline)
|
if (mono_ts > deadline)
|
||||||
{
|
{
|
||||||
UAVCAN_TRACE("Scheduler", "Cleanup with %u processed frames", num_frames_processed_with_last_spin);
|
UAVCAN_TRACE("Scheduler", "Cleanup with %u processed frames", num_frames_processed_with_last_spin);
|
||||||
@@ -139,19 +138,19 @@ void Scheduler::pollCleanup(uint64_t mono_ts, uint32_t num_frames_processed_with
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
int Scheduler::spin(uint64_t monotonic_deadline)
|
int Scheduler::spin(MonotonicTime deadline)
|
||||||
{
|
{
|
||||||
int retval = 0;
|
int retval = 0;
|
||||||
while (true)
|
while (true)
|
||||||
{
|
{
|
||||||
const uint64_t dl = computeDispatcherSpinDeadline(monotonic_deadline);
|
const MonotonicTime dl = computeDispatcherSpinDeadline(deadline);
|
||||||
retval = dispatcher_.spin(dl);
|
retval = dispatcher_.spin(dl);
|
||||||
if (retval < 0)
|
if (retval < 0)
|
||||||
break;
|
break;
|
||||||
|
|
||||||
const uint64_t ts = deadline_scheduler_.pollAndGetMonotonicTimestamp(getSystemClock());
|
const MonotonicTime ts = deadline_scheduler_.pollAndGetMonotonicTimestamp(getSystemClock());
|
||||||
pollCleanup(ts, retval);
|
pollCleanup(ts, retval);
|
||||||
if (ts >= monotonic_deadline)
|
if (ts >= deadline)
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
return retval;
|
return retval;
|
||||||
|
|||||||
@@ -19,7 +19,7 @@ std::string CanRxFrame::toString(StringRepresentation mode) const
|
|||||||
{
|
{
|
||||||
std::ostringstream os;
|
std::ostringstream os;
|
||||||
os << CanFrame::toString(mode)
|
os << CanFrame::toString(mode)
|
||||||
<< " ts_m=" << ts_monotonic << " ts_utc=" << ts_utc << " iface=" << int(iface_index);
|
<< " ts_m=" << ts_mono << " ts_utc=" << ts_utc << " iface=" << int(iface_index);
|
||||||
return os.str();
|
return os.str();
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -88,11 +88,11 @@ void CanTxQueue::registerRejectedFrame()
|
|||||||
rejected_frames_cnt_++;
|
rejected_frames_cnt_++;
|
||||||
}
|
}
|
||||||
|
|
||||||
void CanTxQueue::push(const CanFrame& frame, uint64_t monotonic_tx_deadline, Qos qos)
|
void CanTxQueue::push(const CanFrame& frame, MonotonicTime tx_deadline, Qos qos)
|
||||||
{
|
{
|
||||||
const uint64_t timestamp = sysclock_->getMonotonicMicroseconds();
|
const MonotonicTime timestamp = sysclock_->getMonotonic();
|
||||||
|
|
||||||
if (timestamp >= monotonic_tx_deadline)
|
if (timestamp >= tx_deadline)
|
||||||
{
|
{
|
||||||
UAVCAN_TRACE("CanTxQueue", "Push rejected: already expired");
|
UAVCAN_TRACE("CanTxQueue", "Push rejected: already expired");
|
||||||
registerRejectedFrame();
|
registerRejectedFrame();
|
||||||
@@ -147,14 +147,14 @@ void CanTxQueue::push(const CanFrame& frame, uint64_t monotonic_tx_deadline, Qos
|
|||||||
if (praw == NULL)
|
if (praw == NULL)
|
||||||
return; // Seems that there is no memory at all.
|
return; // Seems that there is no memory at all.
|
||||||
|
|
||||||
Entry* entry = new (praw) Entry(frame, monotonic_tx_deadline, qos);
|
Entry* entry = new (praw) Entry(frame, tx_deadline, qos);
|
||||||
assert(entry);
|
assert(entry);
|
||||||
queue_.insertBefore(entry, PriorityInsertionComparator(frame));
|
queue_.insertBefore(entry, PriorityInsertionComparator(frame));
|
||||||
}
|
}
|
||||||
|
|
||||||
CanTxQueue::Entry* CanTxQueue::peek()
|
CanTxQueue::Entry* CanTxQueue::peek()
|
||||||
{
|
{
|
||||||
const uint64_t timestamp = sysclock_->getMonotonicMicroseconds();
|
const MonotonicTime timestamp = sysclock_->getMonotonic();
|
||||||
Entry* p = queue_.get();
|
Entry* p = queue_.get();
|
||||||
while (p)
|
while (p)
|
||||||
{
|
{
|
||||||
@@ -194,7 +194,7 @@ bool CanTxQueue::topPriorityHigherOrEqual(const CanFrame& rhs_frame) const
|
|||||||
/*
|
/*
|
||||||
* CanIOManager
|
* CanIOManager
|
||||||
*/
|
*/
|
||||||
int CanIOManager::sendToIface(int iface_index, const CanFrame& frame, uint64_t monotonic_tx_deadline)
|
int CanIOManager::sendToIface(int iface_index, const CanFrame& frame, MonotonicTime tx_deadline)
|
||||||
{
|
{
|
||||||
assert(iface_index >= 0 && iface_index < MaxIfaces);
|
assert(iface_index >= 0 && iface_index < MaxIfaces);
|
||||||
ICanIface* const iface = driver_.getIface(iface_index);
|
ICanIface* const iface = driver_.getIface(iface_index);
|
||||||
@@ -203,9 +203,7 @@ int CanIOManager::sendToIface(int iface_index, const CanFrame& frame, uint64_t m
|
|||||||
assert(0); // Nonexistent interface
|
assert(0); // Nonexistent interface
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
const uint64_t timestamp = sysclock_.getMonotonicMicroseconds();
|
const int res = iface->send(frame, tx_deadline);
|
||||||
const uint64_t timeout = (timestamp >= monotonic_tx_deadline) ? 0 : (monotonic_tx_deadline - timestamp);
|
|
||||||
const int res = iface->send(frame, timeout);
|
|
||||||
if (res != 1)
|
if (res != 1)
|
||||||
{
|
{
|
||||||
UAVCAN_TRACE("CanIOManager", "Send failed: code %i, iface %i, frame %s",
|
UAVCAN_TRACE("CanIOManager", "Send failed: code %i, iface %i, frame %s",
|
||||||
@@ -220,7 +218,7 @@ int CanIOManager::sendFromTxQueue(int iface_index)
|
|||||||
CanTxQueue::Entry* entry = tx_queues_[iface_index].peek();
|
CanTxQueue::Entry* entry = tx_queues_[iface_index].peek();
|
||||||
if (entry == NULL)
|
if (entry == NULL)
|
||||||
return 0;
|
return 0;
|
||||||
const int res = sendToIface(iface_index, entry->frame, entry->monotonic_deadline);
|
const int res = sendToIface(iface_index, entry->frame, entry->deadline);
|
||||||
if (res > 0)
|
if (res > 0)
|
||||||
tx_queues_[iface_index].remove(entry);
|
tx_queues_[iface_index].remove(entry);
|
||||||
return res;
|
return res;
|
||||||
@@ -237,12 +235,6 @@ int CanIOManager::makePendingTxMask() const
|
|||||||
return write_mask;
|
return write_mask;
|
||||||
}
|
}
|
||||||
|
|
||||||
uint64_t CanIOManager::getTimeUntilMonotonicDeadline(uint64_t monotonic_deadline) const
|
|
||||||
{
|
|
||||||
const uint64_t timestamp = sysclock_.getMonotonicMicroseconds();
|
|
||||||
return (timestamp >= monotonic_deadline) ? 0 : (monotonic_deadline - timestamp);
|
|
||||||
}
|
|
||||||
|
|
||||||
int CanIOManager::getNumIfaces() const
|
int CanIOManager::getNumIfaces() const
|
||||||
{
|
{
|
||||||
const int num = driver_.getNumIfaces();
|
const int num = driver_.getNumIfaces();
|
||||||
@@ -261,7 +253,7 @@ uint64_t CanIOManager::getNumErrors(int iface_index) const
|
|||||||
return iface->getNumErrors() + tx_queues_[iface_index].getNumRejectedFrames();
|
return iface->getNumErrors() + tx_queues_[iface_index].getNumRejectedFrames();
|
||||||
}
|
}
|
||||||
|
|
||||||
int CanIOManager::send(const CanFrame& frame, uint64_t monotonic_tx_deadline, uint64_t monotonic_blocking_deadline,
|
int CanIOManager::send(const CanFrame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline,
|
||||||
int iface_mask, CanTxQueue::Qos qos)
|
int iface_mask, CanTxQueue::Qos qos)
|
||||||
{
|
{
|
||||||
const int num_ifaces = getNumIfaces();
|
const int num_ifaces = getNumIfaces();
|
||||||
@@ -270,8 +262,8 @@ int CanIOManager::send(const CanFrame& frame, uint64_t monotonic_tx_deadline, ui
|
|||||||
assert((iface_mask & ~all_ifaces_mask) == 0);
|
assert((iface_mask & ~all_ifaces_mask) == 0);
|
||||||
iface_mask &= all_ifaces_mask;
|
iface_mask &= all_ifaces_mask;
|
||||||
|
|
||||||
if (monotonic_blocking_deadline > monotonic_tx_deadline)
|
if (blocking_deadline > tx_deadline)
|
||||||
monotonic_blocking_deadline = monotonic_tx_deadline;
|
blocking_deadline = tx_deadline;
|
||||||
|
|
||||||
int retval = 0;
|
int retval = 0;
|
||||||
|
|
||||||
@@ -280,11 +272,9 @@ int CanIOManager::send(const CanFrame& frame, uint64_t monotonic_tx_deadline, ui
|
|||||||
if (iface_mask == 0)
|
if (iface_mask == 0)
|
||||||
break;
|
break;
|
||||||
int write_mask = iface_mask | makePendingTxMask();
|
int write_mask = iface_mask | makePendingTxMask();
|
||||||
|
|
||||||
const uint64_t timeout = getTimeUntilMonotonicDeadline(monotonic_blocking_deadline);
|
|
||||||
{
|
{
|
||||||
int read_mask = 0;
|
int read_mask = 0;
|
||||||
const int select_res = driver_.select(write_mask, read_mask, timeout);
|
const int select_res = driver_.select(write_mask, read_mask, blocking_deadline);
|
||||||
if (select_res < 0)
|
if (select_res < 0)
|
||||||
return select_res;
|
return select_res;
|
||||||
}
|
}
|
||||||
@@ -303,7 +293,7 @@ int CanIOManager::send(const CanFrame& frame, uint64_t monotonic_tx_deadline, ui
|
|||||||
}
|
}
|
||||||
if (res <= 0)
|
if (res <= 0)
|
||||||
{
|
{
|
||||||
res = sendToIface(i, frame, monotonic_tx_deadline);
|
res = sendToIface(i, frame, tx_deadline);
|
||||||
if (res > 0)
|
if (res > 0)
|
||||||
iface_mask &= ~(1 << i); // Mark transmitted
|
iface_mask &= ~(1 << i); // Mark transmitted
|
||||||
}
|
}
|
||||||
@@ -318,9 +308,10 @@ int CanIOManager::send(const CanFrame& frame, uint64_t monotonic_tx_deadline, ui
|
|||||||
}
|
}
|
||||||
|
|
||||||
// Timeout. Enqueue the frame if wasn't transmitted and leave.
|
// Timeout. Enqueue the frame if wasn't transmitted and leave.
|
||||||
if (write_mask == 0 || timeout == 0)
|
const bool timed_out = sysclock_.getMonotonic() >= blocking_deadline;
|
||||||
|
if (write_mask == 0 || timed_out)
|
||||||
{
|
{
|
||||||
if ((timeout > 0) && (sysclock_.getMonotonicMicroseconds() < monotonic_blocking_deadline))
|
if (!timed_out)
|
||||||
{
|
{
|
||||||
UAVCAN_TRACE("CanIOManager", "Send: Premature timeout in select(), will try again");
|
UAVCAN_TRACE("CanIOManager", "Send: Premature timeout in select(), will try again");
|
||||||
continue;
|
continue;
|
||||||
@@ -328,7 +319,7 @@ int CanIOManager::send(const CanFrame& frame, uint64_t monotonic_tx_deadline, ui
|
|||||||
for (int i = 0; i < num_ifaces; i++)
|
for (int i = 0; i < num_ifaces; i++)
|
||||||
{
|
{
|
||||||
if (iface_mask & (1 << i))
|
if (iface_mask & (1 << i))
|
||||||
tx_queues_[i].push(frame, monotonic_tx_deadline, qos);
|
tx_queues_[i].push(frame, tx_deadline, qos);
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
@@ -336,7 +327,7 @@ int CanIOManager::send(const CanFrame& frame, uint64_t monotonic_tx_deadline, ui
|
|||||||
return retval;
|
return retval;
|
||||||
}
|
}
|
||||||
|
|
||||||
int CanIOManager::receive(CanRxFrame& frame, uint64_t monotonic_deadline)
|
int CanIOManager::receive(CanRxFrame& frame, MonotonicTime blocking_deadline)
|
||||||
{
|
{
|
||||||
const int num_ifaces = getNumIfaces();
|
const int num_ifaces = getNumIfaces();
|
||||||
|
|
||||||
@@ -344,9 +335,8 @@ int CanIOManager::receive(CanRxFrame& frame, uint64_t monotonic_deadline)
|
|||||||
{
|
{
|
||||||
int write_mask = makePendingTxMask();
|
int write_mask = makePendingTxMask();
|
||||||
int read_mask = (1 << num_ifaces) - 1;
|
int read_mask = (1 << num_ifaces) - 1;
|
||||||
const uint64_t timeout = getTimeUntilMonotonicDeadline(monotonic_deadline);
|
|
||||||
{
|
{
|
||||||
const int select_res = driver_.select(write_mask, read_mask, timeout);
|
const int select_res = driver_.select(write_mask, read_mask, blocking_deadline);
|
||||||
if (select_res < 0)
|
if (select_res < 0)
|
||||||
return select_res;
|
return select_res;
|
||||||
}
|
}
|
||||||
@@ -369,7 +359,7 @@ int CanIOManager::receive(CanRxFrame& frame, uint64_t monotonic_deadline)
|
|||||||
assert(0); // Nonexistent interface
|
assert(0); // Nonexistent interface
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
const int res = iface->receive(frame, frame.ts_monotonic, frame.ts_utc);
|
const int res = iface->receive(frame, frame.ts_mono, frame.ts_utc);
|
||||||
if (res == 0)
|
if (res == 0)
|
||||||
{
|
{
|
||||||
assert(0); // select() reported that iface has pending RX frames, but receive() returned none
|
assert(0); // select() reported that iface has pending RX frames, but receive() returned none
|
||||||
@@ -380,7 +370,8 @@ int CanIOManager::receive(CanRxFrame& frame, uint64_t monotonic_deadline)
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
if (timeout == 0) // Timeout checked in the last order - this way we can operate with expired deadline
|
// Timeout checked in the last order - this way we can operate with expired deadline:
|
||||||
|
if (sysclock_.getMonotonic() >= blocking_deadline)
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
return 0;
|
return 0;
|
||||||
|
|||||||
@@ -33,12 +33,12 @@ void Dispatcher::ListenerRegister::remove(TransferListenerBase* listener)
|
|||||||
list_.remove(listener);
|
list_.remove(listener);
|
||||||
}
|
}
|
||||||
|
|
||||||
void Dispatcher::ListenerRegister::cleanup(uint64_t ts_monotonic)
|
void Dispatcher::ListenerRegister::cleanup(MonotonicTime ts)
|
||||||
{
|
{
|
||||||
TransferListenerBase* p = list_.get();
|
TransferListenerBase* p = list_.get();
|
||||||
while (p)
|
while (p)
|
||||||
{
|
{
|
||||||
p->cleanup(ts_monotonic);
|
p->cleanup(ts);
|
||||||
p = p->getNextListNode();
|
p = p->getNextListNode();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -94,13 +94,13 @@ void Dispatcher::handleFrame(const CanRxFrame& can_frame)
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
int Dispatcher::spin(uint64_t monotonic_deadline)
|
int Dispatcher::spin(MonotonicTime deadline)
|
||||||
{
|
{
|
||||||
int num_frames_processed = 0;
|
int num_frames_processed = 0;
|
||||||
do
|
do
|
||||||
{
|
{
|
||||||
CanRxFrame frame;
|
CanRxFrame frame;
|
||||||
const int res = canio_.receive(frame, monotonic_deadline);
|
const int res = canio_.receive(frame, deadline);
|
||||||
if (res < 0)
|
if (res < 0)
|
||||||
return res;
|
return res;
|
||||||
if (res > 0)
|
if (res > 0)
|
||||||
@@ -109,12 +109,12 @@ int Dispatcher::spin(uint64_t monotonic_deadline)
|
|||||||
handleFrame(frame);
|
handleFrame(frame);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
while (sysclock_.getMonotonicMicroseconds() < monotonic_deadline);
|
while (sysclock_.getMonotonic() < deadline);
|
||||||
|
|
||||||
return num_frames_processed;
|
return num_frames_processed;
|
||||||
}
|
}
|
||||||
|
|
||||||
int Dispatcher::send(const Frame& frame, uint64_t monotonic_tx_deadline, uint64_t monotonic_blocking_deadline,
|
int Dispatcher::send(const Frame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline,
|
||||||
CanTxQueue::Qos qos)
|
CanTxQueue::Qos qos)
|
||||||
{
|
{
|
||||||
if (frame.getSrcNodeID() != getSelfNodeID())
|
if (frame.getSrcNodeID() != getSelfNodeID())
|
||||||
@@ -132,15 +132,15 @@ int Dispatcher::send(const Frame& frame, uint64_t monotonic_tx_deadline, uint64_
|
|||||||
}
|
}
|
||||||
const int iface_mask = (1 << canio_.getNumIfaces()) - 1;
|
const int iface_mask = (1 << canio_.getNumIfaces()) - 1;
|
||||||
|
|
||||||
return canio_.send(can_frame, monotonic_tx_deadline, monotonic_blocking_deadline, iface_mask, qos);
|
return canio_.send(can_frame, tx_deadline, blocking_deadline, iface_mask, qos);
|
||||||
}
|
}
|
||||||
|
|
||||||
void Dispatcher::cleanup(uint64_t ts_monotonic)
|
void Dispatcher::cleanup(MonotonicTime ts)
|
||||||
{
|
{
|
||||||
outgoing_transfer_reg_.cleanup(ts_monotonic);
|
outgoing_transfer_reg_.cleanup(ts);
|
||||||
lmsg_.cleanup(ts_monotonic);
|
lmsg_.cleanup(ts);
|
||||||
lsrv_req_.cleanup(ts_monotonic);
|
lsrv_req_.cleanup(ts);
|
||||||
lsrv_resp_.cleanup(ts_monotonic);
|
lsrv_resp_.cleanup(ts);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool Dispatcher::registerMessageListener(TransferListenerBase* listener)
|
bool Dispatcher::registerMessageListener(TransferListenerBase* listener)
|
||||||
|
|||||||
@@ -6,6 +6,7 @@
|
|||||||
#include <cstdio>
|
#include <cstdio>
|
||||||
#include <sstream>
|
#include <sstream>
|
||||||
#include <uavcan/internal/transport/transfer.hpp>
|
#include <uavcan/internal/transport/transfer.hpp>
|
||||||
|
#include <uavcan/internal/transport/can_io.hpp>
|
||||||
|
|
||||||
namespace uavcan
|
namespace uavcan
|
||||||
{
|
{
|
||||||
@@ -225,10 +226,20 @@ std::string Frame::toString() const
|
|||||||
/**
|
/**
|
||||||
* RxFrame
|
* RxFrame
|
||||||
*/
|
*/
|
||||||
|
bool RxFrame::parse(const CanRxFrame& can_frame)
|
||||||
|
{
|
||||||
|
if (!Frame::parse(can_frame))
|
||||||
|
return false;
|
||||||
|
ts_mono_ = can_frame.ts_mono;
|
||||||
|
ts_utc_ = can_frame.ts_utc;
|
||||||
|
iface_index_ = can_frame.iface_index;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
std::string RxFrame::toString() const
|
std::string RxFrame::toString() const
|
||||||
{
|
{
|
||||||
std::ostringstream os; // C++03 doesn't support long long, so we need ostream to print the timestamp
|
std::ostringstream os; // C++03 doesn't support long long, so we need ostream to print the timestamp
|
||||||
os << Frame::toString() << " ts_m=" << ts_monotonic_ << " ts_utc=" << ts_utc_ << " iface=" << int(iface_index_);
|
os << Frame::toString() << " ts_m=" << ts_mono_ << " ts_utc=" << ts_utc_ << " iface=" << int(iface_index_);
|
||||||
return os.str();
|
return os.str();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -49,9 +49,9 @@ int SingleFrameIncomingTransfer::read(unsigned int offset, uint8_t* data, unsign
|
|||||||
/*
|
/*
|
||||||
* MultiFrameIncomingTransfer
|
* MultiFrameIncomingTransfer
|
||||||
*/
|
*/
|
||||||
MultiFrameIncomingTransfer::MultiFrameIncomingTransfer(uint64_t ts_monotonic, uint64_t ts_utc,
|
MultiFrameIncomingTransfer::MultiFrameIncomingTransfer(MonotonicTime ts_mono, UtcTime ts_utc,
|
||||||
const RxFrame& last_frame, TransferBufferAccessor& tba)
|
const RxFrame& last_frame, TransferBufferAccessor& tba)
|
||||||
: IncomingTransfer(ts_monotonic, ts_utc, last_frame.getTransferType(), last_frame.getTransferID(),
|
: IncomingTransfer(ts_mono, ts_utc, last_frame.getTransferType(), last_frame.getTransferID(),
|
||||||
last_frame.getSrcNodeID())
|
last_frame.getSrcNodeID())
|
||||||
, buf_acc_(tba)
|
, buf_acc_(tba)
|
||||||
{
|
{
|
||||||
|
|||||||
@@ -13,9 +13,9 @@
|
|||||||
namespace uavcan
|
namespace uavcan
|
||||||
{
|
{
|
||||||
|
|
||||||
const uint32_t TransferReceiver::DefaultTransferInterval;
|
const uint32_t TransferReceiver::DefaultTransferIntervalUSec;
|
||||||
const uint32_t TransferReceiver::MinTransferInterval;
|
const uint32_t TransferReceiver::MinTransferIntervalUSec;
|
||||||
const uint32_t TransferReceiver::MaxTransferInterval;
|
const uint32_t TransferReceiver::MaxTransferIntervalUSec;
|
||||||
|
|
||||||
TransferReceiver::TidRelation TransferReceiver::getTidRelation(const RxFrame& frame) const
|
TransferReceiver::TidRelation TransferReceiver::getTidRelation(const RxFrame& frame) const
|
||||||
{
|
{
|
||||||
@@ -29,18 +29,17 @@ TransferReceiver::TidRelation TransferReceiver::getTidRelation(const RxFrame& fr
|
|||||||
|
|
||||||
void TransferReceiver::updateTransferTimings()
|
void TransferReceiver::updateTransferTimings()
|
||||||
{
|
{
|
||||||
assert(this_transfer_ts_monotonic_ > 0);
|
assert(!this_transfer_ts_.isZero());
|
||||||
|
|
||||||
const uint64_t prev_prev_ts = prev_transfer_ts_monotonic_;
|
const MonotonicTime prev_prev_ts = prev_transfer_ts_;
|
||||||
prev_transfer_ts_monotonic_ = this_transfer_ts_monotonic_;
|
prev_transfer_ts_ = this_transfer_ts_;
|
||||||
|
|
||||||
if ((prev_prev_ts != 0) &&
|
if ((!prev_prev_ts.isZero()) && (!prev_transfer_ts_.isZero()) && (prev_transfer_ts_ >= prev_prev_ts))
|
||||||
(prev_transfer_ts_monotonic_ != 0) &&
|
|
||||||
(prev_transfer_ts_monotonic_ >= prev_prev_ts))
|
|
||||||
{
|
{
|
||||||
uint64_t interval = prev_transfer_ts_monotonic_ - prev_prev_ts;
|
uint64_t interval_usec = (prev_transfer_ts_ - prev_prev_ts).toUSec();
|
||||||
interval = std::max(std::min(interval, uint64_t(MaxTransferInterval)), uint64_t(MinTransferInterval));
|
interval_usec = std::min(interval_usec, uint64_t(MaxTransferIntervalUSec));
|
||||||
transfer_interval_ = static_cast<uint32_t>((uint64_t(transfer_interval_) * 7 + interval) / 8);
|
interval_usec = std::max(interval_usec, uint64_t(MinTransferIntervalUSec));
|
||||||
|
transfer_interval_usec_ = static_cast<uint32_t>((uint64_t(transfer_interval_usec_) * 7 + interval_usec) / 8);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -117,8 +116,8 @@ TransferReceiver::ResultCode TransferReceiver::receive(const RxFrame& frame, Tra
|
|||||||
// Transfer timestamps are derived from the first frame
|
// Transfer timestamps are derived from the first frame
|
||||||
if (frame.isFirst())
|
if (frame.isFirst())
|
||||||
{
|
{
|
||||||
this_transfer_ts_monotonic_ = frame.getMonotonicTimestamp();
|
this_transfer_ts_ = frame.getMonotonicTimestamp();
|
||||||
first_frame_ts_utc_ = frame.getUtcTimestamp();
|
first_frame_ts_ = frame.getUtcTimestamp();
|
||||||
}
|
}
|
||||||
|
|
||||||
if (frame.isFirst() && frame.isLast())
|
if (frame.isFirst() && frame.isLast())
|
||||||
@@ -158,20 +157,19 @@ TransferReceiver::ResultCode TransferReceiver::receive(const RxFrame& frame, Tra
|
|||||||
return ResultNotComplete;
|
return ResultNotComplete;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool TransferReceiver::isTimedOut(uint64_t ts_monotonic) const
|
bool TransferReceiver::isTimedOut(MonotonicTime current_ts) const
|
||||||
{
|
{
|
||||||
static const uint64_t INTERVAL_MULT = (1 << TransferID::BitLen) / 2 + 1;
|
static const uint64_t INTERVAL_MULT = (1 << TransferID::BitLen) / 2 + 1;
|
||||||
const uint64_t ts = this_transfer_ts_monotonic_;
|
if (current_ts <= this_transfer_ts_)
|
||||||
if (ts_monotonic <= ts)
|
|
||||||
return false;
|
return false;
|
||||||
return (ts_monotonic - ts) > (uint64_t(transfer_interval_) * INTERVAL_MULT);
|
return (current_ts - this_transfer_ts_).toUSec() > (uint64_t(transfer_interval_usec_) * INTERVAL_MULT);
|
||||||
}
|
}
|
||||||
|
|
||||||
TransferReceiver::ResultCode TransferReceiver::addFrame(const RxFrame& frame, TransferBufferAccessor& tba)
|
TransferReceiver::ResultCode TransferReceiver::addFrame(const RxFrame& frame, TransferBufferAccessor& tba)
|
||||||
{
|
{
|
||||||
if ((frame.getMonotonicTimestamp() == 0) ||
|
if ((frame.getMonotonicTimestamp().isZero()) ||
|
||||||
(frame.getMonotonicTimestamp() < prev_transfer_ts_monotonic_) ||
|
(frame.getMonotonicTimestamp() < prev_transfer_ts_) ||
|
||||||
(frame.getMonotonicTimestamp() < this_transfer_ts_monotonic_))
|
(frame.getMonotonicTimestamp() < this_transfer_ts_))
|
||||||
{
|
{
|
||||||
return ResultNotComplete;
|
return ResultNotComplete;
|
||||||
}
|
}
|
||||||
@@ -182,7 +180,7 @@ TransferReceiver::ResultCode TransferReceiver::addFrame(const RxFrame& frame, Tr
|
|||||||
const bool first_fame = frame.isFirst();
|
const bool first_fame = frame.isFirst();
|
||||||
const TidRelation tid_rel = getTidRelation(frame);
|
const TidRelation tid_rel = getTidRelation(frame);
|
||||||
const bool iface_timed_out =
|
const bool iface_timed_out =
|
||||||
(frame.getMonotonicTimestamp() - this_transfer_ts_monotonic_) > (uint64_t(transfer_interval_) * 2);
|
(frame.getMonotonicTimestamp() - this_transfer_ts_).toUSec() > (uint64_t(transfer_interval_usec_) * 2);
|
||||||
|
|
||||||
const bool need_restart = // FSM, the hard way
|
const bool need_restart = // FSM, the hard way
|
||||||
(not_initialized) ||
|
(not_initialized) ||
|
||||||
|
|||||||
@@ -10,8 +10,8 @@
|
|||||||
namespace uavcan
|
namespace uavcan
|
||||||
{
|
{
|
||||||
|
|
||||||
int TransferSender::send(const uint8_t* payload, int payload_len, uint64_t monotonic_tx_deadline,
|
int TransferSender::send(const uint8_t* payload, int payload_len, MonotonicTime tx_deadline,
|
||||||
uint64_t monotonic_blocking_deadline, TransferType transfer_type, NodeID dst_node_id,
|
MonotonicTime blocking_deadline, TransferType transfer_type, NodeID dst_node_id,
|
||||||
TransferID tid)
|
TransferID tid)
|
||||||
{
|
{
|
||||||
Frame frame(data_type_.getID(), transfer_type, dispatcher_.getSelfNodeID(), dst_node_id, 0, tid);
|
Frame frame(data_type_.getID(), transfer_type, dispatcher_.getSelfNodeID(), dst_node_id, 0, tid);
|
||||||
@@ -27,7 +27,7 @@ int TransferSender::send(const uint8_t* payload, int payload_len, uint64_t monot
|
|||||||
}
|
}
|
||||||
frame.makeLast();
|
frame.makeLast();
|
||||||
assert(frame.isLast() && frame.isFirst());
|
assert(frame.isLast() && frame.isFirst());
|
||||||
return dispatcher_.send(frame, monotonic_tx_deadline, monotonic_blocking_deadline, qos_);
|
return dispatcher_.send(frame, tx_deadline, blocking_deadline, qos_);
|
||||||
}
|
}
|
||||||
else // Multi Frame Transfer
|
else // Multi Frame Transfer
|
||||||
{
|
{
|
||||||
@@ -57,7 +57,7 @@ int TransferSender::send(const uint8_t* payload, int payload_len, uint64_t monot
|
|||||||
|
|
||||||
while (true)
|
while (true)
|
||||||
{
|
{
|
||||||
const int send_res = dispatcher_.send(frame, monotonic_tx_deadline, monotonic_blocking_deadline, qos_);
|
const int send_res = dispatcher_.send(frame, tx_deadline, blocking_deadline, qos_);
|
||||||
if (send_res < 0)
|
if (send_res < 0)
|
||||||
return send_res;
|
return send_res;
|
||||||
|
|
||||||
@@ -84,13 +84,13 @@ int TransferSender::send(const uint8_t* payload, int payload_len, uint64_t monot
|
|||||||
return -1; // Return path analysis is apparently broken. There should be no warning, this 'return' is unreachable.
|
return -1; // Return path analysis is apparently broken. There should be no warning, this 'return' is unreachable.
|
||||||
}
|
}
|
||||||
|
|
||||||
int TransferSender::send(const uint8_t* payload, int payload_len, uint64_t monotonic_tx_deadline,
|
int TransferSender::send(const uint8_t* payload, int payload_len, MonotonicTime tx_deadline,
|
||||||
uint64_t monotonic_blocking_deadline, TransferType transfer_type, NodeID dst_node_id)
|
MonotonicTime blocking_deadline, TransferType transfer_type, NodeID dst_node_id)
|
||||||
{
|
{
|
||||||
const OutgoingTransferRegistryKey otr_key(data_type_.getID(), transfer_type, dst_node_id);
|
const OutgoingTransferRegistryKey otr_key(data_type_.getID(), transfer_type, dst_node_id);
|
||||||
|
|
||||||
assert(monotonic_tx_deadline > 0);
|
assert(!tx_deadline.isZero());
|
||||||
const uint64_t otr_deadline = monotonic_tx_deadline + max_transfer_interval_;
|
const MonotonicTime otr_deadline = tx_deadline + max_transfer_interval_;
|
||||||
|
|
||||||
TransferID* const tid = dispatcher_.getOutgoingTransferRegistry().accessOrCreate(otr_key, otr_deadline);
|
TransferID* const tid = dispatcher_.getOutgoingTransferRegistry().accessOrCreate(otr_key, otr_deadline);
|
||||||
if (tid == NULL)
|
if (tid == NULL)
|
||||||
@@ -103,7 +103,7 @@ int TransferSender::send(const uint8_t* payload, int payload_len, uint64_t monot
|
|||||||
const TransferID this_tid = tid->get();
|
const TransferID this_tid = tid->get();
|
||||||
tid->increment();
|
tid->increment();
|
||||||
|
|
||||||
return send(payload, payload_len, monotonic_tx_deadline, monotonic_blocking_deadline, transfer_type,
|
return send(payload, payload_len, tx_deadline, blocking_deadline, transfer_type,
|
||||||
dst_node_id, this_tid);
|
dst_node_id, this_tid);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -0,0 +1,31 @@
|
|||||||
|
/*
|
||||||
|
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <uavcan/time.hpp>
|
||||||
|
#include <uavcan/Timestamp.hpp>
|
||||||
|
|
||||||
|
namespace uavcan
|
||||||
|
{
|
||||||
|
/*
|
||||||
|
* UtcTime
|
||||||
|
*/
|
||||||
|
UtcTime::UtcTime(const Timestamp& ts) // Implicit
|
||||||
|
{
|
||||||
|
operator=(ts);
|
||||||
|
}
|
||||||
|
|
||||||
|
UtcTime& UtcTime::operator=(const Timestamp& ts)
|
||||||
|
{
|
||||||
|
*this = fromUSec(ts.husec * Timestamp::USEC_PER_LSB);
|
||||||
|
return *this;
|
||||||
|
}
|
||||||
|
|
||||||
|
UtcTime::operator Timestamp() const
|
||||||
|
{
|
||||||
|
Timestamp ts;
|
||||||
|
ts.husec = toUSec() / Timestamp::USEC_PER_LSB;
|
||||||
|
return ts;
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
+24
-22
@@ -8,37 +8,39 @@
|
|||||||
namespace uavcan
|
namespace uavcan
|
||||||
{
|
{
|
||||||
|
|
||||||
const uint64_t Timer::InfinitePeriod;
|
void Timer::handleDeadline(MonotonicTime current_timestamp)
|
||||||
|
|
||||||
void Timer::handleMonotonicDeadline(uint64_t monotonic_timestamp)
|
|
||||||
{
|
{
|
||||||
assert(!isRunning());
|
assert(!isRunning());
|
||||||
|
|
||||||
if (period_ != InfinitePeriod)
|
const MonotonicTime scheduled_deadline = getDeadline();
|
||||||
startWithDeadline(getMonotonicDeadline() + period_);
|
|
||||||
|
if (period_ < MonotonicDuration::getInfinite())
|
||||||
|
startWithDeadline(scheduled_deadline + period_);
|
||||||
|
|
||||||
// Application can re-register the timer with different params, it's OK
|
// Application can re-register the timer with different params, it's OK
|
||||||
handleTimerEvent(TimerEvent(this, getMonotonicDeadline(), monotonic_timestamp));
|
handleTimerEvent(TimerEvent(this, scheduled_deadline, current_timestamp));
|
||||||
}
|
}
|
||||||
|
|
||||||
void Timer::startOneShotWithDeadline(uint64_t monotonic_deadline_usec)
|
void Timer::startOneShotWithDeadline(MonotonicTime deadline)
|
||||||
{
|
{
|
||||||
period_ = InfinitePeriod;
|
|
||||||
MonotonicDeadlineHandler::startWithDeadline(monotonic_deadline_usec);
|
|
||||||
}
|
|
||||||
|
|
||||||
void Timer::startOneShotWithDelay(uint64_t delay_usec)
|
|
||||||
{
|
|
||||||
period_ = InfinitePeriod;
|
|
||||||
MonotonicDeadlineHandler::startWithDelay(delay_usec);
|
|
||||||
}
|
|
||||||
|
|
||||||
void Timer::startPeriodic(uint64_t period_usec)
|
|
||||||
{
|
|
||||||
assert(period_usec != InfinitePeriod);
|
|
||||||
stop();
|
stop();
|
||||||
period_ = period_usec;
|
period_ = MonotonicDuration::getInfinite();
|
||||||
MonotonicDeadlineHandler::startWithDelay(period_usec);
|
MonotonicDeadlineHandler::startWithDeadline(deadline);
|
||||||
|
}
|
||||||
|
|
||||||
|
void Timer::startOneShotWithDelay(MonotonicDuration delay)
|
||||||
|
{
|
||||||
|
stop();
|
||||||
|
period_ = MonotonicDuration::getInfinite();
|
||||||
|
MonotonicDeadlineHandler::startWithDelay(delay);
|
||||||
|
}
|
||||||
|
|
||||||
|
void Timer::startPeriodic(MonotonicDuration period)
|
||||||
|
{
|
||||||
|
assert(period < MonotonicDuration::getInfinite());
|
||||||
|
stop();
|
||||||
|
period_ = period;
|
||||||
|
MonotonicDeadlineHandler::startWithDelay(period);
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|||||||
+20
-14
@@ -28,21 +28,21 @@ public:
|
|||||||
utc += usec;
|
utc += usec;
|
||||||
}
|
}
|
||||||
|
|
||||||
uint64_t getMonotonicMicroseconds() const
|
uavcan::MonotonicTime getMonotonic() const
|
||||||
{
|
{
|
||||||
assert(this);
|
assert(this);
|
||||||
const uint64_t res = monotonic;
|
const uint64_t res = monotonic;
|
||||||
advance(monotonic_auto_advance);
|
advance(monotonic_auto_advance);
|
||||||
return res;
|
return uavcan::MonotonicTime::fromUSec(res);
|
||||||
}
|
}
|
||||||
|
|
||||||
uint64_t getUtcMicroseconds() const
|
uavcan::UtcTime getUtc() const
|
||||||
{
|
{
|
||||||
assert(this);
|
assert(this);
|
||||||
return utc;
|
return uavcan::UtcTime::fromUSec(utc);
|
||||||
}
|
}
|
||||||
|
|
||||||
void adjustUtcMicroseconds(uint64_t new_timestamp_usec, int64_t offset_usec)
|
void adjustUtc(uavcan::UtcTime, uavcan::UtcDuration)
|
||||||
{
|
{
|
||||||
assert(0);
|
assert(0);
|
||||||
}
|
}
|
||||||
@@ -52,37 +52,43 @@ public:
|
|||||||
class SystemClockDriver : public uavcan::ISystemClock
|
class SystemClockDriver : public uavcan::ISystemClock
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
uint64_t getMonotonicMicroseconds() const
|
uavcan::MonotonicTime getMonotonic() const
|
||||||
{
|
{
|
||||||
struct timespec ts;
|
struct timespec ts;
|
||||||
const int ret = clock_gettime(CLOCK_MONOTONIC, &ts);
|
const int ret = clock_gettime(CLOCK_MONOTONIC, &ts);
|
||||||
if (ret != 0)
|
if (ret != 0)
|
||||||
{
|
{
|
||||||
assert(0);
|
assert(0);
|
||||||
return 0;
|
return uavcan::MonotonicTime();
|
||||||
}
|
}
|
||||||
return uint64_t(ts.tv_sec) * 1000000UL + ts.tv_nsec / 1000UL;
|
return uavcan::MonotonicTime::fromUSec(uint64_t(ts.tv_sec) * 1000000UL + ts.tv_nsec / 1000UL);
|
||||||
}
|
}
|
||||||
|
|
||||||
uint64_t getUtcMicroseconds() const
|
uavcan::UtcTime getUtc() const
|
||||||
{
|
{
|
||||||
struct timeval tv;
|
struct timeval tv;
|
||||||
const int ret = gettimeofday(&tv, NULL);
|
const int ret = gettimeofday(&tv, NULL);
|
||||||
if (ret != 0)
|
if (ret != 0)
|
||||||
{
|
{
|
||||||
assert(0);
|
assert(0);
|
||||||
return 0;
|
return uavcan::UtcTime();
|
||||||
}
|
}
|
||||||
return uint64_t(tv.tv_sec) * 1000000UL + tv.tv_usec;
|
return uavcan::UtcTime::fromUSec(uint64_t(tv.tv_sec) * 1000000UL + tv.tv_usec);
|
||||||
}
|
}
|
||||||
|
|
||||||
void adjustUtcMicroseconds(uint64_t new_timestamp_usec, int64_t offset_usec)
|
void adjustUtc(uavcan::UtcTime, uavcan::UtcDuration)
|
||||||
{
|
{
|
||||||
assert(0);
|
assert(0);
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
static bool areTimestampsClose(int64_t a, int64_t b, int64_t precision_usec = 10000)
|
static uavcan::MonotonicTime tsMono(uint64_t usec) { return uavcan::MonotonicTime::fromUSec(usec); }
|
||||||
|
static uavcan::UtcTime tsUtc(uint64_t usec) { return uavcan::UtcTime::fromUSec(usec); }
|
||||||
|
|
||||||
|
static uavcan::MonotonicDuration durMono(int64_t usec) { return uavcan::MonotonicDuration::fromUSec(usec); }
|
||||||
|
|
||||||
|
template <typename T>
|
||||||
|
static bool areTimestampsClose(const T& a, const T& b, int64_t precision_usec = 10000)
|
||||||
{
|
{
|
||||||
return std::abs(a - b) < precision_usec;
|
return (a - b).getAbs().toUSec() < precision_usec;
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -47,35 +47,35 @@ TEST(Scheduler, Timers)
|
|||||||
|
|
||||||
ASSERT_EQ(0, sch.getMonotonicDeadlineScheduler().getNumHandlers());
|
ASSERT_EQ(0, sch.getMonotonicDeadlineScheduler().getNumHandlers());
|
||||||
|
|
||||||
const uint64_t start_ts = clock_driver.getMonotonicMicroseconds();
|
const uavcan::MonotonicTime start_ts = clock_driver.getMonotonic();
|
||||||
|
|
||||||
a.startOneShotWithDeadline(start_ts + 100000);
|
a.startOneShotWithDeadline(start_ts + durMono(100000));
|
||||||
b.startPeriodic(1000);
|
b.startPeriodic(durMono(1000));
|
||||||
|
|
||||||
ASSERT_EQ(2, sch.getMonotonicDeadlineScheduler().getNumHandlers());
|
ASSERT_EQ(2, sch.getMonotonicDeadlineScheduler().getNumHandlers());
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Spinning
|
* Spinning
|
||||||
*/
|
*/
|
||||||
ASSERT_EQ(0, sch.spin(start_ts + 1000000));
|
ASSERT_EQ(0, sch.spin(start_ts + durMono(1000000)));
|
||||||
|
|
||||||
ASSERT_EQ(1, tcc.events_a.size());
|
ASSERT_EQ(1, tcc.events_a.size());
|
||||||
ASSERT_TRUE(areTimestampsClose(tcc.events_a[0].scheduled_monotonic_deadline, start_ts + 100000));
|
ASSERT_TRUE(areTimestampsClose(tcc.events_a[0].scheduled_deadline, start_ts + durMono(100000)));
|
||||||
ASSERT_TRUE(areTimestampsClose(tcc.events_a[0].monotonic_timestamp,
|
ASSERT_TRUE(areTimestampsClose(tcc.events_a[0].current_timestamp,
|
||||||
tcc.events_a[0].scheduled_monotonic_deadline));
|
tcc.events_a[0].scheduled_deadline));
|
||||||
ASSERT_EQ(&a, tcc.events_a[0].timer);
|
ASSERT_EQ(&a, tcc.events_a[0].timer);
|
||||||
|
|
||||||
ASSERT_LT(900, tcc.events_b.size());
|
ASSERT_LT(900, tcc.events_b.size());
|
||||||
ASSERT_GT(1100, tcc.events_b.size());
|
ASSERT_GT(1100, tcc.events_b.size());
|
||||||
{
|
{
|
||||||
uint64_t next_expected_deadline = start_ts + 1000;
|
uavcan::MonotonicTime next_expected_deadline = start_ts + durMono(1000);
|
||||||
for (unsigned int i = 0; i < tcc.events_b.size(); i++)
|
for (unsigned int i = 0; i < tcc.events_b.size(); i++)
|
||||||
{
|
{
|
||||||
ASSERT_TRUE(areTimestampsClose(tcc.events_b[i].scheduled_monotonic_deadline, next_expected_deadline));
|
ASSERT_TRUE(areTimestampsClose(tcc.events_b[i].scheduled_deadline, next_expected_deadline));
|
||||||
ASSERT_TRUE(areTimestampsClose(tcc.events_b[i].monotonic_timestamp,
|
ASSERT_TRUE(areTimestampsClose(tcc.events_b[i].current_timestamp,
|
||||||
tcc.events_b[i].scheduled_monotonic_deadline));
|
tcc.events_b[i].scheduled_deadline));
|
||||||
ASSERT_EQ(&b, tcc.events_b[i].timer);
|
ASSERT_EQ(&b, tcc.events_b[i].timer);
|
||||||
next_expected_deadline += 1000;
|
next_expected_deadline += durMono(1000);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -85,12 +85,12 @@ TEST(Scheduler, Timers)
|
|||||||
ASSERT_EQ(1, sch.getMonotonicDeadlineScheduler().getNumHandlers());
|
ASSERT_EQ(1, sch.getMonotonicDeadlineScheduler().getNumHandlers());
|
||||||
|
|
||||||
ASSERT_FALSE(a.isRunning());
|
ASSERT_FALSE(a.isRunning());
|
||||||
ASSERT_EQ(uavcan::Timer::InfinitePeriod, a.getPeriod());
|
ASSERT_EQ(uavcan::MonotonicDuration::getInfinite(), a.getPeriod());
|
||||||
|
|
||||||
ASSERT_TRUE(b.isRunning());
|
ASSERT_TRUE(b.isRunning());
|
||||||
ASSERT_EQ(1000, b.getPeriod());
|
ASSERT_EQ(1000, b.getPeriod().toUSec());
|
||||||
}
|
}
|
||||||
|
|
||||||
ASSERT_EQ(0, sch.getMonotonicDeadlineScheduler().getNumHandlers()); // Both timers were destroyed now
|
ASSERT_EQ(0, sch.getMonotonicDeadlineScheduler().getNumHandlers()); // Both timers were destroyed now
|
||||||
ASSERT_EQ(0, sch.spin(clock_driver.getMonotonicMicroseconds() + 1000)); // Spin some more without timers
|
ASSERT_EQ(0, sch.spin(clock_driver.getMonotonic() + durMono(1000))); // Spin some more without timers
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -21,12 +21,17 @@ public:
|
|||||||
struct FrameWithTime
|
struct FrameWithTime
|
||||||
{
|
{
|
||||||
uavcan::CanFrame frame;
|
uavcan::CanFrame frame;
|
||||||
uint64_t time;
|
uavcan::MonotonicTime time;
|
||||||
|
|
||||||
FrameWithTime(const uavcan::CanFrame& frame, uint64_t time)
|
FrameWithTime(const uavcan::CanFrame& frame, uavcan::MonotonicTime time)
|
||||||
: frame(frame)
|
: frame(frame)
|
||||||
, time(time)
|
, time(time)
|
||||||
{ }
|
{ }
|
||||||
|
|
||||||
|
FrameWithTime(const uavcan::CanFrame& frame, uint64_t time_usec)
|
||||||
|
: frame(frame)
|
||||||
|
, time(uavcan::MonotonicTime::fromUSec(time_usec))
|
||||||
|
{ }
|
||||||
};
|
};
|
||||||
|
|
||||||
std::queue<FrameWithTime> tx; ///< Queue of outgoing frames (bus <-- library)
|
std::queue<FrameWithTime> tx; ///< Queue of outgoing frames (bus <-- library)
|
||||||
@@ -47,7 +52,7 @@ public:
|
|||||||
|
|
||||||
void pushRx(const uavcan::CanFrame& frame)
|
void pushRx(const uavcan::CanFrame& frame)
|
||||||
{
|
{
|
||||||
rx.push(FrameWithTime(frame, iclock.getMonotonicMicroseconds()));
|
rx.push(FrameWithTime(frame, iclock.getMonotonic()));
|
||||||
}
|
}
|
||||||
|
|
||||||
void pushRx(const uavcan::RxFrame& frame)
|
void pushRx(const uavcan::RxFrame& frame)
|
||||||
@@ -57,7 +62,7 @@ public:
|
|||||||
rx.push(FrameWithTime(can_frame, frame.getMonotonicTimestamp()));
|
rx.push(FrameWithTime(can_frame, frame.getMonotonicTimestamp()));
|
||||||
}
|
}
|
||||||
|
|
||||||
bool matchAndPopTx(const uavcan::CanFrame& frame, uint64_t tx_deadline)
|
bool matchAndPopTx(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline)
|
||||||
{
|
{
|
||||||
if (tx.empty())
|
if (tx.empty())
|
||||||
{
|
{
|
||||||
@@ -69,7 +74,12 @@ public:
|
|||||||
return (frame_time.frame == frame) && (frame_time.time == tx_deadline);
|
return (frame_time.frame == frame) && (frame_time.time == tx_deadline);
|
||||||
}
|
}
|
||||||
|
|
||||||
int send(const uavcan::CanFrame& frame, uint64_t tx_timeout_usec)
|
bool matchAndPopTx(const uavcan::CanFrame& frame, uint64_t tx_deadline_usec)
|
||||||
|
{
|
||||||
|
return matchAndPopTx(frame, uavcan::MonotonicTime::fromUSec(tx_deadline_usec));
|
||||||
|
}
|
||||||
|
|
||||||
|
int send(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline)
|
||||||
{
|
{
|
||||||
assert(this);
|
assert(this);
|
||||||
EXPECT_TRUE(writeable); // Shall never be called when not writeable
|
EXPECT_TRUE(writeable); // Shall never be called when not writeable
|
||||||
@@ -77,12 +87,11 @@ public:
|
|||||||
return -1;
|
return -1;
|
||||||
if (!writeable)
|
if (!writeable)
|
||||||
return 0;
|
return 0;
|
||||||
const uint64_t monotonic_deadline = tx_timeout_usec + iclock.getMonotonicMicroseconds();
|
tx.push(FrameWithTime(frame, tx_deadline));
|
||||||
tx.push(FrameWithTime(frame, monotonic_deadline));
|
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
int receive(uavcan::CanFrame& out_frame, uint64_t& out_ts_monotonic_usec, uint64_t& out_ts_utc_usec)
|
int receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic, uavcan::UtcTime& out_ts_utc)
|
||||||
{
|
{
|
||||||
assert(this);
|
assert(this);
|
||||||
EXPECT_TRUE(rx.size()); // Shall never be called when not readable
|
EXPECT_TRUE(rx.size()); // Shall never be called when not readable
|
||||||
@@ -93,8 +102,8 @@ public:
|
|||||||
const FrameWithTime frame = rx.front();
|
const FrameWithTime frame = rx.front();
|
||||||
rx.pop();
|
rx.pop();
|
||||||
out_frame = frame.frame;
|
out_frame = frame.frame;
|
||||||
out_ts_monotonic_usec = frame.time;
|
out_ts_monotonic = frame.time;
|
||||||
out_ts_utc_usec = 0;
|
out_ts_utc = uavcan::UtcTime();
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -119,7 +128,7 @@ public:
|
|||||||
, select_failure(false)
|
, select_failure(false)
|
||||||
{ }
|
{ }
|
||||||
|
|
||||||
int select(int& inout_write_iface_mask, int& inout_read_iface_mask, uint64_t timeout_usec)
|
int select(int& inout_write_iface_mask, int& inout_read_iface_mask, uavcan::MonotonicTime deadline)
|
||||||
{
|
{
|
||||||
assert(this);
|
assert(this);
|
||||||
//std::cout << "Write/read masks: " << inout_write_iface_mask << "/" << inout_read_iface_mask << std::endl;
|
//std::cout << "Write/read masks: " << inout_write_iface_mask << "/" << inout_read_iface_mask << std::endl;
|
||||||
@@ -145,11 +154,19 @@ public:
|
|||||||
inout_read_iface_mask = out_read_mask;
|
inout_read_iface_mask = out_read_mask;
|
||||||
if ((out_write_mask | out_read_mask) == 0)
|
if ((out_write_mask | out_read_mask) == 0)
|
||||||
{
|
{
|
||||||
|
const uavcan::MonotonicTime ts = iclock.getMonotonic();
|
||||||
|
const uavcan::MonotonicDuration diff = deadline - ts;
|
||||||
SystemClockMock* const mock = dynamic_cast<SystemClockMock*>(&iclock);
|
SystemClockMock* const mock = dynamic_cast<SystemClockMock*>(&iclock);
|
||||||
if (mock)
|
if (mock)
|
||||||
mock->advance(timeout_usec); // Emulating timeout
|
{
|
||||||
|
if (diff.isPositive())
|
||||||
|
mock->advance(diff.toUSec()); // Emulating timeout
|
||||||
|
}
|
||||||
else
|
else
|
||||||
usleep(timeout_usec);
|
{
|
||||||
|
if (diff.isPositive())
|
||||||
|
usleep(diff.toUSec());
|
||||||
|
}
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
return 1; // This value is not being checked anyway, it just has to be greater than zero
|
return 1; // This value is not being checked anyway, it just has to be greater than zero
|
||||||
|
|||||||
@@ -17,7 +17,7 @@ TEST(CanIOManager, CanDriverMock)
|
|||||||
// All WR, no RD
|
// All WR, no RD
|
||||||
int mask_wr = 7;
|
int mask_wr = 7;
|
||||||
int mask_rd = 7;
|
int mask_rd = 7;
|
||||||
EXPECT_LT(0, driver.select(mask_wr, mask_rd, 100));
|
EXPECT_LT(0, driver.select(mask_wr, mask_rd, uavcan::MonotonicTime::fromUSec(100)));
|
||||||
EXPECT_EQ(7, mask_wr);
|
EXPECT_EQ(7, mask_wr);
|
||||||
EXPECT_EQ(0, mask_rd);
|
EXPECT_EQ(0, mask_rd);
|
||||||
|
|
||||||
@@ -27,7 +27,7 @@ TEST(CanIOManager, CanDriverMock)
|
|||||||
// No WR, no RD
|
// No WR, no RD
|
||||||
mask_wr = 7;
|
mask_wr = 7;
|
||||||
mask_rd = 7;
|
mask_rd = 7;
|
||||||
EXPECT_EQ(0, driver.select(mask_wr, mask_rd, 100));
|
EXPECT_EQ(0, driver.select(mask_wr, mask_rd, uavcan::MonotonicTime::fromUSec(100)));
|
||||||
EXPECT_EQ(0, mask_wr);
|
EXPECT_EQ(0, mask_wr);
|
||||||
EXPECT_EQ(0, mask_rd);
|
EXPECT_EQ(0, mask_rd);
|
||||||
EXPECT_EQ(100, clockmock.monotonic);
|
EXPECT_EQ(100, clockmock.monotonic);
|
||||||
@@ -38,22 +38,23 @@ TEST(CanIOManager, CanDriverMock)
|
|||||||
driver.ifaces.at(1).pushRx(fr1);
|
driver.ifaces.at(1).pushRx(fr1);
|
||||||
mask_wr = 7;
|
mask_wr = 7;
|
||||||
mask_rd = 6;
|
mask_rd = 6;
|
||||||
EXPECT_LT(0, driver.select(mask_wr, mask_rd, 100));
|
EXPECT_LT(0, driver.select(mask_wr, mask_rd, uavcan::MonotonicTime::fromUSec(100)));
|
||||||
EXPECT_EQ(0, mask_wr);
|
EXPECT_EQ(0, mask_wr);
|
||||||
EXPECT_EQ(2, mask_rd);
|
EXPECT_EQ(2, mask_rd);
|
||||||
CanFrame fr2;
|
CanFrame fr2;
|
||||||
uint64_t ts_monotonic, ts_utc;
|
uavcan::MonotonicTime ts_monotonic;
|
||||||
|
uavcan::UtcTime ts_utc;
|
||||||
EXPECT_EQ(1, driver.getIface(1)->receive(fr2, ts_monotonic, ts_utc));
|
EXPECT_EQ(1, driver.getIface(1)->receive(fr2, ts_monotonic, ts_utc));
|
||||||
EXPECT_EQ(fr1, fr2);
|
EXPECT_EQ(fr1, fr2);
|
||||||
EXPECT_EQ(100, ts_monotonic);
|
EXPECT_EQ(100, ts_monotonic.toUSec());
|
||||||
EXPECT_EQ(0, ts_utc);
|
EXPECT_EQ(0, ts_utc.toUSec());
|
||||||
|
|
||||||
// #0 WR, #1 RD, Select failure
|
// #0 WR, #1 RD, Select failure
|
||||||
driver.ifaces.at(0).writeable = true;
|
driver.ifaces.at(0).writeable = true;
|
||||||
driver.select_failure = true;
|
driver.select_failure = true;
|
||||||
mask_wr = 1;
|
mask_wr = 1;
|
||||||
mask_rd = 7;
|
mask_rd = 7;
|
||||||
EXPECT_EQ(-1, driver.select(mask_wr, mask_rd, 100));
|
EXPECT_EQ(-1, driver.select(mask_wr, mask_rd, uavcan::MonotonicTime::fromUSec(100)));
|
||||||
EXPECT_EQ(1, mask_wr); // Leaving masks unchanged - library must ignore them
|
EXPECT_EQ(1, mask_wr); // Leaving masks unchanged - the library must ignore them
|
||||||
EXPECT_EQ(7, mask_rd);
|
EXPECT_EQ(7, mask_rd);
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -5,9 +5,8 @@
|
|||||||
#include <gtest/gtest.h>
|
#include <gtest/gtest.h>
|
||||||
#include "can.hpp"
|
#include "can.hpp"
|
||||||
|
|
||||||
|
|
||||||
static bool rxFrameEquals(const uavcan::CanRxFrame& rxframe, const uavcan::CanFrame& frame,
|
static bool rxFrameEquals(const uavcan::CanRxFrame& rxframe, const uavcan::CanFrame& frame,
|
||||||
uint64_t timestamp, int iface_index)
|
uint64_t timestamp_usec, int iface_index)
|
||||||
{
|
{
|
||||||
if (static_cast<const uavcan::CanFrame&>(rxframe) != frame)
|
if (static_cast<const uavcan::CanFrame&>(rxframe) != frame)
|
||||||
{
|
{
|
||||||
@@ -16,7 +15,7 @@ static bool rxFrameEquals(const uavcan::CanRxFrame& rxframe, const uavcan::CanFr
|
|||||||
<< " " << frame.toString(uavcan::CanFrame::StrAligned) << std::endl;
|
<< " " << frame.toString(uavcan::CanFrame::StrAligned) << std::endl;
|
||||||
}
|
}
|
||||||
return (static_cast<const uavcan::CanFrame&>(rxframe) == frame) &&
|
return (static_cast<const uavcan::CanFrame&>(rxframe) == frame) &&
|
||||||
(rxframe.ts_monotonic == timestamp) && (rxframe.iface_index == iface_index);
|
(rxframe.ts_mono == uavcan::MonotonicTime::fromUSec(timestamp_usec)) && (rxframe.iface_index == iface_index);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST(CanIOManager, Reception)
|
TEST(CanIOManager, Reception)
|
||||||
@@ -38,7 +37,7 @@ TEST(CanIOManager, Reception)
|
|||||||
* Empty, will time out
|
* Empty, will time out
|
||||||
*/
|
*/
|
||||||
uavcan::CanRxFrame frame;
|
uavcan::CanRxFrame frame;
|
||||||
EXPECT_EQ(0, iomgr.receive(frame, 100));
|
EXPECT_EQ(0, iomgr.receive(frame, tsMono(100)));
|
||||||
EXPECT_EQ(100, clockmock.monotonic);
|
EXPECT_EQ(100, clockmock.monotonic);
|
||||||
EXPECT_EQ(100, clockmock.utc);
|
EXPECT_EQ(100, clockmock.utc);
|
||||||
|
|
||||||
@@ -61,36 +60,36 @@ TEST(CanIOManager, Reception)
|
|||||||
driver.ifaces.at(1).pushRx(frames[1][2]);
|
driver.ifaces.at(1).pushRx(frames[1][2]);
|
||||||
clockmock.advance(10);
|
clockmock.advance(10);
|
||||||
|
|
||||||
EXPECT_EQ(1, iomgr.receive(frame, 0));
|
EXPECT_EQ(1, iomgr.receive(frame, uavcan::MonotonicTime()));
|
||||||
EXPECT_TRUE(rxFrameEquals(frame, frames[0][0], 110, 0));
|
EXPECT_TRUE(rxFrameEquals(frame, frames[0][0], 110, 0));
|
||||||
|
|
||||||
EXPECT_EQ(1, iomgr.receive(frame, 0));
|
EXPECT_EQ(1, iomgr.receive(frame, uavcan::MonotonicTime()));
|
||||||
EXPECT_TRUE(rxFrameEquals(frame, frames[0][1], 120, 0));
|
EXPECT_TRUE(rxFrameEquals(frame, frames[0][1], 120, 0));
|
||||||
|
|
||||||
EXPECT_EQ(1, iomgr.receive(frame, 0));
|
EXPECT_EQ(1, iomgr.receive(frame, uavcan::MonotonicTime()));
|
||||||
EXPECT_TRUE(rxFrameEquals(frame, frames[0][2], 130, 0));
|
EXPECT_TRUE(rxFrameEquals(frame, frames[0][2], 130, 0));
|
||||||
|
|
||||||
EXPECT_EQ(1, iomgr.receive(frame, 0));
|
EXPECT_EQ(1, iomgr.receive(frame, uavcan::MonotonicTime()));
|
||||||
EXPECT_TRUE(rxFrameEquals(frame, frames[1][0], 110, 1));
|
EXPECT_TRUE(rxFrameEquals(frame, frames[1][0], 110, 1));
|
||||||
|
|
||||||
EXPECT_EQ(1, iomgr.receive(frame, 0));
|
EXPECT_EQ(1, iomgr.receive(frame, uavcan::MonotonicTime()));
|
||||||
EXPECT_TRUE(rxFrameEquals(frame, frames[1][1], 120, 1));
|
EXPECT_TRUE(rxFrameEquals(frame, frames[1][1], 120, 1));
|
||||||
|
|
||||||
EXPECT_EQ(1, iomgr.receive(frame, 0));
|
EXPECT_EQ(1, iomgr.receive(frame, uavcan::MonotonicTime()));
|
||||||
EXPECT_TRUE(rxFrameEquals(frame, frames[1][2], 130, 1));
|
EXPECT_TRUE(rxFrameEquals(frame, frames[1][2], 130, 1));
|
||||||
|
|
||||||
EXPECT_EQ(0, iomgr.receive(frame, 0)); // Will time out
|
EXPECT_EQ(0, iomgr.receive(frame, uavcan::MonotonicTime())); // Will time out
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Errors
|
* Errors
|
||||||
*/
|
*/
|
||||||
driver.select_failure = true;
|
driver.select_failure = true;
|
||||||
EXPECT_EQ(-1, iomgr.receive(frame, 0));
|
EXPECT_EQ(-1, iomgr.receive(frame, uavcan::MonotonicTime()));
|
||||||
|
|
||||||
driver.select_failure = false;
|
driver.select_failure = false;
|
||||||
driver.ifaces.at(1).pushRx(frames[0][0]);
|
driver.ifaces.at(1).pushRx(frames[0][0]);
|
||||||
driver.ifaces.at(1).rx_failure = true;
|
driver.ifaces.at(1).rx_failure = true;
|
||||||
EXPECT_EQ(-1, iomgr.receive(frame, 0));
|
EXPECT_EQ(-1, iomgr.receive(frame, uavcan::MonotonicTime()));
|
||||||
|
|
||||||
driver.ifaces.at(0).num_errors = 9000;
|
driver.ifaces.at(0).num_errors = 9000;
|
||||||
driver.ifaces.at(1).num_errors = 100500;
|
driver.ifaces.at(1).num_errors = 100500;
|
||||||
@@ -127,11 +126,11 @@ TEST(CanIOManager, Transmission)
|
|||||||
/*
|
/*
|
||||||
* Simple transmission
|
* Simple transmission
|
||||||
*/
|
*/
|
||||||
EXPECT_EQ(2, iomgr.send(frames[0], 100, 0, ALL_IFACES_MASK, CanTxQueue::Volatile)); // To both
|
EXPECT_EQ(2, iomgr.send(frames[0], tsMono(100), tsMono(0), ALL_IFACES_MASK, CanTxQueue::Volatile)); // To both
|
||||||
EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[0], 100));
|
EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[0], 100));
|
||||||
EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[0], 100));
|
EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[0], 100));
|
||||||
|
|
||||||
EXPECT_EQ(1, iomgr.send(frames[1], 200, 100, 2, CanTxQueue::Persistent)); // To #1
|
EXPECT_EQ(1, iomgr.send(frames[1], tsMono(200), tsMono(100), 2, CanTxQueue::Persistent)); // To #1
|
||||||
EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[1], 200));
|
EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[1], 200));
|
||||||
|
|
||||||
EXPECT_EQ(0, clockmock.monotonic);
|
EXPECT_EQ(0, clockmock.monotonic);
|
||||||
@@ -148,7 +147,7 @@ TEST(CanIOManager, Transmission)
|
|||||||
|
|
||||||
// Sending to both, #0 blocked
|
// Sending to both, #0 blocked
|
||||||
driver.ifaces.at(0).writeable = false;
|
driver.ifaces.at(0).writeable = false;
|
||||||
EXPECT_LT(0, iomgr.send(frames[0], 201, 200, ALL_IFACES_MASK, CanTxQueue::Persistent));
|
EXPECT_LT(0, iomgr.send(frames[0], tsMono(201), tsMono(200), ALL_IFACES_MASK, CanTxQueue::Persistent));
|
||||||
EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[0], 201));
|
EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[0], 201));
|
||||||
EXPECT_EQ(200, clockmock.monotonic);
|
EXPECT_EQ(200, clockmock.monotonic);
|
||||||
EXPECT_EQ(200, clockmock.utc);
|
EXPECT_EQ(200, clockmock.utc);
|
||||||
@@ -158,11 +157,11 @@ TEST(CanIOManager, Transmission)
|
|||||||
|
|
||||||
// Sending to both, both blocked
|
// Sending to both, both blocked
|
||||||
driver.ifaces.at(1).writeable = false;
|
driver.ifaces.at(1).writeable = false;
|
||||||
EXPECT_EQ(0, iomgr.send(frames[1], 777, 300, ALL_IFACES_MASK, CanTxQueue::Volatile));
|
EXPECT_EQ(0, iomgr.send(frames[1], tsMono(777), tsMono(300), ALL_IFACES_MASK, CanTxQueue::Volatile));
|
||||||
EXPECT_EQ(3, pool.getNumUsedBlocks()); // Total 3 frames in TX queue now
|
EXPECT_EQ(3, pool.getNumUsedBlocks()); // Total 3 frames in TX queue now
|
||||||
|
|
||||||
// Sending to #0, both blocked
|
// Sending to #0, both blocked
|
||||||
EXPECT_EQ(0, iomgr.send(frames[2], 888, 400, 1, CanTxQueue::Persistent));
|
EXPECT_EQ(0, iomgr.send(frames[2], tsMono(888), tsMono(400), 1, CanTxQueue::Persistent));
|
||||||
EXPECT_EQ(400, clockmock.monotonic);
|
EXPECT_EQ(400, clockmock.monotonic);
|
||||||
EXPECT_EQ(400, clockmock.utc);
|
EXPECT_EQ(400, clockmock.utc);
|
||||||
EXPECT_TRUE(driver.ifaces.at(0).tx.empty());
|
EXPECT_TRUE(driver.ifaces.at(0).tx.empty());
|
||||||
@@ -176,7 +175,7 @@ TEST(CanIOManager, Transmission)
|
|||||||
// Sending to #1, both writeable
|
// Sending to #1, both writeable
|
||||||
driver.ifaces.at(0).writeable = true;
|
driver.ifaces.at(0).writeable = true;
|
||||||
driver.ifaces.at(1).writeable = true;
|
driver.ifaces.at(1).writeable = true;
|
||||||
EXPECT_LT(0, iomgr.send(frames[0], 999, 500, 2, CanTxQueue::Persistent)); // One frame per each iface will be sent
|
EXPECT_LT(0, iomgr.send(frames[0], tsMono(999), tsMono(500), 2, CanTxQueue::Persistent)); // One frame per each iface will be sent
|
||||||
EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[1], 777)); // Note that frame[0] on iface #0 has expired
|
EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[1], 777)); // Note that frame[0] on iface #0 has expired
|
||||||
EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[0], 999)); // In different order due to prioritization
|
EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[0], 999)); // In different order due to prioritization
|
||||||
EXPECT_TRUE(driver.ifaces.at(0).tx.empty());
|
EXPECT_TRUE(driver.ifaces.at(0).tx.empty());
|
||||||
@@ -184,7 +183,7 @@ TEST(CanIOManager, Transmission)
|
|||||||
|
|
||||||
// Calling receive() to flush the rest two frames
|
// Calling receive() to flush the rest two frames
|
||||||
uavcan::CanRxFrame dummy_rx_frame;
|
uavcan::CanRxFrame dummy_rx_frame;
|
||||||
EXPECT_EQ(0, iomgr.receive(dummy_rx_frame, 0));
|
EXPECT_EQ(0, iomgr.receive(dummy_rx_frame, tsMono(0)));
|
||||||
EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[2], 888));
|
EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[2], 888));
|
||||||
EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[1], 777));
|
EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[1], 777));
|
||||||
|
|
||||||
@@ -202,9 +201,9 @@ TEST(CanIOManager, Transmission)
|
|||||||
driver.ifaces.at(1).writeable = false;
|
driver.ifaces.at(1).writeable = false;
|
||||||
|
|
||||||
// Sending 5 frames, one will be rejected
|
// Sending 5 frames, one will be rejected
|
||||||
EXPECT_EQ(0, iomgr.send(frames[2], 2222, 1000, ALL_IFACES_MASK, CanTxQueue::Persistent));
|
EXPECT_EQ(0, iomgr.send(frames[2], tsMono(2222), tsMono(1000), ALL_IFACES_MASK, CanTxQueue::Persistent));
|
||||||
EXPECT_EQ(0, iomgr.send(frames[0], 3333, 1100, 2, CanTxQueue::Persistent));
|
EXPECT_EQ(0, iomgr.send(frames[0], tsMono(3333), tsMono(1100), 2, CanTxQueue::Persistent));
|
||||||
EXPECT_EQ(0, iomgr.send(frames[1], 4444, 1200, ALL_IFACES_MASK, CanTxQueue::Volatile)); // One frame kicked here
|
EXPECT_EQ(0, iomgr.send(frames[1], tsMono(4444), tsMono(1200), ALL_IFACES_MASK, CanTxQueue::Volatile)); // One frame kicked here
|
||||||
|
|
||||||
// State checks
|
// State checks
|
||||||
EXPECT_EQ(4, pool.getNumUsedBlocks()); // TX queue is full
|
EXPECT_EQ(4, pool.getNumUsedBlocks()); // TX queue is full
|
||||||
@@ -222,12 +221,12 @@ TEST(CanIOManager, Transmission)
|
|||||||
|
|
||||||
// This shall transmit _some_ frames now, at least one per iface (exact number can be changed - it will be OK)
|
// This shall transmit _some_ frames now, at least one per iface (exact number can be changed - it will be OK)
|
||||||
uavcan::CanRxFrame rx_frame;
|
uavcan::CanRxFrame rx_frame;
|
||||||
EXPECT_EQ(1, iomgr.receive(rx_frame, 0)); // Non-blocking
|
EXPECT_EQ(1, iomgr.receive(rx_frame, tsMono(0))); // Non-blocking
|
||||||
EXPECT_TRUE(rxFrameEquals(rx_frame, rx_frames[0], 1200, 0));
|
EXPECT_TRUE(rxFrameEquals(rx_frame, rx_frames[0], 1200, 0));
|
||||||
EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[1], 4444));
|
EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[1], 4444));
|
||||||
EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[0], 3333));
|
EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[0], 3333));
|
||||||
|
|
||||||
EXPECT_EQ(1, iomgr.receive(rx_frame, 0));
|
EXPECT_EQ(1, iomgr.receive(rx_frame, tsMono(0)));
|
||||||
EXPECT_TRUE(rxFrameEquals(rx_frame, rx_frames[1], 1200, 1));
|
EXPECT_TRUE(rxFrameEquals(rx_frame, rx_frames[1], 1200, 1));
|
||||||
EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[2], 2222));
|
EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[2], 2222));
|
||||||
EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[2], 2222)); // Iface #1, frame[1] was rejected (VOLATILE)
|
EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[2], 2222)); // Iface #1, frame[1] was rejected (VOLATILE)
|
||||||
@@ -246,8 +245,8 @@ TEST(CanIOManager, Transmission)
|
|||||||
*/
|
*/
|
||||||
// Select failure
|
// Select failure
|
||||||
driver.select_failure = true;
|
driver.select_failure = true;
|
||||||
EXPECT_EQ(-1, iomgr.receive(rx_frame, 2000));
|
EXPECT_EQ(-1, iomgr.receive(rx_frame, tsMono(2000)));
|
||||||
EXPECT_EQ(-1, iomgr.send(frames[0], 2100, 2000, ALL_IFACES_MASK, CanTxQueue::Volatile));
|
EXPECT_EQ(-1, iomgr.send(frames[0], tsMono(2100), tsMono(2000), ALL_IFACES_MASK, CanTxQueue::Volatile));
|
||||||
EXPECT_EQ(1200, clockmock.monotonic);
|
EXPECT_EQ(1200, clockmock.monotonic);
|
||||||
EXPECT_EQ(1200, clockmock.utc);
|
EXPECT_EQ(1200, clockmock.utc);
|
||||||
|
|
||||||
@@ -257,14 +256,14 @@ TEST(CanIOManager, Transmission)
|
|||||||
driver.ifaces.at(1).writeable = true;
|
driver.ifaces.at(1).writeable = true;
|
||||||
driver.ifaces.at(0).tx_failure = true;
|
driver.ifaces.at(0).tx_failure = true;
|
||||||
driver.ifaces.at(1).tx_failure = true;
|
driver.ifaces.at(1).tx_failure = true;
|
||||||
EXPECT_GE(0, iomgr.send(frames[0], 2200, 0, ALL_IFACES_MASK, CanTxQueue::Persistent)); // Non-blocking - return < 0
|
EXPECT_GE(0, iomgr.send(frames[0], tsMono(2200), tsMono(0), ALL_IFACES_MASK, CanTxQueue::Persistent)); // Non-blocking - return < 0
|
||||||
|
|
||||||
ASSERT_EQ(2, pool.getNumUsedBlocks()); // Untransmitted frames will be buffered
|
ASSERT_EQ(2, pool.getNumUsedBlocks()); // Untransmitted frames will be buffered
|
||||||
|
|
||||||
// Failure removed - transmission shall proceed
|
// Failure removed - transmission shall proceed
|
||||||
driver.ifaces.at(0).tx_failure = false;
|
driver.ifaces.at(0).tx_failure = false;
|
||||||
driver.ifaces.at(1).tx_failure = false;
|
driver.ifaces.at(1).tx_failure = false;
|
||||||
EXPECT_EQ(0, iomgr.receive(rx_frame, 2500));
|
EXPECT_EQ(0, iomgr.receive(rx_frame, tsMono(2500)));
|
||||||
EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[0], 2200));
|
EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[0], 2200));
|
||||||
EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[0], 2200));
|
EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[0], 2200));
|
||||||
EXPECT_EQ(0, pool.getNumUsedBlocks()); // All transmitted
|
EXPECT_EQ(0, pool.getNumUsedBlocks()); // All transmitted
|
||||||
|
|||||||
@@ -33,8 +33,8 @@ static bool isInQueue(uavcan::CanTxQueue& queue, const uavcan::CanFrame& frame)
|
|||||||
|
|
||||||
TEST(CanTxQueue, Qos)
|
TEST(CanTxQueue, Qos)
|
||||||
{
|
{
|
||||||
uavcan::CanTxQueue::Entry e1(makeCanFrame(100, "", EXT), 1000, uavcan::CanTxQueue::Volatile);
|
uavcan::CanTxQueue::Entry e1(makeCanFrame(100, "", EXT), tsMono(1000), uavcan::CanTxQueue::Volatile);
|
||||||
uavcan::CanTxQueue::Entry e2(makeCanFrame(100, "", EXT), 1000, uavcan::CanTxQueue::Volatile);
|
uavcan::CanTxQueue::Entry e2(makeCanFrame(100, "", EXT), tsMono(1000), uavcan::CanTxQueue::Volatile);
|
||||||
|
|
||||||
EXPECT_FALSE(e1.qosHigherThan(e2));
|
EXPECT_FALSE(e1.qosHigherThan(e2));
|
||||||
EXPECT_FALSE(e2.qosHigherThan(e1));
|
EXPECT_FALSE(e2.qosHigherThan(e1));
|
||||||
@@ -86,7 +86,7 @@ TEST(CanTxQueue, TxQueue)
|
|||||||
/*
|
/*
|
||||||
* Priority insertion
|
* Priority insertion
|
||||||
*/
|
*/
|
||||||
queue.push(f4, 100, CanTxQueue::Persistent);
|
queue.push(f4, tsMono(100), CanTxQueue::Persistent);
|
||||||
EXPECT_FALSE(queue.isEmpty());
|
EXPECT_FALSE(queue.isEmpty());
|
||||||
EXPECT_EQ(1, pool32.getNumUsedBlocks());
|
EXPECT_EQ(1, pool32.getNumUsedBlocks());
|
||||||
EXPECT_EQ(f4, queue.peek()->frame);
|
EXPECT_EQ(f4, queue.peek()->frame);
|
||||||
@@ -94,13 +94,13 @@ TEST(CanTxQueue, TxQueue)
|
|||||||
EXPECT_TRUE(queue.topPriorityHigherOrEqual(f4)); // Equal
|
EXPECT_TRUE(queue.topPriorityHigherOrEqual(f4)); // Equal
|
||||||
EXPECT_FALSE(queue.topPriorityHigherOrEqual(f3));
|
EXPECT_FALSE(queue.topPriorityHigherOrEqual(f3));
|
||||||
|
|
||||||
queue.push(f3, 200, CanTxQueue::Persistent);
|
queue.push(f3, tsMono(200), CanTxQueue::Persistent);
|
||||||
EXPECT_EQ(f3, queue.peek()->frame);
|
EXPECT_EQ(f3, queue.peek()->frame);
|
||||||
|
|
||||||
queue.push(f0, 300, CanTxQueue::Volatile);
|
queue.push(f0, tsMono(300), CanTxQueue::Volatile);
|
||||||
EXPECT_EQ(f0, queue.peek()->frame);
|
EXPECT_EQ(f0, queue.peek()->frame);
|
||||||
|
|
||||||
queue.push(f1, 400, CanTxQueue::Volatile);
|
queue.push(f1, tsMono(400), CanTxQueue::Volatile);
|
||||||
EXPECT_EQ(f0, queue.peek()->frame); // Still f0, since it is highest
|
EXPECT_EQ(f0, queue.peek()->frame); // Still f0, since it is highest
|
||||||
EXPECT_TRUE(queue.topPriorityHigherOrEqual(f0)); // Equal
|
EXPECT_TRUE(queue.topPriorityHigherOrEqual(f0)); // Equal
|
||||||
EXPECT_TRUE(queue.topPriorityHigherOrEqual(f1));
|
EXPECT_TRUE(queue.topPriorityHigherOrEqual(f1));
|
||||||
@@ -125,28 +125,28 @@ TEST(CanTxQueue, TxQueue)
|
|||||||
* QoS
|
* QoS
|
||||||
*/
|
*/
|
||||||
EXPECT_FALSE(isInQueue(queue, f2));
|
EXPECT_FALSE(isInQueue(queue, f2));
|
||||||
queue.push(f2, 100, CanTxQueue::Volatile); // Non preempting, will be rejected
|
queue.push(f2, tsMono(100), CanTxQueue::Volatile); // Non preempting, will be rejected
|
||||||
EXPECT_FALSE(isInQueue(queue, f2));
|
EXPECT_FALSE(isInQueue(queue, f2));
|
||||||
|
|
||||||
queue.push(f2, 500, CanTxQueue::Persistent); // Will override f1 (f3 and f4 are presistent)
|
queue.push(f2, tsMono(500), CanTxQueue::Persistent); // Will override f1 (f3 and f4 are presistent)
|
||||||
EXPECT_TRUE(isInQueue(queue, f2));
|
EXPECT_TRUE(isInQueue(queue, f2));
|
||||||
EXPECT_FALSE(isInQueue(queue, f1));
|
EXPECT_FALSE(isInQueue(queue, f1));
|
||||||
EXPECT_EQ(4, getQueueLength(queue));
|
EXPECT_EQ(4, getQueueLength(queue));
|
||||||
EXPECT_EQ(2, queue.getNumRejectedFrames());
|
EXPECT_EQ(2, queue.getNumRejectedFrames());
|
||||||
EXPECT_EQ(f0, queue.peek()->frame); // Check the priority
|
EXPECT_EQ(f0, queue.peek()->frame); // Check the priority
|
||||||
|
|
||||||
queue.push(f5, 600, CanTxQueue::Persistent); // Will override f0 (rest are presistent)
|
queue.push(f5, tsMono(600), CanTxQueue::Persistent); // Will override f0 (rest are presistent)
|
||||||
EXPECT_TRUE(isInQueue(queue, f5));
|
EXPECT_TRUE(isInQueue(queue, f5));
|
||||||
EXPECT_FALSE(isInQueue(queue, f0));
|
EXPECT_FALSE(isInQueue(queue, f0));
|
||||||
EXPECT_EQ(f2, queue.peek()->frame); // Check the priority
|
EXPECT_EQ(f2, queue.peek()->frame); // Check the priority
|
||||||
|
|
||||||
// No volatile frames left now
|
// No volatile frames left now
|
||||||
|
|
||||||
queue.push(f5a, 700, CanTxQueue::Persistent); // Will override f5 (same frame, same QoS)
|
queue.push(f5a, tsMono(700), CanTxQueue::Persistent); // Will override f5 (same frame, same QoS)
|
||||||
EXPECT_TRUE(isInQueue(queue, f5a));
|
EXPECT_TRUE(isInQueue(queue, f5a));
|
||||||
EXPECT_FALSE(isInQueue(queue, f5));
|
EXPECT_FALSE(isInQueue(queue, f5));
|
||||||
|
|
||||||
queue.push(f6, 700, CanTxQueue::Persistent); // Will be rejected (lowest QoS)
|
queue.push(f6, tsMono(700), CanTxQueue::Persistent); // Will be rejected (lowest QoS)
|
||||||
EXPECT_FALSE(isInQueue(queue, f6));
|
EXPECT_FALSE(isInQueue(queue, f6));
|
||||||
|
|
||||||
EXPECT_FALSE(queue.topPriorityHigherOrEqual(f0));
|
EXPECT_FALSE(queue.topPriorityHigherOrEqual(f0));
|
||||||
@@ -165,19 +165,19 @@ TEST(CanTxQueue, TxQueue)
|
|||||||
* Expiration
|
* Expiration
|
||||||
*/
|
*/
|
||||||
clockmock.monotonic = 101;
|
clockmock.monotonic = 101;
|
||||||
queue.push(f0, 800, CanTxQueue::Volatile); // Will replace f4 which is expired now
|
queue.push(f0, tsMono(800), CanTxQueue::Volatile); // Will replace f4 which is expired now
|
||||||
EXPECT_TRUE(isInQueue(queue, f0));
|
EXPECT_TRUE(isInQueue(queue, f0));
|
||||||
EXPECT_FALSE(isInQueue(queue, f4));
|
EXPECT_FALSE(isInQueue(queue, f4));
|
||||||
EXPECT_EQ(6, queue.getNumRejectedFrames());
|
EXPECT_EQ(6, queue.getNumRejectedFrames());
|
||||||
|
|
||||||
clockmock.monotonic = 1001;
|
clockmock.monotonic = 1001;
|
||||||
queue.push(f5, 2000, CanTxQueue::Volatile); // Entire queue is expired
|
queue.push(f5, tsMono(2000), CanTxQueue::Volatile); // Entire queue is expired
|
||||||
EXPECT_TRUE(isInQueue(queue, f5));
|
EXPECT_TRUE(isInQueue(queue, f5));
|
||||||
EXPECT_EQ(1, getQueueLength(queue)); // Just one entry left - f5
|
EXPECT_EQ(1, getQueueLength(queue)); // Just one entry left - f5
|
||||||
EXPECT_EQ(1, pool32.getNumUsedBlocks()); // Make sure there is no leaks
|
EXPECT_EQ(1, pool32.getNumUsedBlocks()); // Make sure there is no leaks
|
||||||
EXPECT_EQ(10, queue.getNumRejectedFrames());
|
EXPECT_EQ(10, queue.getNumRejectedFrames());
|
||||||
|
|
||||||
queue.push(f0, 1000, CanTxQueue::Persistent); // This entry is already expired
|
queue.push(f0, tsMono(1000), CanTxQueue::Persistent); // This entry is already expired
|
||||||
EXPECT_EQ(1, getQueueLength(queue));
|
EXPECT_EQ(1, getQueueLength(queue));
|
||||||
EXPECT_EQ(1, pool32.getNumUsedBlocks());
|
EXPECT_EQ(1, pool32.getNumUsedBlocks());
|
||||||
EXPECT_EQ(11, queue.getNumRejectedFrames());
|
EXPECT_EQ(11, queue.getNumRejectedFrames());
|
||||||
@@ -185,7 +185,7 @@ TEST(CanTxQueue, TxQueue)
|
|||||||
/*
|
/*
|
||||||
* Removing
|
* Removing
|
||||||
*/
|
*/
|
||||||
queue.push(f4, 5000, CanTxQueue::Volatile);
|
queue.push(f4, tsMono(5000), CanTxQueue::Volatile);
|
||||||
EXPECT_EQ(2, getQueueLength(queue));
|
EXPECT_EQ(2, getQueueLength(queue));
|
||||||
EXPECT_TRUE(isInQueue(queue, f4));
|
EXPECT_TRUE(isInQueue(queue, f4));
|
||||||
EXPECT_EQ(f4, queue.peek()->frame);
|
EXPECT_EQ(f4, queue.peek()->frame);
|
||||||
|
|||||||
@@ -130,7 +130,7 @@ TEST(Dispatcher, Reception)
|
|||||||
|
|
||||||
while (true)
|
while (true)
|
||||||
{
|
{
|
||||||
const int res = dispatcher.spin(0);
|
const int res = dispatcher.spin(tsMono(0));
|
||||||
ASSERT_LE(0, res);
|
ASSERT_LE(0, res);
|
||||||
clockmock.advance(100);
|
clockmock.advance(100);
|
||||||
if (res == 0)
|
if (res == 0)
|
||||||
@@ -197,14 +197,14 @@ TEST(Dispatcher, Transmission)
|
|||||||
/*
|
/*
|
||||||
* Transmission
|
* Transmission
|
||||||
*/
|
*/
|
||||||
static const int TX_DEADLINE = 123456;
|
static const uavcan::MonotonicTime TX_DEADLINE = tsMono(123456);
|
||||||
|
|
||||||
// uint_fast16_t data_type_id, TransferType transfer_type, NodeID src_node_id, NodeID dst_node_id,
|
// 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
|
// uint_fast8_t frame_index, TransferID transfer_id, bool last_frame = false
|
||||||
uavcan::Frame frame(123, uavcan::TransferTypeMessageUnicast, SELF_NODE_ID, 2, 0, 0, true);
|
uavcan::Frame frame(123, uavcan::TransferTypeMessageUnicast, SELF_NODE_ID, 2, 0, 0, true);
|
||||||
frame.setPayload(reinterpret_cast<const uint8_t*>("123"), 3);
|
frame.setPayload(reinterpret_cast<const uint8_t*>("123"), 3);
|
||||||
|
|
||||||
ASSERT_EQ(2, dispatcher.send(frame, TX_DEADLINE, 0, uavcan::CanTxQueue::Volatile));
|
ASSERT_EQ(2, dispatcher.send(frame, TX_DEADLINE, tsMono(0), uavcan::CanTxQueue::Volatile));
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Validation
|
* Validation
|
||||||
@@ -234,10 +234,10 @@ TEST(Dispatcher, Spin)
|
|||||||
clockmock.monotonic_auto_advance = 100;
|
clockmock.monotonic_auto_advance = 100;
|
||||||
|
|
||||||
ASSERT_EQ(100, clockmock.monotonic);
|
ASSERT_EQ(100, clockmock.monotonic);
|
||||||
ASSERT_EQ(0, dispatcher.spin(1000));
|
ASSERT_EQ(0, dispatcher.spin(tsMono(1000)));
|
||||||
ASSERT_LE(1000, clockmock.monotonic);
|
ASSERT_LE(1000, clockmock.monotonic);
|
||||||
ASSERT_EQ(0, dispatcher.spin(0));
|
ASSERT_EQ(0, dispatcher.spin(tsMono(0)));
|
||||||
ASSERT_LE(1000, clockmock.monotonic);
|
ASSERT_LE(1000, clockmock.monotonic);
|
||||||
ASSERT_EQ(0, dispatcher.spin(1100));
|
ASSERT_EQ(0, dispatcher.spin(tsMono(1100)));
|
||||||
ASSERT_LE(1100, clockmock.monotonic);
|
ASSERT_LE(1100, clockmock.monotonic);
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -5,13 +5,14 @@
|
|||||||
#include <algorithm>
|
#include <algorithm>
|
||||||
#include <gtest/gtest.h>
|
#include <gtest/gtest.h>
|
||||||
#include <uavcan/internal/transport/transfer_listener.hpp>
|
#include <uavcan/internal/transport/transfer_listener.hpp>
|
||||||
|
#include "../../clock.hpp"
|
||||||
|
|
||||||
|
|
||||||
static uavcan::RxFrame makeFrame()
|
static uavcan::RxFrame makeFrame()
|
||||||
{
|
{
|
||||||
uavcan::RxFrame frame(
|
uavcan::RxFrame frame(
|
||||||
uavcan::Frame(123, uavcan::TransferTypeMessageBroadcast, 1, uavcan::NodeID::Broadcast, 0, 1, true),
|
uavcan::Frame(123, uavcan::TransferTypeMessageBroadcast, 1, uavcan::NodeID::Broadcast, 0, 1, true),
|
||||||
123, 456, 0);
|
tsMono(123), tsUtc(456), 0);
|
||||||
uint8_t data[8];
|
uint8_t data[8];
|
||||||
for (unsigned int i = 0; i < sizeof(data); i++)
|
for (unsigned int i = 0; i < sizeof(data); i++)
|
||||||
data[i] = i;
|
data[i] = i;
|
||||||
|
|||||||
@@ -5,6 +5,7 @@
|
|||||||
#include <algorithm>
|
#include <algorithm>
|
||||||
#include <gtest/gtest.h>
|
#include <gtest/gtest.h>
|
||||||
#include <uavcan/internal/transport/outgoing_transfer_registry.hpp>
|
#include <uavcan/internal/transport/outgoing_transfer_registry.hpp>
|
||||||
|
#include "../../clock.hpp"
|
||||||
|
|
||||||
|
|
||||||
TEST(OutgoingTransferRegistry, Basic)
|
TEST(OutgoingTransferRegistry, Basic)
|
||||||
@@ -13,7 +14,7 @@ TEST(OutgoingTransferRegistry, Basic)
|
|||||||
uavcan::PoolManager<1> poolmgr; // Empty
|
uavcan::PoolManager<1> poolmgr; // Empty
|
||||||
uavcan::OutgoingTransferRegistry<4> otr(poolmgr);
|
uavcan::OutgoingTransferRegistry<4> otr(poolmgr);
|
||||||
|
|
||||||
otr.cleanup(1000);
|
otr.cleanup(tsMono(1000));
|
||||||
|
|
||||||
static const int NUM_KEYS = 5;
|
static const int NUM_KEYS = 5;
|
||||||
const OutgoingTransferRegistryKey keys[NUM_KEYS] =
|
const OutgoingTransferRegistryKey keys[NUM_KEYS] =
|
||||||
@@ -25,50 +26,50 @@ TEST(OutgoingTransferRegistry, Basic)
|
|||||||
OutgoingTransferRegistryKey(456, uavcan::TransferTypeServiceRequest, 2)
|
OutgoingTransferRegistryKey(456, uavcan::TransferTypeServiceRequest, 2)
|
||||||
};
|
};
|
||||||
|
|
||||||
ASSERT_EQ(0, otr.accessOrCreate(keys[0], 1000000)->get());
|
ASSERT_EQ(0, otr.accessOrCreate(keys[0], tsMono(1000000))->get());
|
||||||
ASSERT_EQ(0, otr.accessOrCreate(keys[1], 1000000)->get());
|
ASSERT_EQ(0, otr.accessOrCreate(keys[1], tsMono(1000000))->get());
|
||||||
ASSERT_EQ(0, otr.accessOrCreate(keys[2], 1000000)->get());
|
ASSERT_EQ(0, otr.accessOrCreate(keys[2], tsMono(1000000))->get());
|
||||||
ASSERT_EQ(0, otr.accessOrCreate(keys[3], 1000000)->get());
|
ASSERT_EQ(0, otr.accessOrCreate(keys[3], tsMono(1000000))->get());
|
||||||
ASSERT_FALSE(otr.accessOrCreate(keys[4], 1000000)); // OOM
|
ASSERT_FALSE(otr.accessOrCreate(keys[4], tsMono(1000000))); // OOM
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Incrementing a little
|
* Incrementing a little
|
||||||
*/
|
*/
|
||||||
otr.accessOrCreate(keys[0], 2000000)->increment();
|
otr.accessOrCreate(keys[0], tsMono(2000000))->increment();
|
||||||
otr.accessOrCreate(keys[0], 4000000)->increment();
|
otr.accessOrCreate(keys[0], tsMono(4000000))->increment();
|
||||||
otr.accessOrCreate(keys[0], 3000000)->increment();
|
otr.accessOrCreate(keys[0], tsMono(3000000))->increment();
|
||||||
ASSERT_EQ(3, otr.accessOrCreate(keys[0], 5000000)->get());
|
ASSERT_EQ(3, otr.accessOrCreate(keys[0], tsMono(5000000))->get());
|
||||||
|
|
||||||
otr.accessOrCreate(keys[2], 2000000)->increment();
|
otr.accessOrCreate(keys[2], tsMono(2000000))->increment();
|
||||||
otr.accessOrCreate(keys[2], 3000000)->increment();
|
otr.accessOrCreate(keys[2], tsMono(3000000))->increment();
|
||||||
ASSERT_EQ(2, otr.accessOrCreate(keys[2], 6000000)->get());
|
ASSERT_EQ(2, otr.accessOrCreate(keys[2], tsMono(6000000))->get());
|
||||||
|
|
||||||
otr.accessOrCreate(keys[3], 9000000)->increment();
|
otr.accessOrCreate(keys[3], tsMono(9000000))->increment();
|
||||||
ASSERT_EQ(1, otr.accessOrCreate(keys[3], 4000000)->get());
|
ASSERT_EQ(1, otr.accessOrCreate(keys[3], tsMono(4000000))->get());
|
||||||
|
|
||||||
ASSERT_EQ(0, otr.accessOrCreate(keys[1], 4000000)->get());
|
ASSERT_EQ(0, otr.accessOrCreate(keys[1], tsMono(4000000))->get());
|
||||||
|
|
||||||
ASSERT_FALSE(otr.accessOrCreate(keys[4], 1000000)); // Still OOM
|
ASSERT_FALSE(otr.accessOrCreate(keys[4], tsMono(1000000))); // Still OOM
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Cleaning up
|
* Cleaning up
|
||||||
*/
|
*/
|
||||||
otr.cleanup(4000001); // Kills 1, 3
|
otr.cleanup(tsMono(4000001)); // Kills 1, 3
|
||||||
ASSERT_EQ(0, otr.accessOrCreate(keys[1], 1000000)->get());
|
ASSERT_EQ(0, otr.accessOrCreate(keys[1], tsMono(1000000))->get());
|
||||||
ASSERT_EQ(0, otr.accessOrCreate(keys[3], 1000000)->get());
|
ASSERT_EQ(0, otr.accessOrCreate(keys[3], tsMono(1000000))->get());
|
||||||
otr.accessOrCreate(keys[1], 5000000)->increment();
|
otr.accessOrCreate(keys[1], tsMono(5000000))->increment();
|
||||||
otr.accessOrCreate(keys[3], 5000000)->increment();
|
otr.accessOrCreate(keys[3], tsMono(5000000))->increment();
|
||||||
|
|
||||||
ASSERT_EQ(3, otr.accessOrCreate(keys[0], 5000000)->get());
|
ASSERT_EQ(3, otr.accessOrCreate(keys[0], tsMono(5000000))->get());
|
||||||
ASSERT_EQ(2, otr.accessOrCreate(keys[2], 6000000)->get());
|
ASSERT_EQ(2, otr.accessOrCreate(keys[2], tsMono(6000000))->get());
|
||||||
|
|
||||||
otr.cleanup(5000001); // Kills 1, 3 (He needs a bath, Jud. He stinks of the ground you buried him in.), 0
|
otr.cleanup(tsMono(5000001)); // Kills 1, 3 (He needs a bath, Jud. He stinks of the ground you buried him in.), 0
|
||||||
ASSERT_EQ(0, otr.accessOrCreate(keys[0], 1000000)->get());
|
ASSERT_EQ(0, otr.accessOrCreate(keys[0], tsMono(1000000))->get());
|
||||||
ASSERT_EQ(0, otr.accessOrCreate(keys[1], 1000000)->get());
|
ASSERT_EQ(0, otr.accessOrCreate(keys[1], tsMono(1000000))->get());
|
||||||
ASSERT_EQ(0, otr.accessOrCreate(keys[3], 1000000)->get());
|
ASSERT_EQ(0, otr.accessOrCreate(keys[3], tsMono(1000000))->get());
|
||||||
|
|
||||||
ASSERT_EQ(2, otr.accessOrCreate(keys[2], 1000000)->get());
|
ASSERT_EQ(2, otr.accessOrCreate(keys[2], tsMono(1000000))->get());
|
||||||
|
|
||||||
otr.cleanup(5000001); // Frees some memory for 4
|
otr.cleanup(tsMono(5000001)); // Frees some memory for 4
|
||||||
ASSERT_EQ(0, otr.accessOrCreate(keys[0], 1000000)->get());
|
ASSERT_EQ(0, otr.accessOrCreate(keys[0], tsMono(1000000))->get());
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -228,17 +228,17 @@ TEST(Transfer, RxFrameParse)
|
|||||||
(456 << 19); // Data Type ID
|
(456 << 19); // Data Type ID
|
||||||
|
|
||||||
ASSERT_TRUE(rx_frame.parse(can_rx_frame));
|
ASSERT_TRUE(rx_frame.parse(can_rx_frame));
|
||||||
ASSERT_EQ(0, rx_frame.getMonotonicTimestamp());
|
ASSERT_EQ(0, rx_frame.getMonotonicTimestamp().toUSec());
|
||||||
ASSERT_EQ(0, rx_frame.getIfaceIndex());
|
ASSERT_EQ(0, rx_frame.getIfaceIndex());
|
||||||
|
|
||||||
can_rx_frame.ts_monotonic = 123;
|
can_rx_frame.ts_mono = tsMono(123);
|
||||||
can_rx_frame.iface_index = 2;
|
can_rx_frame.iface_index = 2;
|
||||||
|
|
||||||
Frame frame(456, uavcan::TransferTypeMessageBroadcast, 1, uavcan::NodeID::Broadcast, 0, 0);
|
Frame frame(456, uavcan::TransferTypeMessageBroadcast, 1, uavcan::NodeID::Broadcast, 0, 0);
|
||||||
ASSERT_TRUE(frame.compile(can_rx_frame));
|
ASSERT_TRUE(frame.compile(can_rx_frame));
|
||||||
|
|
||||||
ASSERT_TRUE(rx_frame.parse(can_rx_frame));
|
ASSERT_TRUE(rx_frame.parse(can_rx_frame));
|
||||||
ASSERT_EQ(123, rx_frame.getMonotonicTimestamp());
|
ASSERT_EQ(123, rx_frame.getMonotonicTimestamp().toUSec());
|
||||||
ASSERT_EQ(2, rx_frame.getIfaceIndex());
|
ASSERT_EQ(2, rx_frame.getIfaceIndex());
|
||||||
ASSERT_EQ(456, rx_frame.getDataTypeID());
|
ASSERT_EQ(456, rx_frame.getDataTypeID());
|
||||||
ASSERT_EQ(uavcan::TransferTypeMessageBroadcast, rx_frame.getTransferType());
|
ASSERT_EQ(uavcan::TransferTypeMessageBroadcast, rx_frame.getTransferType());
|
||||||
@@ -252,13 +252,13 @@ TEST(Transfer, FrameToString)
|
|||||||
|
|
||||||
// RX frame default
|
// RX frame default
|
||||||
RxFrame rx_frame;
|
RxFrame rx_frame;
|
||||||
EXPECT_EQ("dtid=0 tt=4 snid=255 dnid=255 idx=0 last=0 tid=0 payload=[] ts_m=0 ts_utc=0 iface=0", rx_frame.toString());
|
EXPECT_EQ("dtid=0 tt=4 snid=255 dnid=255 idx=0 last=0 tid=0 payload=[] ts_m=0.000000 ts_utc=0.000000 iface=0", rx_frame.toString());
|
||||||
|
|
||||||
// RX frame max len
|
// RX frame max len
|
||||||
rx_frame = RxFrame(Frame(Frame::MaxDataTypeID, uavcan::TransferTypeMessageUnicast,
|
rx_frame = RxFrame(Frame(Frame::MaxDataTypeID, uavcan::TransferTypeMessageUnicast,
|
||||||
uavcan::NodeID::Max, uavcan::NodeID::Max - 1, Frame::MaxIndex,
|
uavcan::NodeID::Max, uavcan::NodeID::Max - 1, Frame::MaxIndex,
|
||||||
uavcan::TransferID::Max, true),
|
uavcan::TransferID::Max, true),
|
||||||
0xFFFFFFFFFFFFFFFF, 0xFFFFFFFFFFFFFFFF, 3);
|
uavcan::MonotonicTime::getMax(), uavcan::UtcTime::getMax(), 3);
|
||||||
|
|
||||||
uint8_t data[8];
|
uint8_t data[8];
|
||||||
for (unsigned int i = 0; i < sizeof(data); i++)
|
for (unsigned int i = 0; i < sizeof(data); i++)
|
||||||
@@ -266,7 +266,7 @@ TEST(Transfer, FrameToString)
|
|||||||
rx_frame.setPayload(data, sizeof(data));
|
rx_frame.setPayload(data, sizeof(data));
|
||||||
|
|
||||||
EXPECT_EQ(
|
EXPECT_EQ(
|
||||||
"dtid=1023 tt=3 snid=127 dnid=126 idx=62 last=1 tid=7 payload=[00 01 02 03 04 05 06] ts_m=18446744073709551615 ts_utc=18446744073709551615 iface=3",
|
"dtid=1023 tt=3 snid=127 dnid=126 idx=62 last=1 tid=7 payload=[00 01 02 03 04 05 06] ts_m=18446744073709.551615 ts_utc=18446744073709.551615 iface=3",
|
||||||
rx_frame.toString());
|
rx_frame.toString());
|
||||||
|
|
||||||
// Plain frame default
|
// Plain frame default
|
||||||
|
|||||||
@@ -4,6 +4,7 @@
|
|||||||
|
|
||||||
#include <gtest/gtest.h>
|
#include <gtest/gtest.h>
|
||||||
#include "transfer_test_helpers.hpp"
|
#include "transfer_test_helpers.hpp"
|
||||||
|
#include "../../clock.hpp"
|
||||||
|
|
||||||
|
|
||||||
class TransferListenerEmulator : public IncomingTransferEmulatorBase
|
class TransferListenerEmulator : public IncomingTransferEmulatorBase
|
||||||
@@ -198,7 +199,7 @@ TEST(TransferListener, Cleanup)
|
|||||||
/*
|
/*
|
||||||
* Cleanup with huge timestamp value will remove all entries
|
* Cleanup with huge timestamp value will remove all entries
|
||||||
*/
|
*/
|
||||||
static_cast<uavcan::TransferListenerBase&>(subscriber).cleanup(100000000);
|
static_cast<uavcan::TransferListenerBase&>(subscriber).cleanup(tsMono(100000000));
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Sending the same transfers again - they will be accepted since registres were cleared
|
* Sending the same transfers again - they will be accepted since registres were cleared
|
||||||
|
|||||||
@@ -5,6 +5,7 @@
|
|||||||
#include <algorithm>
|
#include <algorithm>
|
||||||
#include <gtest/gtest.h>
|
#include <gtest/gtest.h>
|
||||||
#include <uavcan/internal/transport/transfer_receiver.hpp>
|
#include <uavcan/internal/transport/transfer_receiver.hpp>
|
||||||
|
#include "../../clock.hpp"
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Beware!
|
* Beware!
|
||||||
@@ -37,7 +38,8 @@ struct RxFrameGenerator
|
|||||||
|
|
||||||
EXPECT_EQ(data.length(), frame.setPayload(reinterpret_cast<const uint8_t*>(data.c_str()), data.length()));
|
EXPECT_EQ(data.length(), frame.setPayload(reinterpret_cast<const uint8_t*>(data.c_str()), data.length()));
|
||||||
|
|
||||||
return uavcan::RxFrame(frame, ts_monotonic, ts_utc, iface_index);
|
return uavcan::RxFrame(frame, uavcan::MonotonicTime::fromUSec(ts_monotonic),
|
||||||
|
uavcan::UtcTime::fromUSec(ts_utc), iface_index);
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
@@ -101,15 +103,15 @@ TEST(TransferReceiver, Basic)
|
|||||||
/*
|
/*
|
||||||
* Empty
|
* Empty
|
||||||
*/
|
*/
|
||||||
ASSERT_EQ(TransferReceiver::DefaultTransferInterval, rcv.getInterval());
|
ASSERT_EQ(TransferReceiver::getDefaultTransferInterval(), rcv.getInterval());
|
||||||
ASSERT_EQ(0, rcv.getLastTransferTimestampMonotonic());
|
ASSERT_EQ(0, rcv.getLastTransferTimestampMonotonic().toUSec());
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Single frame transfer with zero ts, must be ignored
|
* Single frame transfer with zero ts, must be ignored
|
||||||
*/
|
*/
|
||||||
CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "Foo", 0, true, 0, 0), bk));
|
CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "Foo", 0, true, 0, 0), bk));
|
||||||
ASSERT_EQ(TransferReceiver::DefaultTransferInterval, rcv.getInterval());
|
ASSERT_EQ(TransferReceiver::getDefaultTransferInterval(), rcv.getInterval());
|
||||||
ASSERT_EQ(0, rcv.getLastTransferTimestampMonotonic());
|
ASSERT_EQ(0, rcv.getLastTransferTimestampMonotonic().toUSec());
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Valid compound transfer
|
* Valid compound transfer
|
||||||
@@ -120,8 +122,8 @@ TEST(TransferReceiver, Basic)
|
|||||||
|
|
||||||
ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "345678foo"));
|
ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "345678foo"));
|
||||||
ASSERT_EQ(0x1234, rcv.getLastTransferCrc());
|
ASSERT_EQ(0x1234, rcv.getLastTransferCrc());
|
||||||
ASSERT_EQ(TransferReceiver::DefaultTransferInterval, rcv.getInterval()); // Not initialized yet
|
ASSERT_EQ(TransferReceiver::getDefaultTransferInterval(), rcv.getInterval()); // Not initialized yet
|
||||||
ASSERT_EQ(100, rcv.getLastTransferTimestampMonotonic());
|
ASSERT_EQ(100, rcv.getLastTransferTimestampMonotonic().toUSec());
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Compound transfer mixed with invalid frames; buffer was not released explicitly
|
* Compound transfer mixed with invalid frames; buffer was not released explicitly
|
||||||
@@ -138,12 +140,12 @@ TEST(TransferReceiver, Basic)
|
|||||||
|
|
||||||
ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "345678abcdefgh"));
|
ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "345678abcdefgh"));
|
||||||
ASSERT_EQ(0x789A, rcv.getLastTransferCrc());
|
ASSERT_EQ(0x789A, rcv.getLastTransferCrc());
|
||||||
ASSERT_GT(TransferReceiver::DefaultTransferInterval, rcv.getInterval());
|
ASSERT_GT(TransferReceiver::getDefaultTransferInterval(), rcv.getInterval());
|
||||||
ASSERT_LT(TransferReceiver::MinTransferInterval, rcv.getInterval());
|
ASSERT_LT(TransferReceiver::getMinTransferInterval(), rcv.getInterval());
|
||||||
ASSERT_EQ(1000, rcv.getLastTransferTimestampMonotonic());
|
ASSERT_EQ(1000, rcv.getLastTransferTimestampMonotonic().toUSec());
|
||||||
ASSERT_FALSE(rcv.isTimedOut(1000));
|
ASSERT_FALSE(rcv.isTimedOut(tsMono(1000)));
|
||||||
ASSERT_FALSE(rcv.isTimedOut(5000));
|
ASSERT_FALSE(rcv.isTimedOut(tsMono(5000)));
|
||||||
ASSERT_TRUE(rcv.isTimedOut(60000000));
|
ASSERT_TRUE(rcv.isTimedOut(tsMono(60000000)));
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Single-frame transfers
|
* Single-frame transfers
|
||||||
@@ -153,11 +155,11 @@ TEST(TransferReceiver, Basic)
|
|||||||
CHECK_SINGLE_FRAME(rcv.addFrame(gen(0, "qwe", 0, true, 2, 2200), bk));
|
CHECK_SINGLE_FRAME(rcv.addFrame(gen(0, "qwe", 0, true, 2, 2200), bk));
|
||||||
|
|
||||||
ASSERT_FALSE(bufmgr.access(gen.bufmgr_key)); // Buffer must be removed
|
ASSERT_FALSE(bufmgr.access(gen.bufmgr_key)); // Buffer must be removed
|
||||||
ASSERT_GT(TransferReceiver::DefaultTransferInterval, rcv.getInterval());
|
ASSERT_GT(TransferReceiver::getDefaultTransferInterval(), rcv.getInterval());
|
||||||
ASSERT_EQ(2200, rcv.getLastTransferTimestampMonotonic());
|
ASSERT_EQ(2200, rcv.getLastTransferTimestampMonotonic().toUSec());
|
||||||
|
|
||||||
CHECK_SINGLE_FRAME(rcv.addFrame(gen(0, "", 0, true, 3, 2500), bk));
|
CHECK_SINGLE_FRAME(rcv.addFrame(gen(0, "", 0, true, 3, 2500), bk));
|
||||||
ASSERT_EQ(2500, rcv.getLastTransferTimestampMonotonic());
|
ASSERT_EQ(2500, rcv.getLastTransferTimestampMonotonic().toUSec());
|
||||||
|
|
||||||
CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "", 0, true, 0, 3000), bk)); // Old TID
|
CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "", 0, true, 0, 3000), bk)); // Old TID
|
||||||
CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "", 0, true, 1, 3100), bk)); // Old TID
|
CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "", 0, true, 1, 3100), bk)); // Old TID
|
||||||
@@ -166,38 +168,38 @@ TEST(TransferReceiver, Basic)
|
|||||||
CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "", 0, true, 2, 3400), bk)); // Old TID, wrong iface
|
CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "", 0, true, 2, 3400), bk)); // Old TID, wrong iface
|
||||||
CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "", 0, true, 3, 3500), bk)); // Old TID, wrong iface
|
CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "", 0, true, 3, 3500), bk)); // Old TID, wrong iface
|
||||||
CHECK_SINGLE_FRAME(rcv.addFrame(gen(0, "", 0, true, 4, 3600), bk));
|
CHECK_SINGLE_FRAME(rcv.addFrame(gen(0, "", 0, true, 4, 3600), bk));
|
||||||
ASSERT_EQ(3600, rcv.getLastTransferTimestampMonotonic());
|
ASSERT_EQ(3600, rcv.getLastTransferTimestampMonotonic().toUSec());
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Timeouts
|
* Timeouts
|
||||||
*/
|
*/
|
||||||
CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "qwe", 0, true, 1, 100000), bk)); // Wrong iface - ignored
|
CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "qwe", 0, true, 1, 100000), bk)); // Wrong iface - ignored
|
||||||
CHECK_SINGLE_FRAME(rcv.addFrame(gen(1, "qwe", 0, true, 6, 600000), bk)); // Accepted due to iface timeout
|
CHECK_SINGLE_FRAME(rcv.addFrame(gen(1, "qwe", 0, true, 6, 600000), bk)); // Accepted due to iface timeout
|
||||||
ASSERT_EQ(600000, rcv.getLastTransferTimestampMonotonic());
|
ASSERT_EQ(600000, rcv.getLastTransferTimestampMonotonic().toUSec());
|
||||||
|
|
||||||
CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "qwe", 0, true, 7, 600100), bk));// Ignored - old iface 0
|
CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "qwe", 0, true, 7, 600100), bk));// Ignored - old iface 0
|
||||||
CHECK_SINGLE_FRAME(rcv.addFrame(gen(1, "qwe", 0, true, 7, 600100), bk));
|
CHECK_SINGLE_FRAME(rcv.addFrame(gen(1, "qwe", 0, true, 7, 600100), bk));
|
||||||
ASSERT_EQ(600100, rcv.getLastTransferTimestampMonotonic());
|
ASSERT_EQ(600100, rcv.getLastTransferTimestampMonotonic().toUSec());
|
||||||
|
|
||||||
CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "qwe", 0, true, 7, 600100), bk)); // Old TID
|
CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "qwe", 0, true, 7, 600100), bk)); // Old TID
|
||||||
CHECK_SINGLE_FRAME(rcv.addFrame(gen(0, "qwe", 0, true, 7, 100000000), bk));// Accepted - global timeout
|
CHECK_SINGLE_FRAME(rcv.addFrame(gen(0, "qwe", 0, true, 7, 100000000), bk));// Accepted - global timeout
|
||||||
ASSERT_EQ(100000000, rcv.getLastTransferTimestampMonotonic());
|
ASSERT_EQ(100000000, rcv.getLastTransferTimestampMonotonic().toUSec());
|
||||||
|
|
||||||
CHECK_SINGLE_FRAME(rcv.addFrame(gen(0, "qwe", 0, true, 0, 100000100), bk));
|
CHECK_SINGLE_FRAME(rcv.addFrame(gen(0, "qwe", 0, true, 0, 100000100), bk));
|
||||||
ASSERT_EQ(100000100, rcv.getLastTransferTimestampMonotonic());
|
ASSERT_EQ(100000100, rcv.getLastTransferTimestampMonotonic().toUSec());
|
||||||
|
|
||||||
ASSERT_TRUE(rcv.isTimedOut(900000000));
|
ASSERT_TRUE(rcv.isTimedOut(tsMono(900000000)));
|
||||||
CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "\x78\x56" "345678", 0, false, 0, 900000000), bk));// Global timeout
|
CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "\x78\x56" "345678", 0, false, 0, 900000000), bk));// Global timeout
|
||||||
CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "12345678", 0, false, 0, 900000100), bk));// Wrong iface
|
CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "12345678", 0, false, 0, 900000100), bk));// Wrong iface
|
||||||
CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "qwe", 1, true, 0, 900000200), bk));// Wrong iface
|
CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "qwe", 1, true, 0, 900000200), bk));// Wrong iface
|
||||||
CHECK_COMPLETE( rcv.addFrame(gen(1, "qwe", 1, true, 0, 900000200), bk));
|
CHECK_COMPLETE( rcv.addFrame(gen(1, "qwe", 1, true, 0, 900000200), bk));
|
||||||
ASSERT_EQ(900000000, rcv.getLastTransferTimestampMonotonic());
|
ASSERT_EQ(900000000, rcv.getLastTransferTimestampMonotonic().toUSec());
|
||||||
ASSERT_FALSE(rcv.isTimedOut(1000));
|
ASSERT_FALSE(rcv.isTimedOut(tsMono(1000)));
|
||||||
ASSERT_FALSE(rcv.isTimedOut(900000200));
|
ASSERT_FALSE(rcv.isTimedOut(tsMono(900000200)));
|
||||||
ASSERT_TRUE(rcv.isTimedOut(1000 * 1000000));
|
ASSERT_TRUE(rcv.isTimedOut(tsMono(1000 * 1000000)));
|
||||||
ASSERT_LT(TransferReceiver::DefaultTransferInterval, rcv.getInterval());
|
ASSERT_LT(TransferReceiver::getDefaultTransferInterval(), rcv.getInterval());
|
||||||
ASSERT_LE(TransferReceiver::MinTransferInterval, rcv.getInterval());
|
ASSERT_LE(TransferReceiver::getMinTransferInterval(), rcv.getInterval());
|
||||||
ASSERT_GE(TransferReceiver::MaxTransferInterval, rcv.getInterval());
|
ASSERT_GE(TransferReceiver::getMaxTransferInterval(), rcv.getInterval());
|
||||||
ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "345678qwe"));
|
ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "345678qwe"));
|
||||||
ASSERT_EQ(0x5678, rcv.getLastTransferCrc());
|
ASSERT_EQ(0x5678, rcv.getLastTransferCrc());
|
||||||
|
|
||||||
@@ -227,7 +229,7 @@ TEST(TransferReceiver, OutOfBufferSpace_32bytes)
|
|||||||
CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "12345678", 3, false, 1, 100000300), bk)); // 30
|
CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "12345678", 3, false, 1, 100000300), bk)); // 30
|
||||||
CHECK_COMPLETE( rcv.addFrame(gen(1, "12", 4, true, 1, 100000400), bk)); // 32
|
CHECK_COMPLETE( rcv.addFrame(gen(1, "12", 4, true, 1, 100000400), bk)); // 32
|
||||||
|
|
||||||
ASSERT_EQ(100000000, rcv.getLastTransferTimestampMonotonic());
|
ASSERT_EQ(100000000, rcv.getLastTransferTimestampMonotonic().toUSec());
|
||||||
ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "34567812345678123456781234567812"));
|
ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "34567812345678123456781234567812"));
|
||||||
ASSERT_EQ(0x3231, rcv.getLastTransferCrc()); // CRC from "12", which is 0x3231 in little endian
|
ASSERT_EQ(0x3231, rcv.getLastTransferCrc()); // CRC from "12", which is 0x3231 in little endian
|
||||||
|
|
||||||
@@ -240,7 +242,7 @@ TEST(TransferReceiver, OutOfBufferSpace_32bytes)
|
|||||||
CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "12345678", 3, false, 2, 100001200), bk)); // 30
|
CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "12345678", 3, false, 2, 100001200), bk)); // 30
|
||||||
CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "12345678", 4, true, 2, 100001300), bk)); // 38 // EOT, ignored - lost sync
|
CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "12345678", 4, true, 2, 100001300), bk)); // 38 // EOT, ignored - lost sync
|
||||||
|
|
||||||
ASSERT_EQ(100000000, rcv.getLastTransferTimestampMonotonic()); // Timestamp will not be overriden
|
ASSERT_EQ(100000000, rcv.getLastTransferTimestampMonotonic().toUSec()); // Timestamp will not be overriden
|
||||||
ASSERT_FALSE(bufmgr.access(gen.bufmgr_key)); // Buffer should be removed
|
ASSERT_FALSE(bufmgr.access(gen.bufmgr_key)); // Buffer should be removed
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -260,7 +262,7 @@ TEST(TransferReceiver, UnterminatedTransfer)
|
|||||||
content += "12345678";
|
content += "12345678";
|
||||||
}
|
}
|
||||||
CHECK_COMPLETE(rcv.addFrame(gen(1, "12345678", uavcan::Frame::MaxIndex, true, 0, 1100), bk));
|
CHECK_COMPLETE(rcv.addFrame(gen(1, "12345678", uavcan::Frame::MaxIndex, true, 0, 1100), bk));
|
||||||
ASSERT_EQ(1000, rcv.getLastTransferTimestampMonotonic());
|
ASSERT_EQ(1000, rcv.getLastTransferTimestampMonotonic().toUSec());
|
||||||
ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), std::string(content, 2)));
|
ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), std::string(content, 2)));
|
||||||
ASSERT_EQ(0x3231, rcv.getLastTransferCrc());
|
ASSERT_EQ(0x3231, rcv.getLastTransferCrc());
|
||||||
}
|
}
|
||||||
@@ -281,7 +283,7 @@ TEST(TransferReceiver, OutOfOrderFrames)
|
|||||||
CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "--------", 4, true, 7, 100000200), bk)); // Out of order
|
CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "--------", 4, true, 7, 100000200), bk)); // Out of order
|
||||||
CHECK_COMPLETE( rcv.addFrame(gen(1, "abcd", 2, true, 7, 100000400), bk));
|
CHECK_COMPLETE( rcv.addFrame(gen(1, "abcd", 2, true, 7, 100000400), bk));
|
||||||
|
|
||||||
ASSERT_EQ(100000000, rcv.getLastTransferTimestampMonotonic());
|
ASSERT_EQ(100000000, rcv.getLastTransferTimestampMonotonic().toUSec());
|
||||||
ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "345678qwertyuiabcd"));
|
ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "345678qwertyuiabcd"));
|
||||||
ASSERT_EQ(0x3231, rcv.getLastTransferCrc());
|
ASSERT_EQ(0x3231, rcv.getLastTransferCrc());
|
||||||
}
|
}
|
||||||
@@ -307,13 +309,13 @@ TEST(TransferReceiver, IntervalMeasurement)
|
|||||||
|
|
||||||
ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "345678qwertyuiabcd"));
|
ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "345678qwertyuiabcd"));
|
||||||
ASSERT_EQ(0x3231, rcv.getLastTransferCrc());
|
ASSERT_EQ(0x3231, rcv.getLastTransferCrc());
|
||||||
ASSERT_EQ(timestamp, rcv.getLastTransferTimestampMonotonic());
|
ASSERT_EQ(timestamp, rcv.getLastTransferTimestampMonotonic().toUSec());
|
||||||
|
|
||||||
timestamp += INTERVAL;
|
timestamp += INTERVAL;
|
||||||
tid.increment();
|
tid.increment();
|
||||||
}
|
}
|
||||||
|
|
||||||
ASSERT_EQ(INTERVAL, rcv.getInterval());
|
ASSERT_EQ(INTERVAL, rcv.getInterval().toUSec());
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
@@ -375,8 +377,8 @@ TEST(TransferReceiver, UtcTransferTimestamping)
|
|||||||
CHECK_COMPLETE( rcv.addFrame(gen(1, "abcd", 2, true, 0, 3, 0), bk));
|
CHECK_COMPLETE( rcv.addFrame(gen(1, "abcd", 2, true, 0, 3, 0), bk));
|
||||||
|
|
||||||
ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "345678qwertyuiabcd"));
|
ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "345678qwertyuiabcd"));
|
||||||
ASSERT_EQ(1, rcv.getLastTransferTimestampMonotonic());
|
ASSERT_EQ(1, rcv.getLastTransferTimestampMonotonic().toUSec());
|
||||||
ASSERT_EQ(0, rcv.getLastTransferTimestampUtc());
|
ASSERT_EQ(0, rcv.getLastTransferTimestampUtc().toUSec());
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Non-zero UTC timestamp
|
* Non-zero UTC timestamp
|
||||||
@@ -386,15 +388,15 @@ TEST(TransferReceiver, UtcTransferTimestamping)
|
|||||||
CHECK_COMPLETE( rcv.addFrame(gen(1, "abcd", 2, true, 1, 6, 42), bk));
|
CHECK_COMPLETE( rcv.addFrame(gen(1, "abcd", 2, true, 1, 6, 42), bk));
|
||||||
|
|
||||||
ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "345678qwertyuiabcd"));
|
ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "345678qwertyuiabcd"));
|
||||||
ASSERT_EQ(4, rcv.getLastTransferTimestampMonotonic());
|
ASSERT_EQ(4, rcv.getLastTransferTimestampMonotonic().toUSec());
|
||||||
ASSERT_EQ(123, rcv.getLastTransferTimestampUtc());
|
ASSERT_EQ(123, rcv.getLastTransferTimestampUtc().toUSec());
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Single-frame transfers
|
* Single-frame transfers
|
||||||
*/
|
*/
|
||||||
CHECK_SINGLE_FRAME(rcv.addFrame(gen(1, "abc", 0, true, 2, 10, 100000000), bk)); // Exact value is irrelevant
|
CHECK_SINGLE_FRAME(rcv.addFrame(gen(1, "abc", 0, true, 2, 10, 100000000), bk)); // Exact value is irrelevant
|
||||||
ASSERT_EQ(10, rcv.getLastTransferTimestampMonotonic());
|
ASSERT_EQ(10, rcv.getLastTransferTimestampMonotonic().toUSec());
|
||||||
ASSERT_EQ(100000000, rcv.getLastTransferTimestampUtc());
|
ASSERT_EQ(100000000, rcv.getLastTransferTimestampUtc().toUSec());
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Restart recovery
|
* Restart recovery
|
||||||
@@ -404,8 +406,8 @@ TEST(TransferReceiver, UtcTransferTimestamping)
|
|||||||
CHECK_COMPLETE( rcv.addFrame(gen(0, "abcd", 2, true, 1, 100000002, 900000000), bk));
|
CHECK_COMPLETE( rcv.addFrame(gen(0, "abcd", 2, true, 1, 100000002, 900000000), bk));
|
||||||
|
|
||||||
ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "345678qwertyuiabcd"));
|
ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "345678qwertyuiabcd"));
|
||||||
ASSERT_EQ(100000000, rcv.getLastTransferTimestampMonotonic());
|
ASSERT_EQ(100000000, rcv.getLastTransferTimestampMonotonic().toUSec());
|
||||||
ASSERT_EQ(800000000, rcv.getLastTransferTimestampUtc());
|
ASSERT_EQ(800000000, rcv.getLastTransferTimestampUtc().toUSec());
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@@ -13,16 +13,18 @@ static int sendOne(uavcan::TransferSender& sender, const std::string& data,
|
|||||||
uint64_t monotonic_tx_deadline, uint64_t monotonic_blocking_deadline,
|
uint64_t monotonic_tx_deadline, uint64_t monotonic_blocking_deadline,
|
||||||
uavcan::TransferType transfer_type, uavcan::NodeID dst_node_id)
|
uavcan::TransferType transfer_type, uavcan::NodeID dst_node_id)
|
||||||
{
|
{
|
||||||
return sender.send(reinterpret_cast<const uint8_t*>(data.c_str()), data.length(), monotonic_tx_deadline,
|
return sender.send(reinterpret_cast<const uint8_t*>(data.c_str()), data.length(),
|
||||||
monotonic_blocking_deadline, transfer_type, dst_node_id);
|
uavcan::MonotonicTime::fromUSec(monotonic_tx_deadline),
|
||||||
|
uavcan::MonotonicTime::fromUSec(monotonic_blocking_deadline), transfer_type, dst_node_id);
|
||||||
}
|
}
|
||||||
|
|
||||||
static int sendOne(uavcan::TransferSender& sender, const std::string& data,
|
static int sendOne(uavcan::TransferSender& sender, const std::string& data,
|
||||||
uint64_t monotonic_tx_deadline, uint64_t monotonic_blocking_deadline,
|
uint64_t monotonic_tx_deadline, uint64_t monotonic_blocking_deadline,
|
||||||
uavcan::TransferType transfer_type, uavcan::NodeID dst_node_id, uavcan::TransferID tid)
|
uavcan::TransferType transfer_type, uavcan::NodeID dst_node_id, uavcan::TransferID tid)
|
||||||
{
|
{
|
||||||
return sender.send(reinterpret_cast<const uint8_t*>(data.c_str()), data.length(), monotonic_tx_deadline,
|
return sender.send(reinterpret_cast<const uint8_t*>(data.c_str()), data.length(),
|
||||||
monotonic_blocking_deadline, transfer_type, dst_node_id, tid);
|
uavcan::MonotonicTime::fromUSec(monotonic_tx_deadline),
|
||||||
|
uavcan::MonotonicTime::fromUSec(monotonic_blocking_deadline), transfer_type, dst_node_id, tid);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
@@ -119,7 +121,7 @@ TEST(TransferSender, Basic)
|
|||||||
|
|
||||||
while (true)
|
while (true)
|
||||||
{
|
{
|
||||||
const int res = dispatcher_rx.spin(0);
|
const int res = dispatcher_rx.spin(tsMono(0));
|
||||||
ASSERT_LE(0, res);
|
ASSERT_LE(0, res);
|
||||||
clockmock.advance(100);
|
clockmock.advance(100);
|
||||||
if (res == 0)
|
if (res == 0)
|
||||||
|
|||||||
@@ -4,6 +4,7 @@
|
|||||||
|
|
||||||
#include <gtest/gtest.h>
|
#include <gtest/gtest.h>
|
||||||
#include "transfer_test_helpers.hpp"
|
#include "transfer_test_helpers.hpp"
|
||||||
|
#include "../../clock.hpp"
|
||||||
|
|
||||||
|
|
||||||
TEST(TransferTestHelpers, Transfer)
|
TEST(TransferTestHelpers, Transfer)
|
||||||
@@ -15,8 +16,9 @@ TEST(TransferTestHelpers, Transfer)
|
|||||||
uavcan::TransferBufferManager<128, 1> mgr(poolmgr);
|
uavcan::TransferBufferManager<128, 1> mgr(poolmgr);
|
||||||
uavcan::TransferBufferAccessor tba(mgr, uavcan::TransferBufferManagerKey(0, uavcan::TransferTypeMessageUnicast));
|
uavcan::TransferBufferAccessor tba(mgr, uavcan::TransferBufferManagerKey(0, uavcan::TransferTypeMessageUnicast));
|
||||||
|
|
||||||
uavcan::RxFrame frame(uavcan::Frame(123, uavcan::TransferTypeMessageBroadcast, 1, 0, 0, 0, true), 0, 0, 0);
|
uavcan::RxFrame frame(uavcan::Frame(123, uavcan::TransferTypeMessageBroadcast, 1, 0, 0, 0, true),
|
||||||
uavcan::MultiFrameIncomingTransfer mfit(10, 1000, frame, tba);
|
uavcan::MonotonicTime(), uavcan::UtcTime(), 0);
|
||||||
|
uavcan::MultiFrameIncomingTransfer mfit(tsMono(10), tsUtc(1000), frame, tba);
|
||||||
|
|
||||||
// Filling the buffer with data
|
// Filling the buffer with data
|
||||||
static const std::string TEST_DATA = "Kaneda! What do you see? Kaneda! What do you see? Kaneda! Kaneda!!!";
|
static const std::string TEST_DATA = "Kaneda! What do you see? Kaneda! What do you see? Kaneda! Kaneda!!!";
|
||||||
|
|||||||
@@ -15,8 +15,8 @@
|
|||||||
*/
|
*/
|
||||||
struct Transfer
|
struct Transfer
|
||||||
{
|
{
|
||||||
uint64_t ts_monotonic;
|
uavcan::MonotonicTime ts_monotonic;
|
||||||
uint64_t ts_utc;
|
uavcan::UtcTime ts_utc;
|
||||||
uavcan::TransferType transfer_type;
|
uavcan::TransferType transfer_type;
|
||||||
uavcan::TransferID transfer_id;
|
uavcan::TransferID transfer_id;
|
||||||
uavcan::NodeID src_node_id;
|
uavcan::NodeID src_node_id;
|
||||||
@@ -50,7 +50,7 @@ struct Transfer
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
Transfer(uint64_t ts_monotonic, uint64_t ts_utc, uavcan::TransferType transfer_type,
|
Transfer(uavcan::MonotonicTime ts_monotonic, uavcan::UtcTime ts_utc, uavcan::TransferType transfer_type,
|
||||||
uavcan::TransferID transfer_id, uavcan::NodeID src_node_id, uavcan::NodeID dst_node_id,
|
uavcan::TransferID transfer_id, uavcan::NodeID src_node_id, uavcan::NodeID dst_node_id,
|
||||||
const std::string& payload, const uavcan::DataTypeDescriptor& data_type)
|
const std::string& payload, const uavcan::DataTypeDescriptor& data_type)
|
||||||
: ts_monotonic(ts_monotonic)
|
: ts_monotonic(ts_monotonic)
|
||||||
@@ -63,11 +63,24 @@ struct Transfer
|
|||||||
, payload(payload)
|
, payload(payload)
|
||||||
{ }
|
{ }
|
||||||
|
|
||||||
|
Transfer(uint64_t ts_monotonic, uint64_t ts_utc, uavcan::TransferType transfer_type,
|
||||||
|
uavcan::TransferID transfer_id, uavcan::NodeID src_node_id, uavcan::NodeID dst_node_id,
|
||||||
|
const std::string& payload, const uavcan::DataTypeDescriptor& data_type)
|
||||||
|
: ts_monotonic(uavcan::MonotonicTime::fromUSec(ts_monotonic))
|
||||||
|
, ts_utc(uavcan::UtcTime::fromUSec(ts_utc))
|
||||||
|
, transfer_type(transfer_type)
|
||||||
|
, transfer_id(transfer_id)
|
||||||
|
, src_node_id(src_node_id)
|
||||||
|
, dst_node_id(dst_node_id)
|
||||||
|
, data_type(data_type)
|
||||||
|
, payload(payload)
|
||||||
|
{ }
|
||||||
|
|
||||||
bool operator==(const Transfer& rhs) const
|
bool operator==(const Transfer& rhs) const
|
||||||
{
|
{
|
||||||
return
|
return
|
||||||
(ts_monotonic == rhs.ts_monotonic) &&
|
(ts_monotonic == rhs.ts_monotonic) &&
|
||||||
((ts_utc && rhs.ts_utc) ? (ts_utc == rhs.ts_utc) : true) &&
|
((!ts_utc.isZero() && !rhs.ts_utc.isZero()) ? (ts_utc == rhs.ts_utc) : true) &&
|
||||||
(transfer_type == rhs.transfer_type) &&
|
(transfer_type == rhs.transfer_type) &&
|
||||||
(transfer_id == rhs.transfer_id) &&
|
(transfer_id == rhs.transfer_id) &&
|
||||||
(src_node_id == rhs.src_node_id) &&
|
(src_node_id == rhs.src_node_id) &&
|
||||||
@@ -176,8 +189,8 @@ std::vector<uavcan::RxFrame> serializeTransfer(const Transfer& transfer)
|
|||||||
std::vector<uavcan::RxFrame> output;
|
std::vector<uavcan::RxFrame> output;
|
||||||
unsigned int frame_index = 0;
|
unsigned int frame_index = 0;
|
||||||
unsigned int offset = 0;
|
unsigned int offset = 0;
|
||||||
uint64_t ts_monotonic = transfer.ts_monotonic;
|
uavcan::MonotonicTime ts_monotonic = transfer.ts_monotonic;
|
||||||
uint64_t ts_utc = transfer.ts_utc;
|
uavcan::UtcTime ts_utc = transfer.ts_utc;
|
||||||
|
|
||||||
while (true)
|
while (true)
|
||||||
{
|
{
|
||||||
@@ -200,8 +213,8 @@ std::vector<uavcan::RxFrame> serializeTransfer(const Transfer& transfer)
|
|||||||
frame_index++;
|
frame_index++;
|
||||||
|
|
||||||
const uavcan::RxFrame rxfrm(frm, ts_monotonic, ts_utc, 0);
|
const uavcan::RxFrame rxfrm(frm, ts_monotonic, ts_utc, 0);
|
||||||
ts_monotonic += 1;
|
ts_monotonic += uavcan::MonotonicDuration::fromUSec(1);
|
||||||
ts_utc += 1;
|
ts_utc += uavcan::UtcDuration::fromUSec(1);
|
||||||
|
|
||||||
output.push_back(rxfrm);
|
output.push_back(rxfrm);
|
||||||
if (frm.isLast())
|
if (frm.isLast())
|
||||||
@@ -221,14 +234,13 @@ uavcan::DataTypeDescriptor makeDataType(uavcan::DataTypeKind kind, uint16_t id,
|
|||||||
|
|
||||||
class IncomingTransferEmulatorBase
|
class IncomingTransferEmulatorBase
|
||||||
{
|
{
|
||||||
uint64_t ts_;
|
uavcan::MonotonicTime ts_;
|
||||||
uavcan::TransferID tid_;
|
uavcan::TransferID tid_;
|
||||||
uavcan::NodeID dst_node_id_;
|
uavcan::NodeID dst_node_id_;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
IncomingTransferEmulatorBase(uavcan::NodeID dst_node_id)
|
IncomingTransferEmulatorBase(uavcan::NodeID dst_node_id)
|
||||||
: ts_(0)
|
: dst_node_id_(dst_node_id)
|
||||||
, dst_node_id_(dst_node_id)
|
|
||||||
{ }
|
{ }
|
||||||
|
|
||||||
virtual ~IncomingTransferEmulatorBase() { }
|
virtual ~IncomingTransferEmulatorBase() { }
|
||||||
@@ -237,11 +249,12 @@ public:
|
|||||||
const uavcan::DataTypeDescriptor& type,
|
const uavcan::DataTypeDescriptor& type,
|
||||||
uavcan::NodeID dst_node_id_override = uavcan::NodeID())
|
uavcan::NodeID dst_node_id_override = uavcan::NodeID())
|
||||||
{
|
{
|
||||||
ts_ += 100;
|
ts_ += uavcan::MonotonicDuration::fromUSec(100);
|
||||||
|
const uavcan::UtcTime utc = uavcan::UtcTime::fromUSec(ts_.toUSec() + 1000000000ul);
|
||||||
const uavcan::NodeID dst_node_id = (transfer_type == uavcan::TransferTypeMessageBroadcast)
|
const uavcan::NodeID dst_node_id = (transfer_type == uavcan::TransferTypeMessageBroadcast)
|
||||||
? uavcan::NodeID::Broadcast
|
? uavcan::NodeID::Broadcast
|
||||||
: (dst_node_id_override.isValid() ? dst_node_id_override : dst_node_id_);
|
: (dst_node_id_override.isValid() ? dst_node_id_override : dst_node_id_);
|
||||||
const Transfer tr(ts_, ts_ + 1000000000ul, transfer_type, tid_, source_node_id, dst_node_id, payload, type);
|
const Transfer tr(ts_, utc, transfer_type, tid_, source_node_id, dst_node_id, payload, type);
|
||||||
tid_.increment();
|
tid_.increment();
|
||||||
return tr;
|
return tr;
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -50,7 +50,7 @@ TEST(Publisher, Basic)
|
|||||||
msg.payload = "Msg";
|
msg.payload = "Msg";
|
||||||
|
|
||||||
const uint8_t expected_transfer_payload[] = {0x42, 0x72, 0x08, 0xa5, 'M', 's', 'g'};
|
const uint8_t expected_transfer_payload[] = {0x42, 0x72, 0x08, 0xa5, 'M', 's', 'g'};
|
||||||
const uint64_t tx_timeout = publisher.DefaultTxTimeoutUsec;
|
const uint64_t tx_timeout_usec = publisher.getDefaultTxTimeout().toUSec();
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Broadcast
|
* Broadcast
|
||||||
@@ -67,8 +67,8 @@ TEST(Publisher, Basic)
|
|||||||
uavcan::CanFrame expected_can_frame;
|
uavcan::CanFrame expected_can_frame;
|
||||||
ASSERT_TRUE(expected_frame.compile(expected_can_frame));
|
ASSERT_TRUE(expected_frame.compile(expected_can_frame));
|
||||||
|
|
||||||
ASSERT_TRUE(can_driver.ifaces[0].matchAndPopTx(expected_can_frame, tx_timeout + 100));
|
ASSERT_TRUE(can_driver.ifaces[0].matchAndPopTx(expected_can_frame, tx_timeout_usec + 100));
|
||||||
ASSERT_TRUE(can_driver.ifaces[1].matchAndPopTx(expected_can_frame, tx_timeout + 100));
|
ASSERT_TRUE(can_driver.ifaces[1].matchAndPopTx(expected_can_frame, tx_timeout_usec + 100));
|
||||||
ASSERT_TRUE(can_driver.ifaces[0].tx.empty());
|
ASSERT_TRUE(can_driver.ifaces[0].tx.empty());
|
||||||
ASSERT_TRUE(can_driver.ifaces[1].tx.empty());
|
ASSERT_TRUE(can_driver.ifaces[1].tx.empty());
|
||||||
|
|
||||||
@@ -80,8 +80,8 @@ TEST(Publisher, Basic)
|
|||||||
expected_frame.setPayload(expected_transfer_payload, 7);
|
expected_frame.setPayload(expected_transfer_payload, 7);
|
||||||
ASSERT_TRUE(expected_frame.compile(expected_can_frame));
|
ASSERT_TRUE(expected_frame.compile(expected_can_frame));
|
||||||
|
|
||||||
ASSERT_TRUE(can_driver.ifaces[0].matchAndPopTx(expected_can_frame, tx_timeout + 100));
|
ASSERT_TRUE(can_driver.ifaces[0].matchAndPopTx(expected_can_frame, tx_timeout_usec + 100));
|
||||||
ASSERT_TRUE(can_driver.ifaces[1].matchAndPopTx(expected_can_frame, tx_timeout + 100));
|
ASSERT_TRUE(can_driver.ifaces[1].matchAndPopTx(expected_can_frame, tx_timeout_usec + 100));
|
||||||
ASSERT_TRUE(can_driver.ifaces[0].tx.empty());
|
ASSERT_TRUE(can_driver.ifaces[0].tx.empty());
|
||||||
ASSERT_TRUE(can_driver.ifaces[1].tx.empty());
|
ASSERT_TRUE(can_driver.ifaces[1].tx.empty());
|
||||||
}
|
}
|
||||||
@@ -103,8 +103,8 @@ TEST(Publisher, Basic)
|
|||||||
uavcan::CanFrame expected_can_frame;
|
uavcan::CanFrame expected_can_frame;
|
||||||
ASSERT_TRUE(expected_frame.compile(expected_can_frame));
|
ASSERT_TRUE(expected_frame.compile(expected_can_frame));
|
||||||
|
|
||||||
ASSERT_TRUE(can_driver.ifaces[0].matchAndPopTx(expected_can_frame, tx_timeout + 100 + 1000));
|
ASSERT_TRUE(can_driver.ifaces[0].matchAndPopTx(expected_can_frame, tx_timeout_usec + 100 + 1000));
|
||||||
ASSERT_TRUE(can_driver.ifaces[1].matchAndPopTx(expected_can_frame, tx_timeout + 100 + 1000));
|
ASSERT_TRUE(can_driver.ifaces[1].matchAndPopTx(expected_can_frame, tx_timeout_usec + 100 + 1000));
|
||||||
ASSERT_TRUE(can_driver.ifaces[0].tx.empty());
|
ASSERT_TRUE(can_driver.ifaces[0].tx.empty());
|
||||||
ASSERT_TRUE(can_driver.ifaces[1].tx.empty());
|
ASSERT_TRUE(can_driver.ifaces[1].tx.empty());
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -18,8 +18,8 @@ struct SubscriptionListener
|
|||||||
|
|
||||||
struct ReceivedDataStructureCopy
|
struct ReceivedDataStructureCopy
|
||||||
{
|
{
|
||||||
uint64_t ts_monotonic;
|
uavcan::MonotonicTime ts_monotonic;
|
||||||
uint64_t ts_utc;
|
uavcan::UtcTime ts_utc;
|
||||||
uavcan::TransferType transfer_type;
|
uavcan::TransferType transfer_type;
|
||||||
uavcan::TransferID transfer_id;
|
uavcan::TransferID transfer_id;
|
||||||
uavcan::NodeID src_node_id;
|
uavcan::NodeID src_node_id;
|
||||||
@@ -120,7 +120,7 @@ TEST(Subscriber, Basic)
|
|||||||
// uint_fast8_t frame_index, TransferID transfer_id, bool last_frame
|
// uint_fast8_t frame_index, TransferID transfer_id, bool last_frame
|
||||||
uavcan::Frame frame(uavcan::mavlink::Message::DefaultDataTypeID, tt, uavcan::NodeID(i + 100), dni, 0, i, true);
|
uavcan::Frame frame(uavcan::mavlink::Message::DefaultDataTypeID, tt, uavcan::NodeID(i + 100), dni, 0, i, true);
|
||||||
frame.setPayload(transfer_payload, 7);
|
frame.setPayload(transfer_payload, 7);
|
||||||
uavcan::RxFrame rx_frame(frame, clock_driver.getMonotonicMicroseconds(), clock_driver.getUtcMicroseconds(), 0);
|
uavcan::RxFrame rx_frame(frame, clock_driver.getMonotonic(), clock_driver.getUtc(), 0);
|
||||||
rx_frames.push_back(rx_frame);
|
rx_frames.push_back(rx_frame);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -147,7 +147,7 @@ TEST(Subscriber, Basic)
|
|||||||
can_driver.ifaces[1].pushRx(rx_frames[i]);
|
can_driver.ifaces[1].pushRx(rx_frames[i]);
|
||||||
}
|
}
|
||||||
|
|
||||||
ASSERT_LE(0, sch.spin(clock_driver.getMonotonicMicroseconds() + 10000));
|
ASSERT_LE(0, sch.spin(clock_driver.getMonotonic() + durMono(10000)));
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Validation
|
* Validation
|
||||||
@@ -224,13 +224,12 @@ TEST(Subscriber, FailureCount)
|
|||||||
uavcan::Frame frame(uavcan::mavlink::Message::DefaultDataTypeID, uavcan::TransferTypeMessageBroadcast,
|
uavcan::Frame frame(uavcan::mavlink::Message::DefaultDataTypeID, uavcan::TransferTypeMessageBroadcast,
|
||||||
uavcan::NodeID(i + 100), uavcan::NodeID::Broadcast, 0, i, true);
|
uavcan::NodeID(i + 100), uavcan::NodeID::Broadcast, 0, i, true);
|
||||||
// No payload - broken transfer
|
// No payload - broken transfer
|
||||||
uavcan::RxFrame rx_frame(frame, clock_driver.getMonotonicMicroseconds(),
|
uavcan::RxFrame rx_frame(frame, clock_driver.getMonotonic(), clock_driver.getUtc(), 0);
|
||||||
clock_driver.getUtcMicroseconds(), 0);
|
|
||||||
can_driver.ifaces[0].pushRx(rx_frame);
|
can_driver.ifaces[0].pushRx(rx_frame);
|
||||||
can_driver.ifaces[1].pushRx(rx_frame);
|
can_driver.ifaces[1].pushRx(rx_frame);
|
||||||
}
|
}
|
||||||
|
|
||||||
ASSERT_LE(0, sch.spin(clock_driver.getMonotonicMicroseconds() + 10000));
|
ASSERT_LE(0, sch.spin(clock_driver.getMonotonic() + durMono(10000)));
|
||||||
|
|
||||||
ASSERT_EQ(4, sub.getFailureCount());
|
ASSERT_EQ(4, sub.getFailureCount());
|
||||||
|
|
||||||
@@ -276,13 +275,12 @@ TEST(Subscriber, SingleFrameTransfer)
|
|||||||
uavcan::Frame frame(root_ns_a::EmptyMessage::DefaultDataTypeID, uavcan::TransferTypeMessageBroadcast,
|
uavcan::Frame frame(root_ns_a::EmptyMessage::DefaultDataTypeID, uavcan::TransferTypeMessageBroadcast,
|
||||||
uavcan::NodeID(i + 100), uavcan::NodeID::Broadcast, 0, i, true);
|
uavcan::NodeID(i + 100), uavcan::NodeID::Broadcast, 0, i, true);
|
||||||
// No payload - message is empty
|
// No payload - message is empty
|
||||||
uavcan::RxFrame rx_frame(frame, clock_driver.getMonotonicMicroseconds(),
|
uavcan::RxFrame rx_frame(frame, clock_driver.getMonotonic(), clock_driver.getUtc(), 0);
|
||||||
clock_driver.getUtcMicroseconds(), 0);
|
|
||||||
can_driver.ifaces[0].pushRx(rx_frame);
|
can_driver.ifaces[0].pushRx(rx_frame);
|
||||||
can_driver.ifaces[1].pushRx(rx_frame);
|
can_driver.ifaces[1].pushRx(rx_frame);
|
||||||
}
|
}
|
||||||
|
|
||||||
ASSERT_LE(0, sch.spin(clock_driver.getMonotonicMicroseconds() + 10000));
|
ASSERT_LE(0, sch.spin(clock_driver.getMonotonic() + durMono(10000)));
|
||||||
|
|
||||||
ASSERT_EQ(0, sub.getFailureCount());
|
ASSERT_EQ(0, sub.getFailureCount());
|
||||||
|
|
||||||
|
|||||||
@@ -4,6 +4,7 @@
|
|||||||
|
|
||||||
#include <gtest/gtest.h>
|
#include <gtest/gtest.h>
|
||||||
#include <uavcan/time.hpp>
|
#include <uavcan/time.hpp>
|
||||||
|
#include <uavcan/Timestamp.hpp>
|
||||||
|
|
||||||
|
|
||||||
TEST(Time, Monotonic)
|
TEST(Time, Monotonic)
|
||||||
|
|||||||
Reference in New Issue
Block a user