uc_stm32_clock.cpp

00001 /*
00002  * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #include <uavcan_stm32/clock.hpp>
00006 #include <uavcan_stm32/thread.hpp>
00007 #include "internal.hpp"
00008 
00009 #if UAVCAN_STM32_TIMER_NUMBER
00010 
00011 #include <cassert>
00012 #include <cmath>
00013 
00014 /*
00015  * Timer instance
00016  */
00017 # if (UAVCAN_STM32_CHIBIOS && CH_KERNEL_MAJOR == 2) || UAVCAN_STM32_BAREMETAL || UAVCAN_STM32_FREERTOS
00018 #  define TIMX                    UAVCAN_STM32_GLUE2(TIM, UAVCAN_STM32_TIMER_NUMBER)
00019 #  define TIMX_IRQn               UAVCAN_STM32_GLUE3(TIM, UAVCAN_STM32_TIMER_NUMBER, _IRQn)
00020 #  define TIMX_INPUT_CLOCK        STM32_TIMCLK1
00021 # endif
00022 
00023 # if (UAVCAN_STM32_CHIBIOS && (CH_KERNEL_MAJOR == 3 || CH_KERNEL_MAJOR == 4 || CH_KERNEL_MAJOR == 5))
00024 #  define TIMX                    UAVCAN_STM32_GLUE2(STM32_TIM, UAVCAN_STM32_TIMER_NUMBER)
00025 #  define TIMX_IRQn               UAVCAN_STM32_GLUE3(STM32_TIM, UAVCAN_STM32_TIMER_NUMBER, _NUMBER)
00026 #  define TIMX_IRQHandler         UAVCAN_STM32_GLUE3(STM32_TIM, UAVCAN_STM32_TIMER_NUMBER, _HANDLER)
00027 #  define TIMX_INPUT_CLOCK        STM32_TIMCLK1
00028 # else
00029 #  define TIMX_IRQHandler         UAVCAN_STM32_GLUE3(TIM, UAVCAN_STM32_TIMER_NUMBER, _IRQHandler)
00030 # endif
00031 
00032 # if UAVCAN_STM32_NUTTX
00033 #  define TIMX                    UAVCAN_STM32_GLUE3(STM32_TIM, UAVCAN_STM32_TIMER_NUMBER, _BASE)
00034 #  define  TMR_REG(o)              (TIMX + (o))
00035 #  define TIMX_INPUT_CLOCK         UAVCAN_STM32_GLUE3(STM32_APB1_TIM, UAVCAN_STM32_TIMER_NUMBER, _CLKIN)
00036 
00037 #  define TIMX_IRQn                UAVCAN_STM32_GLUE2(STM32_IRQ_TIM, UAVCAN_STM32_TIMER_NUMBER)
00038 # endif
00039 
00040 # if UAVCAN_STM32_TIMER_NUMBER >= 2 && UAVCAN_STM32_TIMER_NUMBER <= 7
00041 #  define TIMX_RCC_ENR           RCC->APB1ENR
00042 #  define TIMX_RCC_RSTR          RCC->APB1RSTR
00043 #  define TIMX_RCC_ENR_MASK      UAVCAN_STM32_GLUE3(RCC_APB1ENR_TIM,  UAVCAN_STM32_TIMER_NUMBER, EN)
00044 #  define TIMX_RCC_RSTR_MASK     UAVCAN_STM32_GLUE3(RCC_APB1RSTR_TIM, UAVCAN_STM32_TIMER_NUMBER, RST)
00045 # else
00046 #  error "This UAVCAN_STM32_TIMER_NUMBER is not supported yet"
00047 # endif
00048 
00049 /**
00050  * UAVCAN_STM32_TIMX_INPUT_CLOCK can be used to manually override the auto-detected timer clock speed.
00051  * This is useful at least with certain versions of ChibiOS which do not support the bit
00052  * RCC_DKCFGR.TIMPRE that is available in newer models of STM32. In that case, if TIMPRE is active,
00053  * the auto-detected value of TIMX_INPUT_CLOCK will be twice lower than the actual clock speed.
00054  * Read this for additional context: http://www.chibios.com/forum/viewtopic.php?f=35&t=3870
00055  * A normal way to use the override feature is to provide an alternative macro, e.g.:
00056  *
00057  *      -DUAVCAN_STM32_TIMX_INPUT_CLOCK=STM32_HCLK
00058  *
00059  * Alternatively, the new clock rate can be specified directly.
00060  */
00061 # ifdef UAVCAN_STM32_TIMX_INPUT_CLOCK
00062 #  undef TIMX_INPUT_CLOCK
00063 #  define TIMX_INPUT_CLOCK      UAVCAN_STM32_TIMX_INPUT_CLOCK
00064 # endif
00065 
00066 extern "C" UAVCAN_STM32_IRQ_HANDLER(TIMX_IRQHandler);
00067 
00068 namespace uavcan_stm32
00069 {
00070 namespace clock
00071 {
00072 namespace
00073 {
00074 
00075 const uavcan::uint32_t USecPerOverflow = 65536;
00076 
00077 Mutex mutex;
00078 
00079 bool initialized = false;
00080 
00081 bool utc_set = false;
00082 bool utc_locked = false;
00083 uavcan::uint32_t utc_jump_cnt = 0;
00084 UtcSyncParams utc_sync_params;
00085 float utc_prev_adj = 0;
00086 float utc_rel_rate_ppm = 0;
00087 float utc_rel_rate_error_integral = 0;
00088 uavcan::int32_t utc_accumulated_correction_nsec = 0;
00089 uavcan::int32_t utc_correction_nsec_per_overflow = 0;
00090 uavcan::MonotonicTime prev_utc_adj_at;
00091 
00092 uavcan::uint64_t time_mono = 0;
00093 uavcan::uint64_t time_utc = 0;
00094 
00095 }
00096 
00097 #if UAVCAN_STM32_BAREMETAL || UAVCAN_STM32_FREERTOS
00098 
00099 static void nvicEnableVector(IRQn_Type irq,  uint8_t prio)
00100 {
00101     #if !defined (USE_HAL_DRIVER)
00102       NVIC_InitTypeDef NVIC_InitStructure;
00103       NVIC_InitStructure.NVIC_IRQChannel = irq;
00104       NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = prio;
00105       NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
00106       NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
00107       NVIC_Init(&NVIC_InitStructure);
00108     #else
00109       HAL_NVIC_SetPriority(irq, prio, 0);
00110       HAL_NVIC_EnableIRQ(irq);
00111     #endif
00112 
00113 }
00114 
00115 #endif
00116 
00117 void init()
00118 {
00119     CriticalSectionLocker lock;
00120     if (initialized)
00121     {
00122         return;
00123     }
00124     initialized = true;
00125 
00126 
00127 # if UAVCAN_STM32_CHIBIOS || UAVCAN_STM32_BAREMETAL || UAVCAN_STM32_FREERTOS
00128     // Power-on and reset
00129     TIMX_RCC_ENR |= TIMX_RCC_ENR_MASK;
00130     TIMX_RCC_RSTR |=  TIMX_RCC_RSTR_MASK;
00131     TIMX_RCC_RSTR &= ~TIMX_RCC_RSTR_MASK;
00132 
00133     // Enable IRQ
00134     nvicEnableVector(TIMX_IRQn,  UAVCAN_STM32_IRQ_PRIORITY_MASK);
00135 
00136 # if (TIMX_INPUT_CLOCK % 1000000) != 0
00137 #  error "No way, timer clock must be divisible by 1e6. FIXME!"
00138 # endif
00139 
00140     // Start the timer
00141     TIMX->ARR  = 0xFFFF;
00142     TIMX->PSC  = (TIMX_INPUT_CLOCK / 1000000) - 1;  // 1 tick == 1 microsecond
00143     TIMX->CR1  = TIM_CR1_URS;
00144     TIMX->SR   = 0;
00145     TIMX->EGR  = TIM_EGR_UG;     // Reload immediately
00146     TIMX->DIER = TIM_DIER_UIE;
00147     TIMX->CR1  = TIM_CR1_CEN;    // Start
00148 
00149 # endif
00150 
00151 # if UAVCAN_STM32_NUTTX
00152 
00153     // Attach IRQ
00154     irq_attach(TIMX_IRQn, &TIMX_IRQHandler);
00155 
00156     // Power-on and reset
00157     modifyreg32(STM32_RCC_APB1ENR, 0, TIMX_RCC_ENR_MASK);
00158     modifyreg32(STM32_RCC_APB1RSTR, 0, TIMX_RCC_RSTR_MASK);
00159     modifyreg32(STM32_RCC_APB1RSTR, TIMX_RCC_RSTR_MASK, 0);
00160 
00161 
00162     // Start the timer
00163     putreg32(0xFFFF, TMR_REG(STM32_BTIM_ARR_OFFSET));
00164     putreg16(((TIMX_INPUT_CLOCK / 1000000)-1), TMR_REG(STM32_BTIM_PSC_OFFSET));
00165     putreg16(BTIM_CR1_URS, TMR_REG(STM32_BTIM_CR1_OFFSET));
00166     putreg16(0, TMR_REG(STM32_BTIM_SR_OFFSET));
00167     putreg16(BTIM_EGR_UG, TMR_REG(STM32_BTIM_EGR_OFFSET)); // Reload immediately
00168     putreg16(BTIM_DIER_UIE, TMR_REG(STM32_BTIM_DIER_OFFSET));
00169     putreg16(BTIM_CR1_CEN, TMR_REG(STM32_BTIM_CR1_OFFSET)); // Start
00170 
00171     // Prioritize and Enable  IRQ
00172 // todo: Currently changing the NVIC_SYSH_HIGH_PRIORITY is HARD faulting
00173 // need to investigate
00174 //    up_prioritize_irq(TIMX_IRQn, NVIC_SYSH_HIGH_PRIORITY);
00175     up_enable_irq(TIMX_IRQn);
00176 
00177 # endif
00178 }
00179 
00180 void setUtc(uavcan::UtcTime time)
00181 {
00182     MutexLocker mlocker(mutex);
00183     UAVCAN_ASSERT(initialized);
00184 
00185     {
00186         CriticalSectionLocker locker;
00187         time_utc = time.toUSec();
00188     }
00189 
00190     utc_set = true;
00191     utc_locked = false;
00192     utc_jump_cnt++;
00193     utc_prev_adj = 0;
00194     utc_rel_rate_ppm = 0;
00195 }
00196 
00197 static uavcan::uint64_t sampleUtcFromCriticalSection()
00198 {
00199 # if UAVCAN_STM32_CHIBIOS || UAVCAN_STM32_BAREMETAL || UAVCAN_STM32_FREERTOS
00200     UAVCAN_ASSERT(initialized);
00201     UAVCAN_ASSERT(TIMX->DIER & TIM_DIER_UIE);
00202 
00203     volatile uavcan::uint64_t time = time_utc;
00204     volatile uavcan::uint32_t cnt = TIMX->CNT;
00205 
00206     if (TIMX->SR & TIM_SR_UIF)
00207     {
00208         cnt = TIMX->CNT;
00209         const uavcan::int32_t add = uavcan::int32_t(USecPerOverflow) +
00210                                     (utc_accumulated_correction_nsec + utc_correction_nsec_per_overflow) / 1000;
00211         time = uavcan::uint64_t(uavcan::int64_t(time) + add);
00212     }
00213     return time + cnt;
00214 # endif
00215 
00216 # if UAVCAN_STM32_NUTTX
00217 
00218     UAVCAN_ASSERT(initialized);
00219     UAVCAN_ASSERT(getreg16(TMR_REG(STM32_BTIM_DIER_OFFSET)) & BTIM_DIER_UIE);
00220 
00221     volatile uavcan::uint64_t time = time_utc;
00222     volatile uavcan::uint32_t cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET));
00223 
00224     if (getreg16(TMR_REG(STM32_BTIM_SR_OFFSET)) & BTIM_SR_UIF)
00225     {
00226         cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET));
00227         const uavcan::int32_t add = uavcan::int32_t(USecPerOverflow) +
00228                                     (utc_accumulated_correction_nsec + utc_correction_nsec_per_overflow) / 1000;
00229         time = uavcan::uint64_t(uavcan::int64_t(time) + add);
00230     }
00231     return time + cnt;
00232 # endif
00233 }
00234 
00235 uavcan::uint64_t getUtcUSecFromCanInterrupt()
00236 {
00237     return utc_set ? sampleUtcFromCriticalSection() : 0;
00238 }
00239 
00240 uavcan::MonotonicTime getMonotonic()
00241 {
00242     uavcan::uint64_t usec = 0;
00243     // Scope Critical section
00244     {
00245         CriticalSectionLocker locker;
00246 
00247         volatile uavcan::uint64_t time = time_mono;
00248 
00249 # if UAVCAN_STM32_CHIBIOS || UAVCAN_STM32_BAREMETAL || UAVCAN_STM32_FREERTOS
00250 
00251         volatile uavcan::uint32_t cnt = TIMX->CNT;
00252         if (TIMX->SR & TIM_SR_UIF)
00253         {
00254             cnt = TIMX->CNT;
00255 # endif
00256 
00257 # if UAVCAN_STM32_NUTTX
00258 
00259         volatile uavcan::uint32_t cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET));
00260 
00261         if (getreg16(TMR_REG(STM32_BTIM_SR_OFFSET)) & BTIM_SR_UIF)
00262         {
00263             cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET));
00264 # endif
00265             time += USecPerOverflow;
00266         }
00267         usec = time + cnt;
00268 
00269 # ifndef NDEBUG
00270     static uavcan::uint64_t prev_usec = 0;      // Self-test
00271     UAVCAN_ASSERT(prev_usec <= usec);
00272     (void)prev_usec;
00273     prev_usec = usec;
00274 # endif
00275    } // End Scope Critical section
00276 
00277    return uavcan::MonotonicTime::fromUSec(usec);
00278 }
00279 
00280 uavcan::UtcTime getUtc()
00281 {
00282     if (utc_set)
00283     {
00284         uavcan::uint64_t usec = 0;
00285         {
00286             CriticalSectionLocker locker;
00287             usec = sampleUtcFromCriticalSection();
00288         }
00289         return uavcan::UtcTime::fromUSec(usec);
00290     }
00291     return uavcan::UtcTime();
00292 }
00293 
00294 static float lowpass(float xold, float xnew, float corner, float dt)
00295 {
00296     const float tau = 1.F / corner;
00297     return (dt * xnew + tau * xold) / (dt + tau);
00298 }
00299 
00300 static void updateRatePID(uavcan::UtcDuration adjustment)
00301 {
00302     const uavcan::MonotonicTime ts = getMonotonic();
00303     const float dt = float((ts - prev_utc_adj_at).toUSec()) / 1e6F;
00304     prev_utc_adj_at = ts;
00305     const float adj_usec = float(adjustment.toUSec());
00306 
00307     /*
00308      * Target relative rate in PPM
00309      * Positive to go faster
00310      */
00311     const float target_rel_rate_ppm = adj_usec * utc_sync_params.offset_p;
00312 
00313     /*
00314      * Current relative rate in PPM
00315      * Positive if the local clock is faster
00316      */
00317     const float new_rel_rate_ppm = (utc_prev_adj - adj_usec) / dt; // rate error in [usec/sec], which is PPM
00318     utc_prev_adj = adj_usec;
00319     utc_rel_rate_ppm = lowpass(utc_rel_rate_ppm, new_rel_rate_ppm, utc_sync_params.rate_error_corner_freq, dt);
00320 
00321     const float rel_rate_error = target_rel_rate_ppm - utc_rel_rate_ppm;
00322 
00323     if (dt > 10)
00324     {
00325         utc_rel_rate_error_integral = 0;
00326     }
00327     else
00328     {
00329         utc_rel_rate_error_integral += rel_rate_error * dt * utc_sync_params.rate_i;
00330         utc_rel_rate_error_integral =
00331             uavcan::max(utc_rel_rate_error_integral, -utc_sync_params.max_rate_correction_ppm);
00332         utc_rel_rate_error_integral =
00333             uavcan::min(utc_rel_rate_error_integral, utc_sync_params.max_rate_correction_ppm);
00334     }
00335 
00336     /*
00337      * Rate controller
00338      */
00339     float total_rate_correction_ppm = rel_rate_error + utc_rel_rate_error_integral;
00340     total_rate_correction_ppm = uavcan::max(total_rate_correction_ppm, -utc_sync_params.max_rate_correction_ppm);
00341     total_rate_correction_ppm = uavcan::min(total_rate_correction_ppm, utc_sync_params.max_rate_correction_ppm);
00342 
00343     utc_correction_nsec_per_overflow = uavcan::int32_t((USecPerOverflow * 1000) * (total_rate_correction_ppm / 1e6F));
00344 
00345 //    syslog("$ adj=%f   rel_rate=%f   rel_rate_eint=%f   tgt_rel_rate=%f   ppm=%f\n",
00346 //              adj_usec, utc_rel_rate_ppm, utc_rel_rate_error_integral, target_rel_rate_ppm,
00347 // total_rate_correction_ppm);
00348 }
00349 
00350 void adjustUtc(uavcan::UtcDuration adjustment)
00351 {
00352     MutexLocker mlocker(mutex);
00353     UAVCAN_ASSERT(initialized);
00354 
00355     if (adjustment.getAbs() > utc_sync_params.min_jump || !utc_set)
00356     {
00357         const uavcan::int64_t adj_usec = adjustment.toUSec();
00358 
00359         {
00360             CriticalSectionLocker locker;
00361             if ((adj_usec < 0) && uavcan::uint64_t(-adj_usec) > time_utc)
00362             {
00363                 time_utc = 1;
00364             }
00365             else
00366             {
00367                 time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + adj_usec);
00368             }
00369         }
00370 
00371         utc_set = true;
00372         utc_locked = false;
00373         utc_jump_cnt++;
00374         utc_prev_adj = 0;
00375         utc_rel_rate_ppm = 0;
00376     }
00377     else
00378     {
00379         updateRatePID(adjustment);
00380 
00381         if (!utc_locked)
00382         {
00383             utc_locked =
00384                 (std::abs(utc_rel_rate_ppm) < utc_sync_params.lock_thres_rate_ppm) &&
00385                 (std::abs(utc_prev_adj) < float(utc_sync_params.lock_thres_offset.toUSec()));
00386         }
00387     }
00388 }
00389 
00390 float getUtcRateCorrectionPPM()
00391 {
00392     MutexLocker mlocker(mutex);
00393     const float rate_correction_mult = float(utc_correction_nsec_per_overflow) / float(USecPerOverflow * 1000);
00394     return 1e6F * rate_correction_mult;
00395 }
00396 
00397 uavcan::uint32_t getUtcJumpCount()
00398 {
00399     MutexLocker mlocker(mutex);
00400     return utc_jump_cnt;
00401 }
00402 
00403 bool isUtcLocked()
00404 {
00405     MutexLocker mlocker(mutex);
00406     return utc_locked;
00407 }
00408 
00409 UtcSyncParams getUtcSyncParams()
00410 {
00411     MutexLocker mlocker(mutex);
00412     return utc_sync_params;
00413 }
00414 
00415 void setUtcSyncParams(const UtcSyncParams& params)
00416 {
00417     MutexLocker mlocker(mutex);
00418     // Add some sanity check
00419     utc_sync_params = params;
00420 }
00421 
00422 } // namespace clock
00423 
00424 SystemClock& SystemClock::instance()
00425 {
00426     static union SystemClockStorage
00427     {
00428         uavcan::uint8_t buffer[sizeof(SystemClock)];
00429         long long _aligner_1;
00430         long double _aligner_2;
00431     } storage;
00432 
00433     SystemClock* const ptr = reinterpret_cast<SystemClock*>(storage.buffer);
00434 
00435     if (!clock::initialized)
00436     {
00437         MutexLocker mlocker(clock::mutex);
00438         clock::init();
00439         new (ptr)SystemClock();
00440     }
00441     return *ptr;
00442 }
00443 
00444 } // namespace uavcan_stm32
00445 
00446 
00447 /**
00448  * Timer interrupt handler
00449  */
00450 
00451 extern "C"
00452 UAVCAN_STM32_IRQ_HANDLER(TIMX_IRQHandler)
00453 {
00454     UAVCAN_STM32_IRQ_PROLOGUE();
00455 
00456 # if UAVCAN_STM32_CHIBIOS || UAVCAN_STM32_BAREMETAL || UAVCAN_STM32_FREERTOS
00457     TIMX->SR = 0;
00458 # endif
00459 # if UAVCAN_STM32_NUTTX
00460     putreg16(0, TMR_REG(STM32_BTIM_SR_OFFSET));
00461 # endif
00462 
00463     using namespace uavcan_stm32::clock;
00464     UAVCAN_ASSERT(initialized);
00465 
00466     time_mono += USecPerOverflow;
00467 
00468     if (utc_set)
00469     {
00470         time_utc += USecPerOverflow;
00471         utc_accumulated_correction_nsec += utc_correction_nsec_per_overflow;
00472         if (std::abs(utc_accumulated_correction_nsec) >= 1000)
00473         {
00474             time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + utc_accumulated_correction_nsec / 1000);
00475             utc_accumulated_correction_nsec %= 1000;
00476         }
00477 
00478         // Correction decay - 1 nsec per 65536 usec
00479         if (utc_correction_nsec_per_overflow > 0)
00480         {
00481             utc_correction_nsec_per_overflow--;
00482         }
00483         else if (utc_correction_nsec_per_overflow < 0)
00484         {
00485             utc_correction_nsec_per_overflow++;
00486         }
00487         else
00488         {
00489             ; // Zero
00490         }
00491     }
00492 
00493     UAVCAN_STM32_IRQ_EPILOGUE();
00494 }
00495 
00496 #endif