Browse Source
Took the existing uavcan_stm32 driver and replaced all bxCAN code with the equivalent for FDCAN following ST Reference Manual RM0433. Note: There is still a bug somewhere in regards to FDCAN2 (probably incorrect setup of the message RAM? Not sure.) But (FD)CAN1 is fully functional (Classic CAN only, no CAN-FD). Also TODO: Configure CAN filters. Right now there are no filters; all incoming messages are accepted.sbg
JacobCrabill
5 years ago
committed by
Daniel Agar
18 changed files with 4053 additions and 3 deletions
@ -0,0 +1,11 @@
@@ -0,0 +1,11 @@
|
||||
STM32H7 platform driver |
||||
===================== |
||||
|
||||
The directory `driver` contains the STM32H7 platform driver for Libuavcan. |
||||
|
||||
A dedicated example application may be added later here. |
||||
For now, please consider the following open source projects as a reference: |
||||
|
||||
- https://github.com/PX4/sapog |
||||
- https://github.com/Zubax/zubax_gnss |
||||
- https://github.com/PX4/Firmware |
||||
@ -0,0 +1,16 @@
@@ -0,0 +1,16 @@
|
||||
include_directories( |
||||
./include |
||||
) |
||||
|
||||
add_library(uavcan_stm32h7_driver STATIC |
||||
./src/uc_stm32h7_can.cpp |
||||
./src/uc_stm32h7_clock.cpp |
||||
./src/uc_stm32h7_thread.cpp |
||||
) |
||||
|
||||
add_dependencies(uavcan_stm32h7_driver uavcan) |
||||
|
||||
install(DIRECTORY include/uavcan_stm32h7 DESTINATION include) |
||||
install(TARGETS uavcan_stm32h7_driver DESTINATION lib) |
||||
|
||||
# vim: set et ft=cmake fenc=utf-8 ff=unix sts=4 sw=4 ts=4 :) |
||||
@ -0,0 +1,9 @@
@@ -0,0 +1,9 @@
|
||||
#
|
||||
# Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
#
|
||||
|
||||
LIBUAVCAN_STM32H7_DIR := $(dir $(lastword $(MAKEFILE_LIST)))
|
||||
|
||||
LIBUAVCAN_STM32H7_SRC := $(shell find $(LIBUAVCAN_STM32H7_DIR)src -type f -name '*.cpp')
|
||||
|
||||
LIBUAVCAN_STM32H7_INC := $(LIBUAVCAN_STM32H7_DIR)include/
|
||||
@ -0,0 +1,28 @@
@@ -0,0 +1,28 @@
|
||||
/*
|
||||
* Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com> |
||||
*/ |
||||
|
||||
#pragma once |
||||
|
||||
/**
|
||||
* OS detection |
||||
*/ |
||||
#ifndef UAVCAN_STM32H7_NUTTX |
||||
# error "Only NuttX is supported" |
||||
#endif |
||||
|
||||
/**
|
||||
* Number of interfaces must be enabled explicitly |
||||
*/ |
||||
#if !defined(UAVCAN_STM32H7_NUM_IFACES) || (UAVCAN_STM32H7_NUM_IFACES != 1 && UAVCAN_STM32H7_NUM_IFACES != 2) |
||||
# error "UAVCAN_STM32H7_NUM_IFACES must be set to either 1 or 2" |
||||
#endif |
||||
|
||||
/**
|
||||
* Any General-Purpose timer (TIM2, TIM3, TIM4, TIM5) |
||||
* e.g. -DUAVCAN_STM32H7_TIMER_NUMBER=2 |
||||
*/ |
||||
#ifndef UAVCAN_STM32H7_TIMER_NUMBER |
||||
// In this case the clock driver should be implemented by the application
|
||||
# define UAVCAN_STM32H7_TIMER_NUMBER 0 |
||||
#endif |
||||
@ -0,0 +1,384 @@
@@ -0,0 +1,384 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com> |
||||
*/ |
||||
|
||||
#pragma once |
||||
|
||||
#include <uavcan_stm32h7/build_config.hpp> |
||||
#include <uavcan_stm32h7/thread.hpp> |
||||
#include <uavcan/driver/can.hpp> |
||||
#include <uavcan_stm32h7/fdcan.hpp> |
||||
|
||||
namespace uavcan_stm32h7 |
||||
{ |
||||
/**
|
||||
* Driver error codes. |
||||
* These values can be returned from driver functions negated. |
||||
*/ |
||||
//static const uavcan::int16_t ErrUnknown = 1000; ///< Reserved for future use
|
||||
static const uavcan::int16_t ErrNotImplemented = 1001; ///< Feature not implemented
|
||||
static const uavcan::int16_t ErrInvalidBitRate = 1002; ///< Bit rate not supported
|
||||
static const uavcan::int16_t ErrLogic = 1003; ///< Internal logic error
|
||||
static const uavcan::int16_t ErrUnsupportedFrame = 1004; ///< Frame not supported (e.g. RTR, CAN FD, etc)
|
||||
static const uavcan::int16_t ErrMsrInakNotSet = 1005; ///< INAK bit of the MSR register is not 1
|
||||
static const uavcan::int16_t ErrMsrInakNotCleared = 1006; ///< INAK bit of the MSR register is not 0
|
||||
static const uavcan::int16_t ErrBitRateNotDetected = 1007; ///< Auto bit rate detection could not be finished
|
||||
static const uavcan::int16_t ErrFilterNumConfigs = 1008; ///< Number of filters is more than supported
|
||||
|
||||
/**
|
||||
* 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) |
||||
{ } |
||||
}; |
||||
|
||||
/**
|
||||
* Single CAN iface. |
||||
* The application shall not use this directly. |
||||
*/ |
||||
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_; |
||||
|
||||
void registerOverflow(); |
||||
|
||||
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 reset(); |
||||
|
||||
unsigned getLength() const { return len_; } |
||||
|
||||
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; |
||||
|
||||
Timings() |
||||
: prescaler(0) |
||||
, sjw(0) |
||||
, bs1(0) |
||||
, bs2(0) |
||||
{ } |
||||
}; |
||||
|
||||
struct TxItem { |
||||
uavcan::MonotonicTime deadline; |
||||
uavcan::CanFrame frame; |
||||
uavcan::uint8_t index; |
||||
//bool pending;
|
||||
bool loopback; |
||||
bool abort_on_error; |
||||
|
||||
TxItem() |
||||
: /*pending(false)
|
||||
,*/ loopback(false) |
||||
, abort_on_error(false) |
||||
{ } |
||||
}; |
||||
|
||||
struct MessageRam { |
||||
uavcan::uint32_t StdIdFilterSA; |
||||
uavcan::uint32_t ExtIdFilterSA; |
||||
uavcan::uint32_t RxFIFO0SA; |
||||
uavcan::uint32_t RxFIFO1SA; |
||||
uavcan::uint32_t TxQueueSA; |
||||
} message_ram_; |
||||
|
||||
enum { NumTxMailboxes = 32 }; // Should match the number of Tx FIFOs available in message RAM
|
||||
enum { NumFilters = 14 }; |
||||
|
||||
RxQueue rx_queue_; |
||||
fdcan::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); |
||||
|
||||
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 configureFilters(const uavcan::CanFilterConfig *filter_configs, |
||||
uavcan::uint16_t num_configs); |
||||
|
||||
virtual uavcan::uint16_t getNumFilters() const { return NumFilters; } |
||||
|
||||
public: |
||||
enum { MaxRxQueueCapacity = 64 }; |
||||
|
||||
enum OperatingMode { |
||||
NormalMode, |
||||
SilentMode |
||||
}; |
||||
|
||||
CanIface(fdcan::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_STM32H7_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); |
||||
|
||||
void handleTxInterrupt(uavcan::uint64_t utc_usec); |
||||
void handleRxInterrupt(uavcan::uint8_t fifo_index); |
||||
|
||||
/**
|
||||
* 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); |
||||
|
||||
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(); } |
||||
|
||||
/**
|
||||
* 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_; } |
||||
|
||||
/**
|
||||
* 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(); |
||||
|
||||
/**
|
||||
* Peak number of TX mailboxes used concurrently since initialization. |
||||
* Range is [1, NumTxMailboxes]. |
||||
* Value at max suggests that priority inversion could be taking place. |
||||
*/ |
||||
uavcan::uint8_t getPeakNumTxMailboxesUsed() const { return uavcan::uint8_t(peak_tx_mailbox_index_ + 1); } |
||||
}; |
||||
|
||||
/**
|
||||
* CAN driver, incorporates all available CAN ifaces. |
||||
* Please avoid direct use, prefer @ref CanInitHelper instead. |
||||
*/ |
||||
class CanDriver : public uavcan::ICanDriver, uavcan::Noncopyable |
||||
{ |
||||
BusEvent update_event_; |
||||
CanIface if0_; |
||||
#if UAVCAN_STM32H7_NUM_IFACES > 1 |
||||
CanIface if1_; |
||||
#endif |
||||
uint8_t num_ifaces_; |
||||
uint32_t enabledInterfaces_; |
||||
|
||||
virtual uavcan::int16_t select(uavcan::CanSelectMasks &inout_masks, |
||||
const uavcan::CanFrame * (& pending_tx)[uavcan::MaxCanIfaces], |
||||
uavcan::MonotonicTime blocking_deadline); |
||||
|
||||
static void initOnce(); |
||||
|
||||
public: |
||||
template <unsigned RxQueueCapacity> |
||||
CanDriver(CanRxItem(&rx_queue_storage)[UAVCAN_STM32H7_NUM_IFACES][RxQueueCapacity]) |
||||
: update_event_(*this) |
||||
, if0_(fdcan::Can[0], update_event_, 0, rx_queue_storage[0], RxQueueCapacity) |
||||
#if UAVCAN_STM32H7_NUM_IFACES > 1 |
||||
, if1_(fdcan::Can[1], update_event_, 1, rx_queue_storage[1], RxQueueCapacity) |
||||
, num_ifaces_(2) |
||||
#else |
||||
, num_ifaces_(1) |
||||
#endif |
||||
, enabledInterfaces_(0x7) |
||||
{ |
||||
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; |
||||
|
||||
/**
|
||||
* 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, const uavcan::uint32_t EnabledInterfaces); |
||||
|
||||
virtual CanIface *getIface(uavcan::uint8_t iface_index); |
||||
|
||||
virtual uavcan::uint8_t getNumIfaces() const { return UAVCAN_STM32H7_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(); |
||||
|
||||
BusEvent &updateEvent() { return update_event_; } |
||||
}; |
||||
|
||||
/**
|
||||
* Helper class. |
||||
* Normally only this class should be used by the application. |
||||
* 145 usec per Extended CAN frame @ 1 Mbps, e.g. 32 RX slots * 145 usec --> 4.6 msec before RX queue overruns. |
||||
*/ |
||||
template <unsigned RxQueueCapacity = 128> |
||||
class CanInitHelper |
||||
{ |
||||
CanRxItem queue_storage_[UAVCAN_STM32H7_NUM_IFACES][RxQueueCapacity]; |
||||
|
||||
public: |
||||
enum { BitRateAutoDetect = 0 }; |
||||
|
||||
CanDriver driver; |
||||
uint32_t enabledInterfaces_; |
||||
|
||||
CanInitHelper(const uavcan::uint32_t EnabledInterfaces = 0x7) : |
||||
driver(queue_storage_), |
||||
enabledInterfaces_(EnabledInterfaces) |
||||
{ } |
||||
|
||||
/**
|
||||
* 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, enabledInterfaces_); |
||||
} |
||||
|
||||
/**
|
||||
* 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, enabledInterfaces_); |
||||
|
||||
} else { |
||||
static const uavcan::uint32_t StandardBitRates[] = { |
||||
1000000, |
||||
500000, |
||||
250000, |
||||
125000 |
||||
}; |
||||
|
||||
for (uavcan::uint8_t br = 0; br < sizeof(StandardBitRates) / sizeof(StandardBitRates[0]); br++) { |
||||
inout_bitrate = StandardBitRates[br]; |
||||
|
||||
const int res = driver.init(inout_bitrate, CanIface::SilentMode, enabledInterfaces_); |
||||
|
||||
delay_callable(); |
||||
|
||||
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, enabledInterfaces_); |
||||
} |
||||
} |
||||
} |
||||
} |
||||
|
||||
return -ErrBitRateNotDetected; |
||||
} |
||||
} |
||||
|
||||
/**
|
||||
* Use this value for listening delay during automatic bit rate detection. |
||||
*/ |
||||
static uavcan::MonotonicDuration getRecommendedListeningDelay() |
||||
{ |
||||
return uavcan::MonotonicDuration::fromMSec(1050); |
||||
} |
||||
}; |
||||
|
||||
} |
||||
@ -0,0 +1,120 @@
@@ -0,0 +1,120 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com> |
||||
*/ |
||||
|
||||
#pragma once |
||||
|
||||
#include <uavcan_stm32h7/build_config.hpp> |
||||
#include <uavcan/driver/system_clock.hpp> |
||||
|
||||
namespace uavcan_stm32h7 |
||||
{ |
||||
|
||||
namespace clock |
||||
{ |
||||
/**
|
||||
* Starts the clock. |
||||
* Can be called multiple times, only the first call will be effective. |
||||
*/ |
||||
void init(); |
||||
|
||||
/**
|
||||
* Returns current monotonic time since the moment when clock::init() was called. |
||||
* This function is thread safe. |
||||
*/ |
||||
uavcan::MonotonicTime getMonotonic(); |
||||
|
||||
/**
|
||||
* Sets the driver's notion of the system UTC. It should be called |
||||
* at startup and any time the system clock is updated from an |
||||
* external source that is not the UAVCAN Timesync master. |
||||
* This function is thread safe. |
||||
*/ |
||||
void setUtc(uavcan::UtcTime time); |
||||
|
||||
/**
|
||||
* Returns UTC time if it has been set, otherwise returns zero time. |
||||
* This function is thread safe. |
||||
*/ |
||||
uavcan::UtcTime getUtc(); |
||||
|
||||
/**
|
||||
* Performs UTC phase and frequency adjustment. |
||||
* The UTC time will be zero until first adjustment has been performed. |
||||
* This function is thread safe. |
||||
*/ |
||||
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
|
||||
|
||||
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)) |
||||
{ } |
||||
}; |
||||
|
||||
/**
|
||||
* Clock rate error. |
||||
* Positive if the hardware timer is slower than reference time. |
||||
* This function is thread safe. |
||||
*/ |
||||
float getUtcRateCorrectionPPM(); |
||||
|
||||
/**
|
||||
* Number of non-gradual adjustments performed so far. |
||||
* Ideally should be zero. |
||||
* This function is thread safe. |
||||
*/ |
||||
uavcan::uint32_t getUtcJumpCount(); |
||||
|
||||
/**
|
||||
* Whether UTC is synchronized and locked. |
||||
* This function is thread safe. |
||||
*/ |
||||
bool isUtcLocked(); |
||||
|
||||
/**
|
||||
* UTC sync params get/set. |
||||
* Both functions are thread safe. |
||||
*/ |
||||
UtcSyncParams getUtcSyncParams(); |
||||
void setUtcSyncParams(const UtcSyncParams ¶ms); |
||||
|
||||
} |
||||
|
||||
/**
|
||||
* Adapter for uavcan::ISystemClock. |
||||
*/ |
||||
class SystemClock : public uavcan::ISystemClock, uavcan::Noncopyable |
||||
{ |
||||
SystemClock() { } |
||||
|
||||
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(); } |
||||
|
||||
/**
|
||||
* Calls clock::init() as needed. |
||||
* This function is thread safe. |
||||
*/ |
||||
static SystemClock &instance(); |
||||
}; |
||||
|
||||
} |
||||
@ -0,0 +1,65 @@
@@ -0,0 +1,65 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com> |
||||
* Bit definitions were copied from NuttX STM32 CAN driver. |
||||
*/ |
||||
|
||||
#pragma once |
||||
|
||||
#include <uavcan_stm32h7/build_config.hpp> |
||||
|
||||
#include <uavcan/uavcan.hpp> |
||||
#include <stdint.h> |
||||
|
||||
#ifndef UAVCAN_CPP_VERSION |
||||
# error UAVCAN_CPP_VERSION |
||||
#endif |
||||
|
||||
#if UAVCAN_CPP_VERSION < UAVCAN_CPP11 |
||||
// #undef'ed at the end of this file
|
||||
# define constexpr const |
||||
#endif |
||||
|
||||
namespace uavcan_stm32h7 |
||||
{ |
||||
namespace fdcan |
||||
{ |
||||
#ifdef CONFIG_ARCH_CHIP_STM32H743ZI |
||||
#include "fdcan_h743xx.h" |
||||
#else |
||||
# error "Unsupported STM32H7 MCU" |
||||
#endif |
||||
|
||||
typedef FDCAN_GlobalTypeDef CanType; |
||||
|
||||
constexpr unsigned long IDE = (0x40000000U); // Identifier Extension
|
||||
constexpr unsigned long STID_MASK = (0x1FFC0000U); // Standard Identifier Mask
|
||||
constexpr unsigned long EXID_MASK = (0x1FFFFFFFU); // Extended Identifier Mask
|
||||
constexpr unsigned long RTR = (0x20000000U); // Remote Transmission Request
|
||||
constexpr unsigned long ESI = (0x80000000U); // Error Frame
|
||||
constexpr unsigned long DLC_MASK = (0x000F0000U); // Data Length Code
|
||||
constexpr uint32_t T0_STID_Pos = 18; |
||||
constexpr uint32_t T0_RTR_Pos = 29; |
||||
constexpr uint32_t T0_XTD_Pos = 30; |
||||
constexpr uint32_t T0_ESI_Pos = 31; |
||||
constexpr uint32_t T1_DLC_Pos = 16; |
||||
constexpr uint32_t T1_BRS_Pos = 20; |
||||
constexpr uint32_t T1_FDF_Pos = 21; |
||||
constexpr uint32_t T1_EFC_Pos = 23; |
||||
constexpr uint32_t T1_MM_Pos = 24; |
||||
|
||||
/**
|
||||
* CANx register sets |
||||
*/ |
||||
CanType *const Can[UAVCAN_STM32H7_NUM_IFACES] = { |
||||
reinterpret_cast<CanType *>(FDCAN1_BASE) |
||||
#if UAVCAN_STM32H7_NUM_IFACES > 1 |
||||
, |
||||
reinterpret_cast<CanType *>(FDCAN2_BASE) |
||||
#endif |
||||
}; |
||||
} |
||||
} |
||||
|
||||
#if UAVCAN_CPP_VERSION < UAVCAN_CPP11 |
||||
# undef constexpr |
||||
#endif |
||||
File diff suppressed because it is too large
Load Diff
@ -0,0 +1,100 @@
@@ -0,0 +1,100 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com> |
||||
*/ |
||||
|
||||
#pragma once |
||||
|
||||
#include <uavcan_stm32h7/build_config.hpp> |
||||
|
||||
#if UAVCAN_STM32H7_NUTTX |
||||
# include <nuttx/config.h> |
||||
# include <nuttx/fs/fs.h> |
||||
# include <poll.h> |
||||
# include <errno.h> |
||||
# include <cstdio> |
||||
# include <ctime> |
||||
# include <cstring> |
||||
#else |
||||
# error "Unknown OS" |
||||
#endif |
||||
|
||||
#include <uavcan/uavcan.hpp> |
||||
|
||||
namespace uavcan_stm32h7 |
||||
{ |
||||
|
||||
class CanDriver; |
||||
|
||||
#if UAVCAN_STM32H7_NUTTX |
||||
|
||||
/**
|
||||
* All bus events are reported as POLLIN. |
||||
*/ |
||||
class BusEvent : uavcan::Noncopyable |
||||
{ |
||||
using SignalCallbackHandler = void(*)(); |
||||
|
||||
SignalCallbackHandler signal_cb_{nullptr}; |
||||
sem_t sem_; |
||||
|
||||
public: |
||||
BusEvent(CanDriver &can_driver); |
||||
~BusEvent(); |
||||
|
||||
void registerSignalCallback(SignalCallbackHandler handler) { signal_cb_ = handler; } |
||||
|
||||
bool wait(uavcan::MonotonicDuration duration); |
||||
|
||||
void signalFromInterrupt(); |
||||
}; |
||||
|
||||
class Mutex |
||||
{ |
||||
pthread_mutex_t mutex_; |
||||
|
||||
public: |
||||
Mutex() |
||||
{ |
||||
init(); |
||||
} |
||||
|
||||
int init() |
||||
{ |
||||
return pthread_mutex_init(&mutex_, UAVCAN_NULLPTR); |
||||
} |
||||
|
||||
int deinit() |
||||
{ |
||||
return pthread_mutex_destroy(&mutex_); |
||||
} |
||||
|
||||
void lock() |
||||
{ |
||||
(void)pthread_mutex_lock(&mutex_); |
||||
} |
||||
|
||||
void unlock() |
||||
{ |
||||
(void)pthread_mutex_unlock(&mutex_); |
||||
} |
||||
}; |
||||
#endif |
||||
|
||||
|
||||
class MutexLocker |
||||
{ |
||||
Mutex &mutex_; |
||||
|
||||
public: |
||||
MutexLocker(Mutex &mutex) |
||||
: mutex_(mutex) |
||||
{ |
||||
mutex_.lock(); |
||||
} |
||||
~MutexLocker() |
||||
{ |
||||
mutex_.unlock(); |
||||
} |
||||
}; |
||||
|
||||
} |
||||
@ -0,0 +1,11 @@
@@ -0,0 +1,11 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com> |
||||
*/ |
||||
|
||||
#pragma once |
||||
|
||||
#include <uavcan/uavcan.hpp> |
||||
|
||||
#include <uavcan_stm32h7/thread.hpp> |
||||
#include <uavcan_stm32h7/clock.hpp> |
||||
#include <uavcan_stm32h7/can.hpp> |
||||
@ -0,0 +1,73 @@
@@ -0,0 +1,73 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com> |
||||
*/ |
||||
|
||||
#pragma once |
||||
|
||||
#include <uavcan_stm32h7/build_config.hpp> |
||||
#include "board_config.h" |
||||
|
||||
#if UAVCAN_STM32H7_NUTTX |
||||
# include <nuttx/arch.h> |
||||
# include <arch/board/board.h> |
||||
# include <hardware/stm32_tim.h> |
||||
# include <syslog.h> |
||||
#else |
||||
# error "Unknown OS" |
||||
#endif |
||||
|
||||
/**
|
||||
* Debug output |
||||
*/ |
||||
#ifndef UAVCAN_STM32H7_LOG |
||||
// syslog() crashes the system in this context
|
||||
// # if UAVCAN_STM32H7_NUTTX && CONFIG_ARCH_LOWPUTC
|
||||
# if 1 |
||||
# define UAVCAN_STM32H7_LOG(fmt, ...) printf("uavcan_stm32: \n" fmt "\n", ##__VA_ARGS__) |
||||
# else |
||||
# define UAVCAN_STM32H7_LOG(...) ((void)0) |
||||
# endif |
||||
#endif |
||||
|
||||
/**
|
||||
* IRQ handler macros |
||||
*/ |
||||
#if UAVCAN_STM32H7_NUTTX |
||||
# define UAVCAN_STM32H7_IRQ_HANDLER(id) int id(int irq, FAR void* context, FAR void *arg) |
||||
# define UAVCAN_STM32H7_IRQ_PROLOGUE() |
||||
# define UAVCAN_STM32H7_IRQ_EPILOGUE() return 0; |
||||
#endif |
||||
|
||||
/**
|
||||
* Glue macros |
||||
*/ |
||||
#define UAVCAN_STM32H7_GLUE2_(A, B) A##B |
||||
#define UAVCAN_STM32H7_GLUE2(A, B) UAVCAN_STM32H7_GLUE2_(A, B) |
||||
|
||||
#define UAVCAN_STM32H7_GLUE3_(A, B, C) A##B##C |
||||
#define UAVCAN_STM32H7_GLUE3(A, B, C) UAVCAN_STM32H7_GLUE3_(A, B, C) |
||||
|
||||
namespace uavcan_stm32h7 |
||||
{ |
||||
#if UAVCAN_STM32H7_NUTTX |
||||
|
||||
struct CriticalSectionLocker { |
||||
const irqstate_t flags_; |
||||
|
||||
CriticalSectionLocker() |
||||
: flags_(enter_critical_section()) |
||||
{ } |
||||
|
||||
~CriticalSectionLocker() |
||||
{ |
||||
leave_critical_section(flags_); |
||||
} |
||||
}; |
||||
|
||||
#endif |
||||
|
||||
namespace clock |
||||
{ |
||||
uavcan::uint64_t getUtcUSecFromCanInterrupt(); |
||||
} |
||||
} |
||||
File diff suppressed because it is too large
Load Diff
@ -0,0 +1,400 @@
@@ -0,0 +1,400 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com> |
||||
*/ |
||||
|
||||
#include <uavcan_stm32h7/clock.hpp> |
||||
#include <uavcan_stm32h7/thread.hpp> |
||||
#include "internal.hpp" |
||||
|
||||
#if UAVCAN_STM32H7_TIMER_NUMBER |
||||
|
||||
#include <cassert> |
||||
#include <math.h> |
||||
|
||||
/*
|
||||
* Timer instance |
||||
*/ |
||||
# if UAVCAN_STM32H7_NUTTX |
||||
# define TIMX UAVCAN_STM32H7_GLUE3(STM32_TIM, UAVCAN_STM32H7_TIMER_NUMBER, _BASE) |
||||
# define TMR_REG(o) (TIMX + (o)) |
||||
# define TIMX_INPUT_CLOCK UAVCAN_STM32H7_GLUE3(STM32_APB1_TIM, UAVCAN_STM32H7_TIMER_NUMBER, _CLKIN) |
||||
|
||||
# define TIMX_IRQn UAVCAN_STM32H7_GLUE2(STM32_IRQ_TIM, UAVCAN_STM32H7_TIMER_NUMBER) |
||||
# endif |
||||
|
||||
# if UAVCAN_STM32H7_TIMER_NUMBER >= 2 && UAVCAN_STM32H7_TIMER_NUMBER <= 7 |
||||
# define TIMX_RCC_ENR RCC->APB1ENR |
||||
# define TIMX_RCC_RSTR RCC->APB1RSTR |
||||
# define TIMX_RCC_ENR_MASK UAVCAN_STM32H7_GLUE3(RCC_APB1ENR_TIM, UAVCAN_STM32H7_TIMER_NUMBER, EN) |
||||
# define TIMX_RCC_RSTR_MASK UAVCAN_STM32H7_GLUE3(RCC_APB1RSTR_TIM, UAVCAN_STM32H7_TIMER_NUMBER, RST) |
||||
# else |
||||
# error "This UAVCAN_STM32H7_TIMER_NUMBER is not supported yet" |
||||
# endif |
||||
|
||||
/**
|
||||
* UAVCAN_STM32H7_TIMX_INPUT_CLOCK can be used to manually override the auto-detected timer clock speed. |
||||
* This is useful at least with certain versions of ChibiOS which do not support the bit |
||||
* RCC_DKCFGR.TIMPRE that is available in newer models of STM32. In that case, if TIMPRE is active, |
||||
* the auto-detected value of TIMX_INPUT_CLOCK will be twice lower than the actual clock speed. |
||||
* Read this for additional context: http://www.chibios.com/forum/viewtopic.php?f=35&t=3870
|
||||
* A normal way to use the override feature is to provide an alternative macro, e.g.: |
||||
* |
||||
* -DUAVCAN_STM32H7_TIMX_INPUT_CLOCK=STM32_HCLK |
||||
* |
||||
* Alternatively, the new clock rate can be specified directly. |
||||
*/ |
||||
# ifdef UAVCAN_STM32H7_TIMX_INPUT_CLOCK |
||||
# undef TIMX_INPUT_CLOCK |
||||
# define TIMX_INPUT_CLOCK UAVCAN_STM32H7_TIMX_INPUT_CLOCK |
||||
# endif |
||||
|
||||
extern "C" UAVCAN_STM32H7_IRQ_HANDLER(TIMX_IRQHandler); |
||||
|
||||
namespace uavcan_stm32h7 |
||||
{ |
||||
namespace clock |
||||
{ |
||||
namespace |
||||
{ |
||||
|
||||
const uavcan::uint32_t USecPerOverflow = 65536; |
||||
|
||||
Mutex mutex; |
||||
|
||||
bool initialized = false; |
||||
|
||||
bool utc_set = false; |
||||
bool utc_locked = false; |
||||
uavcan::uint32_t utc_jump_cnt = 0; |
||||
UtcSyncParams utc_sync_params; |
||||
float utc_prev_adj = 0; |
||||
float utc_rel_rate_ppm = 0; |
||||
float utc_rel_rate_error_integral = 0; |
||||
uavcan::int32_t utc_accumulated_correction_nsec = 0; |
||||
uavcan::int32_t utc_correction_nsec_per_overflow = 0; |
||||
uavcan::MonotonicTime prev_utc_adj_at; |
||||
|
||||
uavcan::uint64_t time_mono = 0; |
||||
uavcan::uint64_t time_utc = 0; |
||||
|
||||
} |
||||
|
||||
|
||||
void init() |
||||
{ |
||||
CriticalSectionLocker lock; |
||||
|
||||
if (initialized) { |
||||
return; |
||||
} |
||||
|
||||
initialized = true; |
||||
|
||||
|
||||
# if UAVCAN_STM32H7_NUTTX |
||||
|
||||
// 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); |
||||
|
||||
|
||||
// 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
|
||||
// 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); |
||||
|
||||
# endif |
||||
} |
||||
|
||||
void setUtc(uavcan::UtcTime time) |
||||
{ |
||||
MutexLocker mlocker(mutex); |
||||
UAVCAN_ASSERT(initialized); |
||||
|
||||
{ |
||||
CriticalSectionLocker locker; |
||||
time_utc = time.toUSec(); |
||||
} |
||||
|
||||
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_STM32H7_NUTTX |
||||
|
||||
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)); |
||||
|
||||
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; |
||||
} |
||||
|
||||
uavcan::MonotonicTime getMonotonic() |
||||
{ |
||||
uavcan::uint64_t usec = 0; |
||||
// Scope Critical section
|
||||
{ |
||||
CriticalSectionLocker locker; |
||||
|
||||
volatile uavcan::uint64_t time = time_mono; |
||||
|
||||
# if UAVCAN_STM32H7_NUTTX |
||||
|
||||
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)); |
||||
# endif |
||||
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; |
||||
# endif |
||||
} // 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(); |
||||
} |
||||
|
||||
static float lowpass(float xold, float xnew, float corner, float dt) |
||||
{ |
||||
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()); |
||||
|
||||
/*
|
||||
* 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); |
||||
|
||||
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); |
||||
} |
||||
|
||||
/*
|
||||
* 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,
|
||||
// total_rate_correction_ppm);
|
||||
} |
||||
|
||||
void adjustUtc(uavcan::UtcDuration adjustment) |
||||
{ |
||||
MutexLocker mlocker(mutex); |
||||
UAVCAN_ASSERT(initialized); |
||||
|
||||
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); |
||||
} |
||||
} |
||||
|
||||
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; |
||||
} |
||||
|
||||
uavcan::uint32_t getUtcJumpCount() |
||||
{ |
||||
MutexLocker mlocker(mutex); |
||||
return utc_jump_cnt; |
||||
} |
||||
|
||||
bool isUtcLocked() |
||||
{ |
||||
MutexLocker mlocker(mutex); |
||||
return utc_locked; |
||||
} |
||||
|
||||
UtcSyncParams getUtcSyncParams() |
||||
{ |
||||
MutexLocker mlocker(mutex); |
||||
return utc_sync_params; |
||||
} |
||||
|
||||
void setUtcSyncParams(const UtcSyncParams ¶ms) |
||||
{ |
||||
MutexLocker mlocker(mutex); |
||||
// Add some sanity check
|
||||
utc_sync_params = params; |
||||
} |
||||
|
||||
} // namespace clock
|
||||
|
||||
SystemClock &SystemClock::instance() |
||||
{ |
||||
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); |
||||
|
||||
if (!clock::initialized) { |
||||
MutexLocker mlocker(clock::mutex); |
||||
clock::init(); |
||||
new (ptr)SystemClock(); |
||||
} |
||||
|
||||
return *ptr; |
||||
} |
||||
|
||||
} // namespace uavcan_stm32h7
|
||||
|
||||
|
||||
/**
|
||||
* Timer interrupt handler |
||||
*/ |
||||
|
||||
extern "C" |
||||
UAVCAN_STM32H7_IRQ_HANDLER(TIMX_IRQHandler) |
||||
{ |
||||
UAVCAN_STM32H7_IRQ_PROLOGUE(); |
||||
|
||||
# if UAVCAN_STM32H7_NUTTX |
||||
putreg16(0, TMR_REG(STM32_BTIM_SR_OFFSET)); |
||||
# endif |
||||
|
||||
using namespace uavcan_stm32h7::clock; |
||||
UAVCAN_ASSERT(initialized); |
||||
|
||||
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; |
||||
} |
||||
|
||||
// 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_STM32H7_IRQ_EPILOGUE(); |
||||
} |
||||
|
||||
#endif |
||||
@ -0,0 +1,67 @@
@@ -0,0 +1,67 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com> |
||||
*/ |
||||
|
||||
#include <uavcan_stm32h7/thread.hpp> |
||||
#include <uavcan_stm32h7/clock.hpp> |
||||
#include <uavcan_stm32h7/can.hpp> |
||||
#include "internal.hpp" |
||||
|
||||
|
||||
namespace uavcan_stm32h7 |
||||
{ |
||||
|
||||
#if UAVCAN_STM32H7_NUTTX |
||||
|
||||
BusEvent::BusEvent(CanDriver &can_driver) |
||||
{ |
||||
sem_init(&sem_, 0, 0); |
||||
sem_setprotocol(&sem_, SEM_PRIO_NONE); |
||||
} |
||||
|
||||
BusEvent::~BusEvent() |
||||
{ |
||||
sem_destroy(&sem_); |
||||
} |
||||
|
||||
bool BusEvent::wait(uavcan::MonotonicDuration duration) |
||||
{ |
||||
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; |
||||
|
||||
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_(); |
||||
} |
||||
} |
||||
|
||||
#endif |
||||
|
||||
} |
||||
Loading…
Reference in new issue