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_SAFETY_BUTTON=y
CONFIG_DRIVERS_TONE_ALARM=y CONFIG_DRIVERS_TONE_ALARM=y
CONFIG_DRIVERS_UAVCAN=y CONFIG_DRIVERS_UAVCAN=y
CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
CONFIG_MODULES_AIRSPEED_SELECTOR=y CONFIG_MODULES_AIRSPEED_SELECTOR=y
CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_CAMERA_FEEDBACK=y
CONFIG_MODULES_COMMANDER=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_COMMON_TELEMETRY=y
CONFIG_DRIVERS_TONE_ALARM=y CONFIG_DRIVERS_TONE_ALARM=y
CONFIG_DRIVERS_UAVCAN=y CONFIG_DRIVERS_UAVCAN=y
CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
CONFIG_MODULES_AIRSPEED_SELECTOR=y CONFIG_MODULES_AIRSPEED_SELECTOR=y
CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
CONFIG_MODULES_BATTERY_STATUS=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_COMMON_TELEMETRY=y
CONFIG_DRIVERS_TONE_ALARM=y CONFIG_DRIVERS_TONE_ALARM=y
CONFIG_DRIVERS_UAVCAN=y CONFIG_DRIVERS_UAVCAN=y
CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
CONFIG_MODULES_AIRSPEED_SELECTOR=y CONFIG_MODULES_AIRSPEED_SELECTOR=y
CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
CONFIG_MODULES_BATTERY_STATUS=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_ADC_ADS1115=n
CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n
CONFIG_DRIVERS_IRLOCK=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_HEATER=n
CONFIG_DRIVERS_UAVCAN=n CONFIG_DRIVERS_UAVCAN=n
CONFIG_CYPHAL_BMS_SUBSCRIBER=y 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_BAROMETERS=n
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n
CONFIG_COMMON_MAGNETOMETER=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_COMMON_TELEMETRY=y
CONFIG_DRIVERS_TONE_ALARM=y CONFIG_DRIVERS_TONE_ALARM=y
CONFIG_DRIVERS_UAVCAN=y CONFIG_DRIVERS_UAVCAN=y
CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=6
CONFIG_MODULES_AIRSPEED_SELECTOR=y CONFIG_MODULES_AIRSPEED_SELECTOR=y
CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
CONFIG_MODULES_BATTERY_STATUS=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_BAROMETERS=n
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n
CONFIG_COMMON_DISTANCE_SENSOR=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_BAROMETERS=n
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n
CONFIG_COMMON_DISTANCE_SENSOR=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_HYGROMETERS=n
CONFIG_COMMON_OSD=n CONFIG_COMMON_OSD=n
CONFIG_DRIVERS_ADC_ADS1115=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_BAROMETERS=n
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n
CONFIG_COMMON_HYGROMETERS=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_COMMON_TELEMETRY=y
CONFIG_DRIVERS_TONE_ALARM=y CONFIG_DRIVERS_TONE_ALARM=y
CONFIG_DRIVERS_UAVCAN=y CONFIG_DRIVERS_UAVCAN=y
CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
CONFIG_MODULES_AIRSPEED_SELECTOR=y CONFIG_MODULES_AIRSPEED_SELECTOR=y
CONFIG_MODULES_BATTERY_STATUS=y CONFIG_MODULES_BATTERY_STATUS=y
CONFIG_MODULES_CAMERA_FEEDBACK=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_SAFETY_BUTTON=y
CONFIG_DRIVERS_TONE_ALARM=y CONFIG_DRIVERS_TONE_ALARM=y
CONFIG_DRIVERS_UAVCAN=y CONFIG_DRIVERS_UAVCAN=y
CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
CONFIG_MODULES_AIRSPEED_SELECTOR=y CONFIG_MODULES_AIRSPEED_SELECTOR=y
CONFIG_MODULES_BATTERY_STATUS=y CONFIG_MODULES_BATTERY_STATUS=y
CONFIG_MODULES_CAMERA_FEEDBACK=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_DRIVERS_UAVCAN=n
CONFIG_MODULES_UXRCE_DDS_CLIENT=n CONFIG_MODULES_UXRCE_DDS_CLIENT=n
CONFIG_MODULES_ZENOH=y 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) set(config_uavcan_num_ifaces ${UAVCAN_INTERFACES} CACHE INTERNAL "UAVCAN interfaces" FORCE)
endif() endif()
if(UAVCAN_TIMER_OVERRIDE)
set(config_uavcan_timer_override ${UAVCAN_TIMER_OVERRIDE} CACHE INTERNAL "UAVCAN TIMER OVERRIDE" FORCE)
endif()
# OPTIONS # OPTIONS
if(CONSTRAINED_FLASH) 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 # error HRT_TIMER_CHANNEL must be a value between 1 and 4
#endif #endif
static volatile hrt_abstime base_time;
/* /*
* Queue of callout entries. * Queue of callout entries.
*/ */
@ -276,6 +278,112 @@ static void hrt_call_reschedule(void);
static void hrt_call_invoke(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); int hrt_ioctl(unsigned int cmd, unsigned long arg);
/* /*
* Specific registers and bits used by PPM sub-functions * 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 * pair. Discourage the compiler from moving loads/stores
* to these outside of the protected range. * to these outside of the protected range.
*/ */
static volatile hrt_abstime base_time;
static volatile uint32_t last_count; static volatile uint32_t last_count;
/* prevent re-entry */ /* prevent re-entry */
@ -683,12 +790,34 @@ hrt_absolute_time(void)
*/ */
if (count < last_count) { if (count < last_count) {
base_time += HRT_COUNTER_PERIOD; 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 */ /* save the count for next time */
last_count = count; last_count = count;
/* compute the current time */ /* compute the current time */
abstime = HRT_COUNTER_SCALE(base_time + count); abstime = HRT_COUNTER_SCALE(base_time + count);
px4_leave_critical_section(flags); 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); __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. * 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_ARCH_CHIP)
if(${CONFIG_NET_CAN} MATCHES "y") if(${CONFIG_NET_CAN} MATCHES "y")
set(UAVCAN_DRIVER "socketcan") set(UAVCAN_DRIVER "socketcan")
set(UAVCAN_TIMER 1)
elseif(${CONFIG_ARCH_CHIP} MATCHES "kinetis") elseif(${CONFIG_ARCH_CHIP} MATCHES "kinetis")
set(UAVCAN_DRIVER "kinetis") set(UAVCAN_DRIVER "kinetis")
set(UAVCAN_TIMER 1)
elseif(${CONFIG_ARCH_CHIP} MATCHES "stm32h7") elseif(${CONFIG_ARCH_CHIP} MATCHES "stm32h7")
set(UAVCAN_DRIVER "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") elseif(${CONFIG_ARCH_CHIP} MATCHES "stm32")
set(UAVCAN_DRIVER "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()
endif() endif()
@ -74,7 +64,6 @@ string(TOUPPER "${UAVCAN_DRIVER}" UAVCAN_DRIVER_UPPER)
add_definitions( add_definitions(
-DUAVCAN_${UAVCAN_DRIVER_UPPER}_${OS_UPPER}=1 -DUAVCAN_${UAVCAN_DRIVER_UPPER}_${OS_UPPER}=1
-DUAVCAN_${UAVCAN_DRIVER_UPPER}_NUM_IFACES=${config_uavcan_num_ifaces} -DUAVCAN_${UAVCAN_DRIVER_UPPER}_NUM_IFACES=${config_uavcan_num_ifaces}
-DUAVCAN_${UAVCAN_DRIVER_UPPER}_TIMER_NUMBER=${UAVCAN_TIMER}
-DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAVCAN_CPP_VERSION=UAVCAN_CPP03
-DUAVCAN_DRIVER=uavcan_${UAVCAN_DRIVER} -DUAVCAN_DRIVER=uavcan_${UAVCAN_DRIVER}
-DUAVCAN_IMPLEMENT_PLACEMENT_NEW=1 -DUAVCAN_IMPLEMENT_PLACEMENT_NEW=1

5
src/drivers/uavcan/Kconfig
View File

@ -96,8 +96,3 @@ depends on DRIVERS_UAVCAN
string "UAVCAN peripheral firmware" string "UAVCAN peripheral firmware"
help help
list of UAVCAN peripheral firmware to build and embed 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 | | Setting | Description |
|-------------------|-----------------------------------------------------------------------------------| |-------------------|-----------------------------------------------------------------------------------|
|UAVCAN_KINETIS_NUM_IFACES | - {1..2} Sets the number of CAN interfaces the SW will support | |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: Things that could be improved:
1. Build time command line configuration of Mailbox/FIFO and filters 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) #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" # error "UAVCAN_KINETIS_NUM_IFACES must be set to either 1 or 2"
#endif #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); 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);
} }
/** /**

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

@ -7,31 +7,7 @@
#include <uavcan_kinetis/thread.hpp> #include <uavcan_kinetis/thread.hpp>
#include "internal.hpp" #include "internal.hpp"
#if UAVCAN_KINETIS_TIMER_NUMBER #include <drivers/drv_hrt.h>
# 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);
namespace uavcan_kinetis namespace uavcan_kinetis
{ {
@ -40,30 +16,12 @@ namespace clock
namespace 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; Mutex mutex;
bool initialized = false; 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() void init()
{ {
CriticalSectionLocker lock; CriticalSectionLocker lock;
@ -73,231 +31,34 @@ void init()
} }
initialized = true; 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) void setUtc(uavcan::UtcTime time)
{ {
MutexLocker mlocker(mutex); // DO NOTHING
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);
} }
uavcan::uint64_t getUtcUSecFromCanInterrupt() uavcan::uint64_t getUtcUSecFromCanInterrupt()
{ {
return utc_set ? sampleUtcFromCriticalSection() : 0; return hrt_absolute_time();
} }
uavcan::MonotonicTime getMonotonic() uavcan::MonotonicTime getMonotonic()
{ {
uavcan::uint64_t usec = 0; uavcan::uint64_t usec = hrt_absolute_time();
// 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
return uavcan::MonotonicTime::fromUSec(usec); return uavcan::MonotonicTime::fromUSec(usec);
} }
uavcan::UtcTime getUtc() uavcan::UtcTime getUtc()
{ {
if (utc_set) { uavcan::uint64_t usec = hrt_absolute_time();
uavcan::uint64_t usec = 0; return uavcan::UtcTime::fromUSec(usec);
{
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) void adjustUtc(uavcan::UtcDuration adjustment)
{ {
MutexLocker mlocker(mutex); const float adj_usec = float(adjustment.toUSec());
UAVCAN_ASSERT(initialized); hrt_absolute_time_adjust(adj_usec);
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;
} }
} // namespace clock } // namespace clock
@ -321,45 +82,5 @@ SystemClock &SystemClock::instance()
return *ptr; 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) #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" # error "UAVCAN_STM32_NUM_IFACES must be set to either 1 or 2"
#endif #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); 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);
} }
/** /**

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

@ -6,49 +6,7 @@
#include <uavcan_stm32/thread.hpp> #include <uavcan_stm32/thread.hpp>
#include "internal.hpp" #include "internal.hpp"
#if UAVCAN_STM32_TIMER_NUMBER #include <drivers/drv_hrt.h>
#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);
namespace uavcan_stm32 namespace uavcan_stm32
{ {
@ -57,26 +15,9 @@ namespace clock
namespace namespace
{ {
const uavcan::uint32_t USecPerOverflow = 65536;
Mutex mutex; Mutex mutex;
bool initialized = false; 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; 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) void setUtc(uavcan::UtcTime time)
{ {
MutexLocker mlocker(mutex); // DO NOTHING
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
} }
uavcan::uint64_t getUtcUSecFromCanInterrupt() uavcan::uint64_t getUtcUSecFromCanInterrupt()
{ {
return utc_set ? sampleUtcFromCriticalSection() : 0; return hrt_absolute_time();
} }
uavcan::MonotonicTime getMonotonic() uavcan::MonotonicTime getMonotonic()
{ {
uavcan::uint64_t usec = 0; uavcan::uint64_t usec = hrt_absolute_time();
// 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
return uavcan::MonotonicTime::fromUSec(usec); return uavcan::MonotonicTime::fromUSec(usec);
} }
uavcan::UtcTime getUtc() uavcan::UtcTime getUtc()
{ {
if (utc_set) { uavcan::uint64_t usec = hrt_absolute_time();
uavcan::uint64_t usec = 0; return uavcan::UtcTime::fromUSec(usec);
{
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) void adjustUtc(uavcan::UtcDuration adjustment)
{ {
MutexLocker mlocker(mutex); const float adj_usec = float(adjustment.toUSec());
UAVCAN_ASSERT(initialized); hrt_absolute_time_adjust(adj_usec);
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;
} }
} // namespace clock } // namespace clock
@ -354,47 +83,3 @@ SystemClock &SystemClock::instance()
} // namespace uavcan_stm32 } // 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) #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" # error "UAVCAN_STM32H7_NUM_IFACES must be set to either 1 or 2"
#endif #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); 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);
} }
/** /**

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

@ -6,49 +6,7 @@
#include <uavcan_stm32h7/thread.hpp> #include <uavcan_stm32h7/thread.hpp>
#include "internal.hpp" #include "internal.hpp"
#if UAVCAN_STM32H7_TIMER_NUMBER #include <drivers/drv_hrt.h>
#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 uavcan_stm32h7
{ {
@ -57,26 +15,9 @@ namespace clock
namespace namespace
{ {
const uavcan::uint32_t USecPerOverflow = 65536;
Mutex mutex; Mutex mutex;
bool initialized = false; 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; 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) void setUtc(uavcan::UtcTime time)
{ {
MutexLocker mlocker(mutex); // DO NOTHING
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() uavcan::uint64_t getUtcUSecFromCanInterrupt()
{ {
return utc_set ? sampleUtcFromCriticalSection() : 0; return hrt_absolute_time();
} }
uavcan::MonotonicTime getMonotonic() uavcan::MonotonicTime getMonotonic()
{ {
uavcan::uint64_t usec = 0; uavcan::uint64_t usec = hrt_absolute_time();
// 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); return uavcan::MonotonicTime::fromUSec(usec);
} }
uavcan::UtcTime getUtc() uavcan::UtcTime getUtc()
{ {
if (utc_set) { uavcan::uint64_t usec = hrt_absolute_time();
uavcan::uint64_t usec = 0; return uavcan::UtcTime::fromUSec(usec);
{
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) void adjustUtc(uavcan::UtcDuration adjustment)
{ {
MutexLocker mlocker(mutex); const float adj_usec = float(adjustment.toUSec());
UAVCAN_ASSERT(initialized); hrt_absolute_time_adjust(adj_usec);
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;
} }
} // namespace clock } // namespace clock
@ -354,47 +83,3 @@ SystemClock &SystemClock::instance()
} // namespace uavcan_stm32h7 } // 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_ARCH_CHIP)
if(${CONFIG_NET_CAN} MATCHES "y") if(${CONFIG_NET_CAN} MATCHES "y")
set(UAVCAN_DRIVER "socketcan") set(UAVCAN_DRIVER "socketcan")
set(UAVCAN_TIMER 1)
elseif(${CONFIG_ARCH_CHIP} MATCHES "kinetis") elseif(${CONFIG_ARCH_CHIP} MATCHES "kinetis")
set(UAVCAN_DRIVER "kinetis") set(UAVCAN_DRIVER "kinetis")
set(UAVCAN_TIMER 1)
elseif(${CONFIG_ARCH_CHIP} MATCHES "stm32h7") elseif(${CONFIG_ARCH_CHIP} MATCHES "stm32h7")
set(UAVCAN_DRIVER "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") elseif(${CONFIG_ARCH_CHIP} MATCHES "stm32")
set(UAVCAN_DRIVER "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()
endif() endif()
@ -74,7 +64,6 @@ string(TOUPPER "${UAVCAN_DRIVER}" UAVCAN_DRIVER_UPPER)
add_definitions( add_definitions(
-DUAVCAN_${UAVCAN_DRIVER_UPPER}_${OS_UPPER}=1 -DUAVCAN_${UAVCAN_DRIVER_UPPER}_${OS_UPPER}=1
-DUAVCAN_${UAVCAN_DRIVER_UPPER}_NUM_IFACES=${config_uavcan_num_ifaces} -DUAVCAN_${UAVCAN_DRIVER_UPPER}_NUM_IFACES=${config_uavcan_num_ifaces}
-DUAVCAN_${UAVCAN_DRIVER_UPPER}_TIMER_NUMBER=${UAVCAN_TIMER}
-DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAVCAN_CPP_VERSION=UAVCAN_CPP03
-DUAVCAN_DRIVER=uavcan_${UAVCAN_DRIVER} -DUAVCAN_DRIVER=uavcan_${UAVCAN_DRIVER}
-DUAVCAN_IMPLEMENT_PLACEMENT_NEW=1 -DUAVCAN_IMPLEMENT_PLACEMENT_NEW=1