From 803222dcc3811c286cabf19bb60c6fa10694d1b5 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 26 Apr 2014 17:48:42 +0400 Subject: [PATCH] STM32: New clock sync algorithm --- .../driver/include/uavcan_stm32/clock.hpp | 40 ++-- .../stm32/driver/src/uc_stm32_clock.cpp | 210 ++++++++++-------- .../stm32/test_stm32f107/src/main.cpp | 8 +- 3 files changed, 150 insertions(+), 108 deletions(-) diff --git a/libuavcan_drivers/stm32/driver/include/uavcan_stm32/clock.hpp b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/clock.hpp index d2954574e1..b1d6a758f9 100644 --- a/libuavcan_drivers/stm32/driver/include/uavcan_stm32/clock.hpp +++ b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/clock.hpp @@ -37,38 +37,46 @@ uavcan::UtcTime getUtc(); void adjustUtc(uavcan::UtcDuration adjustment); /** - * Clock speed error. + * UTC clock synchronization parameters + */ +struct UtcSyncParams +{ + float p = 0.01; ///< Correction PPM per 1 usec error + float i_fwd = 0.0001; + float i_rev = i_fwd * 10.0; + float rate_error_corner_freq = 0.05; + float max_rate_correction_ppm = 300; + float lock_thres_rate_ppm = 10.0; + uavcan::UtcDuration lock_thres_offset = uavcan::UtcDuration::fromMSec(4); + uavcan::UtcDuration min_jump = uavcan::UtcDuration::fromMSec(10); ///< Min error to jump rather than change rate +}; + +/** + * Clock rate error. * Positive if the hardware timer is slower than reference time. * This function is thread safe. */ -uavcan::int32_t getUtcSpeedCorrectionPPM(); - -/** - * Sets maximum absolute UTC speed correction in ppm. - * This function is thread safe. - */ -void setMaxUtcSpeedCorrectionPPM(uavcan::uint32_t ppm); +float getUtcRateCorrectionPPM(); /** * Number of non-gradual adjustments performed so far. * Ideally should be zero. * This function is thread safe. */ -uavcan::uint32_t getUtcAjdustmentJumpCount(); +uavcan::uint32_t getUtcJumpCount(); /** - * Returns clock error sampled at previous UTC adjustment. - * Positive if the hardware timer is slower than reference time. + * Whether UTC is synchronized and locked. * This function is thread safe. */ -uavcan::UtcDuration getPrevUtcAdjustment(); +bool isUtcLocked(); /** - * Sets minimum absolute time error to perform non-gradual jump adjustment rather than speed change. - * The parameter must be positive. - * This function is thread safe. + * UTC sync params get/set. + * Both functions are thread safe. */ -void setMinUtcJump(uavcan::UtcDuration adj); +UtcSyncParams getUtcSyncParams(); +void setUtcSyncParams(const UtcSyncParams& params); } diff --git a/libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp b/libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp index 41913071ff..5c3f919569 100644 --- a/libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp +++ b/libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp @@ -3,6 +3,7 @@ */ #include +#include #include #include #include "internal.hpp" @@ -31,25 +32,26 @@ namespace clock namespace { +const uavcan::uint32_t USecPerOverflow = 65536; + Mutex mutex; bool initialized = false; bool utc_set = false; - -// TODO: Clock speed adjustment is suboptimal, shall be reimplemented. +bool utc_locked = false; uavcan::uint32_t utc_jump_cnt = 0; -uavcan::int32_t utc_correction_usec_per_overflow_x16 = 0; -uavcan::int64_t prev_adjustment = 0; - -uavcan::UtcDuration min_utc_jump = uavcan::UtcDuration::fromMSec(10); -uavcan::int32_t max_utc_speed_correction_x16 = 20 * 16; +UtcSyncParams utc_sync_params; +float utc_prev_adj = 0; +float utc_inv_rate_error_ppm = 0; +float utc_integrated_error = 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; -const uavcan::uint32_t USecPerOverflow = 65536; - } void init() @@ -83,35 +85,25 @@ void init() TIMX->CR1 = TIM_CR1_CEN; // Start } -static uavcan::uint64_t sampleFromCriticalSection(const volatile uavcan::uint64_t* const value) +static uavcan::uint64_t sampleUtcFromCriticalSection() { assert(initialized); assert(TIMX->DIER & TIM_DIER_UIE); - volatile uavcan::uint64_t time = *value; + volatile uavcan::uint64_t time = time_utc; volatile uavcan::uint32_t cnt = TIMX->CNT; if (TIMX->SR & TIM_SR_UIF) { - /* - * The timer has overflowed either before or after CNT sample was obtained. - * We need to sample it once more to be sure that the obtained - * counter value has wrapped over zero. - */ cnt = TIMX->CNT; - /* - * The timer interrupt was set, but not handled yet. - * Thus we need to adjust the tick counter manually. - */ - time += USecPerOverflow; + time += USecPerOverflow + (utc_accumulated_correction_nsec + utc_correction_nsec_per_overflow) / 1000; } - return time + cnt; } uavcan::uint64_t getUtcUSecFromCanInterrupt() { - return utc_set ? sampleFromCriticalSection(&time_utc) : 0; + return utc_set ? sampleUtcFromCriticalSection() : 0; } uavcan::MonotonicTime getMonotonic() @@ -119,7 +111,16 @@ uavcan::MonotonicTime getMonotonic() uavcan::uint64_t usec = 0; { CriticalSectionLocker locker; - usec = sampleFromCriticalSection(&time_mono); + + volatile uavcan::uint64_t time = time_mono; + volatile uavcan::uint32_t cnt = TIMX->CNT; + if (TIMX->SR & TIM_SR_UIF) + { + cnt = TIMX->CNT; + time += USecPerOverflow; + } + usec = time + cnt; + #if !NDEBUG static uavcan::uint64_t prev_usec = 0; // Self-test assert(prev_usec <= usec); @@ -136,36 +137,65 @@ uavcan::UtcTime getUtc() uavcan::uint64_t usec = 0; { CriticalSectionLocker locker; - usec = sampleFromCriticalSection(&time_utc); + 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 = (ts - prev_utc_adj_at).toUSec() / 1e6F; + prev_utc_adj_at = ts; + + /* + * Rate error with lowpass filter + */ + const float adj_usec = adjustment.toUSec(); + const float new_inverted_rate_error_ppm = (adj_usec - utc_prev_adj) / dt;// rate error in [usec/sec], which is PPM + utc_prev_adj = adj_usec; + utc_inv_rate_error_ppm = + lowpass(utc_inv_rate_error_ppm, new_inverted_rate_error_ppm, utc_sync_params.rate_error_corner_freq, dt); + + /* + * Long term offset error + */ + if (dt < 10) + { + const float i = ((adj_usec > 0) == (utc_integrated_error > 0)) ? utc_sync_params.i_fwd : utc_sync_params.i_rev; + utc_integrated_error += adj_usec * dt * i; + utc_integrated_error = std::max(utc_integrated_error, -utc_sync_params.max_rate_correction_ppm); + utc_integrated_error = std::min(utc_integrated_error, utc_sync_params.max_rate_correction_ppm); + } + else + { + utc_integrated_error = 0; + } + + /* + * Compute final correction + */ + float rate_correction_ppm = utc_inv_rate_error_ppm + utc_integrated_error + adj_usec * utc_sync_params.p; + rate_correction_ppm = std::max(rate_correction_ppm, -utc_sync_params.max_rate_correction_ppm); + rate_correction_ppm = std::min(rate_correction_ppm, utc_sync_params.max_rate_correction_ppm); + + utc_correction_nsec_per_overflow = (USecPerOverflow * 1000) * (rate_correction_ppm / 1e6F); +} + void adjustUtc(uavcan::UtcDuration adjustment) { MutexLocker mlocker(mutex); - assert(initialized); - /* - * Naive speed adjustment - discrete PI controller. - */ - const uavcan::int64_t adj_delta = adjustment.toUSec() - prev_adjustment; - prev_adjustment = adjustment.toUSec(); - - utc_correction_usec_per_overflow_x16 += adjustment.isPositive() ? 1 : -1; - utc_correction_usec_per_overflow_x16 += (adj_delta > 0) ? 1 : -1; - - utc_correction_usec_per_overflow_x16 = std::max(utc_correction_usec_per_overflow_x16,-max_utc_speed_correction_x16); - utc_correction_usec_per_overflow_x16 = std::min(utc_correction_usec_per_overflow_x16, max_utc_speed_correction_x16); - - /* - * Clock value adjustment - * For small adjustments we will rely only on speed change - */ - if (adjustment.getAbs() > min_utc_jump || !utc_set) + if (adjustment.getAbs() > utc_sync_params.min_jump || !utc_set) { const uavcan::int64_t adj_usec = adjustment.toUSec(); @@ -181,53 +211,55 @@ void adjustUtc(uavcan::UtcDuration adjustment) } } - if (utc_set) + utc_set = true; + utc_locked = false; + utc_jump_cnt++; + utc_prev_adj = 0; + utc_inv_rate_error_ppm = 0; + } + else + { + updateRatePID(adjustment); + + if (!utc_locked) { - utc_jump_cnt++; - } - else - { - utc_set = true; - utc_correction_usec_per_overflow_x16 = 0; + utc_locked = + (std::abs(utc_inv_rate_error_ppm) < utc_sync_params.lock_thres_rate_ppm) && + (std::abs(utc_prev_adj) < utc_sync_params.lock_thres_offset.toUSec()); } } } -uavcan::int32_t getUtcSpeedCorrectionPPM() +float getUtcRateCorrectionPPM() { MutexLocker mlocker(mutex); - return uavcan::int64_t((utc_correction_usec_per_overflow_x16 * 1000000) / 16) / USecPerOverflow; + const float rate_correction_mult = utc_correction_nsec_per_overflow / float(USecPerOverflow * 1000); + return 1e6F * rate_correction_mult; } -void setMaxUtcSpeedCorrectionPPM(uavcan::uint32_t ppm) -{ - MutexLocker mlocker(mutex); - max_utc_speed_correction_x16 = (USecPerOverflow * 16LL * uavcan::int64_t(ppm)) / 1000000; -} - -uavcan::uint32_t getUtcAjdustmentJumpCount() +uavcan::uint32_t getUtcJumpCount() { MutexLocker mlocker(mutex); return utc_jump_cnt; } -uavcan::UtcDuration getPrevUtcAdjustment() +bool isUtcLocked() { MutexLocker mlocker(mutex); - return uavcan::UtcDuration::fromUSec(prev_adjustment); + return utc_locked; } -void setMinUtcJump(uavcan::UtcDuration adj) +UtcSyncParams getUtcSyncParams() { MutexLocker mlocker(mutex); - if (adj.isPositive()) - { - min_utc_jump = adj; - } - else - { - assert(0); - } + return utc_sync_params; +} + +void setUtcSyncParams(const UtcSyncParams& params) +{ + MutexLocker mlocker(mutex); + // Add some sanity check + utc_sync_params = params; } } // namespace clock @@ -269,27 +301,29 @@ UAVCAN_STM32_IRQ_HANDLER(TIMX_IRQHandler) assert(initialized); time_mono += USecPerOverflow; + if (utc_set) { - // Values below 16 are ignored - time_utc += USecPerOverflow + (utc_correction_usec_per_overflow_x16 / 16); - - // Correction slowly decays - static uavcan::uint8_t reductor; - if (reductor++ == 0) + time_utc += USecPerOverflow; + utc_accumulated_correction_nsec += utc_correction_nsec_per_overflow; + if (std::abs(utc_accumulated_correction_nsec) >= 1000) { - if (utc_correction_usec_per_overflow_x16 > 0) - { - utc_correction_usec_per_overflow_x16--; - } - else if (utc_correction_usec_per_overflow_x16 < 0) - { - utc_correction_usec_per_overflow_x16++; - } - else - { - ; // Nothing to do - } + 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 } } diff --git a/libuavcan_drivers/stm32/test_stm32f107/src/main.cpp b/libuavcan_drivers/stm32/test_stm32f107/src/main.cpp index f04cc5cd98..b4b9577c52 100644 --- a/libuavcan_drivers/stm32/test_stm32f107/src/main.cpp +++ b/libuavcan_drivers/stm32/test_stm32f107/src/main.cpp @@ -181,10 +181,10 @@ int main() } const uavcan::UtcTime utc = uavcan_stm32::clock::getUtc(); - lowsyslog("UTC %lu sec Absolute correction: %li usec Speed correction: %liPPM Jumps: %lu\n", + lowsyslog("UTC %lu sec Rate corr: %fPPM Jumps: %lu Locked: %i\n", static_cast(utc.toMSec() / 1000), - static_cast(uavcan_stm32::clock::getPrevUtcAdjustment().toUSec()), - uavcan_stm32::clock::getUtcSpeedCorrectionPPM(), - uavcan_stm32::clock::getUtcAjdustmentJumpCount()); + uavcan_stm32::clock::getUtcRateCorrectionPPM(), + uavcan_stm32::clock::getUtcJumpCount(), + int(uavcan_stm32::clock::isUtcLocked())); } }