forked from Archive/PX4-Autopilot
UAVCAN: Add STM32H7 FDCAN Driver
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.
This commit is contained in:
JacobCrabill
Daniel Agar
parent
08d2226043
commit
62799d9aca
|
|
@ -10,7 +10,7 @@ px4_add_board(
|
|||
BUILD_BOOTLOADER
|
||||
IO px4_io-v2_default
|
||||
TESTING
|
||||
UAVCAN_INTERFACES 2 # - No H7 or FD can support in UAVCAN
|
||||
UAVCAN_INTERFACES 2
|
||||
SERIAL_PORTS
|
||||
# IO DEBUG:/dev/ttyS0
|
||||
TEL1:/dev/ttyS1
|
||||
|
|
@ -55,7 +55,7 @@ px4_add_board(
|
|||
telemetry # all available telemetry drivers
|
||||
test_ppm
|
||||
tone_alarm
|
||||
# uavcan - No H7 or FD can support in UAVCAN yet
|
||||
uavcan
|
||||
MODULES
|
||||
airspeed_selector
|
||||
attitude_estimator_q
|
||||
|
|
|
|||
|
|
@ -192,6 +192,9 @@
|
|||
#define STM32_RCC_D3CCIPR_SPI6SRC RCC_D3CCIPR_SPI6SEL_PLL2 /* SPI6 clock source */
|
||||
#define STM32_RCC_D2CCIP2R_USBSRC RCC_D2CCIP2R_USBSEL_PLL3 /* USB 1 and 2 clock source */
|
||||
#define STM32_RCC_D3CCIPR_ADCSEL RCC_D3CCIPR_ADCSEL_PLL2 /* ADC 1 2 3 clock source */
|
||||
#define STM32_RCC_D2CCIP1R_FDCANSEL RCC_D2CCIP1R_FDCANSEL_HSE /* FDCAN 1 2 clock source */
|
||||
|
||||
#define STM32_FDCANCLK STM32_HSE_FREQUENCY
|
||||
|
||||
/* FLASH wait states */
|
||||
#define BOARD_FLASH_WAITSTATES 2
|
||||
|
|
|
|||
|
|
@ -43,9 +43,12 @@ if(CONFIG_ARCH_CHIP)
|
|||
if(${CONFIG_ARCH_CHIP} MATCHES "kinetis")
|
||||
set(UAVCAN_DRIVER "kinetis")
|
||||
set(UAVCAN_TIMER 1)
|
||||
elseif(${CONFIG_ARCH_CHIP} MATCHES "stm32h7")
|
||||
set(UAVCAN_DRIVER "stm32h7")
|
||||
set(UAVCAN_TIMER 5) # The default timer is TIM5
|
||||
elseif(${CONFIG_ARCH_CHIP} MATCHES "stm32")
|
||||
set(UAVCAN_DRIVER "stm32")
|
||||
set(UAVCAN_TIMER 5) # The default timer the 5
|
||||
set(UAVCAN_TIMER 5) # The default timer is TIM5
|
||||
endif()
|
||||
endif()
|
||||
|
||||
|
|
|
|||
|
|
@ -40,6 +40,8 @@
|
|||
# include <uavcan_kinetis/uavcan_kinetis.hpp>
|
||||
#elif defined(UAVCAN_STM32_NUTTX)
|
||||
# include <uavcan_stm32/uavcan_stm32.hpp>
|
||||
#elif defined(UAVCAN_STM32H7_NUTTX)
|
||||
# include <uavcan_stm32h7/uavcan_stm32h7.hpp>
|
||||
#else
|
||||
# error "Unsupported driver"
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -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 @@
|
|||
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 @@
|
|||
#
|
||||
# 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 @@
|
|||
/*
|
||||
* 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 @@
|
|||
/*
|
||||
* 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 @@
|
|||
/*
|
||||
* 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 @@
|
|||
/*
|
||||
* 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 @@
|
|||
/*
|
||||
* 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 @@
|
|||
/*
|
||||
* 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 @@
|
|||
/*
|
||||
* 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 @@
|
|||
/*
|
||||
* 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 @@
|
|||
/*
|
||||
* 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