dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

[WIP] use HRT for UAVCAN monotonic time and introduce HRT time sync

This commit is contained in:
Daniel Agar 2024-03-13 18:04:57 -04:00
parent ad50afda10
commit 0d3daf3efd
30 changed files with 161 additions and 1160 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
boards/ark/fmu-v6x/default.px4board
View File

@ -33,7 +33,6 @@ CONFIG_DRIVERS_RC_INPUT=y
CONFIG_DRIVERS_SAFETY_BUTTON=y
CONFIG_DRIVERS_TONE_ALARM=y
CONFIG_DRIVERS_UAVCAN=y
CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
CONFIG_MODULES_AIRSPEED_SELECTOR=y
CONFIG_MODULES_CAMERA_FEEDBACK=y
CONFIG_MODULES_COMMANDER=y

1
boards/cuav/nora/default.px4board
View File

@ -35,7 +35,6 @@ CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
CONFIG_COMMON_TELEMETRY=y
CONFIG_DRIVERS_TONE_ALARM=y
CONFIG_DRIVERS_UAVCAN=y
CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
CONFIG_MODULES_AIRSPEED_SELECTOR=y
CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
CONFIG_MODULES_BATTERY_STATUS=y

1
boards/cuav/x7pro/default.px4board
View File

@ -36,7 +36,6 @@ CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
CONFIG_COMMON_TELEMETRY=y
CONFIG_DRIVERS_TONE_ALARM=y
CONFIG_DRIVERS_UAVCAN=y
CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
CONFIG_MODULES_AIRSPEED_SELECTOR=y
CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
CONFIG_MODULES_BATTERY_STATUS=y

1
boards/cuav/x7pro/test.px4board
View File

@ -1,4 +1,3 @@
# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
CONFIG_DRIVERS_ADC_ADS1115=n
CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n
CONFIG_DRIVERS_IRLOCK=n

1
boards/px4/fmu-v5/cyphal.px4board
View File

@ -1,4 +1,3 @@
# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
CONFIG_DRIVERS_HEATER=n
CONFIG_DRIVERS_UAVCAN=n
CONFIG_CYPHAL_BMS_SUBSCRIBER=y

1
boards/px4/fmu-v5/debug.px4board
View File

@ -1,4 +1,3 @@
# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
CONFIG_COMMON_BAROMETERS=n
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n
CONFIG_COMMON_MAGNETOMETER=n

1
boards/px4/fmu-v5/default.px4board
View File

@ -40,7 +40,6 @@ CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
CONFIG_COMMON_TELEMETRY=y
CONFIG_DRIVERS_TONE_ALARM=y
CONFIG_DRIVERS_UAVCAN=y
CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=6
CONFIG_MODULES_AIRSPEED_SELECTOR=y
CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
CONFIG_MODULES_BATTERY_STATUS=y

1
boards/px4/fmu-v5/protected.px4board
View File

@ -1,4 +1,3 @@
# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
CONFIG_COMMON_BAROMETERS=n
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n
CONFIG_COMMON_DISTANCE_SENSOR=n

1
boards/px4/fmu-v5/stackcheck.px4board
View File

@ -1,4 +1,3 @@
# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
CONFIG_COMMON_BAROMETERS=n
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n
CONFIG_COMMON_DISTANCE_SENSOR=n

1
boards/px4/fmu-v5/test.px4board
View File

@ -1,4 +1,3 @@
# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
CONFIG_COMMON_HYGROMETERS=n
CONFIG_COMMON_OSD=n
CONFIG_DRIVERS_ADC_ADS1115=n

1
boards/px4/fmu-v5/uavcanv0periph.px4board
View File

@ -1,4 +1,3 @@
# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
CONFIG_COMMON_BAROMETERS=n
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n
CONFIG_COMMON_HYGROMETERS=n

1
boards/px4/fmu-v6c/default.px4board
View File

@ -30,7 +30,6 @@ CONFIG_DRIVERS_PX4IO=y
CONFIG_COMMON_TELEMETRY=y
CONFIG_DRIVERS_TONE_ALARM=y
CONFIG_DRIVERS_UAVCAN=y
CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
CONFIG_MODULES_AIRSPEED_SELECTOR=y
CONFIG_MODULES_BATTERY_STATUS=y
CONFIG_MODULES_CAMERA_FEEDBACK=y

1
boards/px4/fmu-v6x/default.px4board
View File

@ -42,7 +42,6 @@ CONFIG_DRIVERS_RC_INPUT=y
CONFIG_DRIVERS_SAFETY_BUTTON=y
CONFIG_DRIVERS_TONE_ALARM=y
CONFIG_DRIVERS_UAVCAN=y
CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
CONFIG_MODULES_AIRSPEED_SELECTOR=y
CONFIG_MODULES_BATTERY_STATUS=y
CONFIG_MODULES_CAMERA_FEEDBACK=y

1
boards/px4/fmu-v6x/zenoh.px4board
View File

@ -1,4 +1,3 @@
# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
CONFIG_DRIVERS_UAVCAN=n
CONFIG_MODULES_UXRCE_DDS_CLIENT=n
CONFIG_MODULES_ZENOH=y

4
cmake/kconfig.cmake
View File

@ -333,10 +333,6 @@ if(EXISTS ${BOARD_DEFCONFIG})
set(config_uavcan_num_ifaces ${UAVCAN_INTERFACES} CACHE INTERNAL "UAVCAN interfaces" FORCE)
endif()
if(UAVCAN_TIMER_OVERRIDE)
set(config_uavcan_timer_override ${UAVCAN_TIMER_OVERRIDE} CACHE INTERNAL "UAVCAN TIMER OVERRIDE" FORCE)
endif()
# OPTIONS
if(CONSTRAINED_FLASH)

131
platforms/nuttx/src/px4/stm/stm32_common/hrt/hrt.c
View File

@ -244,6 +244,8 @@
# error HRT_TIMER_CHANNEL must be a value between 1 and 4
#endif
static volatile hrt_abstime base_time;
/*
* Queue of callout entries.
*/
@ -276,6 +278,112 @@ static void hrt_call_reschedule(void);
static void hrt_call_invoke(void);
#define HRT_TIME_SYNC
#if defined(HRT_TIME_SYNC)
#include <math.h>
static bool sync_set = false;
static bool sync_locked = false;
static uint32_t sync_jump_cnt = 0;
static float sync_prev_adj = 0;
static float sync_rel_rate_ppm = 0;
static float sync_rel_rate_error_integral = 0;
static int32_t sync_accumulated_correction_nsec = 0;
static int32_t sync_correction_nsec_per_overflow = 0;
static hrt_abstime prev_sync_adj_at;
static const float sync_offset_p = 0.01f; ///< PPM per one usec error
static const float sync_rate_i = 0.02f; ///< PPM per one PPM error for second
static const float sync_rate_error_corner_freq = 0.01f;
static const float sync_max_rate_correction_ppm = 300.0f;
static const float sync_lock_thres_rate_ppm = 2.f;
static const hrt_abstime sync_lock_thres_offset = 4000; ///< usec
static const hrt_abstime sync_min_jump = 10000; ///< Min error to jump rather than change rate
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(float adj_usec)
{
const hrt_abstime ts = hrt_absolute_time();
const float dt = (ts - prev_sync_adj_at) / 1e6f;
prev_sync_adj_at = ts;
/*
* Target relative rate in PPM
* Positive to go faster
*/
const float target_rel_rate_ppm = adj_usec * sync_offset_p;
/*
* Current relative rate in PPM
* Positive if the local clock is faster
*/
const float new_rel_rate_ppm = (sync_prev_adj - adj_usec) / dt; // rate error in [usec/sec], which is PPM
sync_prev_adj = adj_usec;
sync_rel_rate_ppm = lowpass(sync_rel_rate_ppm, new_rel_rate_ppm, sync_rate_error_corner_freq, dt);
const float rel_rate_error = target_rel_rate_ppm - sync_rel_rate_ppm;
if (dt > 10) {
sync_rel_rate_error_integral = 0;
} else {
sync_rel_rate_error_integral += rel_rate_error * dt * sync_rate_i;
sync_rel_rate_error_integral = fmaxf(sync_rel_rate_error_integral, -sync_max_rate_correction_ppm);
sync_rel_rate_error_integral = fminf(sync_rel_rate_error_integral, sync_max_rate_correction_ppm);
}
/* Rate controller */
float total_rate_correction_ppm = rel_rate_error + sync_rel_rate_error_integral;
total_rate_correction_ppm = fmaxf(total_rate_correction_ppm, -sync_max_rate_correction_ppm);
total_rate_correction_ppm = fminf(total_rate_correction_ppm, sync_max_rate_correction_ppm);
sync_correction_nsec_per_overflow = (HRT_COUNTER_PERIOD * 1000) * (total_rate_correction_ppm / 1e6f);
// syslog(LOG_INFO, "$ adj=%f rel_rate=%f rel_rate_eint=%f tgt_rel_rate=%f ppm=%f\n",
// (double)adj_usec, (double)sync_rel_rate_ppm, (double)sync_rel_rate_error_integral, (double)target_rel_rate_ppm,
// (double)total_rate_correction_ppm);
}
void hrt_absolute_time_adjust(int64_t adjustment)
{
irqstate_t flags = px4_enter_critical_section();
if (fabsf((float)adjustment) > (float)sync_min_jump || !sync_set) {
const hrt_abstime time_prev = base_time;
base_time += adjustment;
syslog(LOG_NOTICE, "HRT: resetting %llu -> %llu\n", time_prev, base_time);
sync_set = true;
sync_locked = false;
sync_jump_cnt++;
sync_prev_adj = 0;
sync_rel_rate_ppm = 0;
} else {
updateRatePID(adjustment);
if (!sync_locked) {
sync_locked =
(fabsf(sync_rel_rate_ppm) < sync_lock_thres_rate_ppm) &&
(fabsf(sync_prev_adj) < sync_lock_thres_offset);
}
}
px4_leave_critical_section(flags);
}
#endif // HRT_TIME_SYNC
int hrt_ioctl(unsigned int cmd, unsigned long arg);
/*
* Specific registers and bits used by PPM sub-functions
@ -665,7 +773,6 @@ hrt_absolute_time(void)
* pair. Discourage the compiler from moving loads/stores
* to these outside of the protected range.
*/
static volatile hrt_abstime base_time;
static volatile uint32_t last_count;
/* prevent re-entry */
@ -683,12 +790,34 @@ hrt_absolute_time(void)
*/
if (count < last_count) {
base_time += HRT_COUNTER_PERIOD;
#if defined(HRT_TIME_SYNC)
if (sync_set) {
sync_accumulated_correction_nsec += sync_correction_nsec_per_overflow;
if (abs(sync_accumulated_correction_nsec) >= 1000) {
base_time += sync_accumulated_correction_nsec / 1000;
sync_accumulated_correction_nsec %= 1000;
}
// Correction decay - 1 nsec per 65536 usec
if (sync_correction_nsec_per_overflow > 0) {
sync_correction_nsec_per_overflow--;
} else if (sync_correction_nsec_per_overflow < 0) {
sync_correction_nsec_per_overflow++;
}
}
#endif /* HRT_TIME_SYNC */
}
/* save the count for next time */
last_count = count;
/* compute the current time */
abstime = HRT_COUNTER_SCALE(base_time + count);
px4_leave_critical_section(flags);

5
src/drivers/drv_hrt.h
View File

@ -131,6 +131,11 @@ typedef struct latency_boardctl {
*/
__EXPORT extern hrt_abstime hrt_absolute_time(void);
/**
* Adjust the synchronized clock.
*/
__EXPORT extern void hrt_absolute_time_adjust(int64_t adjustment);
/**
* Convert a timespec to absolute time.
*/

11
src/drivers/uavcan/CMakeLists.txt
View File

@ -42,22 +42,12 @@ set(UAVCAN_PLATFORM "generic")
if(CONFIG_ARCH_CHIP)
if(${CONFIG_NET_CAN} MATCHES "y")
set(UAVCAN_DRIVER "socketcan")
set(UAVCAN_TIMER 1)
elseif(${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
if (DEFINED config_uavcan_timer_override)
set (UAVCAN_TIMER ${config_uavcan_timer_override})
endif()
elseif(${CONFIG_ARCH_CHIP} MATCHES "stm32")
set(UAVCAN_DRIVER "stm32")
set(UAVCAN_TIMER 6) # The default timer is TIM6
if (DEFINED config_uavcan_timer_override)
set (UAVCAN_TIMER ${config_uavcan_timer_override})
endif()
endif()
endif()
@ -74,7 +64,6 @@ string(TOUPPER "${UAVCAN_DRIVER}" UAVCAN_DRIVER_UPPER)
add_definitions(
-DUAVCAN_${UAVCAN_DRIVER_UPPER}_${OS_UPPER}=1
-DUAVCAN_${UAVCAN_DRIVER_UPPER}_NUM_IFACES=${config_uavcan_num_ifaces}
-DUAVCAN_${UAVCAN_DRIVER_UPPER}_TIMER_NUMBER=${UAVCAN_TIMER}
-DUAVCAN_CPP_VERSION=UAVCAN_CPP03
-DUAVCAN_DRIVER=uavcan_${UAVCAN_DRIVER}
-DUAVCAN_IMPLEMENT_PLACEMENT_NEW=1

5
src/drivers/uavcan/Kconfig
View File

@ -96,8 +96,3 @@ depends on DRIVERS_UAVCAN
string "UAVCAN peripheral firmware"
help
list of UAVCAN peripheral firmware to build and embed
menuconfig BOARD_UAVCAN_TIMER_OVERRIDE
depends on DRIVERS_UAVCAN
int "UAVCAN timer override"
default 0

1
src/drivers/uavcan/uavcan_drivers/kinetis/README.md
View File

@ -15,7 +15,6 @@ and the following commandline defines:
| Setting | Description |
|-------------------|-----------------------------------------------------------------------------------|
|UAVCAN_KINETIS_NUM_IFACES | - {1..2} Sets the number of CAN interfaces the SW will support |
|UAVCAN_KINETIS_TIMER_NUMBER | - {0..3} Sets the Periodic Interrupt Timer (PITn) channel |
Things that could be improved:
1. Build time command line configuration of Mailbox/FIFO and filters

9
src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/build_config.hpp
View File

@ -18,12 +18,3 @@
#if !defined(UAVCAN_KINETIS_NUM_IFACES) || (UAVCAN_KINETIS_NUM_IFACES != 1 && UAVCAN_KINETIS_NUM_IFACES != 2)
# error "UAVCAN_KINETIS_NUM_IFACES must be set to either 1 or 2"
#endif
/**
* Any PIT timer channel (PIT0-PIT3)
* e.g. -DUAVCAN_KINETIS_TIMER_NUMBER=2
*/
#ifndef UAVCAN_KINETIS_TIMER_NUMBER
// In this case the clock driver should be implemented by the application
# define UAVCAN_KINETIS_TIMER_NUMBER 0
#endif

51
src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/clock.hpp
View File

@ -46,57 +46,6 @@ uavcan::UtcTime getUtc();
*/
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 &params);
}
/**

297
src/drivers/uavcan/uavcan_drivers/kinetis/driver/src/uc_kinetis_clock.cpp
View File

@ -7,31 +7,7 @@
#include <uavcan_kinetis/thread.hpp>
#include "internal.hpp"
#if UAVCAN_KINETIS_TIMER_NUMBER
# include <cassert>
# include <math.h>
/*
* Timer instance
* todo:Consider using Lifetime Timer support
*/
# define TIMX_IRQHandler UAVCAN_KINETIS_GLUE3(PIT, UAVCAN_KINETIS_TIMER_NUMBER, _IRQHandler)
# define TIMX (KINETIS_PIT_BASE + (UAVCAN_KINETIS_TIMER_NUMBER << 4))
# define TMR_REG(o) (TIMX + (o))
# define TIMX_INPUT_CLOCK BOARD_BUS_FREQ
# define TIMX_INTERRUPT_FREQ 16
# define TIMX_IRQn UAVCAN_KINETIS_GLUE2(KINETIS_IRQ_PITCH, UAVCAN_KINETIS_TIMER_NUMBER)
# if UAVCAN_KINETIS_TIMER_NUMBER >= 0 && UAVCAN_KINETIS_TIMER_NUMBER <= 3
# define KINETIS_PIT_LDVAL_OFFSET KINETIS_PIT_LDVAL0_OFFSET
# define KINETIS_PIT_CVAL_OFFSET KINETIS_PIT_CVAL0_OFFSET
# define KINETIS_PIT_TCTRL_OFFSET KINETIS_PIT_TCTRL0_OFFSET
# define KINETIS_PIT_TFLG_OFFSET KINETIS_PIT_TFLG0_OFFSET
# else
# error "This UAVCAN_KINETIS_TIMER_NUMBER is not supported yet"
# endif
extern "C" UAVCAN_KINETIS_IRQ_HANDLER(TIMX_IRQHandler);
#include <drivers/drv_hrt.h>
namespace uavcan_kinetis
{
@ -40,30 +16,12 @@ namespace clock
namespace
{
const uavcan::uint32_t CountsPerPeriod = (TIMX_INPUT_CLOCK / TIMX_INTERRUPT_FREQ);
const uavcan::uint32_t CountsPerUs = (TIMX_INPUT_CLOCK / 1000000);
const uavcan::uint32_t USecPerOverflow = (1000000 / TIMX_INTERRUPT_FREQ);
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;
@ -73,231 +31,34 @@ void init()
}
initialized = true;
// Attach IRQ
irq_attach(TIMX_IRQn, &TIMX_IRQHandler, NULL);
// Power-on Clock
modifyreg32(KINETIS_SIM_SCGC6, 0, SIM_SCGC6_PIT);
// Enable module
putreg32(0, KINETIS_PIT_MCR);
// 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);
#endif
up_enable_irq(TIMX_IRQn);
}
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()
{
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));
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);
// DO NOTHING
}
uavcan::uint64_t getUtcUSecFromCanInterrupt()
{
return utc_set ? sampleUtcFromCriticalSection() : 0;
return hrt_absolute_time();
}
uavcan::MonotonicTime getMonotonic()
{
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));
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);
} // End Scope Critical section
uavcan::uint64_t usec = hrt_absolute_time();
return uavcan::MonotonicTime::fromUSec(usec);
}
uavcan::UtcTime getUtc()
{
if (utc_set) {
uavcan::uint64_t usec = 0;
{
CriticalSectionLocker locker;
usec = sampleUtcFromCriticalSection();
}
uavcan::uint64_t usec = hrt_absolute_time();
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 &params)
{
MutexLocker mlocker(mutex);
// Add some sanity check
utc_sync_params = params;
const float adj_usec = float(adjustment.toUSec());
hrt_absolute_time_adjust(adj_usec);
}
} // namespace clock
@ -321,45 +82,5 @@ SystemClock &SystemClock::instance()
return *ptr;
}
} // namespace uavcan_kinetis
} // namespace uavcan_stm32
/**
* Timer interrupt handler
*/
extern "C"
UAVCAN_KINETIS_IRQ_HANDLER(TIMX_IRQHandler)
{
putreg32(PIT_TFLG_TIF, TMR_REG(KINETIS_PIT_TFLG_OFFSET));
using namespace uavcan_kinetis::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
}
}
return 0;
}
#endif

9
src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/build_config.hpp
View File

@ -17,12 +17,3 @@
#if !defined(UAVCAN_STM32_NUM_IFACES) || (UAVCAN_STM32_NUM_IFACES != 1 && UAVCAN_STM32_NUM_IFACES != 2)
# error "UAVCAN_STM32_NUM_IFACES must be set to either 1 or 2"
#endif
/**
* Any General-Purpose timer (TIM2, TIM3, TIM4, TIM5)
* e.g. -DUAVCAN_STM32_TIMER_NUMBER=2
*/
#ifndef UAVCAN_STM32_TIMER_NUMBER
// In this case the clock driver should be implemented by the application
# define UAVCAN_STM32_TIMER_NUMBER 0
#endif

50
src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/clock.hpp
View File

@ -45,56 +45,6 @@ uavcan::UtcTime getUtc();
*/
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 &params);
}
/**

329
src/drivers/uavcan/uavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp
View File

@ -6,49 +6,7 @@
#include <uavcan_stm32/thread.hpp>
#include "internal.hpp"
#if UAVCAN_STM32_TIMER_NUMBER
#include <cassert>
#include <math.h>
/*
* Timer instance
*/
# if UAVCAN_STM32_NUTTX
# define TIMX UAVCAN_STM32_GLUE3(STM32_TIM, UAVCAN_STM32_TIMER_NUMBER, _BASE)
# define TMR_REG(o) (TIMX + (o))
# define TIMX_INPUT_CLOCK UAVCAN_STM32_GLUE3(STM32_APB1_TIM, UAVCAN_STM32_TIMER_NUMBER, _CLKIN)
# define TIMX_IRQn UAVCAN_STM32_GLUE2(STM32_IRQ_TIM, UAVCAN_STM32_TIMER_NUMBER)
# endif
# if UAVCAN_STM32_TIMER_NUMBER >= 2 && UAVCAN_STM32_TIMER_NUMBER <= 7
# define TIMX_RCC_ENR RCC->APB1ENR
# define TIMX_RCC_RSTR RCC->APB1RSTR
# define TIMX_RCC_ENR_MASK UAVCAN_STM32_GLUE3(RCC_APB1ENR_TIM, UAVCAN_STM32_TIMER_NUMBER, EN)
# define TIMX_RCC_RSTR_MASK UAVCAN_STM32_GLUE3(RCC_APB1RSTR_TIM, UAVCAN_STM32_TIMER_NUMBER, RST)
# else
# error "This UAVCAN_STM32_TIMER_NUMBER is not supported yet"
# endif
/**
* UAVCAN_STM32_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_STM32_TIMX_INPUT_CLOCK=STM32_HCLK
*
* Alternatively, the new clock rate can be specified directly.
*/
# ifdef UAVCAN_STM32_TIMX_INPUT_CLOCK
# undef TIMX_INPUT_CLOCK
# define TIMX_INPUT_CLOCK UAVCAN_STM32_TIMX_INPUT_CLOCK
# endif
extern "C" UAVCAN_STM32_IRQ_HANDLER(TIMX_IRQHandler);
#include <drivers/drv_hrt.h>
namespace uavcan_stm32
{
@ -57,26 +15,9 @@ 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;
}
@ -89,246 +30,34 @@ void init()
}
initialized = true;
# if UAVCAN_STM32_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_STM32_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
// DO NOTHING
}
uavcan::uint64_t getUtcUSecFromCanInterrupt()
{
return utc_set ? sampleUtcFromCriticalSection() : 0;
return hrt_absolute_time();
}
uavcan::MonotonicTime getMonotonic()
{
uavcan::uint64_t usec = 0;
// Scope Critical section
{
CriticalSectionLocker locker;
volatile uavcan::uint64_t time = time_mono;
# if UAVCAN_STM32_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
uavcan::uint64_t usec = hrt_absolute_time();
return uavcan::MonotonicTime::fromUSec(usec);
}
uavcan::UtcTime getUtc()
{
if (utc_set) {
uavcan::uint64_t usec = 0;
{
CriticalSectionLocker locker;
usec = sampleUtcFromCriticalSection();
}
uavcan::uint64_t usec = hrt_absolute_time();
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 &params)
{
MutexLocker mlocker(mutex);
// Add some sanity check
utc_sync_params = params;
const float adj_usec = float(adjustment.toUSec());
hrt_absolute_time_adjust(adj_usec);
}
} // namespace clock
@ -354,47 +83,3 @@ SystemClock &SystemClock::instance()
} // namespace uavcan_stm32
/**
* Timer interrupt handler
*/
extern "C"
UAVCAN_STM32_IRQ_HANDLER(TIMX_IRQHandler)
{
UAVCAN_STM32_IRQ_PROLOGUE();
# if UAVCAN_STM32_NUTTX
putreg16(0, TMR_REG(STM32_BTIM_SR_OFFSET));
# endif
using namespace uavcan_stm32::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_STM32_IRQ_EPILOGUE();
}
#endif

9
src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include/uavcan_stm32h7/build_config.hpp
View File

@ -17,12 +17,3 @@
#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

50
src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include/uavcan_stm32h7/clock.hpp
View File

@ -45,56 +45,6 @@ uavcan::UtcTime getUtc();
*/
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 &params);
}
/**

329
src/drivers/uavcan/uavcan_drivers/stm32h7/driver/src/uc_stm32h7_clock.cpp
View File

@ -6,49 +6,7 @@
#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);
#include <drivers/drv_hrt.h>
namespace uavcan_stm32h7
{
@ -57,26 +15,9 @@ 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;
}
@ -89,246 +30,34 @@ void init()
}
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
// DO NOTHING
}
uavcan::uint64_t getUtcUSecFromCanInterrupt()
{
return utc_set ? sampleUtcFromCriticalSection() : 0;
return hrt_absolute_time();
}
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
uavcan::uint64_t usec = hrt_absolute_time();
return uavcan::MonotonicTime::fromUSec(usec);
}
uavcan::UtcTime getUtc()
{
if (utc_set) {
uavcan::uint64_t usec = 0;
{
CriticalSectionLocker locker;
usec = sampleUtcFromCriticalSection();
}
uavcan::uint64_t usec = hrt_absolute_time();
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 &params)
{
MutexLocker mlocker(mutex);
// Add some sanity check
utc_sync_params = params;
const float adj_usec = float(adjustment.toUSec());
hrt_absolute_time_adjust(adj_usec);
}
} // namespace clock
@ -354,47 +83,3 @@ SystemClock &SystemClock::instance()
} // 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

11
src/drivers/uavcannode/CMakeLists.txt
View File

@ -42,22 +42,12 @@ set(UAVCAN_PLATFORM "generic")
if(CONFIG_ARCH_CHIP)
if(${CONFIG_NET_CAN} MATCHES "y")
set(UAVCAN_DRIVER "socketcan")
set(UAVCAN_TIMER 1)
elseif(${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
if (DEFINED config_uavcan_timer_override)
set (UAVCAN_TIMER ${config_uavcan_timer_override})
endif()
elseif(${CONFIG_ARCH_CHIP} MATCHES "stm32")
set(UAVCAN_DRIVER "stm32")
set(UAVCAN_TIMER 5) # The default timer is TIM5
if (DEFINED config_uavcan_timer_override)
set (UAVCAN_TIMER ${config_uavcan_timer_override})
endif()
endif()
endif()
@ -74,7 +64,6 @@ string(TOUPPER "${UAVCAN_DRIVER}" UAVCAN_DRIVER_UPPER)
add_definitions(
-DUAVCAN_${UAVCAN_DRIVER_UPPER}_${OS_UPPER}=1
-DUAVCAN_${UAVCAN_DRIVER_UPPER}_NUM_IFACES=${config_uavcan_num_ifaces}
-DUAVCAN_${UAVCAN_DRIVER_UPPER}_TIMER_NUMBER=${UAVCAN_TIMER}
-DUAVCAN_CPP_VERSION=UAVCAN_CPP03
-DUAVCAN_DRIVER=uavcan_${UAVCAN_DRIVER}
-DUAVCAN_IMPLEMENT_PLACEMENT_NEW=1