diff --git a/boards/ark/fmu-v6x/default.px4board b/boards/ark/fmu-v6x/default.px4board index 4018e493a2..9dfb052880 100644 --- a/boards/ark/fmu-v6x/default.px4board +++ b/boards/ark/fmu-v6x/default.px4board @@ -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 diff --git a/boards/cuav/nora/default.px4board b/boards/cuav/nora/default.px4board index f312504ee5..b9dcdcce57 100644 --- a/boards/cuav/nora/default.px4board +++ b/boards/cuav/nora/default.px4board @@ -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 diff --git a/boards/cuav/x7pro/default.px4board b/boards/cuav/x7pro/default.px4board index 99e831c129..edb4610053 100644 --- a/boards/cuav/x7pro/default.px4board +++ b/boards/cuav/x7pro/default.px4board @@ -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 diff --git a/boards/cuav/x7pro/test.px4board b/boards/cuav/x7pro/test.px4board index 3968d0283d..17d80e5023 100644 --- a/boards/cuav/x7pro/test.px4board +++ b/boards/cuav/x7pro/test.px4board @@ -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 diff --git a/boards/px4/fmu-v5/cyphal.px4board b/boards/px4/fmu-v5/cyphal.px4board index bb753f2713..d674c9f0f9 100644 --- a/boards/px4/fmu-v5/cyphal.px4board +++ b/boards/px4/fmu-v5/cyphal.px4board @@ -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 diff --git a/boards/px4/fmu-v5/debug.px4board b/boards/px4/fmu-v5/debug.px4board index 0b8ab3fc3e..e641d7ec1d 100644 --- a/boards/px4/fmu-v5/debug.px4board +++ b/boards/px4/fmu-v5/debug.px4board @@ -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 diff --git a/boards/px4/fmu-v5/default.px4board b/boards/px4/fmu-v5/default.px4board index bfb8b6af46..c91a090890 100644 --- a/boards/px4/fmu-v5/default.px4board +++ b/boards/px4/fmu-v5/default.px4board @@ -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 diff --git a/boards/px4/fmu-v5/protected.px4board b/boards/px4/fmu-v5/protected.px4board index e5371b2a7d..7d9f5306fb 100644 --- a/boards/px4/fmu-v5/protected.px4board +++ b/boards/px4/fmu-v5/protected.px4board @@ -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 diff --git a/boards/px4/fmu-v5/stackcheck.px4board b/boards/px4/fmu-v5/stackcheck.px4board index 812a0f7295..7801549d84 100644 --- a/boards/px4/fmu-v5/stackcheck.px4board +++ b/boards/px4/fmu-v5/stackcheck.px4board @@ -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 diff --git a/boards/px4/fmu-v5/test.px4board b/boards/px4/fmu-v5/test.px4board index 2dcd6925f2..7a24c29b3c 100644 --- a/boards/px4/fmu-v5/test.px4board +++ b/boards/px4/fmu-v5/test.px4board @@ -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 diff --git a/boards/px4/fmu-v5/uavcanv0periph.px4board b/boards/px4/fmu-v5/uavcanv0periph.px4board index c9e9823c03..b85b3578a3 100644 --- a/boards/px4/fmu-v5/uavcanv0periph.px4board +++ b/boards/px4/fmu-v5/uavcanv0periph.px4board @@ -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 diff --git a/boards/px4/fmu-v6c/default.px4board b/boards/px4/fmu-v6c/default.px4board index 148e6326fb..772044f0e4 100644 --- a/boards/px4/fmu-v6c/default.px4board +++ b/boards/px4/fmu-v6c/default.px4board @@ -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 diff --git a/boards/px4/fmu-v6x/default.px4board b/boards/px4/fmu-v6x/default.px4board index 4e21f0fd08..961b01d8ef 100644 --- a/boards/px4/fmu-v6x/default.px4board +++ b/boards/px4/fmu-v6x/default.px4board @@ -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 diff --git a/boards/px4/fmu-v6x/zenoh.px4board b/boards/px4/fmu-v6x/zenoh.px4board index cb14fde935..8a1f29a084 100644 --- a/boards/px4/fmu-v6x/zenoh.px4board +++ b/boards/px4/fmu-v6x/zenoh.px4board @@ -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 diff --git a/cmake/kconfig.cmake b/cmake/kconfig.cmake index eb418bf6cc..2b170e91aa 100644 --- a/cmake/kconfig.cmake +++ b/cmake/kconfig.cmake @@ -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) diff --git a/platforms/nuttx/src/px4/stm/stm32_common/hrt/hrt.c b/platforms/nuttx/src/px4/stm/stm32_common/hrt/hrt.c index f31c240fed..03f35be96f 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/hrt/hrt.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/hrt/hrt.c @@ -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 + +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); diff --git a/src/drivers/drv_hrt.h b/src/drivers/drv_hrt.h index 3e295abfc9..65bbc99154 100644 --- a/src/drivers/drv_hrt.h +++ b/src/drivers/drv_hrt.h @@ -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. */ diff --git a/src/drivers/uavcan/CMakeLists.txt b/src/drivers/uavcan/CMakeLists.txt index 4146edab05..42e2a5bf22 100644 --- a/src/drivers/uavcan/CMakeLists.txt +++ b/src/drivers/uavcan/CMakeLists.txt @@ -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 diff --git a/src/drivers/uavcan/Kconfig b/src/drivers/uavcan/Kconfig index 1d91acb622..833eb00343 100644 --- a/src/drivers/uavcan/Kconfig +++ b/src/drivers/uavcan/Kconfig @@ -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 diff --git a/src/drivers/uavcan/uavcan_drivers/kinetis/README.md b/src/drivers/uavcan/uavcan_drivers/kinetis/README.md index 5d04cdd2b0..aa0f45ebca 100644 --- a/src/drivers/uavcan/uavcan_drivers/kinetis/README.md +++ b/src/drivers/uavcan/uavcan_drivers/kinetis/README.md @@ -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 diff --git a/src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/build_config.hpp b/src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/build_config.hpp index 02e27d3a80..6d2d127b0a 100644 --- a/src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/build_config.hpp +++ b/src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/build_config.hpp @@ -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 diff --git a/src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/clock.hpp b/src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/clock.hpp index f593feb60c..e3fb0573ea 100644 --- a/src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/clock.hpp +++ b/src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/clock.hpp @@ -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 ¶ms); - } /** diff --git a/src/drivers/uavcan/uavcan_drivers/kinetis/driver/src/uc_kinetis_clock.cpp b/src/drivers/uavcan/uavcan_drivers/kinetis/driver/src/uc_kinetis_clock.cpp index 9560284094..842b66bd2e 100644 --- a/src/drivers/uavcan/uavcan_drivers/kinetis/driver/src/uc_kinetis_clock.cpp +++ b/src/drivers/uavcan/uavcan_drivers/kinetis/driver/src/uc_kinetis_clock.cpp @@ -7,31 +7,7 @@ #include #include "internal.hpp" -#if UAVCAN_KINETIS_TIMER_NUMBER -# include -# include - -/* - * 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 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(); - } - 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); + uavcan::uint64_t usec = hrt_absolute_time(); + return uavcan::UtcTime::fromUSec(usec); } void adjustUtc(uavcan::UtcDuration adjustment) { - MutexLocker mlocker(mutex); - UAVCAN_ASSERT(initialized); - - if (adjustment.getAbs() > utc_sync_params.min_jump || !utc_set) { - const uavcan::int64_t adj_usec = adjustment.toUSec(); - - { - CriticalSectionLocker locker; - - if ((adj_usec < 0) && uavcan::uint64_t(-adj_usec) > time_utc) { - time_utc = 1; - - } else { - time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + adj_usec); - } - } - - utc_set = true; - utc_locked = false; - utc_jump_cnt++; - utc_prev_adj = 0; - utc_rel_rate_ppm = 0; - - } else { - updateRatePID(adjustment); - - if (!utc_locked) { - utc_locked = - (std::abs(utc_rel_rate_ppm) < utc_sync_params.lock_thres_rate_ppm) && - (std::abs(utc_prev_adj) < float(utc_sync_params.lock_thres_offset.toUSec())); - } - } -} - -float getUtcRateCorrectionPPM() -{ - MutexLocker mlocker(mutex); - const float rate_correction_mult = float(utc_correction_nsec_per_overflow) / float(USecPerOverflow * 1000); - return 1e6F * rate_correction_mult; -} - -uavcan::uint32_t getUtcJumpCount() -{ - MutexLocker mlocker(mutex); - return utc_jump_cnt; -} - -bool isUtcLocked() -{ - MutexLocker mlocker(mutex); - return utc_locked; -} - -UtcSyncParams getUtcSyncParams() -{ - MutexLocker mlocker(mutex); - return utc_sync_params; -} - -void setUtcSyncParams(const UtcSyncParams ¶ms) -{ - MutexLocker mlocker(mutex); - // Add some sanity check - utc_sync_params = params; + 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 diff --git a/src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/build_config.hpp b/src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/build_config.hpp index 56e06ec576..ffc7b2c53f 100644 --- a/src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/build_config.hpp +++ b/src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/build_config.hpp @@ -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 diff --git a/src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/clock.hpp b/src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/clock.hpp index fcfff84da2..a32a0b0dc9 100644 --- a/src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/clock.hpp +++ b/src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/clock.hpp @@ -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 ¶ms); - } /** diff --git a/src/drivers/uavcan/uavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp b/src/drivers/uavcan/uavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp index c2e0216ea2..c54c4afd8c 100644 --- a/src/drivers/uavcan/uavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp +++ b/src/drivers/uavcan/uavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp @@ -6,49 +6,7 @@ #include #include "internal.hpp" -#if UAVCAN_STM32_TIMER_NUMBER - -#include -#include - -/* - * 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 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(); - } - 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); + uavcan::uint64_t usec = hrt_absolute_time(); + return uavcan::UtcTime::fromUSec(usec); } void adjustUtc(uavcan::UtcDuration adjustment) { - MutexLocker mlocker(mutex); - UAVCAN_ASSERT(initialized); - - if (adjustment.getAbs() > utc_sync_params.min_jump || !utc_set) { - const uavcan::int64_t adj_usec = adjustment.toUSec(); - - { - CriticalSectionLocker locker; - - if ((adj_usec < 0) && uavcan::uint64_t(-adj_usec) > time_utc) { - time_utc = 1; - - } else { - time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + adj_usec); - } - } - - utc_set = true; - utc_locked = false; - utc_jump_cnt++; - utc_prev_adj = 0; - utc_rel_rate_ppm = 0; - - } else { - updateRatePID(adjustment); - - if (!utc_locked) { - utc_locked = - (std::abs(utc_rel_rate_ppm) < utc_sync_params.lock_thres_rate_ppm) && - (std::abs(utc_prev_adj) < float(utc_sync_params.lock_thres_offset.toUSec())); - } - } -} - -float getUtcRateCorrectionPPM() -{ - MutexLocker mlocker(mutex); - const float rate_correction_mult = float(utc_correction_nsec_per_overflow) / float(USecPerOverflow * 1000); - return 1e6F * rate_correction_mult; -} - -uavcan::uint32_t getUtcJumpCount() -{ - MutexLocker mlocker(mutex); - return utc_jump_cnt; -} - -bool isUtcLocked() -{ - MutexLocker mlocker(mutex); - return utc_locked; -} - -UtcSyncParams getUtcSyncParams() -{ - MutexLocker mlocker(mutex); - return utc_sync_params; -} - -void setUtcSyncParams(const UtcSyncParams ¶ms) -{ - MutexLocker mlocker(mutex); - // Add some sanity check - utc_sync_params = params; + 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 diff --git a/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include/uavcan_stm32h7/build_config.hpp b/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include/uavcan_stm32h7/build_config.hpp index 769545f6d2..275be429e8 100644 --- a/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include/uavcan_stm32h7/build_config.hpp +++ b/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include/uavcan_stm32h7/build_config.hpp @@ -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 diff --git a/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include/uavcan_stm32h7/clock.hpp b/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include/uavcan_stm32h7/clock.hpp index 98d1c2988e..f3ddb6e40b 100644 --- a/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include/uavcan_stm32h7/clock.hpp +++ b/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include/uavcan_stm32h7/clock.hpp @@ -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 ¶ms); - } /** diff --git a/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/src/uc_stm32h7_clock.cpp b/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/src/uc_stm32h7_clock.cpp index 72c4170a1b..53527eb22b 100644 --- a/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/src/uc_stm32h7_clock.cpp +++ b/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/src/uc_stm32h7_clock.cpp @@ -6,49 +6,7 @@ #include #include "internal.hpp" -#if UAVCAN_STM32H7_TIMER_NUMBER - -#include -#include - -/* - * 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 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(); - } - 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); + uavcan::uint64_t usec = hrt_absolute_time(); + return uavcan::UtcTime::fromUSec(usec); } void adjustUtc(uavcan::UtcDuration adjustment) { - MutexLocker mlocker(mutex); - UAVCAN_ASSERT(initialized); - - if (adjustment.getAbs() > utc_sync_params.min_jump || !utc_set) { - const uavcan::int64_t adj_usec = adjustment.toUSec(); - - { - CriticalSectionLocker locker; - - if ((adj_usec < 0) && uavcan::uint64_t(-adj_usec) > time_utc) { - time_utc = 1; - - } else { - time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + adj_usec); - } - } - - utc_set = true; - utc_locked = false; - utc_jump_cnt++; - utc_prev_adj = 0; - utc_rel_rate_ppm = 0; - - } else { - updateRatePID(adjustment); - - if (!utc_locked) { - utc_locked = - (std::abs(utc_rel_rate_ppm) < utc_sync_params.lock_thres_rate_ppm) && - (std::abs(utc_prev_adj) < float(utc_sync_params.lock_thres_offset.toUSec())); - } - } -} - -float getUtcRateCorrectionPPM() -{ - MutexLocker mlocker(mutex); - const float rate_correction_mult = float(utc_correction_nsec_per_overflow) / float(USecPerOverflow * 1000); - return 1e6F * rate_correction_mult; -} - -uavcan::uint32_t getUtcJumpCount() -{ - MutexLocker mlocker(mutex); - return utc_jump_cnt; -} - -bool isUtcLocked() -{ - MutexLocker mlocker(mutex); - return utc_locked; -} - -UtcSyncParams getUtcSyncParams() -{ - MutexLocker mlocker(mutex); - return utc_sync_params; -} - -void setUtcSyncParams(const UtcSyncParams ¶ms) -{ - MutexLocker mlocker(mutex); - // Add some sanity check - utc_sync_params = params; + 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 diff --git a/src/drivers/uavcannode/CMakeLists.txt b/src/drivers/uavcannode/CMakeLists.txt index 21366ad064..02d1b5582d 100644 --- a/src/drivers/uavcannode/CMakeLists.txt +++ b/src/drivers/uavcannode/CMakeLists.txt @@ -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