forked from Archive/PX4-Autopilot
uavcan drivers: update code style
Pure refactoring, just running 'make format'.
This commit is contained in:
Beat Küng
parent
6049a95073
commit
6622f04feb
|
|
@ -11,7 +11,6 @@ exec find boards msg src platforms \
|
|||
-path msg/templates/urtps -prune -o \
|
||||
-path platforms/nuttx/NuttX -prune -o \
|
||||
-path src/drivers/uavcan/libuavcan -prune -o \
|
||||
-path src/drivers/uavcan/uavcan_drivers -prune -o \
|
||||
-path src/lib/DriverFramework -prune -o \
|
||||
-path src/lib/ecl -prune -o \
|
||||
-path src/lib/matrix -prune -o \
|
||||
|
|
|
|||
|
|
@ -32,16 +32,15 @@ static const uavcan::int16_t ErrMcrSOFTRSTNotCleared = 1011; ///< MCR_SOFTRST
|
|||
* RX queue item.
|
||||
* The application shall not use this directly.
|
||||
*/
|
||||
struct CanRxItem
|
||||
{
|
||||
uavcan::uint64_t utc_usec;
|
||||
uavcan::CanFrame frame;
|
||||
uavcan::CanIOFlags flags;
|
||||
CanRxItem()
|
||||
: utc_usec(0),
|
||||
flags(0)
|
||||
{
|
||||
}
|
||||
struct CanRxItem {
|
||||
uavcan::uint64_t utc_usec;
|
||||
uavcan::CanFrame frame;
|
||||
uavcan::CanIOFlags flags;
|
||||
CanRxItem()
|
||||
: utc_usec(0),
|
||||
flags(0)
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
@ -49,215 +48,218 @@ struct CanRxItem
|
|||
* The application shall not use this directly.
|
||||
*/
|
||||
class CanIface : public uavcan::ICanIface
|
||||
, uavcan::Noncopyable
|
||||
, uavcan::Noncopyable
|
||||
{
|
||||
const uavcan::uint32_t FIFO_IFLAG1 = flexcan::CAN_FIFO_NE | flexcan::CAN_FIFO_WARN | flexcan::CAN_FIFO_OV;
|
||||
class RxQueue
|
||||
{
|
||||
CanRxItem* const buf_;
|
||||
const uavcan::uint8_t capacity_;
|
||||
uavcan::uint8_t in_;
|
||||
uavcan::uint8_t out_;
|
||||
uavcan::uint8_t len_;
|
||||
uavcan::uint32_t overflow_cnt_;
|
||||
const uavcan::uint32_t FIFO_IFLAG1 = flexcan::CAN_FIFO_NE | flexcan::CAN_FIFO_WARN | flexcan::CAN_FIFO_OV;
|
||||
class RxQueue
|
||||
{
|
||||
CanRxItem *const buf_;
|
||||
const uavcan::uint8_t capacity_;
|
||||
uavcan::uint8_t in_;
|
||||
uavcan::uint8_t out_;
|
||||
uavcan::uint8_t len_;
|
||||
uavcan::uint32_t overflow_cnt_;
|
||||
|
||||
void registerOverflow();
|
||||
void registerOverflow();
|
||||
|
||||
public:
|
||||
RxQueue(CanRxItem* buf, uavcan::uint8_t capacity)
|
||||
: buf_(buf),
|
||||
capacity_(capacity),
|
||||
in_(0),
|
||||
out_(0),
|
||||
len_(0),
|
||||
overflow_cnt_(0)
|
||||
{
|
||||
}
|
||||
public:
|
||||
RxQueue(CanRxItem *buf, uavcan::uint8_t capacity)
|
||||
: buf_(buf),
|
||||
capacity_(capacity),
|
||||
in_(0),
|
||||
out_(0),
|
||||
len_(0),
|
||||
overflow_cnt_(0)
|
||||
{
|
||||
}
|
||||
|
||||
void push(const uavcan::CanFrame& frame, const uint64_t& utc_usec, uavcan::CanIOFlags flags);
|
||||
void pop(uavcan::CanFrame& out_frame, uavcan::uint64_t& out_utc_usec, uavcan::CanIOFlags& out_flags);
|
||||
void push(const uavcan::CanFrame &frame, const uint64_t &utc_usec, uavcan::CanIOFlags flags);
|
||||
void pop(uavcan::CanFrame &out_frame, uavcan::uint64_t &out_utc_usec, uavcan::CanIOFlags &out_flags);
|
||||
|
||||
void reset();
|
||||
unsigned getLength() const {
|
||||
return len_;
|
||||
}
|
||||
void reset();
|
||||
unsigned getLength() const
|
||||
{
|
||||
return len_;
|
||||
}
|
||||
|
||||
uavcan::uint32_t getOverflowCount() const {
|
||||
return overflow_cnt_;
|
||||
}
|
||||
};
|
||||
uavcan::uint32_t getOverflowCount() const
|
||||
{
|
||||
return overflow_cnt_;
|
||||
}
|
||||
};
|
||||
|
||||
struct Timings
|
||||
{
|
||||
uavcan::uint8_t prescaler;
|
||||
uavcan::uint8_t rjw;
|
||||
uavcan::uint8_t pseg1;
|
||||
uavcan::uint8_t pseg2;
|
||||
uavcan::uint8_t propseg;
|
||||
struct Timings {
|
||||
uavcan::uint8_t prescaler;
|
||||
uavcan::uint8_t rjw;
|
||||
uavcan::uint8_t pseg1;
|
||||
uavcan::uint8_t pseg2;
|
||||
uavcan::uint8_t propseg;
|
||||
|
||||
Timings()
|
||||
: prescaler(0),
|
||||
rjw(0),
|
||||
pseg1(0),
|
||||
pseg2(0),
|
||||
propseg(0)
|
||||
{
|
||||
}
|
||||
};
|
||||
Timings()
|
||||
: prescaler(0),
|
||||
rjw(0),
|
||||
pseg1(0),
|
||||
pseg2(0),
|
||||
propseg(0)
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
struct TxItem
|
||||
{
|
||||
uavcan::MonotonicTime deadline;
|
||||
uavcan::CanFrame frame;
|
||||
enum {free = 0, busy, abort } pending;
|
||||
bool loopback;
|
||||
bool abort_on_error;
|
||||
struct TxItem {
|
||||
uavcan::MonotonicTime deadline;
|
||||
uavcan::CanFrame frame;
|
||||
enum {free = 0, busy, abort } pending;
|
||||
bool loopback;
|
||||
bool abort_on_error;
|
||||
|
||||
TxItem()
|
||||
: pending(free),
|
||||
loopback(false),
|
||||
abort_on_error(false)
|
||||
{
|
||||
}
|
||||
};
|
||||
TxItem()
|
||||
: pending(free),
|
||||
loopback(false),
|
||||
abort_on_error(false)
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
enum { NumTxMesgBuffers = 6 };
|
||||
enum { NumFilters = 16 };
|
||||
enum { NumTxMesgBuffers = 6 };
|
||||
enum { NumFilters = 16 };
|
||||
|
||||
uavcan::uint8_t MaxMB;
|
||||
uavcan::uint8_t MaxMB;
|
||||
|
||||
RxQueue rx_queue_;
|
||||
flexcan::CanType* const can_;
|
||||
uavcan::uint64_t error_cnt_;
|
||||
uavcan::uint64_t fifo_warn_cnt_;
|
||||
uavcan::uint32_t pending_aborts_;
|
||||
uavcan::uint32_t served_aborts_cnt_;
|
||||
BusEvent& update_event_;
|
||||
TxItem pending_tx_[NumTxMesgBuffers];
|
||||
uavcan::uint8_t peak_tx_mailbox_index_;
|
||||
const uavcan::uint8_t self_index_;
|
||||
bool had_activity_;
|
||||
RxQueue rx_queue_;
|
||||
flexcan::CanType *const can_;
|
||||
uavcan::uint64_t error_cnt_;
|
||||
uavcan::uint64_t fifo_warn_cnt_;
|
||||
uavcan::uint32_t pending_aborts_;
|
||||
uavcan::uint32_t served_aborts_cnt_;
|
||||
BusEvent &update_event_;
|
||||
TxItem pending_tx_[NumTxMesgBuffers];
|
||||
uavcan::uint8_t peak_tx_mailbox_index_;
|
||||
const uavcan::uint8_t self_index_;
|
||||
bool had_activity_;
|
||||
|
||||
int computeTimings(uavcan::uint32_t target_bitrate, Timings& out_timings);
|
||||
int computeTimings(uavcan::uint32_t target_bitrate, Timings &out_timings);
|
||||
|
||||
virtual uavcan::int16_t send(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline,
|
||||
uavcan::CanIOFlags flags);
|
||||
virtual uavcan::int16_t send(const uavcan::CanFrame &frame, uavcan::MonotonicTime tx_deadline,
|
||||
uavcan::CanIOFlags flags);
|
||||
|
||||
virtual uavcan::int16_t receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic,
|
||||
uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags);
|
||||
virtual uavcan::int16_t receive(uavcan::CanFrame &out_frame, uavcan::MonotonicTime &out_ts_monotonic,
|
||||
uavcan::UtcTime &out_ts_utc, uavcan::CanIOFlags &out_flags);
|
||||
|
||||
virtual uavcan::int16_t configureFilters(const uavcan::CanFilterConfig* filter_configs,
|
||||
uavcan::uint16_t num_configs);
|
||||
virtual uavcan::int16_t configureFilters(const uavcan::CanFilterConfig *filter_configs,
|
||||
uavcan::uint16_t num_configs);
|
||||
|
||||
virtual uavcan::uint16_t getNumFilters() const {
|
||||
return NumFilters;
|
||||
}
|
||||
virtual uavcan::uint16_t getNumFilters() const
|
||||
{
|
||||
return NumFilters;
|
||||
}
|
||||
|
||||
void handleTxMailboxInterrupt(uavcan::uint8_t mailbox_index, bool txok, uavcan::uint64_t utc_usec);
|
||||
void handleTxMailboxInterrupt(uavcan::uint8_t mailbox_index, bool txok, uavcan::uint64_t utc_usec);
|
||||
|
||||
bool waitMCRChange(uavcan::uint32_t mask, bool target_state);
|
||||
void setMCR(uavcan::uint32_t mask, bool target_state);
|
||||
bool waitMCRChange(uavcan::uint32_t mask, bool target_state);
|
||||
void setMCR(uavcan::uint32_t mask, bool target_state);
|
||||
|
||||
bool setEnable(bool enable_true);
|
||||
uavcan::int16_t doReset(Timings timings);
|
||||
bool waitFreezeAckChange(bool target_state);
|
||||
void setFreeze(bool freeze_true);
|
||||
bool setEnable(bool enable_true);
|
||||
uavcan::int16_t doReset(Timings timings);
|
||||
bool waitFreezeAckChange(bool target_state);
|
||||
void setFreeze(bool freeze_true);
|
||||
|
||||
public:
|
||||
enum { MaxRxQueueCapacity = 254 };
|
||||
enum { MaxRxQueueCapacity = 254 };
|
||||
|
||||
enum OperatingMode
|
||||
{
|
||||
NormalMode,
|
||||
SilentMode
|
||||
};
|
||||
enum OperatingMode {
|
||||
NormalMode,
|
||||
SilentMode
|
||||
};
|
||||
|
||||
CanIface(flexcan::CanType* can, BusEvent& update_event, uavcan::uint8_t self_index,
|
||||
CanRxItem* rx_queue_buffer, uavcan::uint8_t rx_queue_capacity)
|
||||
: MaxMB(flexcan::HWMaxMB),
|
||||
rx_queue_(rx_queue_buffer, rx_queue_capacity),
|
||||
can_(can),
|
||||
error_cnt_(0),
|
||||
fifo_warn_cnt_(0),
|
||||
pending_aborts_(0),
|
||||
served_aborts_cnt_(0),
|
||||
update_event_(update_event),
|
||||
peak_tx_mailbox_index_(0),
|
||||
self_index_(self_index),
|
||||
had_activity_(false)
|
||||
{
|
||||
UAVCAN_ASSERT(self_index_ < UAVCAN_KINETIS_NUM_IFACES);
|
||||
}
|
||||
CanIface(flexcan::CanType *can, BusEvent &update_event, uavcan::uint8_t self_index,
|
||||
CanRxItem *rx_queue_buffer, uavcan::uint8_t rx_queue_capacity)
|
||||
: MaxMB(flexcan::HWMaxMB),
|
||||
rx_queue_(rx_queue_buffer, rx_queue_capacity),
|
||||
can_(can),
|
||||
error_cnt_(0),
|
||||
fifo_warn_cnt_(0),
|
||||
pending_aborts_(0),
|
||||
served_aborts_cnt_(0),
|
||||
update_event_(update_event),
|
||||
peak_tx_mailbox_index_(0),
|
||||
self_index_(self_index),
|
||||
had_activity_(false)
|
||||
{
|
||||
UAVCAN_ASSERT(self_index_ < UAVCAN_KINETIS_NUM_IFACES);
|
||||
}
|
||||
|
||||
/**
|
||||
* Initializes the hardware CAN controller.
|
||||
* Assumes:
|
||||
* - Iface clock is enabled
|
||||
* - Iface has been resetted via RCC
|
||||
* - Caller will configure NVIC by itself
|
||||
*/
|
||||
int init(const uavcan::uint32_t bitrate, const OperatingMode mode);
|
||||
/**
|
||||
* Initializes the hardware CAN controller.
|
||||
* Assumes:
|
||||
* - Iface clock is enabled
|
||||
* - Iface has been resetted via RCC
|
||||
* - Caller will configure NVIC by itself
|
||||
*/
|
||||
int init(const uavcan::uint32_t bitrate, const OperatingMode mode);
|
||||
|
||||
void handleTxInterrupt(uavcan::uint32_t tx_iflags, uavcan::uint64_t utc_usec);
|
||||
void handleRxInterrupt(uavcan::uint32_t rx_iflags, uavcan::uint64_t utc_usec);
|
||||
void handleTxInterrupt(uavcan::uint32_t tx_iflags, uavcan::uint64_t utc_usec);
|
||||
void handleRxInterrupt(uavcan::uint32_t rx_iflags, uavcan::uint64_t utc_usec);
|
||||
|
||||
/**
|
||||
* This method is used to count errors and abort transmission on error if necessary.
|
||||
* This functionality used to be implemented in the SCE interrupt handler, but that approach was
|
||||
* generating too much processing overhead, especially on disconnected interfaces.
|
||||
*
|
||||
* Should be called from RX ISR, TX ISR, and select(); interrupts must be enabled.
|
||||
*/
|
||||
void pollErrorFlagsFromISR();
|
||||
/**
|
||||
* This method is used to count errors and abort transmission on error if necessary.
|
||||
* This functionality used to be implemented in the SCE interrupt handler, but that approach was
|
||||
* generating too much processing overhead, especially on disconnected interfaces.
|
||||
*
|
||||
* Should be called from RX ISR, TX ISR, and select(); interrupts must be enabled.
|
||||
*/
|
||||
void pollErrorFlagsFromISR();
|
||||
|
||||
void discardTimedOutTxMailboxes(uavcan::MonotonicTime current_time);
|
||||
void discardTimedOutTxMailboxes(uavcan::MonotonicTime current_time);
|
||||
|
||||
bool canAcceptNewTxFrame(const uavcan::CanFrame& frame) const;
|
||||
bool isRxBufferEmpty() const;
|
||||
bool canAcceptNewTxFrame(const uavcan::CanFrame &frame) const;
|
||||
bool isRxBufferEmpty() const;
|
||||
|
||||
/**
|
||||
* Number of RX frames lost due to queue overflow.
|
||||
* This is an atomic read, it doesn't require a critical section.
|
||||
*/
|
||||
uavcan::uint32_t getRxQueueOverflowCount() const {
|
||||
return rx_queue_.getOverflowCount();
|
||||
}
|
||||
/**
|
||||
* Number of RX frames lost due to queue overflow.
|
||||
* This is an atomic read, it doesn't require a critical section.
|
||||
*/
|
||||
uavcan::uint32_t getRxQueueOverflowCount() const
|
||||
{
|
||||
return rx_queue_.getOverflowCount();
|
||||
}
|
||||
|
||||
/**
|
||||
* Total number of hardware failures and other kinds of errors (e.g. queue overruns).
|
||||
* May increase continuously if the interface is not connected to the bus.
|
||||
*/
|
||||
virtual uavcan::uint64_t getErrorCount() const;
|
||||
/**
|
||||
* Total number of hardware failures and other kinds of errors (e.g. queue overruns).
|
||||
* May increase continuously if the interface is not connected to the bus.
|
||||
*/
|
||||
virtual uavcan::uint64_t getErrorCount() const;
|
||||
|
||||
/**
|
||||
* Number of times the driver exercised library's requirement to abort transmission on first error.
|
||||
* This is an atomic read, it doesn't require a critical section.
|
||||
* See @ref uavcan::CanIOFlagAbortOnError.
|
||||
*/
|
||||
uavcan::uint32_t getVoluntaryTxAbortCount() const {
|
||||
return served_aborts_cnt_;
|
||||
}
|
||||
/**
|
||||
* Number of times the driver exercised library's requirement to abort transmission on first error.
|
||||
* This is an atomic read, it doesn't require a critical section.
|
||||
* See @ref uavcan::CanIOFlagAbortOnError.
|
||||
*/
|
||||
uavcan::uint32_t getVoluntaryTxAbortCount() const
|
||||
{
|
||||
return served_aborts_cnt_;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the number of frames pending in the RX queue.
|
||||
* This is intended for debug use only.
|
||||
*/
|
||||
unsigned getRxQueueLength() const;
|
||||
/**
|
||||
* Returns the number of frames pending in the RX queue.
|
||||
* This is intended for debug use only.
|
||||
*/
|
||||
unsigned getRxQueueLength() const;
|
||||
|
||||
/**
|
||||
* Whether this iface had at least one successful IO since the previous call of this method.
|
||||
* This is designed for use with iface activity LEDs.
|
||||
*/
|
||||
bool hadActivity();
|
||||
/**
|
||||
* Whether this iface had at least one successful IO since the previous call of this method.
|
||||
* This is designed for use with iface activity LEDs.
|
||||
*/
|
||||
bool hadActivity();
|
||||
|
||||
/**
|
||||
* Peak number of TX mailboxes used concurrently since initialization.
|
||||
* Range is [1, 3].
|
||||
* Value of 3 suggests that priority inversion could be taking place.
|
||||
*/
|
||||
uavcan::uint8_t getPeakNumTxMailboxesUsed() const {
|
||||
return uavcan::uint8_t(peak_tx_mailbox_index_ + 1);
|
||||
}
|
||||
/**
|
||||
* Peak number of TX mailboxes used concurrently since initialization.
|
||||
* Range is [1, 3].
|
||||
* Value of 3 suggests that priority inversion could be taking place.
|
||||
*/
|
||||
uavcan::uint8_t getPeakNumTxMailboxesUsed() const
|
||||
{
|
||||
return uavcan::uint8_t(peak_tx_mailbox_index_ + 1);
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
@ -265,61 +267,62 @@ public:
|
|||
* Please avoid direct use, prefer @ref CanInitHelper instead.
|
||||
*/
|
||||
class CanDriver : public uavcan::ICanDriver
|
||||
, uavcan::Noncopyable
|
||||
, uavcan::Noncopyable
|
||||
{
|
||||
BusEvent update_event_;
|
||||
CanIface if0_;
|
||||
BusEvent update_event_;
|
||||
CanIface if0_;
|
||||
#if UAVCAN_KINETIS_NUM_IFACES > 1
|
||||
CanIface if1_;
|
||||
CanIface if1_;
|
||||
#endif
|
||||
|
||||
virtual uavcan::int16_t select(uavcan::CanSelectMasks& inout_masks,
|
||||
const uavcan::CanFrame* (&pending_tx)[uavcan::MaxCanIfaces],
|
||||
uavcan::MonotonicTime blocking_deadline);
|
||||
virtual uavcan::int16_t select(uavcan::CanSelectMasks &inout_masks,
|
||||
const uavcan::CanFrame * (&pending_tx)[uavcan::MaxCanIfaces],
|
||||
uavcan::MonotonicTime blocking_deadline);
|
||||
|
||||
static void initOnce();
|
||||
static void initOnce();
|
||||
|
||||
public:
|
||||
template <unsigned RxQueueCapacity>
|
||||
CanDriver(CanRxItem (&rx_queue_storage)[UAVCAN_KINETIS_NUM_IFACES][RxQueueCapacity])
|
||||
: update_event_(*this),
|
||||
if0_(flexcan::Can[0], update_event_, 0, rx_queue_storage[0], RxQueueCapacity)
|
||||
template <unsigned RxQueueCapacity>
|
||||
CanDriver(CanRxItem(&rx_queue_storage)[UAVCAN_KINETIS_NUM_IFACES][RxQueueCapacity])
|
||||
: update_event_(*this),
|
||||
if0_(flexcan::Can[0], update_event_, 0, rx_queue_storage[0], RxQueueCapacity)
|
||||
#if UAVCAN_KINETIS_NUM_IFACES > 1
|
||||
, if1_(flexcan::Can[1], update_event_, 1, rx_queue_storage[1], RxQueueCapacity)
|
||||
, if1_(flexcan::Can[1], update_event_, 1, rx_queue_storage[1], RxQueueCapacity)
|
||||
#endif
|
||||
{
|
||||
uavcan::StaticAssert<(RxQueueCapacity <= CanIface::MaxRxQueueCapacity)>::check();
|
||||
}
|
||||
{
|
||||
uavcan::StaticAssert < (RxQueueCapacity <= CanIface::MaxRxQueueCapacity) >::check();
|
||||
}
|
||||
|
||||
/**
|
||||
* This function returns select masks indicating which interfaces are available for read/write.
|
||||
*/
|
||||
uavcan::CanSelectMasks makeSelectMasks(const uavcan::CanFrame* (&pending_tx)[uavcan::MaxCanIfaces]) const;
|
||||
/**
|
||||
* This function returns select masks indicating which interfaces are available for read/write.
|
||||
*/
|
||||
uavcan::CanSelectMasks makeSelectMasks(const uavcan::CanFrame * (&pending_tx)[uavcan::MaxCanIfaces]) const;
|
||||
|
||||
/**
|
||||
* Whether there's at least one interface where receive() would return a frame.
|
||||
*/
|
||||
bool hasReadableInterfaces() const;
|
||||
/**
|
||||
* Whether there's at least one interface where receive() would return a frame.
|
||||
*/
|
||||
bool hasReadableInterfaces() const;
|
||||
|
||||
/**
|
||||
* Returns zero if OK.
|
||||
* Returns negative value if failed (e.g. invalid bitrate).
|
||||
*/
|
||||
int init(const uavcan::uint32_t bitrate, const CanIface::OperatingMode mode);
|
||||
/**
|
||||
* Returns zero if OK.
|
||||
* Returns negative value if failed (e.g. invalid bitrate).
|
||||
*/
|
||||
int init(const uavcan::uint32_t bitrate, const CanIface::OperatingMode mode);
|
||||
|
||||
virtual CanIface* getIface(uavcan::uint8_t iface_index);
|
||||
virtual CanIface *getIface(uavcan::uint8_t iface_index);
|
||||
|
||||
virtual uavcan::uint8_t getNumIfaces() const {
|
||||
return UAVCAN_KINETIS_NUM_IFACES;
|
||||
}
|
||||
virtual uavcan::uint8_t getNumIfaces() const
|
||||
{
|
||||
return UAVCAN_KINETIS_NUM_IFACES;
|
||||
}
|
||||
|
||||
/**
|
||||
* Whether at least one iface had at least one successful IO since previous call of this method.
|
||||
* This is designed for use with iface activity LEDs.
|
||||
*/
|
||||
bool hadActivity();
|
||||
/**
|
||||
* Whether at least one iface had at least one successful IO since previous call of this method.
|
||||
* This is designed for use with iface activity LEDs.
|
||||
*/
|
||||
bool hadActivity();
|
||||
|
||||
BusEvent& updateEvent() { return update_event_; }
|
||||
BusEvent &updateEvent() { return update_event_; }
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
@ -330,92 +333,85 @@ public:
|
|||
template <unsigned RxQueueCapacity = 128>
|
||||
class CanInitHelper
|
||||
{
|
||||
CanRxItem queue_storage_[UAVCAN_KINETIS_NUM_IFACES][RxQueueCapacity];
|
||||
CanRxItem queue_storage_[UAVCAN_KINETIS_NUM_IFACES][RxQueueCapacity];
|
||||
|
||||
public:
|
||||
enum { BitRateAutoDetect = 0 };
|
||||
enum { BitRateAutoDetect = 0 };
|
||||
|
||||
CanDriver driver;
|
||||
CanDriver driver;
|
||||
|
||||
CanInitHelper() :
|
||||
driver(queue_storage_)
|
||||
{
|
||||
}
|
||||
CanInitHelper() :
|
||||
driver(queue_storage_)
|
||||
{
|
||||
}
|
||||
|
||||
/**
|
||||
* This overload simply configures the provided bitrate.
|
||||
* Auto bit rate detection will not be performed.
|
||||
* Bitrate value must be positive.
|
||||
* @return Negative value on error; non-negative on success. Refer to constants Err*.
|
||||
*/
|
||||
int init(uavcan::uint32_t bitrate)
|
||||
{
|
||||
return driver.init(bitrate, CanIface::NormalMode);
|
||||
}
|
||||
/**
|
||||
* This overload simply configures the provided bitrate.
|
||||
* Auto bit rate detection will not be performed.
|
||||
* Bitrate value must be positive.
|
||||
* @return Negative value on error; non-negative on success. Refer to constants Err*.
|
||||
*/
|
||||
int init(uavcan::uint32_t bitrate)
|
||||
{
|
||||
return driver.init(bitrate, CanIface::NormalMode);
|
||||
}
|
||||
|
||||
/**
|
||||
* This function can either initialize the driver at a fixed bit rate, or it can perform
|
||||
* automatic bit rate detection. For theory please refer to the CiA application note #801.
|
||||
*
|
||||
* @param delay_callable A callable entity that suspends execution for strictly more than one second.
|
||||
* The callable entity will be invoked without arguments.
|
||||
* @ref getRecommendedListeningDelay().
|
||||
*
|
||||
* @param inout_bitrate Fixed bit rate or zero. Zero invokes the bit rate detection process.
|
||||
* If auto detection was used, the function will update the argument
|
||||
* with established bit rate. In case of an error the value will be undefined.
|
||||
*
|
||||
* @return Negative value on error; non-negative on success. Refer to constants Err*.
|
||||
*/
|
||||
template <typename DelayCallable>
|
||||
int init(DelayCallable delay_callable, uavcan::uint32_t& inout_bitrate = BitRateAutoDetect)
|
||||
{
|
||||
if (inout_bitrate > 0)
|
||||
{
|
||||
return driver.init(inout_bitrate, CanIface::NormalMode);
|
||||
}
|
||||
else
|
||||
{
|
||||
static const uavcan::uint32_t StandardBitRates[] =
|
||||
{
|
||||
1000000,
|
||||
500000,
|
||||
250000,
|
||||
125000
|
||||
};
|
||||
/**
|
||||
* This function can either initialize the driver at a fixed bit rate, or it can perform
|
||||
* automatic bit rate detection. For theory please refer to the CiA application note #801.
|
||||
*
|
||||
* @param delay_callable A callable entity that suspends execution for strictly more than one second.
|
||||
* The callable entity will be invoked without arguments.
|
||||
* @ref getRecommendedListeningDelay().
|
||||
*
|
||||
* @param inout_bitrate Fixed bit rate or zero. Zero invokes the bit rate detection process.
|
||||
* If auto detection was used, the function will update the argument
|
||||
* with established bit rate. In case of an error the value will be undefined.
|
||||
*
|
||||
* @return Negative value on error; non-negative on success. Refer to constants Err*.
|
||||
*/
|
||||
template <typename DelayCallable>
|
||||
int init(DelayCallable delay_callable, uavcan::uint32_t &inout_bitrate = BitRateAutoDetect)
|
||||
{
|
||||
if (inout_bitrate > 0) {
|
||||
return driver.init(inout_bitrate, CanIface::NormalMode);
|
||||
|
||||
for (uavcan::uint8_t br = 0; br < sizeof(StandardBitRates) / sizeof(StandardBitRates[0]); br++)
|
||||
{
|
||||
inout_bitrate = StandardBitRates[br];
|
||||
} else {
|
||||
static const uavcan::uint32_t StandardBitRates[] = {
|
||||
1000000,
|
||||
500000,
|
||||
250000,
|
||||
125000
|
||||
};
|
||||
|
||||
const int res = driver.init(inout_bitrate, CanIface::SilentMode);
|
||||
for (uavcan::uint8_t br = 0; br < sizeof(StandardBitRates) / sizeof(StandardBitRates[0]); br++) {
|
||||
inout_bitrate = StandardBitRates[br];
|
||||
|
||||
delay_callable();
|
||||
const int res = driver.init(inout_bitrate, CanIface::SilentMode);
|
||||
|
||||
if (res >= 0)
|
||||
{
|
||||
for (uavcan::uint8_t iface = 0; iface < driver.getNumIfaces(); iface++)
|
||||
{
|
||||
if (!driver.getIface(iface)->isRxBufferEmpty())
|
||||
{
|
||||
// Re-initializing in normal mode
|
||||
return driver.init(inout_bitrate, CanIface::NormalMode);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
delay_callable();
|
||||
|
||||
return -ErrBitRateNotDetected;
|
||||
}
|
||||
}
|
||||
if (res >= 0) {
|
||||
for (uavcan::uint8_t iface = 0; iface < driver.getNumIfaces(); iface++) {
|
||||
if (!driver.getIface(iface)->isRxBufferEmpty()) {
|
||||
// Re-initializing in normal mode
|
||||
return driver.init(inout_bitrate, CanIface::NormalMode);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Use this value for listening delay during automatic bit rate detection.
|
||||
*/
|
||||
static uavcan::MonotonicDuration getRecommendedListeningDelay()
|
||||
{
|
||||
return uavcan::MonotonicDuration::fromMSec(1050);
|
||||
}
|
||||
return -ErrBitRateNotDetected;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Use this value for listening delay during automatic bit rate detection.
|
||||
*/
|
||||
static uavcan::MonotonicDuration getRecommendedListeningDelay()
|
||||
{
|
||||
return uavcan::MonotonicDuration::fromMSec(1050);
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -49,26 +49,25 @@ void adjustUtc(uavcan::UtcDuration adjustment);
|
|||
/**
|
||||
* UTC clock synchronization parameters
|
||||
*/
|
||||
struct UtcSyncParams
|
||||
{
|
||||
float offset_p; ///< PPM per one usec error
|
||||
float rate_i; ///< PPM per one PPM error for second
|
||||
float rate_error_corner_freq;
|
||||
float max_rate_correction_ppm;
|
||||
float lock_thres_rate_ppm;
|
||||
uavcan::UtcDuration lock_thres_offset;
|
||||
uavcan::UtcDuration min_jump; ///< Min error to jump rather than change rate
|
||||
struct UtcSyncParams {
|
||||
float offset_p; ///< PPM per one usec error
|
||||
float rate_i; ///< PPM per one PPM error for second
|
||||
float rate_error_corner_freq;
|
||||
float max_rate_correction_ppm;
|
||||
float lock_thres_rate_ppm;
|
||||
uavcan::UtcDuration lock_thres_offset;
|
||||
uavcan::UtcDuration min_jump; ///< Min error to jump rather than change rate
|
||||
|
||||
UtcSyncParams()
|
||||
: offset_p(0.01F),
|
||||
rate_i(0.02F),
|
||||
rate_error_corner_freq(0.01F),
|
||||
max_rate_correction_ppm(300.0F),
|
||||
lock_thres_rate_ppm(2.0F),
|
||||
lock_thres_offset(uavcan::UtcDuration::fromMSec(4)),
|
||||
min_jump(uavcan::UtcDuration::fromMSec(10))
|
||||
{
|
||||
}
|
||||
UtcSyncParams()
|
||||
: offset_p(0.01F),
|
||||
rate_i(0.02F),
|
||||
rate_error_corner_freq(0.01F),
|
||||
max_rate_correction_ppm(300.0F),
|
||||
lock_thres_rate_ppm(2.0F),
|
||||
lock_thres_offset(uavcan::UtcDuration::fromMSec(4)),
|
||||
min_jump(uavcan::UtcDuration::fromMSec(10))
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
@ -96,7 +95,7 @@ bool isUtcLocked();
|
|||
* Both functions are thread safe.
|
||||
*/
|
||||
UtcSyncParams getUtcSyncParams();
|
||||
void setUtcSyncParams(const UtcSyncParams& params);
|
||||
void setUtcSyncParams(const UtcSyncParams ¶ms);
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -104,28 +103,32 @@ void setUtcSyncParams(const UtcSyncParams& params);
|
|||
* Adapter for uavcan::ISystemClock.
|
||||
*/
|
||||
class SystemClock : public uavcan::ISystemClock
|
||||
, uavcan::Noncopyable
|
||||
, uavcan::Noncopyable
|
||||
{
|
||||
SystemClock() {
|
||||
}
|
||||
SystemClock()
|
||||
{
|
||||
}
|
||||
|
||||
virtual void adjustUtc(uavcan::UtcDuration adjustment) {
|
||||
clock::adjustUtc(adjustment);
|
||||
}
|
||||
virtual void adjustUtc(uavcan::UtcDuration adjustment)
|
||||
{
|
||||
clock::adjustUtc(adjustment);
|
||||
}
|
||||
|
||||
public:
|
||||
virtual uavcan::MonotonicTime getMonotonic() const {
|
||||
return clock::getMonotonic();
|
||||
}
|
||||
virtual uavcan::UtcTime getUtc() const {
|
||||
return clock::getUtc();
|
||||
}
|
||||
virtual uavcan::MonotonicTime getMonotonic() const
|
||||
{
|
||||
return clock::getMonotonic();
|
||||
}
|
||||
virtual uavcan::UtcTime getUtc() const
|
||||
{
|
||||
return clock::getUtc();
|
||||
}
|
||||
|
||||
/**
|
||||
* Calls clock::init() as needed.
|
||||
* This function is thread safe.
|
||||
*/
|
||||
static SystemClock& instance();
|
||||
/**
|
||||
* Calls clock::init() as needed.
|
||||
* This function is thread safe.
|
||||
*/
|
||||
static SystemClock &instance();
|
||||
};
|
||||
|
||||
}
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load Diff
|
|
@ -28,68 +28,68 @@ class CanDriver;
|
|||
*/
|
||||
class BusEvent : uavcan::Noncopyable
|
||||
{
|
||||
using SignalCallbackHandler = void(*)();
|
||||
using SignalCallbackHandler = void(*)();
|
||||
|
||||
SignalCallbackHandler signal_cb_{nullptr};
|
||||
sem_t sem_;
|
||||
SignalCallbackHandler signal_cb_{nullptr};
|
||||
sem_t sem_;
|
||||
public:
|
||||
|
||||
BusEvent(CanDriver& can_driver);
|
||||
~BusEvent();
|
||||
BusEvent(CanDriver &can_driver);
|
||||
~BusEvent();
|
||||
|
||||
void registerSignalCallback(SignalCallbackHandler handler) { signal_cb_ = handler; }
|
||||
void registerSignalCallback(SignalCallbackHandler handler) { signal_cb_ = handler; }
|
||||
|
||||
bool wait(uavcan::MonotonicDuration duration);
|
||||
bool wait(uavcan::MonotonicDuration duration);
|
||||
|
||||
void signalFromInterrupt();
|
||||
void signalFromInterrupt();
|
||||
};
|
||||
|
||||
class Mutex
|
||||
{
|
||||
pthread_mutex_t mutex_;
|
||||
pthread_mutex_t mutex_;
|
||||
|
||||
public:
|
||||
Mutex()
|
||||
{
|
||||
init();
|
||||
}
|
||||
Mutex()
|
||||
{
|
||||
init();
|
||||
}
|
||||
|
||||
int init()
|
||||
{
|
||||
return pthread_mutex_init(&mutex_, UAVCAN_NULLPTR);
|
||||
}
|
||||
int init()
|
||||
{
|
||||
return pthread_mutex_init(&mutex_, UAVCAN_NULLPTR);
|
||||
}
|
||||
|
||||
int deinit()
|
||||
{
|
||||
return pthread_mutex_destroy(&mutex_);
|
||||
}
|
||||
int deinit()
|
||||
{
|
||||
return pthread_mutex_destroy(&mutex_);
|
||||
}
|
||||
|
||||
void lock()
|
||||
{
|
||||
(void)pthread_mutex_lock(&mutex_);
|
||||
}
|
||||
void lock()
|
||||
{
|
||||
(void)pthread_mutex_lock(&mutex_);
|
||||
}
|
||||
|
||||
void unlock()
|
||||
{
|
||||
(void)pthread_mutex_unlock(&mutex_);
|
||||
}
|
||||
void unlock()
|
||||
{
|
||||
(void)pthread_mutex_unlock(&mutex_);
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
class MutexLocker
|
||||
{
|
||||
Mutex& mutex_;
|
||||
Mutex &mutex_;
|
||||
|
||||
public:
|
||||
MutexLocker(Mutex& mutex)
|
||||
: mutex_(mutex)
|
||||
{
|
||||
mutex_.lock();
|
||||
}
|
||||
~MutexLocker()
|
||||
{
|
||||
mutex_.unlock();
|
||||
}
|
||||
MutexLocker(Mutex &mutex)
|
||||
: mutex_(mutex)
|
||||
{
|
||||
mutex_.lock();
|
||||
}
|
||||
~MutexLocker()
|
||||
{
|
||||
mutex_.unlock();
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -48,19 +48,18 @@ namespace uavcan_kinetis
|
|||
{
|
||||
#if UAVCAN_KINETIS_NUTTX
|
||||
|
||||
struct CriticalSectionLocker
|
||||
{
|
||||
const irqstate_t flags_;
|
||||
struct CriticalSectionLocker {
|
||||
const irqstate_t flags_;
|
||||
|
||||
CriticalSectionLocker()
|
||||
: flags_(enter_critical_section())
|
||||
{
|
||||
}
|
||||
CriticalSectionLocker()
|
||||
: flags_(enter_critical_section())
|
||||
{
|
||||
}
|
||||
|
||||
~CriticalSectionLocker()
|
||||
{
|
||||
leave_critical_section(flags_);
|
||||
}
|
||||
~CriticalSectionLocker()
|
||||
{
|
||||
leave_critical_section(flags_);
|
||||
}
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -66,167 +66,166 @@ uavcan::uint64_t time_utc = 0;
|
|||
|
||||
void init()
|
||||
{
|
||||
CriticalSectionLocker lock;
|
||||
if (initialized)
|
||||
{
|
||||
return;
|
||||
}
|
||||
initialized = true;
|
||||
CriticalSectionLocker lock;
|
||||
|
||||
// Attach IRQ
|
||||
irq_attach(TIMX_IRQn, &TIMX_IRQHandler, NULL);
|
||||
if (initialized) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Power-on Clock
|
||||
modifyreg32(KINETIS_SIM_SCGC6, 0, SIM_SCGC6_PIT);
|
||||
initialized = true;
|
||||
|
||||
// Enable module
|
||||
putreg32(0, KINETIS_PIT_MCR);
|
||||
// Attach IRQ
|
||||
irq_attach(TIMX_IRQn, &TIMX_IRQHandler, NULL);
|
||||
|
||||
// Start the timer
|
||||
// Power-on Clock
|
||||
modifyreg32(KINETIS_SIM_SCGC6, 0, SIM_SCGC6_PIT);
|
||||
|
||||
putreg32(CountsPerPeriod - 1, TMR_REG(KINETIS_PIT_LDVAL_OFFSET));
|
||||
putreg32(PIT_TCTRL_TEN | PIT_TCTRL_TIE, TMR_REG(KINETIS_PIT_TCTRL_OFFSET)); // Start
|
||||
// Enable module
|
||||
putreg32(0, KINETIS_PIT_MCR);
|
||||
|
||||
// Prioritize and Enable IRQ
|
||||
// Start the timer
|
||||
|
||||
putreg32(CountsPerPeriod - 1, TMR_REG(KINETIS_PIT_LDVAL_OFFSET));
|
||||
putreg32(PIT_TCTRL_TEN | PIT_TCTRL_TIE, TMR_REG(KINETIS_PIT_TCTRL_OFFSET)); // Start
|
||||
|
||||
// Prioritize and Enable IRQ
|
||||
|
||||
#if 0
|
||||
// This has to be off or uses the default priority
|
||||
// Without the ability to point the vector
|
||||
// Directly to this ISR this will reenter the
|
||||
// exception_common and cause the interrupt
|
||||
// Stack pointer to be reset
|
||||
up_prioritize_irq(TIMX_IRQn, NVIC_SYSH_HIGH_PRIORITY);
|
||||
// This has to be off or uses the default priority
|
||||
// Without the ability to point the vector
|
||||
// Directly to this ISR this will reenter the
|
||||
// exception_common and cause the interrupt
|
||||
// Stack pointer to be reset
|
||||
up_prioritize_irq(TIMX_IRQn, NVIC_SYSH_HIGH_PRIORITY);
|
||||
#endif
|
||||
up_enable_irq(TIMX_IRQn);
|
||||
up_enable_irq(TIMX_IRQn);
|
||||
}
|
||||
|
||||
void setUtc(uavcan::UtcTime time)
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
UAVCAN_ASSERT(initialized);
|
||||
MutexLocker mlocker(mutex);
|
||||
UAVCAN_ASSERT(initialized);
|
||||
|
||||
{
|
||||
CriticalSectionLocker locker;
|
||||
time_utc = time.toUSec();
|
||||
}
|
||||
{
|
||||
CriticalSectionLocker locker;
|
||||
time_utc = time.toUSec();
|
||||
}
|
||||
|
||||
utc_set = true;
|
||||
utc_locked = false;
|
||||
utc_jump_cnt++;
|
||||
utc_prev_adj = 0;
|
||||
utc_rel_rate_ppm = 0;
|
||||
utc_set = true;
|
||||
utc_locked = false;
|
||||
utc_jump_cnt++;
|
||||
utc_prev_adj = 0;
|
||||
utc_rel_rate_ppm = 0;
|
||||
}
|
||||
|
||||
static uavcan::uint64_t sampleUtcFromCriticalSection()
|
||||
{
|
||||
UAVCAN_ASSERT(initialized);
|
||||
UAVCAN_ASSERT(getreg32(TMR_REG(KINETIS_PIT_TCTRL_OFFSET)) & PIT_TCTRL_TIE);
|
||||
UAVCAN_ASSERT(initialized);
|
||||
UAVCAN_ASSERT(getreg32(TMR_REG(KINETIS_PIT_TCTRL_OFFSET)) & PIT_TCTRL_TIE);
|
||||
|
||||
volatile uavcan::uint64_t time = time_utc;
|
||||
volatile uavcan::uint32_t cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_OFFSET));
|
||||
volatile uavcan::uint64_t time = time_utc;
|
||||
volatile uavcan::uint32_t cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_OFFSET));
|
||||
|
||||
if (getreg32(TMR_REG(KINETIS_PIT_TFLG_OFFSET)) & PIT_TFLG_TIF)
|
||||
{
|
||||
cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_OFFSET));
|
||||
const uavcan::int32_t add = uavcan::int32_t(USecPerOverflow) +
|
||||
(utc_accumulated_correction_nsec + utc_correction_nsec_per_overflow) / 1000;
|
||||
time = uavcan::uint64_t(uavcan::int64_t(time) + add);
|
||||
}
|
||||
return time + (cnt / CountsPerUs);
|
||||
if (getreg32(TMR_REG(KINETIS_PIT_TFLG_OFFSET)) & PIT_TFLG_TIF) {
|
||||
cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_OFFSET));
|
||||
const uavcan::int32_t add = uavcan::int32_t(USecPerOverflow) +
|
||||
(utc_accumulated_correction_nsec + utc_correction_nsec_per_overflow) / 1000;
|
||||
time = uavcan::uint64_t(uavcan::int64_t(time) + add);
|
||||
}
|
||||
|
||||
return time + (cnt / CountsPerUs);
|
||||
}
|
||||
|
||||
uavcan::uint64_t getUtcUSecFromCanInterrupt()
|
||||
{
|
||||
return utc_set ? sampleUtcFromCriticalSection() : 0;
|
||||
return utc_set ? sampleUtcFromCriticalSection() : 0;
|
||||
}
|
||||
|
||||
uavcan::MonotonicTime getMonotonic()
|
||||
{
|
||||
uavcan::uint64_t usec = 0;
|
||||
// Scope Critical section
|
||||
{
|
||||
CriticalSectionLocker locker;
|
||||
uavcan::uint64_t usec = 0;
|
||||
// Scope Critical section
|
||||
{
|
||||
CriticalSectionLocker locker;
|
||||
|
||||
volatile uavcan::uint64_t time = time_mono;
|
||||
volatile uavcan::uint32_t cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_OFFSET));
|
||||
volatile uavcan::uint64_t time = time_mono;
|
||||
volatile uavcan::uint32_t cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_OFFSET));
|
||||
|
||||
if (getreg32(TMR_REG(KINETIS_PIT_TFLG_OFFSET)) & PIT_TFLG_TIF)
|
||||
{
|
||||
cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_OFFSET));
|
||||
time += USecPerOverflow;
|
||||
}
|
||||
usec = time + (cnt / CountsPerUs);
|
||||
if (getreg32(TMR_REG(KINETIS_PIT_TFLG_OFFSET)) & PIT_TFLG_TIF) {
|
||||
cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_OFFSET));
|
||||
time += USecPerOverflow;
|
||||
}
|
||||
|
||||
} // End Scope Critical section
|
||||
usec = time + (cnt / CountsPerUs);
|
||||
|
||||
return uavcan::MonotonicTime::fromUSec(usec);
|
||||
} // End Scope Critical section
|
||||
|
||||
return uavcan::MonotonicTime::fromUSec(usec);
|
||||
}
|
||||
|
||||
|
||||
uavcan::UtcTime getUtc()
|
||||
{
|
||||
if (utc_set)
|
||||
{
|
||||
uavcan::uint64_t usec = 0;
|
||||
{
|
||||
CriticalSectionLocker locker;
|
||||
usec = sampleUtcFromCriticalSection();
|
||||
}
|
||||
return uavcan::UtcTime::fromUSec(usec);
|
||||
}
|
||||
return uavcan::UtcTime();
|
||||
if (utc_set) {
|
||||
uavcan::uint64_t usec = 0;
|
||||
{
|
||||
CriticalSectionLocker locker;
|
||||
usec = sampleUtcFromCriticalSection();
|
||||
}
|
||||
return uavcan::UtcTime::fromUSec(usec);
|
||||
}
|
||||
|
||||
return uavcan::UtcTime();
|
||||
}
|
||||
|
||||
static float lowpass(float xold, float xnew, float corner, float dt)
|
||||
{
|
||||
const float tau = 1.F / corner;
|
||||
return (dt * xnew + tau * xold) / (dt + tau);
|
||||
const float tau = 1.F / corner;
|
||||
return (dt * xnew + tau * xold) / (dt + tau);
|
||||
}
|
||||
|
||||
static void updateRatePID(uavcan::UtcDuration adjustment)
|
||||
{
|
||||
const uavcan::MonotonicTime ts = getMonotonic();
|
||||
const float dt = float((ts - prev_utc_adj_at).toUSec()) / 1e6F;
|
||||
prev_utc_adj_at = ts;
|
||||
const float adj_usec = float(adjustment.toUSec());
|
||||
const uavcan::MonotonicTime ts = getMonotonic();
|
||||
const float dt = float((ts - prev_utc_adj_at).toUSec()) / 1e6F;
|
||||
prev_utc_adj_at = ts;
|
||||
const float adj_usec = float(adjustment.toUSec());
|
||||
|
||||
/*
|
||||
* Target relative rate in PPM
|
||||
* Positive to go faster
|
||||
*/
|
||||
const float target_rel_rate_ppm = adj_usec * utc_sync_params.offset_p;
|
||||
/*
|
||||
* Target relative rate in PPM
|
||||
* Positive to go faster
|
||||
*/
|
||||
const float target_rel_rate_ppm = adj_usec * utc_sync_params.offset_p;
|
||||
|
||||
/*
|
||||
* Current relative rate in PPM
|
||||
* Positive if the local clock is faster
|
||||
*/
|
||||
const float new_rel_rate_ppm = (utc_prev_adj - adj_usec) / dt; // rate error in [usec/sec], which is PPM
|
||||
utc_prev_adj = adj_usec;
|
||||
utc_rel_rate_ppm = lowpass(utc_rel_rate_ppm, new_rel_rate_ppm, utc_sync_params.rate_error_corner_freq, dt);
|
||||
/*
|
||||
* Current relative rate in PPM
|
||||
* Positive if the local clock is faster
|
||||
*/
|
||||
const float new_rel_rate_ppm = (utc_prev_adj - adj_usec) / dt; // rate error in [usec/sec], which is PPM
|
||||
utc_prev_adj = adj_usec;
|
||||
utc_rel_rate_ppm = lowpass(utc_rel_rate_ppm, new_rel_rate_ppm, utc_sync_params.rate_error_corner_freq, dt);
|
||||
|
||||
const float rel_rate_error = target_rel_rate_ppm - utc_rel_rate_ppm;
|
||||
const float rel_rate_error = target_rel_rate_ppm - utc_rel_rate_ppm;
|
||||
|
||||
if (dt > 10)
|
||||
{
|
||||
utc_rel_rate_error_integral = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
utc_rel_rate_error_integral += rel_rate_error * dt * utc_sync_params.rate_i;
|
||||
utc_rel_rate_error_integral =
|
||||
uavcan::max(utc_rel_rate_error_integral, -utc_sync_params.max_rate_correction_ppm);
|
||||
utc_rel_rate_error_integral =
|
||||
uavcan::min(utc_rel_rate_error_integral, utc_sync_params.max_rate_correction_ppm);
|
||||
}
|
||||
if (dt > 10) {
|
||||
utc_rel_rate_error_integral = 0;
|
||||
|
||||
/*
|
||||
* Rate controller
|
||||
*/
|
||||
float total_rate_correction_ppm = rel_rate_error + utc_rel_rate_error_integral;
|
||||
total_rate_correction_ppm = uavcan::max(total_rate_correction_ppm, -utc_sync_params.max_rate_correction_ppm);
|
||||
total_rate_correction_ppm = uavcan::min(total_rate_correction_ppm, utc_sync_params.max_rate_correction_ppm);
|
||||
} else {
|
||||
utc_rel_rate_error_integral += rel_rate_error * dt * utc_sync_params.rate_i;
|
||||
utc_rel_rate_error_integral =
|
||||
uavcan::max(utc_rel_rate_error_integral, -utc_sync_params.max_rate_correction_ppm);
|
||||
utc_rel_rate_error_integral =
|
||||
uavcan::min(utc_rel_rate_error_integral, utc_sync_params.max_rate_correction_ppm);
|
||||
}
|
||||
|
||||
utc_correction_nsec_per_overflow = uavcan::int32_t((USecPerOverflow * 1000) * (total_rate_correction_ppm / 1e6F));
|
||||
/*
|
||||
* Rate controller
|
||||
*/
|
||||
float total_rate_correction_ppm = rel_rate_error + utc_rel_rate_error_integral;
|
||||
total_rate_correction_ppm = uavcan::max(total_rate_correction_ppm, -utc_sync_params.max_rate_correction_ppm);
|
||||
total_rate_correction_ppm = uavcan::min(total_rate_correction_ppm, utc_sync_params.max_rate_correction_ppm);
|
||||
|
||||
utc_correction_nsec_per_overflow = uavcan::int32_t((USecPerOverflow * 1000) * (total_rate_correction_ppm / 1e6F));
|
||||
|
||||
// syslog("$ adj=%f rel_rate=%f rel_rate_eint=%f tgt_rel_rate=%f ppm=%f\n",
|
||||
// adj_usec, utc_rel_rate_ppm, utc_rel_rate_error_integral, target_rel_rate_ppm,
|
||||
|
|
@ -235,96 +234,91 @@ static void updateRatePID(uavcan::UtcDuration adjustment)
|
|||
|
||||
void adjustUtc(uavcan::UtcDuration adjustment)
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
UAVCAN_ASSERT(initialized);
|
||||
MutexLocker mlocker(mutex);
|
||||
UAVCAN_ASSERT(initialized);
|
||||
|
||||
if (adjustment.getAbs() > utc_sync_params.min_jump || !utc_set)
|
||||
{
|
||||
const uavcan::int64_t adj_usec = adjustment.toUSec();
|
||||
if (adjustment.getAbs() > utc_sync_params.min_jump || !utc_set) {
|
||||
const uavcan::int64_t adj_usec = adjustment.toUSec();
|
||||
|
||||
{
|
||||
CriticalSectionLocker locker;
|
||||
if ((adj_usec < 0) && uavcan::uint64_t(-adj_usec) > time_utc)
|
||||
{
|
||||
time_utc = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + adj_usec);
|
||||
}
|
||||
}
|
||||
{
|
||||
CriticalSectionLocker locker;
|
||||
|
||||
utc_set = true;
|
||||
utc_locked = false;
|
||||
utc_jump_cnt++;
|
||||
utc_prev_adj = 0;
|
||||
utc_rel_rate_ppm = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
updateRatePID(adjustment);
|
||||
if ((adj_usec < 0) && uavcan::uint64_t(-adj_usec) > time_utc) {
|
||||
time_utc = 1;
|
||||
|
||||
if (!utc_locked)
|
||||
{
|
||||
utc_locked =
|
||||
(std::abs(utc_rel_rate_ppm) < utc_sync_params.lock_thres_rate_ppm) &&
|
||||
(std::abs(utc_prev_adj) < float(utc_sync_params.lock_thres_offset.toUSec()));
|
||||
}
|
||||
}
|
||||
} else {
|
||||
time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + adj_usec);
|
||||
}
|
||||
}
|
||||
|
||||
utc_set = true;
|
||||
utc_locked = false;
|
||||
utc_jump_cnt++;
|
||||
utc_prev_adj = 0;
|
||||
utc_rel_rate_ppm = 0;
|
||||
|
||||
} else {
|
||||
updateRatePID(adjustment);
|
||||
|
||||
if (!utc_locked) {
|
||||
utc_locked =
|
||||
(std::abs(utc_rel_rate_ppm) < utc_sync_params.lock_thres_rate_ppm) &&
|
||||
(std::abs(utc_prev_adj) < float(utc_sync_params.lock_thres_offset.toUSec()));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
float getUtcRateCorrectionPPM()
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
const float rate_correction_mult = float(utc_correction_nsec_per_overflow) / float(USecPerOverflow * 1000);
|
||||
return 1e6F * rate_correction_mult;
|
||||
MutexLocker mlocker(mutex);
|
||||
const float rate_correction_mult = float(utc_correction_nsec_per_overflow) / float(USecPerOverflow * 1000);
|
||||
return 1e6F * rate_correction_mult;
|
||||
}
|
||||
|
||||
uavcan::uint32_t getUtcJumpCount()
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
return utc_jump_cnt;
|
||||
MutexLocker mlocker(mutex);
|
||||
return utc_jump_cnt;
|
||||
}
|
||||
|
||||
bool isUtcLocked()
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
return utc_locked;
|
||||
MutexLocker mlocker(mutex);
|
||||
return utc_locked;
|
||||
}
|
||||
|
||||
UtcSyncParams getUtcSyncParams()
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
return utc_sync_params;
|
||||
MutexLocker mlocker(mutex);
|
||||
return utc_sync_params;
|
||||
}
|
||||
|
||||
void setUtcSyncParams(const UtcSyncParams& params)
|
||||
void setUtcSyncParams(const UtcSyncParams ¶ms)
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
// Add some sanity check
|
||||
utc_sync_params = params;
|
||||
MutexLocker mlocker(mutex);
|
||||
// Add some sanity check
|
||||
utc_sync_params = params;
|
||||
}
|
||||
|
||||
} // namespace clock
|
||||
|
||||
SystemClock& SystemClock::instance()
|
||||
SystemClock &SystemClock::instance()
|
||||
{
|
||||
static union SystemClockStorage
|
||||
{
|
||||
uavcan::uint8_t buffer[sizeof(SystemClock)];
|
||||
long long _aligner_1;
|
||||
long double _aligner_2;
|
||||
} storage;
|
||||
static union SystemClockStorage {
|
||||
uavcan::uint8_t buffer[sizeof(SystemClock)];
|
||||
long long _aligner_1;
|
||||
long double _aligner_2;
|
||||
} storage;
|
||||
|
||||
SystemClock* const ptr = reinterpret_cast<SystemClock*>(storage.buffer);
|
||||
SystemClock *const ptr = reinterpret_cast<SystemClock *>(storage.buffer);
|
||||
|
||||
if (!clock::initialized)
|
||||
{
|
||||
MutexLocker mlocker(clock::mutex);
|
||||
clock::init();
|
||||
new (ptr)SystemClock();
|
||||
}
|
||||
return *ptr;
|
||||
if (!clock::initialized) {
|
||||
MutexLocker mlocker(clock::mutex);
|
||||
clock::init();
|
||||
new (ptr)SystemClock();
|
||||
}
|
||||
|
||||
return *ptr;
|
||||
}
|
||||
|
||||
} // namespace uavcan_kinetis
|
||||
|
|
@ -337,38 +331,35 @@ SystemClock& SystemClock::instance()
|
|||
extern "C"
|
||||
UAVCAN_KINETIS_IRQ_HANDLER(TIMX_IRQHandler)
|
||||
{
|
||||
putreg32(PIT_TFLG_TIF, TMR_REG(KINETIS_PIT_TFLG_OFFSET));
|
||||
putreg32(PIT_TFLG_TIF, TMR_REG(KINETIS_PIT_TFLG_OFFSET));
|
||||
|
||||
using namespace uavcan_kinetis::clock;
|
||||
UAVCAN_ASSERT(initialized);
|
||||
using namespace uavcan_kinetis::clock;
|
||||
UAVCAN_ASSERT(initialized);
|
||||
|
||||
time_mono += USecPerOverflow;
|
||||
time_mono += USecPerOverflow;
|
||||
|
||||
if (utc_set)
|
||||
{
|
||||
time_utc += USecPerOverflow;
|
||||
utc_accumulated_correction_nsec += utc_correction_nsec_per_overflow;
|
||||
if (std::abs(utc_accumulated_correction_nsec) >= 1000)
|
||||
{
|
||||
time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + utc_accumulated_correction_nsec / 1000);
|
||||
utc_accumulated_correction_nsec %= 1000;
|
||||
}
|
||||
if (utc_set) {
|
||||
time_utc += USecPerOverflow;
|
||||
utc_accumulated_correction_nsec += utc_correction_nsec_per_overflow;
|
||||
|
||||
// Correction decay - 1 nsec per 65536 usec
|
||||
if (utc_correction_nsec_per_overflow > 0)
|
||||
{
|
||||
utc_correction_nsec_per_overflow--;
|
||||
}
|
||||
else if (utc_correction_nsec_per_overflow < 0)
|
||||
{
|
||||
utc_correction_nsec_per_overflow++;
|
||||
}
|
||||
else
|
||||
{
|
||||
; // Zero
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
if (std::abs(utc_accumulated_correction_nsec) >= 1000) {
|
||||
time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + utc_accumulated_correction_nsec / 1000);
|
||||
utc_accumulated_correction_nsec %= 1000;
|
||||
}
|
||||
|
||||
// Correction decay - 1 nsec per 65536 usec
|
||||
if (utc_correction_nsec_per_overflow > 0) {
|
||||
utc_correction_nsec_per_overflow--;
|
||||
|
||||
} else if (utc_correction_nsec_per_overflow < 0) {
|
||||
utc_correction_nsec_per_overflow++;
|
||||
|
||||
} else {
|
||||
; // Zero
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load Diff
|
|
@ -11,50 +11,53 @@
|
|||
namespace uavcan_kinetis
|
||||
{
|
||||
|
||||
BusEvent::BusEvent(CanDriver& can_driver)
|
||||
BusEvent::BusEvent(CanDriver &can_driver)
|
||||
{
|
||||
sem_init(&sem_, 0, 0);
|
||||
sem_setprotocol(&sem_, SEM_PRIO_NONE);
|
||||
sem_init(&sem_, 0, 0);
|
||||
sem_setprotocol(&sem_, SEM_PRIO_NONE);
|
||||
}
|
||||
|
||||
BusEvent::~BusEvent()
|
||||
{
|
||||
sem_destroy(&sem_);
|
||||
sem_destroy(&sem_);
|
||||
}
|
||||
|
||||
bool BusEvent::wait(uavcan::MonotonicDuration duration)
|
||||
{
|
||||
if (duration.isPositive()) {
|
||||
timespec abstime;
|
||||
if (duration.isPositive()) {
|
||||
timespec abstime;
|
||||
|
||||
if (clock_gettime(CLOCK_REALTIME, &abstime) == 0) {
|
||||
const unsigned billion = 1000 * 1000 * 1000;
|
||||
uint64_t nsecs = abstime.tv_nsec + (uint64_t)duration.toUSec() * 1000;
|
||||
abstime.tv_sec += nsecs / billion;
|
||||
nsecs -= (nsecs / billion) * billion;
|
||||
abstime.tv_nsec = nsecs;
|
||||
if (clock_gettime(CLOCK_REALTIME, &abstime) == 0) {
|
||||
const unsigned billion = 1000 * 1000 * 1000;
|
||||
uint64_t nsecs = abstime.tv_nsec + (uint64_t)duration.toUSec() * 1000;
|
||||
abstime.tv_sec += nsecs / billion;
|
||||
nsecs -= (nsecs / billion) * billion;
|
||||
abstime.tv_nsec = nsecs;
|
||||
|
||||
int ret;
|
||||
while ((ret = sem_timedwait(&sem_, &abstime)) == -1 && errno == EINTR);
|
||||
if (ret == -1) { // timed out or error
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
int ret;
|
||||
|
||||
while ((ret = sem_timedwait(&sem_, &abstime)) == -1 && errno == EINTR);
|
||||
|
||||
if (ret == -1) { // timed out or error
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
void BusEvent::signalFromInterrupt()
|
||||
{
|
||||
if (sem_.semcount <= 0)
|
||||
{
|
||||
(void)sem_post(&sem_);
|
||||
}
|
||||
if (signal_cb_)
|
||||
{
|
||||
signal_cb_();
|
||||
}
|
||||
if (sem_.semcount <= 0) {
|
||||
(void)sem_post(&sem_);
|
||||
}
|
||||
|
||||
if (signal_cb_) {
|
||||
signal_cb_();
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -24,262 +24,257 @@ namespace uavcan_stm32
|
|||
namespace bxcan
|
||||
{
|
||||
|
||||
struct TxMailboxType
|
||||
{
|
||||
volatile uint32_t TIR;
|
||||
volatile uint32_t TDTR;
|
||||
volatile uint32_t TDLR;
|
||||
volatile uint32_t TDHR;
|
||||
struct TxMailboxType {
|
||||
volatile uint32_t TIR;
|
||||
volatile uint32_t TDTR;
|
||||
volatile uint32_t TDLR;
|
||||
volatile uint32_t TDHR;
|
||||
};
|
||||
|
||||
struct RxMailboxType
|
||||
{
|
||||
volatile uint32_t RIR;
|
||||
volatile uint32_t RDTR;
|
||||
volatile uint32_t RDLR;
|
||||
volatile uint32_t RDHR;
|
||||
struct RxMailboxType {
|
||||
volatile uint32_t RIR;
|
||||
volatile uint32_t RDTR;
|
||||
volatile uint32_t RDLR;
|
||||
volatile uint32_t RDHR;
|
||||
};
|
||||
|
||||
struct FilterRegisterType
|
||||
{
|
||||
volatile uint32_t FR1;
|
||||
volatile uint32_t FR2;
|
||||
struct FilterRegisterType {
|
||||
volatile uint32_t FR1;
|
||||
volatile uint32_t FR2;
|
||||
};
|
||||
|
||||
struct CanType
|
||||
{
|
||||
volatile uint32_t MCR; /*!< CAN master control register, Address offset: 0x00 */
|
||||
volatile uint32_t MSR; /*!< CAN master status register, Address offset: 0x04 */
|
||||
volatile uint32_t TSR; /*!< CAN transmit status register, Address offset: 0x08 */
|
||||
volatile uint32_t RF0R; /*!< CAN receive FIFO 0 register, Address offset: 0x0C */
|
||||
volatile uint32_t RF1R; /*!< CAN receive FIFO 1 register, Address offset: 0x10 */
|
||||
volatile uint32_t IER; /*!< CAN interrupt enable register, Address offset: 0x14 */
|
||||
volatile uint32_t ESR; /*!< CAN error status register, Address offset: 0x18 */
|
||||
volatile uint32_t BTR; /*!< CAN bit timing register, Address offset: 0x1C */
|
||||
uint32_t RESERVED0[88]; /*!< Reserved, 0x020 - 0x17F */
|
||||
TxMailboxType TxMailbox[3]; /*!< CAN Tx MailBox, Address offset: 0x180 - 0x1AC */
|
||||
RxMailboxType RxMailbox[2]; /*!< CAN FIFO MailBox, Address offset: 0x1B0 - 0x1CC */
|
||||
uint32_t RESERVED1[12]; /*!< Reserved, 0x1D0 - 0x1FF */
|
||||
volatile uint32_t FMR; /*!< CAN filter master register, Address offset: 0x200 */
|
||||
volatile uint32_t FM1R; /*!< CAN filter mode register, Address offset: 0x204 */
|
||||
uint32_t RESERVED2; /*!< Reserved, 0x208 */
|
||||
volatile uint32_t FS1R; /*!< CAN filter scale register, Address offset: 0x20C */
|
||||
uint32_t RESERVED3; /*!< Reserved, 0x210 */
|
||||
volatile uint32_t FFA1R; /*!< CAN filter FIFO assignment register, Address offset: 0x214 */
|
||||
uint32_t RESERVED4; /*!< Reserved, 0x218 */
|
||||
volatile uint32_t FA1R; /*!< CAN filter activation register, Address offset: 0x21C */
|
||||
uint32_t RESERVED5[8]; /*!< Reserved, 0x220-0x23F */
|
||||
FilterRegisterType FilterRegister[28]; /*!< CAN Filter Register, Address offset: 0x240-0x31C */
|
||||
struct CanType {
|
||||
volatile uint32_t MCR; /*!< CAN master control register, Address offset: 0x00 */
|
||||
volatile uint32_t MSR; /*!< CAN master status register, Address offset: 0x04 */
|
||||
volatile uint32_t TSR; /*!< CAN transmit status register, Address offset: 0x08 */
|
||||
volatile uint32_t RF0R; /*!< CAN receive FIFO 0 register, Address offset: 0x0C */
|
||||
volatile uint32_t RF1R; /*!< CAN receive FIFO 1 register, Address offset: 0x10 */
|
||||
volatile uint32_t IER; /*!< CAN interrupt enable register, Address offset: 0x14 */
|
||||
volatile uint32_t ESR; /*!< CAN error status register, Address offset: 0x18 */
|
||||
volatile uint32_t BTR; /*!< CAN bit timing register, Address offset: 0x1C */
|
||||
uint32_t RESERVED0[88]; /*!< Reserved, 0x020 - 0x17F */
|
||||
TxMailboxType TxMailbox[3]; /*!< CAN Tx MailBox, Address offset: 0x180 - 0x1AC */
|
||||
RxMailboxType RxMailbox[2]; /*!< CAN FIFO MailBox, Address offset: 0x1B0 - 0x1CC */
|
||||
uint32_t RESERVED1[12]; /*!< Reserved, 0x1D0 - 0x1FF */
|
||||
volatile uint32_t FMR; /*!< CAN filter master register, Address offset: 0x200 */
|
||||
volatile uint32_t FM1R; /*!< CAN filter mode register, Address offset: 0x204 */
|
||||
uint32_t RESERVED2; /*!< Reserved, 0x208 */
|
||||
volatile uint32_t FS1R; /*!< CAN filter scale register, Address offset: 0x20C */
|
||||
uint32_t RESERVED3; /*!< Reserved, 0x210 */
|
||||
volatile uint32_t FFA1R; /*!< CAN filter FIFO assignment register, Address offset: 0x214 */
|
||||
uint32_t RESERVED4; /*!< Reserved, 0x218 */
|
||||
volatile uint32_t FA1R; /*!< CAN filter activation register, Address offset: 0x21C */
|
||||
uint32_t RESERVED5[8]; /*!< Reserved, 0x220-0x23F */
|
||||
FilterRegisterType FilterRegister[28]; /*!< CAN Filter Register, Address offset: 0x240-0x31C */
|
||||
};
|
||||
|
||||
/**
|
||||
* CANx register sets
|
||||
*/
|
||||
CanType* const Can[UAVCAN_STM32_NUM_IFACES] =
|
||||
{
|
||||
reinterpret_cast<CanType*>(0x40006400)
|
||||
CanType *const Can[UAVCAN_STM32_NUM_IFACES] = {
|
||||
reinterpret_cast<CanType *>(0x40006400)
|
||||
#if UAVCAN_STM32_NUM_IFACES > 1
|
||||
,
|
||||
reinterpret_cast<CanType*>(0x40006800)
|
||||
,
|
||||
reinterpret_cast<CanType *>(0x40006800)
|
||||
#endif
|
||||
};
|
||||
|
||||
/* CAN master control register */
|
||||
|
||||
constexpr unsigned long MCR_INRQ = (1U << 0); /* Bit 0: Initialization Request */
|
||||
constexpr unsigned long MCR_SLEEP = (1U << 1); /* Bit 1: Sleep Mode Request */
|
||||
constexpr unsigned long MCR_TXFP = (1U << 2); /* Bit 2: Transmit FIFO Priority */
|
||||
constexpr unsigned long MCR_RFLM = (1U << 3); /* Bit 3: Receive FIFO Locked Mode */
|
||||
constexpr unsigned long MCR_NART = (1U << 4); /* Bit 4: No Automatic Retransmission */
|
||||
constexpr unsigned long MCR_AWUM = (1U << 5); /* Bit 5: Automatic Wakeup Mode */
|
||||
constexpr unsigned long MCR_ABOM = (1U << 6); /* Bit 6: Automatic Bus-Off Management */
|
||||
constexpr unsigned long MCR_TTCM = (1U << 7); /* Bit 7: Time Triggered Communication Mode Enable */
|
||||
constexpr unsigned long MCR_RESET = (1U << 15);/* Bit 15: bxCAN software master reset */
|
||||
constexpr unsigned long MCR_DBF = (1U << 16);/* Bit 16: Debug freeze */
|
||||
constexpr unsigned long MCR_INRQ = (1U << 0); /* Bit 0: Initialization Request */
|
||||
constexpr unsigned long MCR_SLEEP = (1U << 1); /* Bit 1: Sleep Mode Request */
|
||||
constexpr unsigned long MCR_TXFP = (1U << 2); /* Bit 2: Transmit FIFO Priority */
|
||||
constexpr unsigned long MCR_RFLM = (1U << 3); /* Bit 3: Receive FIFO Locked Mode */
|
||||
constexpr unsigned long MCR_NART = (1U << 4); /* Bit 4: No Automatic Retransmission */
|
||||
constexpr unsigned long MCR_AWUM = (1U << 5); /* Bit 5: Automatic Wakeup Mode */
|
||||
constexpr unsigned long MCR_ABOM = (1U << 6); /* Bit 6: Automatic Bus-Off Management */
|
||||
constexpr unsigned long MCR_TTCM = (1U << 7); /* Bit 7: Time Triggered Communication Mode Enable */
|
||||
constexpr unsigned long MCR_RESET = (1U << 15); /* Bit 15: bxCAN software master reset */
|
||||
constexpr unsigned long MCR_DBF = (1U << 16); /* Bit 16: Debug freeze */
|
||||
|
||||
/* CAN master status register */
|
||||
|
||||
constexpr unsigned long MSR_INAK = (1U << 0); /* Bit 0: Initialization Acknowledge */
|
||||
constexpr unsigned long MSR_SLAK = (1U << 1); /* Bit 1: Sleep Acknowledge */
|
||||
constexpr unsigned long MSR_ERRI = (1U << 2); /* Bit 2: Error Interrupt */
|
||||
constexpr unsigned long MSR_WKUI = (1U << 3); /* Bit 3: Wakeup Interrupt */
|
||||
constexpr unsigned long MSR_SLAKI = (1U << 4); /* Bit 4: Sleep acknowledge interrupt */
|
||||
constexpr unsigned long MSR_TXM = (1U << 8); /* Bit 8: Transmit Mode */
|
||||
constexpr unsigned long MSR_RXM = (1U << 9); /* Bit 9: Receive Mode */
|
||||
constexpr unsigned long MSR_SAMP = (1U << 10);/* Bit 10: Last Sample Point */
|
||||
constexpr unsigned long MSR_RX = (1U << 11);/* Bit 11: CAN Rx Signal */
|
||||
constexpr unsigned long MSR_INAK = (1U << 0); /* Bit 0: Initialization Acknowledge */
|
||||
constexpr unsigned long MSR_SLAK = (1U << 1); /* Bit 1: Sleep Acknowledge */
|
||||
constexpr unsigned long MSR_ERRI = (1U << 2); /* Bit 2: Error Interrupt */
|
||||
constexpr unsigned long MSR_WKUI = (1U << 3); /* Bit 3: Wakeup Interrupt */
|
||||
constexpr unsigned long MSR_SLAKI = (1U << 4); /* Bit 4: Sleep acknowledge interrupt */
|
||||
constexpr unsigned long MSR_TXM = (1U << 8); /* Bit 8: Transmit Mode */
|
||||
constexpr unsigned long MSR_RXM = (1U << 9); /* Bit 9: Receive Mode */
|
||||
constexpr unsigned long MSR_SAMP = (1U << 10); /* Bit 10: Last Sample Point */
|
||||
constexpr unsigned long MSR_RX = (1U << 11); /* Bit 11: CAN Rx Signal */
|
||||
|
||||
/* CAN transmit status register */
|
||||
|
||||
constexpr unsigned long TSR_RQCP0 = (1U << 0); /* Bit 0: Request Completed Mailbox 0 */
|
||||
constexpr unsigned long TSR_TXOK0 = (1U << 1); /* Bit 1 : Transmission OK of Mailbox 0 */
|
||||
constexpr unsigned long TSR_ALST0 = (1U << 2); /* Bit 2 : Arbitration Lost for Mailbox 0 */
|
||||
constexpr unsigned long TSR_TERR0 = (1U << 3); /* Bit 3 : Transmission Error of Mailbox 0 */
|
||||
constexpr unsigned long TSR_ABRQ0 = (1U << 7); /* Bit 7 : Abort Request for Mailbox 0 */
|
||||
constexpr unsigned long TSR_RQCP1 = (1U << 8); /* Bit 8 : Request Completed Mailbox 1 */
|
||||
constexpr unsigned long TSR_TXOK1 = (1U << 9); /* Bit 9 : Transmission OK of Mailbox 1 */
|
||||
constexpr unsigned long TSR_ALST1 = (1U << 10);/* Bit 10 : Arbitration Lost for Mailbox 1 */
|
||||
constexpr unsigned long TSR_TERR1 = (1U << 11);/* Bit 11 : Transmission Error of Mailbox 1 */
|
||||
constexpr unsigned long TSR_ABRQ1 = (1U << 15);/* Bit 15 : Abort Request for Mailbox 1 */
|
||||
constexpr unsigned long TSR_RQCP2 = (1U << 16);/* Bit 16 : Request Completed Mailbox 2 */
|
||||
constexpr unsigned long TSR_TXOK2 = (1U << 17);/* Bit 17 : Transmission OK of Mailbox 2 */
|
||||
constexpr unsigned long TSR_ALST2 = (1U << 18);/* Bit 18: Arbitration Lost for Mailbox 2 */
|
||||
constexpr unsigned long TSR_TERR2 = (1U << 19);/* Bit 19: Transmission Error of Mailbox 2 */
|
||||
constexpr unsigned long TSR_ABRQ2 = (1U << 23);/* Bit 23: Abort Request for Mailbox 2 */
|
||||
constexpr unsigned long TSR_CODE_SHIFT = (24U); /* Bits 25-24: Mailbox Code */
|
||||
constexpr unsigned long TSR_CODE_MASK = (3U << TSR_CODE_SHIFT);
|
||||
constexpr unsigned long TSR_TME0 = (1U << 26);/* Bit 26: Transmit Mailbox 0 Empty */
|
||||
constexpr unsigned long TSR_TME1 = (1U << 27);/* Bit 27: Transmit Mailbox 1 Empty */
|
||||
constexpr unsigned long TSR_TME2 = (1U << 28);/* Bit 28: Transmit Mailbox 2 Empty */
|
||||
constexpr unsigned long TSR_LOW0 = (1U << 29);/* Bit 29: Lowest Priority Flag for Mailbox 0 */
|
||||
constexpr unsigned long TSR_LOW1 = (1U << 30);/* Bit 30: Lowest Priority Flag for Mailbox 1 */
|
||||
constexpr unsigned long TSR_LOW2 = (1U << 31);/* Bit 31: Lowest Priority Flag for Mailbox 2 */
|
||||
constexpr unsigned long TSR_RQCP0 = (1U << 0); /* Bit 0: Request Completed Mailbox 0 */
|
||||
constexpr unsigned long TSR_TXOK0 = (1U << 1); /* Bit 1 : Transmission OK of Mailbox 0 */
|
||||
constexpr unsigned long TSR_ALST0 = (1U << 2); /* Bit 2 : Arbitration Lost for Mailbox 0 */
|
||||
constexpr unsigned long TSR_TERR0 = (1U << 3); /* Bit 3 : Transmission Error of Mailbox 0 */
|
||||
constexpr unsigned long TSR_ABRQ0 = (1U << 7); /* Bit 7 : Abort Request for Mailbox 0 */
|
||||
constexpr unsigned long TSR_RQCP1 = (1U << 8); /* Bit 8 : Request Completed Mailbox 1 */
|
||||
constexpr unsigned long TSR_TXOK1 = (1U << 9); /* Bit 9 : Transmission OK of Mailbox 1 */
|
||||
constexpr unsigned long TSR_ALST1 = (1U << 10); /* Bit 10 : Arbitration Lost for Mailbox 1 */
|
||||
constexpr unsigned long TSR_TERR1 = (1U << 11); /* Bit 11 : Transmission Error of Mailbox 1 */
|
||||
constexpr unsigned long TSR_ABRQ1 = (1U << 15); /* Bit 15 : Abort Request for Mailbox 1 */
|
||||
constexpr unsigned long TSR_RQCP2 = (1U << 16); /* Bit 16 : Request Completed Mailbox 2 */
|
||||
constexpr unsigned long TSR_TXOK2 = (1U << 17); /* Bit 17 : Transmission OK of Mailbox 2 */
|
||||
constexpr unsigned long TSR_ALST2 = (1U << 18); /* Bit 18: Arbitration Lost for Mailbox 2 */
|
||||
constexpr unsigned long TSR_TERR2 = (1U << 19); /* Bit 19: Transmission Error of Mailbox 2 */
|
||||
constexpr unsigned long TSR_ABRQ2 = (1U << 23); /* Bit 23: Abort Request for Mailbox 2 */
|
||||
constexpr unsigned long TSR_CODE_SHIFT = (24U); /* Bits 25-24: Mailbox Code */
|
||||
constexpr unsigned long TSR_CODE_MASK = (3U << TSR_CODE_SHIFT);
|
||||
constexpr unsigned long TSR_TME0 = (1U << 26); /* Bit 26: Transmit Mailbox 0 Empty */
|
||||
constexpr unsigned long TSR_TME1 = (1U << 27); /* Bit 27: Transmit Mailbox 1 Empty */
|
||||
constexpr unsigned long TSR_TME2 = (1U << 28); /* Bit 28: Transmit Mailbox 2 Empty */
|
||||
constexpr unsigned long TSR_LOW0 = (1U << 29); /* Bit 29: Lowest Priority Flag for Mailbox 0 */
|
||||
constexpr unsigned long TSR_LOW1 = (1U << 30); /* Bit 30: Lowest Priority Flag for Mailbox 1 */
|
||||
constexpr unsigned long TSR_LOW2 = (1U << 31); /* Bit 31: Lowest Priority Flag for Mailbox 2 */
|
||||
|
||||
/* CAN receive FIFO 0/1 registers */
|
||||
|
||||
constexpr unsigned long RFR_FMP_SHIFT = (0U); /* Bits 1-0: FIFO Message Pending */
|
||||
constexpr unsigned long RFR_FMP_MASK = (3U << RFR_FMP_SHIFT);
|
||||
constexpr unsigned long RFR_FULL = (1U << 3); /* Bit 3: FIFO 0 Full */
|
||||
constexpr unsigned long RFR_FOVR = (1U << 4); /* Bit 4: FIFO 0 Overrun */
|
||||
constexpr unsigned long RFR_RFOM = (1U << 5); /* Bit 5: Release FIFO 0 Output Mailbox */
|
||||
constexpr unsigned long RFR_FMP_SHIFT = (0U); /* Bits 1-0: FIFO Message Pending */
|
||||
constexpr unsigned long RFR_FMP_MASK = (3U << RFR_FMP_SHIFT);
|
||||
constexpr unsigned long RFR_FULL = (1U << 3); /* Bit 3: FIFO 0 Full */
|
||||
constexpr unsigned long RFR_FOVR = (1U << 4); /* Bit 4: FIFO 0 Overrun */
|
||||
constexpr unsigned long RFR_RFOM = (1U << 5); /* Bit 5: Release FIFO 0 Output Mailbox */
|
||||
|
||||
/* CAN interrupt enable register */
|
||||
|
||||
constexpr unsigned long IER_TMEIE = (1U << 0); /* Bit 0: Transmit Mailbox Empty Interrupt Enable */
|
||||
constexpr unsigned long IER_FMPIE0 = (1U << 1); /* Bit 1: FIFO Message Pending Interrupt Enable */
|
||||
constexpr unsigned long IER_FFIE0 = (1U << 2); /* Bit 2: FIFO Full Interrupt Enable */
|
||||
constexpr unsigned long IER_FOVIE0 = (1U << 3); /* Bit 3: FIFO Overrun Interrupt Enable */
|
||||
constexpr unsigned long IER_FMPIE1 = (1U << 4); /* Bit 4: FIFO Message Pending Interrupt Enable */
|
||||
constexpr unsigned long IER_FFIE1 = (1U << 5); /* Bit 5: FIFO Full Interrupt Enable */
|
||||
constexpr unsigned long IER_FOVIE1 = (1U << 6); /* Bit 6: FIFO Overrun Interrupt Enable */
|
||||
constexpr unsigned long IER_EWGIE = (1U << 8); /* Bit 8: Error Warning Interrupt Enable */
|
||||
constexpr unsigned long IER_EPVIE = (1U << 9); /* Bit 9: Error Passive Interrupt Enable */
|
||||
constexpr unsigned long IER_BOFIE = (1U << 10);/* Bit 10: Bus-Off Interrupt Enable */
|
||||
constexpr unsigned long IER_LECIE = (1U << 11);/* Bit 11: Last Error Code Interrupt Enable */
|
||||
constexpr unsigned long IER_ERRIE = (1U << 15);/* Bit 15: Error Interrupt Enable */
|
||||
constexpr unsigned long IER_WKUIE = (1U << 16);/* Bit 16: Wakeup Interrupt Enable */
|
||||
constexpr unsigned long IER_SLKIE = (1U << 17);/* Bit 17: Sleep Interrupt Enable */
|
||||
constexpr unsigned long IER_TMEIE = (1U << 0); /* Bit 0: Transmit Mailbox Empty Interrupt Enable */
|
||||
constexpr unsigned long IER_FMPIE0 = (1U << 1); /* Bit 1: FIFO Message Pending Interrupt Enable */
|
||||
constexpr unsigned long IER_FFIE0 = (1U << 2); /* Bit 2: FIFO Full Interrupt Enable */
|
||||
constexpr unsigned long IER_FOVIE0 = (1U << 3); /* Bit 3: FIFO Overrun Interrupt Enable */
|
||||
constexpr unsigned long IER_FMPIE1 = (1U << 4); /* Bit 4: FIFO Message Pending Interrupt Enable */
|
||||
constexpr unsigned long IER_FFIE1 = (1U << 5); /* Bit 5: FIFO Full Interrupt Enable */
|
||||
constexpr unsigned long IER_FOVIE1 = (1U << 6); /* Bit 6: FIFO Overrun Interrupt Enable */
|
||||
constexpr unsigned long IER_EWGIE = (1U << 8); /* Bit 8: Error Warning Interrupt Enable */
|
||||
constexpr unsigned long IER_EPVIE = (1U << 9); /* Bit 9: Error Passive Interrupt Enable */
|
||||
constexpr unsigned long IER_BOFIE = (1U << 10); /* Bit 10: Bus-Off Interrupt Enable */
|
||||
constexpr unsigned long IER_LECIE = (1U << 11); /* Bit 11: Last Error Code Interrupt Enable */
|
||||
constexpr unsigned long IER_ERRIE = (1U << 15); /* Bit 15: Error Interrupt Enable */
|
||||
constexpr unsigned long IER_WKUIE = (1U << 16); /* Bit 16: Wakeup Interrupt Enable */
|
||||
constexpr unsigned long IER_SLKIE = (1U << 17); /* Bit 17: Sleep Interrupt Enable */
|
||||
|
||||
/* CAN error status register */
|
||||
|
||||
constexpr unsigned long ESR_EWGF = (1U << 0); /* Bit 0: Error Warning Flag */
|
||||
constexpr unsigned long ESR_EPVF = (1U << 1); /* Bit 1: Error Passive Flag */
|
||||
constexpr unsigned long ESR_BOFF = (1U << 2); /* Bit 2: Bus-Off Flag */
|
||||
constexpr unsigned long ESR_LEC_SHIFT = (4U); /* Bits 6-4: Last Error Code */
|
||||
constexpr unsigned long ESR_LEC_MASK = (7U << ESR_LEC_SHIFT);
|
||||
constexpr unsigned long ESR_NOERROR = (0U << ESR_LEC_SHIFT);/* 000: No Error */
|
||||
constexpr unsigned long ESR_STUFFERROR = (1U << ESR_LEC_SHIFT);/* 001: Stuff Error */
|
||||
constexpr unsigned long ESR_FORMERROR = (2U << ESR_LEC_SHIFT);/* 010: Form Error */
|
||||
constexpr unsigned long ESR_ACKERROR = (3U << ESR_LEC_SHIFT);/* 011: Acknowledgment Error */
|
||||
constexpr unsigned long ESR_BRECERROR = (4U << ESR_LEC_SHIFT);/* 100: Bit recessive Error */
|
||||
constexpr unsigned long ESR_BDOMERROR = (5U << ESR_LEC_SHIFT);/* 101: Bit dominant Error */
|
||||
constexpr unsigned long ESR_CRCERRPR = (6U << ESR_LEC_SHIFT);/* 110: CRC Error */
|
||||
constexpr unsigned long ESR_SWERROR = (7U << ESR_LEC_SHIFT);/* 111: Set by software */
|
||||
constexpr unsigned long ESR_TEC_SHIFT = (16U); /* Bits 23-16: LS byte of the 9-bit Transmit Error Counter */
|
||||
constexpr unsigned long ESR_TEC_MASK = (0xFFU << ESR_TEC_SHIFT);
|
||||
constexpr unsigned long ESR_REC_SHIFT = (24U); /* Bits 31-24: Receive Error Counter */
|
||||
constexpr unsigned long ESR_REC_MASK = (0xFFU << ESR_REC_SHIFT);
|
||||
constexpr unsigned long ESR_EWGF = (1U << 0); /* Bit 0: Error Warning Flag */
|
||||
constexpr unsigned long ESR_EPVF = (1U << 1); /* Bit 1: Error Passive Flag */
|
||||
constexpr unsigned long ESR_BOFF = (1U << 2); /* Bit 2: Bus-Off Flag */
|
||||
constexpr unsigned long ESR_LEC_SHIFT = (4U); /* Bits 6-4: Last Error Code */
|
||||
constexpr unsigned long ESR_LEC_MASK = (7U << ESR_LEC_SHIFT);
|
||||
constexpr unsigned long ESR_NOERROR = (0U << ESR_LEC_SHIFT); /* 000: No Error */
|
||||
constexpr unsigned long ESR_STUFFERROR = (1U << ESR_LEC_SHIFT); /* 001: Stuff Error */
|
||||
constexpr unsigned long ESR_FORMERROR = (2U << ESR_LEC_SHIFT); /* 010: Form Error */
|
||||
constexpr unsigned long ESR_ACKERROR = (3U << ESR_LEC_SHIFT); /* 011: Acknowledgment Error */
|
||||
constexpr unsigned long ESR_BRECERROR = (4U << ESR_LEC_SHIFT); /* 100: Bit recessive Error */
|
||||
constexpr unsigned long ESR_BDOMERROR = (5U << ESR_LEC_SHIFT); /* 101: Bit dominant Error */
|
||||
constexpr unsigned long ESR_CRCERRPR = (6U << ESR_LEC_SHIFT); /* 110: CRC Error */
|
||||
constexpr unsigned long ESR_SWERROR = (7U << ESR_LEC_SHIFT); /* 111: Set by software */
|
||||
constexpr unsigned long ESR_TEC_SHIFT = (16U); /* Bits 23-16: LS byte of the 9-bit Transmit Error Counter */
|
||||
constexpr unsigned long ESR_TEC_MASK = (0xFFU << ESR_TEC_SHIFT);
|
||||
constexpr unsigned long ESR_REC_SHIFT = (24U); /* Bits 31-24: Receive Error Counter */
|
||||
constexpr unsigned long ESR_REC_MASK = (0xFFU << ESR_REC_SHIFT);
|
||||
|
||||
/* CAN bit timing register */
|
||||
|
||||
constexpr unsigned long BTR_BRP_SHIFT = (0U); /* Bits 9-0: Baud Rate Prescaler */
|
||||
constexpr unsigned long BTR_BRP_MASK = (0x03FFU << BTR_BRP_SHIFT);
|
||||
constexpr unsigned long BTR_TS1_SHIFT = (16U); /* Bits 19-16: Time Segment 1 */
|
||||
constexpr unsigned long BTR_TS1_MASK = (0x0FU << BTR_TS1_SHIFT);
|
||||
constexpr unsigned long BTR_TS2_SHIFT = (20U); /* Bits 22-20: Time Segment 2 */
|
||||
constexpr unsigned long BTR_TS2_MASK = (7U << BTR_TS2_SHIFT);
|
||||
constexpr unsigned long BTR_SJW_SHIFT = (24U); /* Bits 25-24: Resynchronization Jump Width */
|
||||
constexpr unsigned long BTR_SJW_MASK = (3U << BTR_SJW_SHIFT);
|
||||
constexpr unsigned long BTR_LBKM = (1U << 30);/* Bit 30: Loop Back Mode (Debug);*/
|
||||
constexpr unsigned long BTR_SILM = (1U << 31);/* Bit 31: Silent Mode (Debug);*/
|
||||
constexpr unsigned long BTR_BRP_SHIFT = (0U); /* Bits 9-0: Baud Rate Prescaler */
|
||||
constexpr unsigned long BTR_BRP_MASK = (0x03FFU << BTR_BRP_SHIFT);
|
||||
constexpr unsigned long BTR_TS1_SHIFT = (16U); /* Bits 19-16: Time Segment 1 */
|
||||
constexpr unsigned long BTR_TS1_MASK = (0x0FU << BTR_TS1_SHIFT);
|
||||
constexpr unsigned long BTR_TS2_SHIFT = (20U); /* Bits 22-20: Time Segment 2 */
|
||||
constexpr unsigned long BTR_TS2_MASK = (7U << BTR_TS2_SHIFT);
|
||||
constexpr unsigned long BTR_SJW_SHIFT = (24U); /* Bits 25-24: Resynchronization Jump Width */
|
||||
constexpr unsigned long BTR_SJW_MASK = (3U << BTR_SJW_SHIFT);
|
||||
constexpr unsigned long BTR_LBKM = (1U << 30); /* Bit 30: Loop Back Mode (Debug);*/
|
||||
constexpr unsigned long BTR_SILM = (1U << 31); /* Bit 31: Silent Mode (Debug);*/
|
||||
|
||||
constexpr unsigned long BTR_BRP_MAX = (1024U); /* Maximum BTR value (without decrement);*/
|
||||
constexpr unsigned long BTR_TSEG1_MAX = (16U); /* Maximum TSEG1 value (without decrement);*/
|
||||
constexpr unsigned long BTR_TSEG2_MAX = (8U); /* Maximum TSEG2 value (without decrement);*/
|
||||
constexpr unsigned long BTR_BRP_MAX = (1024U); /* Maximum BTR value (without decrement);*/
|
||||
constexpr unsigned long BTR_TSEG1_MAX = (16U); /* Maximum TSEG1 value (without decrement);*/
|
||||
constexpr unsigned long BTR_TSEG2_MAX = (8U); /* Maximum TSEG2 value (without decrement);*/
|
||||
|
||||
/* TX mailbox identifier register */
|
||||
|
||||
constexpr unsigned long TIR_TXRQ = (1U << 0); /* Bit 0: Transmit Mailbox Request */
|
||||
constexpr unsigned long TIR_RTR = (1U << 1); /* Bit 1: Remote Transmission Request */
|
||||
constexpr unsigned long TIR_IDE = (1U << 2); /* Bit 2: Identifier Extension */
|
||||
constexpr unsigned long TIR_EXID_SHIFT = (3U); /* Bit 3-31: Extended Identifier */
|
||||
constexpr unsigned long TIR_EXID_MASK = (0x1FFFFFFFU << TIR_EXID_SHIFT);
|
||||
constexpr unsigned long TIR_STID_SHIFT = (21U); /* Bits 21-31: Standard Identifier */
|
||||
constexpr unsigned long TIR_STID_MASK = (0x07FFU << TIR_STID_SHIFT);
|
||||
constexpr unsigned long TIR_TXRQ = (1U << 0); /* Bit 0: Transmit Mailbox Request */
|
||||
constexpr unsigned long TIR_RTR = (1U << 1); /* Bit 1: Remote Transmission Request */
|
||||
constexpr unsigned long TIR_IDE = (1U << 2); /* Bit 2: Identifier Extension */
|
||||
constexpr unsigned long TIR_EXID_SHIFT = (3U); /* Bit 3-31: Extended Identifier */
|
||||
constexpr unsigned long TIR_EXID_MASK = (0x1FFFFFFFU << TIR_EXID_SHIFT);
|
||||
constexpr unsigned long TIR_STID_SHIFT = (21U); /* Bits 21-31: Standard Identifier */
|
||||
constexpr unsigned long TIR_STID_MASK = (0x07FFU << TIR_STID_SHIFT);
|
||||
|
||||
/* Mailbox data length control and time stamp register */
|
||||
|
||||
constexpr unsigned long TDTR_DLC_SHIFT = (0U); /* Bits 3:0: Data Length Code */
|
||||
constexpr unsigned long TDTR_DLC_MASK = (0x0FU << TDTR_DLC_SHIFT);
|
||||
constexpr unsigned long TDTR_TGT = (1U << 8); /* Bit 8: Transmit Global Time */
|
||||
constexpr unsigned long TDTR_TIME_SHIFT = (16U); /* Bits 31:16: Message Time Stamp */
|
||||
constexpr unsigned long TDTR_TIME_MASK = (0xFFFFU << TDTR_TIME_SHIFT);
|
||||
constexpr unsigned long TDTR_DLC_SHIFT = (0U); /* Bits 3:0: Data Length Code */
|
||||
constexpr unsigned long TDTR_DLC_MASK = (0x0FU << TDTR_DLC_SHIFT);
|
||||
constexpr unsigned long TDTR_TGT = (1U << 8); /* Bit 8: Transmit Global Time */
|
||||
constexpr unsigned long TDTR_TIME_SHIFT = (16U); /* Bits 31:16: Message Time Stamp */
|
||||
constexpr unsigned long TDTR_TIME_MASK = (0xFFFFU << TDTR_TIME_SHIFT);
|
||||
|
||||
/* Mailbox data low register */
|
||||
|
||||
constexpr unsigned long TDLR_DATA0_SHIFT = (0U); /* Bits 7-0: Data Byte 0 */
|
||||
constexpr unsigned long TDLR_DATA0_MASK = (0xFFU << TDLR_DATA0_SHIFT);
|
||||
constexpr unsigned long TDLR_DATA1_SHIFT = (8U); /* Bits 15-8: Data Byte 1 */
|
||||
constexpr unsigned long TDLR_DATA1_MASK = (0xFFU << TDLR_DATA1_SHIFT);
|
||||
constexpr unsigned long TDLR_DATA2_SHIFT = (16U); /* Bits 23-16: Data Byte 2 */
|
||||
constexpr unsigned long TDLR_DATA2_MASK = (0xFFU << TDLR_DATA2_SHIFT);
|
||||
constexpr unsigned long TDLR_DATA3_SHIFT = (24U); /* Bits 31-24: Data Byte 3 */
|
||||
constexpr unsigned long TDLR_DATA3_MASK = (0xFFU << TDLR_DATA3_SHIFT);
|
||||
constexpr unsigned long TDLR_DATA0_SHIFT = (0U); /* Bits 7-0: Data Byte 0 */
|
||||
constexpr unsigned long TDLR_DATA0_MASK = (0xFFU << TDLR_DATA0_SHIFT);
|
||||
constexpr unsigned long TDLR_DATA1_SHIFT = (8U); /* Bits 15-8: Data Byte 1 */
|
||||
constexpr unsigned long TDLR_DATA1_MASK = (0xFFU << TDLR_DATA1_SHIFT);
|
||||
constexpr unsigned long TDLR_DATA2_SHIFT = (16U); /* Bits 23-16: Data Byte 2 */
|
||||
constexpr unsigned long TDLR_DATA2_MASK = (0xFFU << TDLR_DATA2_SHIFT);
|
||||
constexpr unsigned long TDLR_DATA3_SHIFT = (24U); /* Bits 31-24: Data Byte 3 */
|
||||
constexpr unsigned long TDLR_DATA3_MASK = (0xFFU << TDLR_DATA3_SHIFT);
|
||||
|
||||
/* Mailbox data high register */
|
||||
|
||||
constexpr unsigned long TDHR_DATA4_SHIFT = (0U); /* Bits 7-0: Data Byte 4 */
|
||||
constexpr unsigned long TDHR_DATA4_MASK = (0xFFU << TDHR_DATA4_SHIFT);
|
||||
constexpr unsigned long TDHR_DATA5_SHIFT = (8U); /* Bits 15-8: Data Byte 5 */
|
||||
constexpr unsigned long TDHR_DATA5_MASK = (0xFFU << TDHR_DATA5_SHIFT);
|
||||
constexpr unsigned long TDHR_DATA6_SHIFT = (16U); /* Bits 23-16: Data Byte 6 */
|
||||
constexpr unsigned long TDHR_DATA6_MASK = (0xFFU << TDHR_DATA6_SHIFT);
|
||||
constexpr unsigned long TDHR_DATA7_SHIFT = (24U); /* Bits 31-24: Data Byte 7 */
|
||||
constexpr unsigned long TDHR_DATA7_MASK = (0xFFU << TDHR_DATA7_SHIFT);
|
||||
constexpr unsigned long TDHR_DATA4_SHIFT = (0U); /* Bits 7-0: Data Byte 4 */
|
||||
constexpr unsigned long TDHR_DATA4_MASK = (0xFFU << TDHR_DATA4_SHIFT);
|
||||
constexpr unsigned long TDHR_DATA5_SHIFT = (8U); /* Bits 15-8: Data Byte 5 */
|
||||
constexpr unsigned long TDHR_DATA5_MASK = (0xFFU << TDHR_DATA5_SHIFT);
|
||||
constexpr unsigned long TDHR_DATA6_SHIFT = (16U); /* Bits 23-16: Data Byte 6 */
|
||||
constexpr unsigned long TDHR_DATA6_MASK = (0xFFU << TDHR_DATA6_SHIFT);
|
||||
constexpr unsigned long TDHR_DATA7_SHIFT = (24U); /* Bits 31-24: Data Byte 7 */
|
||||
constexpr unsigned long TDHR_DATA7_MASK = (0xFFU << TDHR_DATA7_SHIFT);
|
||||
|
||||
/* Rx FIFO mailbox identifier register */
|
||||
|
||||
constexpr unsigned long RIR_RTR = (1U << 1); /* Bit 1: Remote Transmission Request */
|
||||
constexpr unsigned long RIR_IDE = (1U << 2); /* Bit 2: Identifier Extension */
|
||||
constexpr unsigned long RIR_EXID_SHIFT = (3U); /* Bit 3-31: Extended Identifier */
|
||||
constexpr unsigned long RIR_EXID_MASK = (0x1FFFFFFFU << RIR_EXID_SHIFT);
|
||||
constexpr unsigned long RIR_STID_SHIFT = (21U); /* Bits 21-31: Standard Identifier */
|
||||
constexpr unsigned long RIR_STID_MASK = (0x07FFU << RIR_STID_SHIFT);
|
||||
constexpr unsigned long RIR_RTR = (1U << 1); /* Bit 1: Remote Transmission Request */
|
||||
constexpr unsigned long RIR_IDE = (1U << 2); /* Bit 2: Identifier Extension */
|
||||
constexpr unsigned long RIR_EXID_SHIFT = (3U); /* Bit 3-31: Extended Identifier */
|
||||
constexpr unsigned long RIR_EXID_MASK = (0x1FFFFFFFU << RIR_EXID_SHIFT);
|
||||
constexpr unsigned long RIR_STID_SHIFT = (21U); /* Bits 21-31: Standard Identifier */
|
||||
constexpr unsigned long RIR_STID_MASK = (0x07FFU << RIR_STID_SHIFT);
|
||||
|
||||
/* Receive FIFO mailbox data length control and time stamp register */
|
||||
|
||||
constexpr unsigned long RDTR_DLC_SHIFT = (0U); /* Bits 3:0: Data Length Code */
|
||||
constexpr unsigned long RDTR_DLC_MASK = (0x0FU << RDTR_DLC_SHIFT);
|
||||
constexpr unsigned long RDTR_FM_SHIFT = (8U); /* Bits 15-8: Filter Match Index */
|
||||
constexpr unsigned long RDTR_FM_MASK = (0xFFU << RDTR_FM_SHIFT);
|
||||
constexpr unsigned long RDTR_TIME_SHIFT = (16U); /* Bits 31:16: Message Time Stamp */
|
||||
constexpr unsigned long RDTR_TIME_MASK = (0xFFFFU << RDTR_TIME_SHIFT);
|
||||
constexpr unsigned long RDTR_DLC_SHIFT = (0U); /* Bits 3:0: Data Length Code */
|
||||
constexpr unsigned long RDTR_DLC_MASK = (0x0FU << RDTR_DLC_SHIFT);
|
||||
constexpr unsigned long RDTR_FM_SHIFT = (8U); /* Bits 15-8: Filter Match Index */
|
||||
constexpr unsigned long RDTR_FM_MASK = (0xFFU << RDTR_FM_SHIFT);
|
||||
constexpr unsigned long RDTR_TIME_SHIFT = (16U); /* Bits 31:16: Message Time Stamp */
|
||||
constexpr unsigned long RDTR_TIME_MASK = (0xFFFFU << RDTR_TIME_SHIFT);
|
||||
|
||||
/* Receive FIFO mailbox data low register */
|
||||
|
||||
constexpr unsigned long RDLR_DATA0_SHIFT = (0U); /* Bits 7-0: Data Byte 0 */
|
||||
constexpr unsigned long RDLR_DATA0_MASK = (0xFFU << RDLR_DATA0_SHIFT);
|
||||
constexpr unsigned long RDLR_DATA1_SHIFT = (8U); /* Bits 15-8: Data Byte 1 */
|
||||
constexpr unsigned long RDLR_DATA1_MASK = (0xFFU << RDLR_DATA1_SHIFT);
|
||||
constexpr unsigned long RDLR_DATA2_SHIFT = (16U); /* Bits 23-16: Data Byte 2 */
|
||||
constexpr unsigned long RDLR_DATA2_MASK = (0xFFU << RDLR_DATA2_SHIFT);
|
||||
constexpr unsigned long RDLR_DATA3_SHIFT = (24U); /* Bits 31-24: Data Byte 3 */
|
||||
constexpr unsigned long RDLR_DATA3_MASK = (0xFFU << RDLR_DATA3_SHIFT);
|
||||
constexpr unsigned long RDLR_DATA0_SHIFT = (0U); /* Bits 7-0: Data Byte 0 */
|
||||
constexpr unsigned long RDLR_DATA0_MASK = (0xFFU << RDLR_DATA0_SHIFT);
|
||||
constexpr unsigned long RDLR_DATA1_SHIFT = (8U); /* Bits 15-8: Data Byte 1 */
|
||||
constexpr unsigned long RDLR_DATA1_MASK = (0xFFU << RDLR_DATA1_SHIFT);
|
||||
constexpr unsigned long RDLR_DATA2_SHIFT = (16U); /* Bits 23-16: Data Byte 2 */
|
||||
constexpr unsigned long RDLR_DATA2_MASK = (0xFFU << RDLR_DATA2_SHIFT);
|
||||
constexpr unsigned long RDLR_DATA3_SHIFT = (24U); /* Bits 31-24: Data Byte 3 */
|
||||
constexpr unsigned long RDLR_DATA3_MASK = (0xFFU << RDLR_DATA3_SHIFT);
|
||||
|
||||
/* Receive FIFO mailbox data high register */
|
||||
|
||||
constexpr unsigned long RDHR_DATA4_SHIFT = (0U); /* Bits 7-0: Data Byte 4 */
|
||||
constexpr unsigned long RDHR_DATA4_MASK = (0xFFU << RDHR_DATA4_SHIFT);
|
||||
constexpr unsigned long RDHR_DATA5_SHIFT = (8U); /* Bits 15-8: Data Byte 5 */
|
||||
constexpr unsigned long RDHR_DATA5_MASK = (0xFFU << RDHR_DATA5_SHIFT);
|
||||
constexpr unsigned long RDHR_DATA6_SHIFT = (16U); /* Bits 23-16: Data Byte 6 */
|
||||
constexpr unsigned long RDHR_DATA6_MASK = (0xFFU << RDHR_DATA6_SHIFT);
|
||||
constexpr unsigned long RDHR_DATA7_SHIFT = (24U); /* Bits 31-24: Data Byte 7 */
|
||||
constexpr unsigned long RDHR_DATA7_MASK = (0xFFU << RDHR_DATA7_SHIFT);
|
||||
constexpr unsigned long RDHR_DATA4_SHIFT = (0U); /* Bits 7-0: Data Byte 4 */
|
||||
constexpr unsigned long RDHR_DATA4_MASK = (0xFFU << RDHR_DATA4_SHIFT);
|
||||
constexpr unsigned long RDHR_DATA5_SHIFT = (8U); /* Bits 15-8: Data Byte 5 */
|
||||
constexpr unsigned long RDHR_DATA5_MASK = (0xFFU << RDHR_DATA5_SHIFT);
|
||||
constexpr unsigned long RDHR_DATA6_SHIFT = (16U); /* Bits 23-16: Data Byte 6 */
|
||||
constexpr unsigned long RDHR_DATA6_MASK = (0xFFU << RDHR_DATA6_SHIFT);
|
||||
constexpr unsigned long RDHR_DATA7_SHIFT = (24U); /* Bits 31-24: Data Byte 7 */
|
||||
constexpr unsigned long RDHR_DATA7_MASK = (0xFFU << RDHR_DATA7_SHIFT);
|
||||
|
||||
/* CAN filter master register */
|
||||
|
||||
constexpr unsigned long FMR_FINIT = (1U << 0); /* Bit 0: Filter Init Mode */
|
||||
constexpr unsigned long FMR_FINIT = (1U << 0); /* Bit 0: Filter Init Mode */
|
||||
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -29,15 +29,14 @@ static const uavcan::int16_t ErrFilterNumConfigs = 1008; ///< Number of f
|
|||
* RX queue item.
|
||||
* The application shall not use this directly.
|
||||
*/
|
||||
struct CanRxItem
|
||||
{
|
||||
uavcan::uint64_t utc_usec;
|
||||
uavcan::CanFrame frame;
|
||||
uavcan::CanIOFlags flags;
|
||||
CanRxItem()
|
||||
: utc_usec(0)
|
||||
, flags(0)
|
||||
{ }
|
||||
struct CanRxItem {
|
||||
uavcan::uint64_t utc_usec;
|
||||
uavcan::CanFrame frame;
|
||||
uavcan::CanIOFlags flags;
|
||||
CanRxItem()
|
||||
: utc_usec(0)
|
||||
, flags(0)
|
||||
{ }
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
@ -46,185 +45,182 @@ struct CanRxItem
|
|||
*/
|
||||
class CanIface : public uavcan::ICanIface, uavcan::Noncopyable
|
||||
{
|
||||
class RxQueue
|
||||
{
|
||||
CanRxItem* const buf_;
|
||||
const uavcan::uint8_t capacity_;
|
||||
uavcan::uint8_t in_;
|
||||
uavcan::uint8_t out_;
|
||||
uavcan::uint8_t len_;
|
||||
uavcan::uint32_t overflow_cnt_;
|
||||
class RxQueue
|
||||
{
|
||||
CanRxItem *const buf_;
|
||||
const uavcan::uint8_t capacity_;
|
||||
uavcan::uint8_t in_;
|
||||
uavcan::uint8_t out_;
|
||||
uavcan::uint8_t len_;
|
||||
uavcan::uint32_t overflow_cnt_;
|
||||
|
||||
void registerOverflow();
|
||||
void registerOverflow();
|
||||
|
||||
public:
|
||||
RxQueue(CanRxItem* buf, uavcan::uint8_t capacity)
|
||||
: buf_(buf)
|
||||
, capacity_(capacity)
|
||||
, in_(0)
|
||||
, out_(0)
|
||||
, len_(0)
|
||||
, overflow_cnt_(0)
|
||||
{ }
|
||||
public:
|
||||
RxQueue(CanRxItem *buf, uavcan::uint8_t capacity)
|
||||
: buf_(buf)
|
||||
, capacity_(capacity)
|
||||
, in_(0)
|
||||
, out_(0)
|
||||
, len_(0)
|
||||
, overflow_cnt_(0)
|
||||
{ }
|
||||
|
||||
void push(const uavcan::CanFrame& frame, const uint64_t& utc_usec, uavcan::CanIOFlags flags);
|
||||
void pop(uavcan::CanFrame& out_frame, uavcan::uint64_t& out_utc_usec, uavcan::CanIOFlags& out_flags);
|
||||
void push(const uavcan::CanFrame &frame, const uint64_t &utc_usec, uavcan::CanIOFlags flags);
|
||||
void pop(uavcan::CanFrame &out_frame, uavcan::uint64_t &out_utc_usec, uavcan::CanIOFlags &out_flags);
|
||||
|
||||
void reset();
|
||||
void reset();
|
||||
|
||||
unsigned getLength() const { return len_; }
|
||||
unsigned getLength() const { return len_; }
|
||||
|
||||
uavcan::uint32_t getOverflowCount() const { return overflow_cnt_; }
|
||||
};
|
||||
uavcan::uint32_t getOverflowCount() const { return overflow_cnt_; }
|
||||
};
|
||||
|
||||
struct Timings
|
||||
{
|
||||
uavcan::uint16_t prescaler;
|
||||
uavcan::uint8_t sjw;
|
||||
uavcan::uint8_t bs1;
|
||||
uavcan::uint8_t bs2;
|
||||
struct Timings {
|
||||
uavcan::uint16_t prescaler;
|
||||
uavcan::uint8_t sjw;
|
||||
uavcan::uint8_t bs1;
|
||||
uavcan::uint8_t bs2;
|
||||
|
||||
Timings()
|
||||
: prescaler(0)
|
||||
, sjw(0)
|
||||
, bs1(0)
|
||||
, bs2(0)
|
||||
{ }
|
||||
};
|
||||
Timings()
|
||||
: prescaler(0)
|
||||
, sjw(0)
|
||||
, bs1(0)
|
||||
, bs2(0)
|
||||
{ }
|
||||
};
|
||||
|
||||
struct TxItem
|
||||
{
|
||||
uavcan::MonotonicTime deadline;
|
||||
uavcan::CanFrame frame;
|
||||
bool pending;
|
||||
bool loopback;
|
||||
bool abort_on_error;
|
||||
struct TxItem {
|
||||
uavcan::MonotonicTime deadline;
|
||||
uavcan::CanFrame frame;
|
||||
bool pending;
|
||||
bool loopback;
|
||||
bool abort_on_error;
|
||||
|
||||
TxItem()
|
||||
: pending(false)
|
||||
, loopback(false)
|
||||
, abort_on_error(false)
|
||||
{ }
|
||||
};
|
||||
TxItem()
|
||||
: pending(false)
|
||||
, loopback(false)
|
||||
, abort_on_error(false)
|
||||
{ }
|
||||
};
|
||||
|
||||
enum { NumTxMailboxes = 3 };
|
||||
enum { NumFilters = 14 };
|
||||
enum { NumTxMailboxes = 3 };
|
||||
enum { NumFilters = 14 };
|
||||
|
||||
static const uavcan::uint32_t TSR_ABRQx[NumTxMailboxes];
|
||||
static const uavcan::uint32_t TSR_ABRQx[NumTxMailboxes];
|
||||
|
||||
RxQueue rx_queue_;
|
||||
bxcan::CanType* const can_;
|
||||
uavcan::uint64_t error_cnt_;
|
||||
uavcan::uint32_t served_aborts_cnt_;
|
||||
BusEvent& update_event_;
|
||||
TxItem pending_tx_[NumTxMailboxes];
|
||||
uavcan::uint8_t peak_tx_mailbox_index_;
|
||||
const uavcan::uint8_t self_index_;
|
||||
bool had_activity_;
|
||||
RxQueue rx_queue_;
|
||||
bxcan::CanType *const can_;
|
||||
uavcan::uint64_t error_cnt_;
|
||||
uavcan::uint32_t served_aborts_cnt_;
|
||||
BusEvent &update_event_;
|
||||
TxItem pending_tx_[NumTxMailboxes];
|
||||
uavcan::uint8_t peak_tx_mailbox_index_;
|
||||
const uavcan::uint8_t self_index_;
|
||||
bool had_activity_;
|
||||
|
||||
int computeTimings(uavcan::uint32_t target_bitrate, Timings& out_timings);
|
||||
int computeTimings(uavcan::uint32_t target_bitrate, Timings &out_timings);
|
||||
|
||||
virtual uavcan::int16_t send(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline,
|
||||
uavcan::CanIOFlags flags);
|
||||
virtual uavcan::int16_t send(const uavcan::CanFrame &frame, uavcan::MonotonicTime tx_deadline,
|
||||
uavcan::CanIOFlags flags);
|
||||
|
||||
virtual uavcan::int16_t receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic,
|
||||
uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags);
|
||||
virtual uavcan::int16_t receive(uavcan::CanFrame &out_frame, uavcan::MonotonicTime &out_ts_monotonic,
|
||||
uavcan::UtcTime &out_ts_utc, uavcan::CanIOFlags &out_flags);
|
||||
|
||||
virtual uavcan::int16_t configureFilters(const uavcan::CanFilterConfig* filter_configs,
|
||||
uavcan::uint16_t num_configs);
|
||||
virtual uavcan::int16_t configureFilters(const uavcan::CanFilterConfig *filter_configs,
|
||||
uavcan::uint16_t num_configs);
|
||||
|
||||
virtual uavcan::uint16_t getNumFilters() const { return NumFilters; }
|
||||
virtual uavcan::uint16_t getNumFilters() const { return NumFilters; }
|
||||
|
||||
void handleTxMailboxInterrupt(uavcan::uint8_t mailbox_index, bool txok, uavcan::uint64_t utc_usec);
|
||||
void handleTxMailboxInterrupt(uavcan::uint8_t mailbox_index, bool txok, uavcan::uint64_t utc_usec);
|
||||
|
||||
bool waitMsrINakBitStateChange(bool target_state);
|
||||
bool waitMsrINakBitStateChange(bool target_state);
|
||||
|
||||
public:
|
||||
enum { MaxRxQueueCapacity = 254 };
|
||||
enum { MaxRxQueueCapacity = 254 };
|
||||
|
||||
enum OperatingMode
|
||||
{
|
||||
NormalMode,
|
||||
SilentMode
|
||||
};
|
||||
enum OperatingMode {
|
||||
NormalMode,
|
||||
SilentMode
|
||||
};
|
||||
|
||||
CanIface(bxcan::CanType* can, BusEvent& update_event, uavcan::uint8_t self_index,
|
||||
CanRxItem* rx_queue_buffer, uavcan::uint8_t rx_queue_capacity)
|
||||
: rx_queue_(rx_queue_buffer, rx_queue_capacity)
|
||||
, can_(can)
|
||||
, error_cnt_(0)
|
||||
, served_aborts_cnt_(0)
|
||||
, update_event_(update_event)
|
||||
, peak_tx_mailbox_index_(0)
|
||||
, self_index_(self_index)
|
||||
, had_activity_(false)
|
||||
{
|
||||
UAVCAN_ASSERT(self_index_ < UAVCAN_STM32_NUM_IFACES);
|
||||
}
|
||||
CanIface(bxcan::CanType *can, BusEvent &update_event, uavcan::uint8_t self_index,
|
||||
CanRxItem *rx_queue_buffer, uavcan::uint8_t rx_queue_capacity)
|
||||
: rx_queue_(rx_queue_buffer, rx_queue_capacity)
|
||||
, can_(can)
|
||||
, error_cnt_(0)
|
||||
, served_aborts_cnt_(0)
|
||||
, update_event_(update_event)
|
||||
, peak_tx_mailbox_index_(0)
|
||||
, self_index_(self_index)
|
||||
, had_activity_(false)
|
||||
{
|
||||
UAVCAN_ASSERT(self_index_ < UAVCAN_STM32_NUM_IFACES);
|
||||
}
|
||||
|
||||
/**
|
||||
* Initializes the hardware CAN controller.
|
||||
* Assumes:
|
||||
* - Iface clock is enabled
|
||||
* - Iface has been resetted via RCC
|
||||
* - Caller will configure NVIC by itself
|
||||
*/
|
||||
int init(const uavcan::uint32_t bitrate, const OperatingMode mode);
|
||||
/**
|
||||
* Initializes the hardware CAN controller.
|
||||
* Assumes:
|
||||
* - Iface clock is enabled
|
||||
* - Iface has been resetted via RCC
|
||||
* - Caller will configure NVIC by itself
|
||||
*/
|
||||
int init(const uavcan::uint32_t bitrate, const OperatingMode mode);
|
||||
|
||||
void handleTxInterrupt(uavcan::uint64_t utc_usec);
|
||||
void handleRxInterrupt(uavcan::uint8_t fifo_index, uavcan::uint64_t utc_usec);
|
||||
void handleTxInterrupt(uavcan::uint64_t utc_usec);
|
||||
void handleRxInterrupt(uavcan::uint8_t fifo_index, uavcan::uint64_t utc_usec);
|
||||
|
||||
/**
|
||||
* This method is used to count errors and abort transmission on error if necessary.
|
||||
* This functionality used to be implemented in the SCE interrupt handler, but that approach was
|
||||
* generating too much processing overhead, especially on disconnected interfaces.
|
||||
*
|
||||
* Should be called from RX ISR, TX ISR, and select(); interrupts must be enabled.
|
||||
*/
|
||||
void pollErrorFlagsFromISR();
|
||||
/**
|
||||
* This method is used to count errors and abort transmission on error if necessary.
|
||||
* This functionality used to be implemented in the SCE interrupt handler, but that approach was
|
||||
* generating too much processing overhead, especially on disconnected interfaces.
|
||||
*
|
||||
* Should be called from RX ISR, TX ISR, and select(); interrupts must be enabled.
|
||||
*/
|
||||
void pollErrorFlagsFromISR();
|
||||
|
||||
void discardTimedOutTxMailboxes(uavcan::MonotonicTime current_time);
|
||||
void discardTimedOutTxMailboxes(uavcan::MonotonicTime current_time);
|
||||
|
||||
bool canAcceptNewTxFrame(const uavcan::CanFrame& frame) const;
|
||||
bool isRxBufferEmpty() const;
|
||||
bool canAcceptNewTxFrame(const uavcan::CanFrame &frame) const;
|
||||
bool isRxBufferEmpty() const;
|
||||
|
||||
/**
|
||||
* Number of RX frames lost due to queue overflow.
|
||||
* This is an atomic read, it doesn't require a critical section.
|
||||
*/
|
||||
uavcan::uint32_t getRxQueueOverflowCount() const { return rx_queue_.getOverflowCount(); }
|
||||
/**
|
||||
* Number of RX frames lost due to queue overflow.
|
||||
* This is an atomic read, it doesn't require a critical section.
|
||||
*/
|
||||
uavcan::uint32_t getRxQueueOverflowCount() const { return rx_queue_.getOverflowCount(); }
|
||||
|
||||
/**
|
||||
* Total number of hardware failures and other kinds of errors (e.g. queue overruns).
|
||||
* May increase continuously if the interface is not connected to the bus.
|
||||
*/
|
||||
virtual uavcan::uint64_t getErrorCount() const;
|
||||
/**
|
||||
* Total number of hardware failures and other kinds of errors (e.g. queue overruns).
|
||||
* May increase continuously if the interface is not connected to the bus.
|
||||
*/
|
||||
virtual uavcan::uint64_t getErrorCount() const;
|
||||
|
||||
/**
|
||||
* Number of times the driver exercised library's requirement to abort transmission on first error.
|
||||
* This is an atomic read, it doesn't require a critical section.
|
||||
* See @ref uavcan::CanIOFlagAbortOnError.
|
||||
*/
|
||||
uavcan::uint32_t getVoluntaryTxAbortCount() const { return served_aborts_cnt_; }
|
||||
/**
|
||||
* Number of times the driver exercised library's requirement to abort transmission on first error.
|
||||
* This is an atomic read, it doesn't require a critical section.
|
||||
* See @ref uavcan::CanIOFlagAbortOnError.
|
||||
*/
|
||||
uavcan::uint32_t getVoluntaryTxAbortCount() const { return served_aborts_cnt_; }
|
||||
|
||||
/**
|
||||
* Returns the number of frames pending in the RX queue.
|
||||
* This is intended for debug use only.
|
||||
*/
|
||||
unsigned getRxQueueLength() const;
|
||||
/**
|
||||
* Returns the number of frames pending in the RX queue.
|
||||
* This is intended for debug use only.
|
||||
*/
|
||||
unsigned getRxQueueLength() const;
|
||||
|
||||
/**
|
||||
* Whether this iface had at least one successful IO since the previous call of this method.
|
||||
* This is designed for use with iface activity LEDs.
|
||||
*/
|
||||
bool hadActivity();
|
||||
/**
|
||||
* Whether this iface had at least one successful IO since the previous call of this method.
|
||||
* This is designed for use with iface activity LEDs.
|
||||
*/
|
||||
bool hadActivity();
|
||||
|
||||
/**
|
||||
* Peak number of TX mailboxes used concurrently since initialization.
|
||||
* Range is [1, 3].
|
||||
* Value of 3 suggests that priority inversion could be taking place.
|
||||
*/
|
||||
uavcan::uint8_t getPeakNumTxMailboxesUsed() const { return uavcan::uint8_t(peak_tx_mailbox_index_ + 1); }
|
||||
/**
|
||||
* Peak number of TX mailboxes used concurrently since initialization.
|
||||
* Range is [1, 3].
|
||||
* Value of 3 suggests that priority inversion could be taking place.
|
||||
*/
|
||||
uavcan::uint8_t getPeakNumTxMailboxesUsed() const { return uavcan::uint8_t(peak_tx_mailbox_index_ + 1); }
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
@ -233,57 +229,57 @@ public:
|
|||
*/
|
||||
class CanDriver : public uavcan::ICanDriver, uavcan::Noncopyable
|
||||
{
|
||||
BusEvent update_event_;
|
||||
CanIface if0_;
|
||||
BusEvent update_event_;
|
||||
CanIface if0_;
|
||||
#if UAVCAN_STM32_NUM_IFACES > 1
|
||||
CanIface if1_;
|
||||
CanIface if1_;
|
||||
#endif
|
||||
|
||||
virtual uavcan::int16_t select(uavcan::CanSelectMasks& inout_masks,
|
||||
const uavcan::CanFrame* (& pending_tx)[uavcan::MaxCanIfaces],
|
||||
uavcan::MonotonicTime blocking_deadline);
|
||||
virtual uavcan::int16_t select(uavcan::CanSelectMasks &inout_masks,
|
||||
const uavcan::CanFrame * (& pending_tx)[uavcan::MaxCanIfaces],
|
||||
uavcan::MonotonicTime blocking_deadline);
|
||||
|
||||
static void initOnce();
|
||||
static void initOnce();
|
||||
|
||||
public:
|
||||
template <unsigned RxQueueCapacity>
|
||||
CanDriver(CanRxItem (&rx_queue_storage)[UAVCAN_STM32_NUM_IFACES][RxQueueCapacity])
|
||||
: update_event_(*this)
|
||||
, if0_(bxcan::Can[0], update_event_, 0, rx_queue_storage[0], RxQueueCapacity)
|
||||
template <unsigned RxQueueCapacity>
|
||||
CanDriver(CanRxItem(&rx_queue_storage)[UAVCAN_STM32_NUM_IFACES][RxQueueCapacity])
|
||||
: update_event_(*this)
|
||||
, if0_(bxcan::Can[0], update_event_, 0, rx_queue_storage[0], RxQueueCapacity)
|
||||
#if UAVCAN_STM32_NUM_IFACES > 1
|
||||
, if1_(bxcan::Can[1], update_event_, 1, rx_queue_storage[1], RxQueueCapacity)
|
||||
, if1_(bxcan::Can[1], update_event_, 1, rx_queue_storage[1], RxQueueCapacity)
|
||||
#endif
|
||||
{
|
||||
uavcan::StaticAssert<(RxQueueCapacity <= CanIface::MaxRxQueueCapacity)>::check();
|
||||
}
|
||||
{
|
||||
uavcan::StaticAssert < (RxQueueCapacity <= CanIface::MaxRxQueueCapacity) >::check();
|
||||
}
|
||||
|
||||
/**
|
||||
* This function returns select masks indicating which interfaces are available for read/write.
|
||||
*/
|
||||
uavcan::CanSelectMasks makeSelectMasks(const uavcan::CanFrame* (& pending_tx)[uavcan::MaxCanIfaces]) const;
|
||||
/**
|
||||
* This function returns select masks indicating which interfaces are available for read/write.
|
||||
*/
|
||||
uavcan::CanSelectMasks makeSelectMasks(const uavcan::CanFrame * (& pending_tx)[uavcan::MaxCanIfaces]) const;
|
||||
|
||||
/**
|
||||
* Whether there's at least one interface where receive() would return a frame.
|
||||
*/
|
||||
bool hasReadableInterfaces() const;
|
||||
/**
|
||||
* Whether there's at least one interface where receive() would return a frame.
|
||||
*/
|
||||
bool hasReadableInterfaces() const;
|
||||
|
||||
/**
|
||||
* Returns zero if OK.
|
||||
* Returns negative value if failed (e.g. invalid bitrate).
|
||||
*/
|
||||
int init(const uavcan::uint32_t bitrate, const CanIface::OperatingMode mode);
|
||||
/**
|
||||
* Returns zero if OK.
|
||||
* Returns negative value if failed (e.g. invalid bitrate).
|
||||
*/
|
||||
int init(const uavcan::uint32_t bitrate, const CanIface::OperatingMode mode);
|
||||
|
||||
virtual CanIface* getIface(uavcan::uint8_t iface_index);
|
||||
virtual CanIface *getIface(uavcan::uint8_t iface_index);
|
||||
|
||||
virtual uavcan::uint8_t getNumIfaces() const { return UAVCAN_STM32_NUM_IFACES; }
|
||||
virtual uavcan::uint8_t getNumIfaces() const { return UAVCAN_STM32_NUM_IFACES; }
|
||||
|
||||
/**
|
||||
* Whether at least one iface had at least one successful IO since previous call of this method.
|
||||
* This is designed for use with iface activity LEDs.
|
||||
*/
|
||||
bool hadActivity();
|
||||
/**
|
||||
* Whether at least one iface had at least one successful IO since previous call of this method.
|
||||
* This is designed for use with iface activity LEDs.
|
||||
*/
|
||||
bool hadActivity();
|
||||
|
||||
BusEvent& updateEvent() { return update_event_; }
|
||||
BusEvent &updateEvent() { return update_event_; }
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
@ -294,91 +290,84 @@ public:
|
|||
template <unsigned RxQueueCapacity = 128>
|
||||
class CanInitHelper
|
||||
{
|
||||
CanRxItem queue_storage_[UAVCAN_STM32_NUM_IFACES][RxQueueCapacity];
|
||||
CanRxItem queue_storage_[UAVCAN_STM32_NUM_IFACES][RxQueueCapacity];
|
||||
|
||||
public:
|
||||
enum { BitRateAutoDetect = 0 };
|
||||
enum { BitRateAutoDetect = 0 };
|
||||
|
||||
CanDriver driver;
|
||||
CanDriver driver;
|
||||
|
||||
CanInitHelper() :
|
||||
driver(queue_storage_)
|
||||
{ }
|
||||
CanInitHelper() :
|
||||
driver(queue_storage_)
|
||||
{ }
|
||||
|
||||
/**
|
||||
* This overload simply configures the provided bitrate.
|
||||
* Auto bit rate detection will not be performed.
|
||||
* Bitrate value must be positive.
|
||||
* @return Negative value on error; non-negative on success. Refer to constants Err*.
|
||||
*/
|
||||
int init(uavcan::uint32_t bitrate)
|
||||
{
|
||||
return driver.init(bitrate, CanIface::NormalMode);
|
||||
}
|
||||
/**
|
||||
* This overload simply configures the provided bitrate.
|
||||
* Auto bit rate detection will not be performed.
|
||||
* Bitrate value must be positive.
|
||||
* @return Negative value on error; non-negative on success. Refer to constants Err*.
|
||||
*/
|
||||
int init(uavcan::uint32_t bitrate)
|
||||
{
|
||||
return driver.init(bitrate, CanIface::NormalMode);
|
||||
}
|
||||
|
||||
/**
|
||||
* This function can either initialize the driver at a fixed bit rate, or it can perform
|
||||
* automatic bit rate detection. For theory please refer to the CiA application note #801.
|
||||
*
|
||||
* @param delay_callable A callable entity that suspends execution for strictly more than one second.
|
||||
* The callable entity will be invoked without arguments.
|
||||
* @ref getRecommendedListeningDelay().
|
||||
*
|
||||
* @param inout_bitrate Fixed bit rate or zero. Zero invokes the bit rate detection process.
|
||||
* If auto detection was used, the function will update the argument
|
||||
* with established bit rate. In case of an error the value will be undefined.
|
||||
*
|
||||
* @return Negative value on error; non-negative on success. Refer to constants Err*.
|
||||
*/
|
||||
template <typename DelayCallable>
|
||||
int init(DelayCallable delay_callable, uavcan::uint32_t& inout_bitrate = BitRateAutoDetect)
|
||||
{
|
||||
if (inout_bitrate > 0)
|
||||
{
|
||||
return driver.init(inout_bitrate, CanIface::NormalMode);
|
||||
}
|
||||
else
|
||||
{
|
||||
static const uavcan::uint32_t StandardBitRates[] =
|
||||
{
|
||||
1000000,
|
||||
500000,
|
||||
250000,
|
||||
125000
|
||||
};
|
||||
/**
|
||||
* This function can either initialize the driver at a fixed bit rate, or it can perform
|
||||
* automatic bit rate detection. For theory please refer to the CiA application note #801.
|
||||
*
|
||||
* @param delay_callable A callable entity that suspends execution for strictly more than one second.
|
||||
* The callable entity will be invoked without arguments.
|
||||
* @ref getRecommendedListeningDelay().
|
||||
*
|
||||
* @param inout_bitrate Fixed bit rate or zero. Zero invokes the bit rate detection process.
|
||||
* If auto detection was used, the function will update the argument
|
||||
* with established bit rate. In case of an error the value will be undefined.
|
||||
*
|
||||
* @return Negative value on error; non-negative on success. Refer to constants Err*.
|
||||
*/
|
||||
template <typename DelayCallable>
|
||||
int init(DelayCallable delay_callable, uavcan::uint32_t &inout_bitrate = BitRateAutoDetect)
|
||||
{
|
||||
if (inout_bitrate > 0) {
|
||||
return driver.init(inout_bitrate, CanIface::NormalMode);
|
||||
|
||||
for (uavcan::uint8_t br = 0; br < sizeof(StandardBitRates) / sizeof(StandardBitRates[0]); br++)
|
||||
{
|
||||
inout_bitrate = StandardBitRates[br];
|
||||
} else {
|
||||
static const uavcan::uint32_t StandardBitRates[] = {
|
||||
1000000,
|
||||
500000,
|
||||
250000,
|
||||
125000
|
||||
};
|
||||
|
||||
const int res = driver.init(inout_bitrate, CanIface::SilentMode);
|
||||
for (uavcan::uint8_t br = 0; br < sizeof(StandardBitRates) / sizeof(StandardBitRates[0]); br++) {
|
||||
inout_bitrate = StandardBitRates[br];
|
||||
|
||||
delay_callable();
|
||||
const int res = driver.init(inout_bitrate, CanIface::SilentMode);
|
||||
|
||||
if (res >= 0)
|
||||
{
|
||||
for (uavcan::uint8_t iface = 0; iface < driver.getNumIfaces(); iface++)
|
||||
{
|
||||
if (!driver.getIface(iface)->isRxBufferEmpty())
|
||||
{
|
||||
// Re-initializing in normal mode
|
||||
return driver.init(inout_bitrate, CanIface::NormalMode);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
delay_callable();
|
||||
|
||||
return -ErrBitRateNotDetected;
|
||||
}
|
||||
}
|
||||
if (res >= 0) {
|
||||
for (uavcan::uint8_t iface = 0; iface < driver.getNumIfaces(); iface++) {
|
||||
if (!driver.getIface(iface)->isRxBufferEmpty()) {
|
||||
// Re-initializing in normal mode
|
||||
return driver.init(inout_bitrate, CanIface::NormalMode);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Use this value for listening delay during automatic bit rate detection.
|
||||
*/
|
||||
static uavcan::MonotonicDuration getRecommendedListeningDelay()
|
||||
{
|
||||
return uavcan::MonotonicDuration::fromMSec(1050);
|
||||
}
|
||||
return -ErrBitRateNotDetected;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Use this value for listening delay during automatic bit rate detection.
|
||||
*/
|
||||
static uavcan::MonotonicDuration getRecommendedListeningDelay()
|
||||
{
|
||||
return uavcan::MonotonicDuration::fromMSec(1050);
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -48,25 +48,24 @@ void adjustUtc(uavcan::UtcDuration adjustment);
|
|||
/**
|
||||
* UTC clock synchronization parameters
|
||||
*/
|
||||
struct UtcSyncParams
|
||||
{
|
||||
float offset_p; ///< PPM per one usec error
|
||||
float rate_i; ///< PPM per one PPM error for second
|
||||
float rate_error_corner_freq;
|
||||
float max_rate_correction_ppm;
|
||||
float lock_thres_rate_ppm;
|
||||
uavcan::UtcDuration lock_thres_offset;
|
||||
uavcan::UtcDuration min_jump; ///< Min error to jump rather than change rate
|
||||
struct UtcSyncParams {
|
||||
float offset_p; ///< PPM per one usec error
|
||||
float rate_i; ///< PPM per one PPM error for second
|
||||
float rate_error_corner_freq;
|
||||
float max_rate_correction_ppm;
|
||||
float lock_thres_rate_ppm;
|
||||
uavcan::UtcDuration lock_thres_offset;
|
||||
uavcan::UtcDuration min_jump; ///< Min error to jump rather than change rate
|
||||
|
||||
UtcSyncParams()
|
||||
: offset_p(0.01F)
|
||||
, rate_i(0.02F)
|
||||
, rate_error_corner_freq(0.01F)
|
||||
, max_rate_correction_ppm(300.0F)
|
||||
, lock_thres_rate_ppm(2.0F)
|
||||
, lock_thres_offset(uavcan::UtcDuration::fromMSec(4))
|
||||
, min_jump(uavcan::UtcDuration::fromMSec(10))
|
||||
{ }
|
||||
UtcSyncParams()
|
||||
: offset_p(0.01F)
|
||||
, rate_i(0.02F)
|
||||
, rate_error_corner_freq(0.01F)
|
||||
, max_rate_correction_ppm(300.0F)
|
||||
, lock_thres_rate_ppm(2.0F)
|
||||
, lock_thres_offset(uavcan::UtcDuration::fromMSec(4))
|
||||
, min_jump(uavcan::UtcDuration::fromMSec(10))
|
||||
{ }
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
@ -94,7 +93,7 @@ bool isUtcLocked();
|
|||
* Both functions are thread safe.
|
||||
*/
|
||||
UtcSyncParams getUtcSyncParams();
|
||||
void setUtcSyncParams(const UtcSyncParams& params);
|
||||
void setUtcSyncParams(const UtcSyncParams ¶ms);
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -103,19 +102,19 @@ void setUtcSyncParams(const UtcSyncParams& params);
|
|||
*/
|
||||
class SystemClock : public uavcan::ISystemClock, uavcan::Noncopyable
|
||||
{
|
||||
SystemClock() { }
|
||||
SystemClock() { }
|
||||
|
||||
virtual void adjustUtc(uavcan::UtcDuration adjustment) { clock::adjustUtc(adjustment); }
|
||||
virtual void adjustUtc(uavcan::UtcDuration adjustment) { clock::adjustUtc(adjustment); }
|
||||
|
||||
public:
|
||||
virtual uavcan::MonotonicTime getMonotonic() const { return clock::getMonotonic(); }
|
||||
virtual uavcan::UtcTime getUtc() const { return clock::getUtc(); }
|
||||
virtual uavcan::MonotonicTime getMonotonic() const { return clock::getMonotonic(); }
|
||||
virtual uavcan::UtcTime getUtc() const { return clock::getUtc(); }
|
||||
|
||||
/**
|
||||
* Calls clock::init() as needed.
|
||||
* This function is thread safe.
|
||||
*/
|
||||
static SystemClock& instance();
|
||||
/**
|
||||
* Calls clock::init() as needed.
|
||||
* This function is thread safe.
|
||||
*/
|
||||
static SystemClock &instance();
|
||||
};
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -32,69 +32,69 @@ class CanDriver;
|
|||
*/
|
||||
class BusEvent : uavcan::Noncopyable
|
||||
{
|
||||
using SignalCallbackHandler = void(*)();
|
||||
using SignalCallbackHandler = void(*)();
|
||||
|
||||
SignalCallbackHandler signal_cb_{nullptr};
|
||||
sem_t sem_;
|
||||
SignalCallbackHandler signal_cb_{nullptr};
|
||||
sem_t sem_;
|
||||
|
||||
public:
|
||||
BusEvent(CanDriver& can_driver);
|
||||
~BusEvent();
|
||||
BusEvent(CanDriver &can_driver);
|
||||
~BusEvent();
|
||||
|
||||
void registerSignalCallback(SignalCallbackHandler handler) { signal_cb_ = handler; }
|
||||
void registerSignalCallback(SignalCallbackHandler handler) { signal_cb_ = handler; }
|
||||
|
||||
bool wait(uavcan::MonotonicDuration duration);
|
||||
bool wait(uavcan::MonotonicDuration duration);
|
||||
|
||||
void signalFromInterrupt();
|
||||
void signalFromInterrupt();
|
||||
};
|
||||
|
||||
class Mutex
|
||||
{
|
||||
pthread_mutex_t mutex_;
|
||||
pthread_mutex_t mutex_;
|
||||
|
||||
public:
|
||||
Mutex()
|
||||
{
|
||||
init();
|
||||
}
|
||||
Mutex()
|
||||
{
|
||||
init();
|
||||
}
|
||||
|
||||
int init()
|
||||
{
|
||||
return pthread_mutex_init(&mutex_, UAVCAN_NULLPTR);
|
||||
}
|
||||
int init()
|
||||
{
|
||||
return pthread_mutex_init(&mutex_, UAVCAN_NULLPTR);
|
||||
}
|
||||
|
||||
int deinit()
|
||||
{
|
||||
return pthread_mutex_destroy(&mutex_);
|
||||
}
|
||||
int deinit()
|
||||
{
|
||||
return pthread_mutex_destroy(&mutex_);
|
||||
}
|
||||
|
||||
void lock()
|
||||
{
|
||||
(void)pthread_mutex_lock(&mutex_);
|
||||
}
|
||||
void lock()
|
||||
{
|
||||
(void)pthread_mutex_lock(&mutex_);
|
||||
}
|
||||
|
||||
void unlock()
|
||||
{
|
||||
(void)pthread_mutex_unlock(&mutex_);
|
||||
}
|
||||
void unlock()
|
||||
{
|
||||
(void)pthread_mutex_unlock(&mutex_);
|
||||
}
|
||||
};
|
||||
#endif
|
||||
|
||||
|
||||
class MutexLocker
|
||||
{
|
||||
Mutex& mutex_;
|
||||
Mutex &mutex_;
|
||||
|
||||
public:
|
||||
MutexLocker(Mutex& mutex)
|
||||
: mutex_(mutex)
|
||||
{
|
||||
mutex_.lock();
|
||||
}
|
||||
~MutexLocker()
|
||||
{
|
||||
mutex_.unlock();
|
||||
}
|
||||
MutexLocker(Mutex &mutex)
|
||||
: mutex_(mutex)
|
||||
{
|
||||
mutex_.lock();
|
||||
}
|
||||
~MutexLocker()
|
||||
{
|
||||
mutex_.unlock();
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -50,18 +50,17 @@ namespace uavcan_stm32
|
|||
{
|
||||
#if UAVCAN_STM32_NUTTX
|
||||
|
||||
struct CriticalSectionLocker
|
||||
{
|
||||
const irqstate_t flags_;
|
||||
struct CriticalSectionLocker {
|
||||
const irqstate_t flags_;
|
||||
|
||||
CriticalSectionLocker()
|
||||
: flags_(enter_critical_section())
|
||||
{ }
|
||||
CriticalSectionLocker()
|
||||
: flags_(enter_critical_section())
|
||||
{ }
|
||||
|
||||
~CriticalSectionLocker()
|
||||
{
|
||||
leave_critical_section(flags_);
|
||||
}
|
||||
~CriticalSectionLocker()
|
||||
{
|
||||
leave_critical_section(flags_);
|
||||
}
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load Diff
|
|
@ -82,182 +82,181 @@ uavcan::uint64_t time_utc = 0;
|
|||
|
||||
void init()
|
||||
{
|
||||
CriticalSectionLocker lock;
|
||||
if (initialized)
|
||||
{
|
||||
return;
|
||||
}
|
||||
initialized = true;
|
||||
CriticalSectionLocker lock;
|
||||
|
||||
if (initialized) {
|
||||
return;
|
||||
}
|
||||
|
||||
initialized = true;
|
||||
|
||||
|
||||
# if UAVCAN_STM32_NUTTX
|
||||
|
||||
// Attach IRQ
|
||||
irq_attach(TIMX_IRQn, &TIMX_IRQHandler, NULL);
|
||||
// Attach IRQ
|
||||
irq_attach(TIMX_IRQn, &TIMX_IRQHandler, NULL);
|
||||
|
||||
// Power-on and reset
|
||||
modifyreg32(STM32_RCC_APB1ENR, 0, TIMX_RCC_ENR_MASK);
|
||||
modifyreg32(STM32_RCC_APB1RSTR, 0, TIMX_RCC_RSTR_MASK);
|
||||
modifyreg32(STM32_RCC_APB1RSTR, TIMX_RCC_RSTR_MASK, 0);
|
||||
// Power-on and reset
|
||||
modifyreg32(STM32_RCC_APB1ENR, 0, TIMX_RCC_ENR_MASK);
|
||||
modifyreg32(STM32_RCC_APB1RSTR, 0, TIMX_RCC_RSTR_MASK);
|
||||
modifyreg32(STM32_RCC_APB1RSTR, TIMX_RCC_RSTR_MASK, 0);
|
||||
|
||||
|
||||
// Start the timer
|
||||
putreg32(0xFFFF, TMR_REG(STM32_BTIM_ARR_OFFSET));
|
||||
putreg16(((TIMX_INPUT_CLOCK / 1000000)-1), TMR_REG(STM32_BTIM_PSC_OFFSET));
|
||||
putreg16(BTIM_CR1_URS, TMR_REG(STM32_BTIM_CR1_OFFSET));
|
||||
putreg16(0, TMR_REG(STM32_BTIM_SR_OFFSET));
|
||||
putreg16(BTIM_EGR_UG, TMR_REG(STM32_BTIM_EGR_OFFSET)); // Reload immediately
|
||||
putreg16(BTIM_DIER_UIE, TMR_REG(STM32_BTIM_DIER_OFFSET));
|
||||
putreg16(BTIM_CR1_CEN, TMR_REG(STM32_BTIM_CR1_OFFSET)); // Start
|
||||
// Start the timer
|
||||
putreg32(0xFFFF, TMR_REG(STM32_BTIM_ARR_OFFSET));
|
||||
putreg16(((TIMX_INPUT_CLOCK / 1000000) - 1), TMR_REG(STM32_BTIM_PSC_OFFSET));
|
||||
putreg16(BTIM_CR1_URS, TMR_REG(STM32_BTIM_CR1_OFFSET));
|
||||
putreg16(0, TMR_REG(STM32_BTIM_SR_OFFSET));
|
||||
putreg16(BTIM_EGR_UG, TMR_REG(STM32_BTIM_EGR_OFFSET)); // Reload immediately
|
||||
putreg16(BTIM_DIER_UIE, TMR_REG(STM32_BTIM_DIER_OFFSET));
|
||||
putreg16(BTIM_CR1_CEN, TMR_REG(STM32_BTIM_CR1_OFFSET)); // Start
|
||||
|
||||
// Prioritize and Enable IRQ
|
||||
// Prioritize and Enable IRQ
|
||||
// todo: Currently changing the NVIC_SYSH_HIGH_PRIORITY is HARD faulting
|
||||
// need to investigate
|
||||
// up_prioritize_irq(TIMX_IRQn, NVIC_SYSH_HIGH_PRIORITY);
|
||||
up_enable_irq(TIMX_IRQn);
|
||||
up_enable_irq(TIMX_IRQn);
|
||||
|
||||
# endif
|
||||
}
|
||||
|
||||
void setUtc(uavcan::UtcTime time)
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
UAVCAN_ASSERT(initialized);
|
||||
MutexLocker mlocker(mutex);
|
||||
UAVCAN_ASSERT(initialized);
|
||||
|
||||
{
|
||||
CriticalSectionLocker locker;
|
||||
time_utc = time.toUSec();
|
||||
}
|
||||
{
|
||||
CriticalSectionLocker locker;
|
||||
time_utc = time.toUSec();
|
||||
}
|
||||
|
||||
utc_set = true;
|
||||
utc_locked = false;
|
||||
utc_jump_cnt++;
|
||||
utc_prev_adj = 0;
|
||||
utc_rel_rate_ppm = 0;
|
||||
utc_set = true;
|
||||
utc_locked = false;
|
||||
utc_jump_cnt++;
|
||||
utc_prev_adj = 0;
|
||||
utc_rel_rate_ppm = 0;
|
||||
}
|
||||
|
||||
static uavcan::uint64_t sampleUtcFromCriticalSection()
|
||||
{
|
||||
# if UAVCAN_STM32_NUTTX
|
||||
|
||||
UAVCAN_ASSERT(initialized);
|
||||
UAVCAN_ASSERT(getreg16(TMR_REG(STM32_BTIM_DIER_OFFSET)) & BTIM_DIER_UIE);
|
||||
UAVCAN_ASSERT(initialized);
|
||||
UAVCAN_ASSERT(getreg16(TMR_REG(STM32_BTIM_DIER_OFFSET)) & BTIM_DIER_UIE);
|
||||
|
||||
volatile uavcan::uint64_t time = time_utc;
|
||||
volatile uavcan::uint32_t cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET));
|
||||
volatile uavcan::uint64_t time = time_utc;
|
||||
volatile uavcan::uint32_t cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET));
|
||||
|
||||
if (getreg16(TMR_REG(STM32_BTIM_SR_OFFSET)) & BTIM_SR_UIF)
|
||||
{
|
||||
cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET));
|
||||
const uavcan::int32_t add = uavcan::int32_t(USecPerOverflow) +
|
||||
(utc_accumulated_correction_nsec + utc_correction_nsec_per_overflow) / 1000;
|
||||
time = uavcan::uint64_t(uavcan::int64_t(time) + add);
|
||||
}
|
||||
return time + cnt;
|
||||
if (getreg16(TMR_REG(STM32_BTIM_SR_OFFSET)) & BTIM_SR_UIF) {
|
||||
cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET));
|
||||
const uavcan::int32_t add = uavcan::int32_t(USecPerOverflow) +
|
||||
(utc_accumulated_correction_nsec + utc_correction_nsec_per_overflow) / 1000;
|
||||
time = uavcan::uint64_t(uavcan::int64_t(time) + add);
|
||||
}
|
||||
|
||||
return time + cnt;
|
||||
# endif
|
||||
}
|
||||
|
||||
uavcan::uint64_t getUtcUSecFromCanInterrupt()
|
||||
{
|
||||
return utc_set ? sampleUtcFromCriticalSection() : 0;
|
||||
return utc_set ? sampleUtcFromCriticalSection() : 0;
|
||||
}
|
||||
|
||||
uavcan::MonotonicTime getMonotonic()
|
||||
{
|
||||
uavcan::uint64_t usec = 0;
|
||||
// Scope Critical section
|
||||
{
|
||||
CriticalSectionLocker locker;
|
||||
uavcan::uint64_t usec = 0;
|
||||
// Scope Critical section
|
||||
{
|
||||
CriticalSectionLocker locker;
|
||||
|
||||
volatile uavcan::uint64_t time = time_mono;
|
||||
volatile uavcan::uint64_t time = time_mono;
|
||||
|
||||
# if UAVCAN_STM32_NUTTX
|
||||
|
||||
volatile uavcan::uint32_t cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET));
|
||||
volatile uavcan::uint32_t cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET));
|
||||
|
||||
if (getreg16(TMR_REG(STM32_BTIM_SR_OFFSET)) & BTIM_SR_UIF)
|
||||
{
|
||||
cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET));
|
||||
if (getreg16(TMR_REG(STM32_BTIM_SR_OFFSET)) & BTIM_SR_UIF) {
|
||||
cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET));
|
||||
# endif
|
||||
time += USecPerOverflow;
|
||||
}
|
||||
usec = time + cnt;
|
||||
time += USecPerOverflow;
|
||||
}
|
||||
|
||||
usec = time + cnt;
|
||||
|
||||
# ifndef NDEBUG
|
||||
static uavcan::uint64_t prev_usec = 0; // Self-test
|
||||
UAVCAN_ASSERT(prev_usec <= usec);
|
||||
(void)prev_usec;
|
||||
prev_usec = usec;
|
||||
static uavcan::uint64_t prev_usec = 0; // Self-test
|
||||
UAVCAN_ASSERT(prev_usec <= usec);
|
||||
(void)prev_usec;
|
||||
prev_usec = usec;
|
||||
# endif
|
||||
} // End Scope Critical section
|
||||
} // End Scope Critical section
|
||||
|
||||
return uavcan::MonotonicTime::fromUSec(usec);
|
||||
return uavcan::MonotonicTime::fromUSec(usec);
|
||||
}
|
||||
|
||||
uavcan::UtcTime getUtc()
|
||||
{
|
||||
if (utc_set)
|
||||
{
|
||||
uavcan::uint64_t usec = 0;
|
||||
{
|
||||
CriticalSectionLocker locker;
|
||||
usec = sampleUtcFromCriticalSection();
|
||||
}
|
||||
return uavcan::UtcTime::fromUSec(usec);
|
||||
}
|
||||
return uavcan::UtcTime();
|
||||
if (utc_set) {
|
||||
uavcan::uint64_t usec = 0;
|
||||
{
|
||||
CriticalSectionLocker locker;
|
||||
usec = sampleUtcFromCriticalSection();
|
||||
}
|
||||
return uavcan::UtcTime::fromUSec(usec);
|
||||
}
|
||||
|
||||
return uavcan::UtcTime();
|
||||
}
|
||||
|
||||
static float lowpass(float xold, float xnew, float corner, float dt)
|
||||
{
|
||||
const float tau = 1.F / corner;
|
||||
return (dt * xnew + tau * xold) / (dt + tau);
|
||||
const float tau = 1.F / corner;
|
||||
return (dt * xnew + tau * xold) / (dt + tau);
|
||||
}
|
||||
|
||||
static void updateRatePID(uavcan::UtcDuration adjustment)
|
||||
{
|
||||
const uavcan::MonotonicTime ts = getMonotonic();
|
||||
const float dt = float((ts - prev_utc_adj_at).toUSec()) / 1e6F;
|
||||
prev_utc_adj_at = ts;
|
||||
const float adj_usec = float(adjustment.toUSec());
|
||||
const uavcan::MonotonicTime ts = getMonotonic();
|
||||
const float dt = float((ts - prev_utc_adj_at).toUSec()) / 1e6F;
|
||||
prev_utc_adj_at = ts;
|
||||
const float adj_usec = float(adjustment.toUSec());
|
||||
|
||||
/*
|
||||
* Target relative rate in PPM
|
||||
* Positive to go faster
|
||||
*/
|
||||
const float target_rel_rate_ppm = adj_usec * utc_sync_params.offset_p;
|
||||
/*
|
||||
* Target relative rate in PPM
|
||||
* Positive to go faster
|
||||
*/
|
||||
const float target_rel_rate_ppm = adj_usec * utc_sync_params.offset_p;
|
||||
|
||||
/*
|
||||
* Current relative rate in PPM
|
||||
* Positive if the local clock is faster
|
||||
*/
|
||||
const float new_rel_rate_ppm = (utc_prev_adj - adj_usec) / dt; // rate error in [usec/sec], which is PPM
|
||||
utc_prev_adj = adj_usec;
|
||||
utc_rel_rate_ppm = lowpass(utc_rel_rate_ppm, new_rel_rate_ppm, utc_sync_params.rate_error_corner_freq, dt);
|
||||
/*
|
||||
* Current relative rate in PPM
|
||||
* Positive if the local clock is faster
|
||||
*/
|
||||
const float new_rel_rate_ppm = (utc_prev_adj - adj_usec) / dt; // rate error in [usec/sec], which is PPM
|
||||
utc_prev_adj = adj_usec;
|
||||
utc_rel_rate_ppm = lowpass(utc_rel_rate_ppm, new_rel_rate_ppm, utc_sync_params.rate_error_corner_freq, dt);
|
||||
|
||||
const float rel_rate_error = target_rel_rate_ppm - utc_rel_rate_ppm;
|
||||
const float rel_rate_error = target_rel_rate_ppm - utc_rel_rate_ppm;
|
||||
|
||||
if (dt > 10)
|
||||
{
|
||||
utc_rel_rate_error_integral = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
utc_rel_rate_error_integral += rel_rate_error * dt * utc_sync_params.rate_i;
|
||||
utc_rel_rate_error_integral =
|
||||
uavcan::max(utc_rel_rate_error_integral, -utc_sync_params.max_rate_correction_ppm);
|
||||
utc_rel_rate_error_integral =
|
||||
uavcan::min(utc_rel_rate_error_integral, utc_sync_params.max_rate_correction_ppm);
|
||||
}
|
||||
if (dt > 10) {
|
||||
utc_rel_rate_error_integral = 0;
|
||||
|
||||
/*
|
||||
* Rate controller
|
||||
*/
|
||||
float total_rate_correction_ppm = rel_rate_error + utc_rel_rate_error_integral;
|
||||
total_rate_correction_ppm = uavcan::max(total_rate_correction_ppm, -utc_sync_params.max_rate_correction_ppm);
|
||||
total_rate_correction_ppm = uavcan::min(total_rate_correction_ppm, utc_sync_params.max_rate_correction_ppm);
|
||||
} else {
|
||||
utc_rel_rate_error_integral += rel_rate_error * dt * utc_sync_params.rate_i;
|
||||
utc_rel_rate_error_integral =
|
||||
uavcan::max(utc_rel_rate_error_integral, -utc_sync_params.max_rate_correction_ppm);
|
||||
utc_rel_rate_error_integral =
|
||||
uavcan::min(utc_rel_rate_error_integral, utc_sync_params.max_rate_correction_ppm);
|
||||
}
|
||||
|
||||
utc_correction_nsec_per_overflow = uavcan::int32_t((USecPerOverflow * 1000) * (total_rate_correction_ppm / 1e6F));
|
||||
/*
|
||||
* Rate controller
|
||||
*/
|
||||
float total_rate_correction_ppm = rel_rate_error + utc_rel_rate_error_integral;
|
||||
total_rate_correction_ppm = uavcan::max(total_rate_correction_ppm, -utc_sync_params.max_rate_correction_ppm);
|
||||
total_rate_correction_ppm = uavcan::min(total_rate_correction_ppm, utc_sync_params.max_rate_correction_ppm);
|
||||
|
||||
utc_correction_nsec_per_overflow = uavcan::int32_t((USecPerOverflow * 1000) * (total_rate_correction_ppm / 1e6F));
|
||||
|
||||
// syslog("$ adj=%f rel_rate=%f rel_rate_eint=%f tgt_rel_rate=%f ppm=%f\n",
|
||||
// adj_usec, utc_rel_rate_ppm, utc_rel_rate_error_integral, target_rel_rate_ppm,
|
||||
|
|
@ -266,96 +265,91 @@ static void updateRatePID(uavcan::UtcDuration adjustment)
|
|||
|
||||
void adjustUtc(uavcan::UtcDuration adjustment)
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
UAVCAN_ASSERT(initialized);
|
||||
MutexLocker mlocker(mutex);
|
||||
UAVCAN_ASSERT(initialized);
|
||||
|
||||
if (adjustment.getAbs() > utc_sync_params.min_jump || !utc_set)
|
||||
{
|
||||
const uavcan::int64_t adj_usec = adjustment.toUSec();
|
||||
if (adjustment.getAbs() > utc_sync_params.min_jump || !utc_set) {
|
||||
const uavcan::int64_t adj_usec = adjustment.toUSec();
|
||||
|
||||
{
|
||||
CriticalSectionLocker locker;
|
||||
if ((adj_usec < 0) && uavcan::uint64_t(-adj_usec) > time_utc)
|
||||
{
|
||||
time_utc = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + adj_usec);
|
||||
}
|
||||
}
|
||||
{
|
||||
CriticalSectionLocker locker;
|
||||
|
||||
utc_set = true;
|
||||
utc_locked = false;
|
||||
utc_jump_cnt++;
|
||||
utc_prev_adj = 0;
|
||||
utc_rel_rate_ppm = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
updateRatePID(adjustment);
|
||||
if ((adj_usec < 0) && uavcan::uint64_t(-adj_usec) > time_utc) {
|
||||
time_utc = 1;
|
||||
|
||||
if (!utc_locked)
|
||||
{
|
||||
utc_locked =
|
||||
(std::abs(utc_rel_rate_ppm) < utc_sync_params.lock_thres_rate_ppm) &&
|
||||
(std::abs(utc_prev_adj) < float(utc_sync_params.lock_thres_offset.toUSec()));
|
||||
}
|
||||
}
|
||||
} else {
|
||||
time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + adj_usec);
|
||||
}
|
||||
}
|
||||
|
||||
utc_set = true;
|
||||
utc_locked = false;
|
||||
utc_jump_cnt++;
|
||||
utc_prev_adj = 0;
|
||||
utc_rel_rate_ppm = 0;
|
||||
|
||||
} else {
|
||||
updateRatePID(adjustment);
|
||||
|
||||
if (!utc_locked) {
|
||||
utc_locked =
|
||||
(std::abs(utc_rel_rate_ppm) < utc_sync_params.lock_thres_rate_ppm) &&
|
||||
(std::abs(utc_prev_adj) < float(utc_sync_params.lock_thres_offset.toUSec()));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
float getUtcRateCorrectionPPM()
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
const float rate_correction_mult = float(utc_correction_nsec_per_overflow) / float(USecPerOverflow * 1000);
|
||||
return 1e6F * rate_correction_mult;
|
||||
MutexLocker mlocker(mutex);
|
||||
const float rate_correction_mult = float(utc_correction_nsec_per_overflow) / float(USecPerOverflow * 1000);
|
||||
return 1e6F * rate_correction_mult;
|
||||
}
|
||||
|
||||
uavcan::uint32_t getUtcJumpCount()
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
return utc_jump_cnt;
|
||||
MutexLocker mlocker(mutex);
|
||||
return utc_jump_cnt;
|
||||
}
|
||||
|
||||
bool isUtcLocked()
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
return utc_locked;
|
||||
MutexLocker mlocker(mutex);
|
||||
return utc_locked;
|
||||
}
|
||||
|
||||
UtcSyncParams getUtcSyncParams()
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
return utc_sync_params;
|
||||
MutexLocker mlocker(mutex);
|
||||
return utc_sync_params;
|
||||
}
|
||||
|
||||
void setUtcSyncParams(const UtcSyncParams& params)
|
||||
void setUtcSyncParams(const UtcSyncParams ¶ms)
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
// Add some sanity check
|
||||
utc_sync_params = params;
|
||||
MutexLocker mlocker(mutex);
|
||||
// Add some sanity check
|
||||
utc_sync_params = params;
|
||||
}
|
||||
|
||||
} // namespace clock
|
||||
|
||||
SystemClock& SystemClock::instance()
|
||||
SystemClock &SystemClock::instance()
|
||||
{
|
||||
static union SystemClockStorage
|
||||
{
|
||||
uavcan::uint8_t buffer[sizeof(SystemClock)];
|
||||
long long _aligner_1;
|
||||
long double _aligner_2;
|
||||
} storage;
|
||||
static union SystemClockStorage {
|
||||
uavcan::uint8_t buffer[sizeof(SystemClock)];
|
||||
long long _aligner_1;
|
||||
long double _aligner_2;
|
||||
} storage;
|
||||
|
||||
SystemClock* const ptr = reinterpret_cast<SystemClock*>(storage.buffer);
|
||||
SystemClock *const ptr = reinterpret_cast<SystemClock *>(storage.buffer);
|
||||
|
||||
if (!clock::initialized)
|
||||
{
|
||||
MutexLocker mlocker(clock::mutex);
|
||||
clock::init();
|
||||
new (ptr)SystemClock();
|
||||
}
|
||||
return *ptr;
|
||||
if (!clock::initialized) {
|
||||
MutexLocker mlocker(clock::mutex);
|
||||
clock::init();
|
||||
new (ptr)SystemClock();
|
||||
}
|
||||
|
||||
return *ptr;
|
||||
}
|
||||
|
||||
} // namespace uavcan_stm32
|
||||
|
|
@ -368,43 +362,39 @@ SystemClock& SystemClock::instance()
|
|||
extern "C"
|
||||
UAVCAN_STM32_IRQ_HANDLER(TIMX_IRQHandler)
|
||||
{
|
||||
UAVCAN_STM32_IRQ_PROLOGUE();
|
||||
UAVCAN_STM32_IRQ_PROLOGUE();
|
||||
|
||||
# if UAVCAN_STM32_NUTTX
|
||||
putreg16(0, TMR_REG(STM32_BTIM_SR_OFFSET));
|
||||
putreg16(0, TMR_REG(STM32_BTIM_SR_OFFSET));
|
||||
# endif
|
||||
|
||||
using namespace uavcan_stm32::clock;
|
||||
UAVCAN_ASSERT(initialized);
|
||||
using namespace uavcan_stm32::clock;
|
||||
UAVCAN_ASSERT(initialized);
|
||||
|
||||
time_mono += USecPerOverflow;
|
||||
time_mono += USecPerOverflow;
|
||||
|
||||
if (utc_set)
|
||||
{
|
||||
time_utc += USecPerOverflow;
|
||||
utc_accumulated_correction_nsec += utc_correction_nsec_per_overflow;
|
||||
if (std::abs(utc_accumulated_correction_nsec) >= 1000)
|
||||
{
|
||||
time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + utc_accumulated_correction_nsec / 1000);
|
||||
utc_accumulated_correction_nsec %= 1000;
|
||||
}
|
||||
if (utc_set) {
|
||||
time_utc += USecPerOverflow;
|
||||
utc_accumulated_correction_nsec += utc_correction_nsec_per_overflow;
|
||||
|
||||
// Correction decay - 1 nsec per 65536 usec
|
||||
if (utc_correction_nsec_per_overflow > 0)
|
||||
{
|
||||
utc_correction_nsec_per_overflow--;
|
||||
}
|
||||
else if (utc_correction_nsec_per_overflow < 0)
|
||||
{
|
||||
utc_correction_nsec_per_overflow++;
|
||||
}
|
||||
else
|
||||
{
|
||||
; // Zero
|
||||
}
|
||||
}
|
||||
if (std::abs(utc_accumulated_correction_nsec) >= 1000) {
|
||||
time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + utc_accumulated_correction_nsec / 1000);
|
||||
utc_accumulated_correction_nsec %= 1000;
|
||||
}
|
||||
|
||||
UAVCAN_STM32_IRQ_EPILOGUE();
|
||||
// Correction decay - 1 nsec per 65536 usec
|
||||
if (utc_correction_nsec_per_overflow > 0) {
|
||||
utc_correction_nsec_per_overflow--;
|
||||
|
||||
} else if (utc_correction_nsec_per_overflow < 0) {
|
||||
utc_correction_nsec_per_overflow++;
|
||||
|
||||
} else {
|
||||
; // Zero
|
||||
}
|
||||
}
|
||||
|
||||
UAVCAN_STM32_IRQ_EPILOGUE();
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -13,50 +13,53 @@ namespace uavcan_stm32
|
|||
|
||||
#if UAVCAN_STM32_NUTTX
|
||||
|
||||
BusEvent::BusEvent(CanDriver& can_driver)
|
||||
BusEvent::BusEvent(CanDriver &can_driver)
|
||||
{
|
||||
sem_init(&sem_, 0, 0);
|
||||
sem_setprotocol(&sem_, SEM_PRIO_NONE);
|
||||
sem_init(&sem_, 0, 0);
|
||||
sem_setprotocol(&sem_, SEM_PRIO_NONE);
|
||||
}
|
||||
|
||||
BusEvent::~BusEvent()
|
||||
{
|
||||
sem_destroy(&sem_);
|
||||
sem_destroy(&sem_);
|
||||
}
|
||||
|
||||
bool BusEvent::wait(uavcan::MonotonicDuration duration)
|
||||
{
|
||||
if (duration.isPositive()) {
|
||||
timespec abstime;
|
||||
if (duration.isPositive()) {
|
||||
timespec abstime;
|
||||
|
||||
if (clock_gettime(CLOCK_REALTIME, &abstime) == 0) {
|
||||
const unsigned billion = 1000 * 1000 * 1000;
|
||||
uint64_t nsecs = abstime.tv_nsec + (uint64_t)duration.toUSec() * 1000;
|
||||
abstime.tv_sec += nsecs / billion;
|
||||
nsecs -= (nsecs / billion) * billion;
|
||||
abstime.tv_nsec = nsecs;
|
||||
if (clock_gettime(CLOCK_REALTIME, &abstime) == 0) {
|
||||
const unsigned billion = 1000 * 1000 * 1000;
|
||||
uint64_t nsecs = abstime.tv_nsec + (uint64_t)duration.toUSec() * 1000;
|
||||
abstime.tv_sec += nsecs / billion;
|
||||
nsecs -= (nsecs / billion) * billion;
|
||||
abstime.tv_nsec = nsecs;
|
||||
|
||||
int ret;
|
||||
while ((ret = sem_timedwait(&sem_, &abstime)) == -1 && errno == EINTR);
|
||||
if (ret == -1) { // timed out or error
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
int ret;
|
||||
|
||||
while ((ret = sem_timedwait(&sem_, &abstime)) == -1 && errno == EINTR);
|
||||
|
||||
if (ret == -1) { // timed out or error
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
void BusEvent::signalFromInterrupt()
|
||||
{
|
||||
if (sem_.semcount <= 0)
|
||||
{
|
||||
(void)sem_post(&sem_);
|
||||
}
|
||||
if (signal_cb_)
|
||||
{
|
||||
signal_cb_();
|
||||
}
|
||||
if (sem_.semcount <= 0) {
|
||||
(void)sem_post(&sem_);
|
||||
}
|
||||
|
||||
if (signal_cb_) {
|
||||
signal_cb_();
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
|||
Loading…
Reference in New Issue