uc_stm32_clock.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
3  */
4 
5 #include <uavcan_stm32/clock.hpp>
6 #include <uavcan_stm32/thread.hpp>
7 #include "internal.hpp"
8 
9 #if UAVCAN_STM32_TIMER_NUMBER
10 
11 #include <cassert>
12 #include <cmath>
13 
14 /*
15  * Timer instance
16  */
17 # if (UAVCAN_STM32_CHIBIOS && CH_KERNEL_MAJOR == 2) || UAVCAN_STM32_BAREMETAL || UAVCAN_STM32_FREERTOS
18 # define TIMX UAVCAN_STM32_GLUE2(TIM, UAVCAN_STM32_TIMER_NUMBER)
19 # define TIMX_IRQn UAVCAN_STM32_GLUE3(TIM, UAVCAN_STM32_TIMER_NUMBER, _IRQn)
20 # define TIMX_INPUT_CLOCK STM32_TIMCLK1
21 # endif
22 
23 # if (UAVCAN_STM32_CHIBIOS && (CH_KERNEL_MAJOR == 3 || CH_KERNEL_MAJOR == 4 || CH_KERNEL_MAJOR == 5))
24 # define TIMX UAVCAN_STM32_GLUE2(STM32_TIM, UAVCAN_STM32_TIMER_NUMBER)
25 # define TIMX_IRQn UAVCAN_STM32_GLUE3(STM32_TIM, UAVCAN_STM32_TIMER_NUMBER, _NUMBER)
26 # define TIMX_IRQHandler UAVCAN_STM32_GLUE3(STM32_TIM, UAVCAN_STM32_TIMER_NUMBER, _HANDLER)
27 # define TIMX_INPUT_CLOCK STM32_TIMCLK1
28 # else
29 # define TIMX_IRQHandler UAVCAN_STM32_GLUE3(TIM, UAVCAN_STM32_TIMER_NUMBER, _IRQHandler)
30 # endif
31 
32 # if UAVCAN_STM32_NUTTX
33 # define TIMX UAVCAN_STM32_GLUE3(STM32_TIM, UAVCAN_STM32_TIMER_NUMBER, _BASE)
34 # define TMR_REG(o) (TIMX + (o))
35 # define TIMX_INPUT_CLOCK UAVCAN_STM32_GLUE3(STM32_APB1_TIM, UAVCAN_STM32_TIMER_NUMBER, _CLKIN)
36 
37 # define TIMX_IRQn UAVCAN_STM32_GLUE2(STM32_IRQ_TIM, UAVCAN_STM32_TIMER_NUMBER)
38 # endif
39 
40 # if UAVCAN_STM32_TIMER_NUMBER >= 2 && UAVCAN_STM32_TIMER_NUMBER <= 7
41 # if defined(STM32L4xx)
42 # define TIMX_RCC_ENR RCC->APB1ENR1
43 # define TIMX_RCC_RSTR RCC->APB1RSTR1
44 # define TIMX_RCC_ENR_MASK UAVCAN_STM32_GLUE3(RCC_APB1ENR1_TIM, UAVCAN_STM32_TIMER_NUMBER, EN)
45 # define TIMX_RCC_RSTR_MASK UAVCAN_STM32_GLUE3(RCC_APB1RSTR1_TIM, UAVCAN_STM32_TIMER_NUMBER, RST)
46 # else
47 # define TIMX_RCC_ENR RCC->APB1ENR
48 # define TIMX_RCC_RSTR RCC->APB1RSTR
49 # define TIMX_RCC_ENR_MASK UAVCAN_STM32_GLUE3(RCC_APB1ENR_TIM, UAVCAN_STM32_TIMER_NUMBER, EN)
50 # define TIMX_RCC_RSTR_MASK UAVCAN_STM32_GLUE3(RCC_APB1RSTR_TIM, UAVCAN_STM32_TIMER_NUMBER, RST)
51 # endif
52 # else
53 # error "This UAVCAN_STM32_TIMER_NUMBER is not supported yet"
54 # endif
55 
68 # ifdef UAVCAN_STM32_TIMX_INPUT_CLOCK
69 # undef TIMX_INPUT_CLOCK
70 # define TIMX_INPUT_CLOCK UAVCAN_STM32_TIMX_INPUT_CLOCK
71 # endif
72 
73 extern "C" UAVCAN_STM32_IRQ_HANDLER(TIMX_IRQHandler);
74 
75 namespace uavcan_stm32
76 {
77 namespace clock
78 {
79 namespace
80 {
81 
82 const uavcan::uint32_t USecPerOverflow = 65536;
83 
84 Mutex mutex;
85 
86 bool initialized = false;
87 
88 bool utc_set = false;
89 bool utc_locked = false;
90 uavcan::uint32_t utc_jump_cnt = 0;
91 UtcSyncParams utc_sync_params;
92 float utc_prev_adj = 0;
93 float utc_rel_rate_ppm = 0;
94 float utc_rel_rate_error_integral = 0;
95 uavcan::int32_t utc_accumulated_correction_nsec = 0;
96 uavcan::int32_t utc_correction_nsec_per_overflow = 0;
97 uavcan::MonotonicTime prev_utc_adj_at;
98 
99 uavcan::uint64_t time_mono = 0;
100 uavcan::uint64_t time_utc = 0;
101 
102 }
103 
104 #if UAVCAN_STM32_BAREMETAL || UAVCAN_STM32_FREERTOS
105 
106 static void nvicEnableVector(IRQn_Type irq, uint8_t prio)
107 {
108  #if !defined (USE_HAL_DRIVER)
109  NVIC_InitTypeDef NVIC_InitStructure;
110  NVIC_InitStructure.NVIC_IRQChannel = irq;
111  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = prio;
112  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
113  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
114  NVIC_Init(&NVIC_InitStructure);
115  #else
116  HAL_NVIC_SetPriority(irq, prio, 0);
117  HAL_NVIC_EnableIRQ(irq);
118  #endif
119 
120 }
121 
122 #endif
123 
124 void init()
125 {
126  CriticalSectionLocker lock;
127  if (initialized)
128  {
129  return;
130  }
131  initialized = true;
132 
133 
134 # if UAVCAN_STM32_CHIBIOS || UAVCAN_STM32_BAREMETAL || UAVCAN_STM32_FREERTOS
135  // Power-on and reset
136  TIMX_RCC_ENR |= TIMX_RCC_ENR_MASK;
137  TIMX_RCC_RSTR |= TIMX_RCC_RSTR_MASK;
138  TIMX_RCC_RSTR &= ~TIMX_RCC_RSTR_MASK;
139 
140  // Enable IRQ
141  nvicEnableVector(TIMX_IRQn, UAVCAN_STM32_IRQ_PRIORITY_MASK);
142 
143 # if (TIMX_INPUT_CLOCK % 1000000) != 0
144 # error "No way, timer clock must be divisible by 1e6. FIXME!"
145 # endif
146 
147  // Start the timer
148  TIMX->ARR = 0xFFFF;
149  TIMX->PSC = (TIMX_INPUT_CLOCK / 1000000) - 1; // 1 tick == 1 microsecond
150  TIMX->CR1 = TIM_CR1_URS;
151  TIMX->SR = 0;
152  TIMX->EGR = TIM_EGR_UG; // Reload immediately
153  TIMX->DIER = TIM_DIER_UIE;
154  TIMX->CR1 = TIM_CR1_CEN; // Start
155 
156 # endif
157 
158 # if UAVCAN_STM32_NUTTX
159 
160  // Attach IRQ
161  irq_attach(TIMX_IRQn, &TIMX_IRQHandler, NULL);
162 
163  // Power-on and reset
164  modifyreg32(STM32_RCC_APB1ENR, 0, TIMX_RCC_ENR_MASK);
165  modifyreg32(STM32_RCC_APB1RSTR, 0, TIMX_RCC_RSTR_MASK);
166  modifyreg32(STM32_RCC_APB1RSTR, TIMX_RCC_RSTR_MASK, 0);
167 
168 
169  // Start the timer
170  putreg32(0xFFFF, TMR_REG(STM32_BTIM_ARR_OFFSET));
171  putreg16(((TIMX_INPUT_CLOCK / 1000000)-1), TMR_REG(STM32_BTIM_PSC_OFFSET));
172  putreg16(BTIM_CR1_URS, TMR_REG(STM32_BTIM_CR1_OFFSET));
173  putreg16(0, TMR_REG(STM32_BTIM_SR_OFFSET));
174  putreg16(BTIM_EGR_UG, TMR_REG(STM32_BTIM_EGR_OFFSET)); // Reload immediately
175  putreg16(BTIM_DIER_UIE, TMR_REG(STM32_BTIM_DIER_OFFSET));
176  putreg16(BTIM_CR1_CEN, TMR_REG(STM32_BTIM_CR1_OFFSET)); // Start
177 
178  // Prioritize and Enable IRQ
179 // todo: Currently changing the NVIC_SYSH_HIGH_PRIORITY is HARD faulting
180 // need to investigate
181 // up_prioritize_irq(TIMX_IRQn, NVIC_SYSH_HIGH_PRIORITY);
182  up_enable_irq(TIMX_IRQn);
183 
184 # endif
185 }
186 
187 void setUtc(uavcan::UtcTime time)
188 {
189  MutexLocker mlocker(mutex);
190  UAVCAN_ASSERT(initialized);
191 
192  {
193  CriticalSectionLocker locker;
194  time_utc = time.toUSec();
195  }
196 
197  utc_set = true;
198  utc_locked = false;
199  utc_jump_cnt++;
200  utc_prev_adj = 0;
201  utc_rel_rate_ppm = 0;
202 }
203 
204 static uavcan::uint64_t sampleUtcFromCriticalSection()
205 {
206 # if UAVCAN_STM32_CHIBIOS || UAVCAN_STM32_BAREMETAL || UAVCAN_STM32_FREERTOS
207  UAVCAN_ASSERT(initialized);
208  UAVCAN_ASSERT(TIMX->DIER & TIM_DIER_UIE);
209 
210  volatile uavcan::uint64_t time = time_utc;
211  volatile uavcan::uint32_t cnt = TIMX->CNT;
212 
213  if (TIMX->SR & TIM_SR_UIF)
214  {
215  cnt = TIMX->CNT;
216  const uavcan::int32_t add = uavcan::int32_t(USecPerOverflow) +
217  (utc_accumulated_correction_nsec + utc_correction_nsec_per_overflow) / 1000;
218  time = uavcan::uint64_t(uavcan::int64_t(time) + add);
219  }
220  return time + cnt;
221 # endif
222 
223 # if UAVCAN_STM32_NUTTX
224 
225  UAVCAN_ASSERT(initialized);
226  UAVCAN_ASSERT(getreg16(TMR_REG(STM32_BTIM_DIER_OFFSET)) & BTIM_DIER_UIE);
227 
228  volatile uavcan::uint64_t time = time_utc;
229  volatile uavcan::uint32_t cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET));
230 
231  if (getreg16(TMR_REG(STM32_BTIM_SR_OFFSET)) & BTIM_SR_UIF)
232  {
233  cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET));
234  const uavcan::int32_t add = uavcan::int32_t(USecPerOverflow) +
235  (utc_accumulated_correction_nsec + utc_correction_nsec_per_overflow) / 1000;
236  time = uavcan::uint64_t(uavcan::int64_t(time) + add);
237  }
238  return time + cnt;
239 # endif
240 }
241 
242 uavcan::uint64_t getUtcUSecFromCanInterrupt()
243 {
244  return utc_set ? sampleUtcFromCriticalSection() : 0;
245 }
246 
247 uavcan::MonotonicTime getMonotonic()
248 {
249  uavcan::uint64_t usec = 0;
250  // Scope Critical section
251  {
252  CriticalSectionLocker locker;
253 
254  volatile uavcan::uint64_t time = time_mono;
255 
256 # if UAVCAN_STM32_CHIBIOS || UAVCAN_STM32_BAREMETAL || UAVCAN_STM32_FREERTOS
257 
258  volatile uavcan::uint32_t cnt = TIMX->CNT;
259  if (TIMX->SR & TIM_SR_UIF)
260  {
261  cnt = TIMX->CNT;
262 # endif
263 
264 # if UAVCAN_STM32_NUTTX
265 
266  volatile uavcan::uint32_t cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET));
267 
268  if (getreg16(TMR_REG(STM32_BTIM_SR_OFFSET)) & BTIM_SR_UIF)
269  {
270  cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET));
271 # endif
272  time += USecPerOverflow;
273  }
274  usec = time + cnt;
275 
276 # ifndef NDEBUG
277  static uavcan::uint64_t prev_usec = 0; // Self-test
278  UAVCAN_ASSERT(prev_usec <= usec);
279  (void)prev_usec;
280  prev_usec = usec;
281 # endif
282  } // End Scope Critical section
283 
284  return uavcan::MonotonicTime::fromUSec(usec);
285 }
286 
287 uavcan::UtcTime getUtc()
288 {
289  if (utc_set)
290  {
291  uavcan::uint64_t usec = 0;
292  {
293  CriticalSectionLocker locker;
294  usec = sampleUtcFromCriticalSection();
295  }
296  return uavcan::UtcTime::fromUSec(usec);
297  }
298  return uavcan::UtcTime();
299 }
300 
301 static float lowpass(float xold, float xnew, float corner, float dt)
302 {
303  const float tau = 1.F / corner;
304  return (dt * xnew + tau * xold) / (dt + tau);
305 }
306 
307 static void updateRatePID(uavcan::UtcDuration adjustment)
308 {
309  const uavcan::MonotonicTime ts = getMonotonic();
310  const float dt = float((ts - prev_utc_adj_at).toUSec()) / 1e6F;
311  prev_utc_adj_at = ts;
312  const float adj_usec = float(adjustment.toUSec());
313 
314  /*
315  * Target relative rate in PPM
316  * Positive to go faster
317  */
318  const float target_rel_rate_ppm = adj_usec * utc_sync_params.offset_p;
319 
320  /*
321  * Current relative rate in PPM
322  * Positive if the local clock is faster
323  */
324  const float new_rel_rate_ppm = (utc_prev_adj - adj_usec) / dt; // rate error in [usec/sec], which is PPM
325  utc_prev_adj = adj_usec;
326  utc_rel_rate_ppm = lowpass(utc_rel_rate_ppm, new_rel_rate_ppm, utc_sync_params.rate_error_corner_freq, dt);
327 
328  const float rel_rate_error = target_rel_rate_ppm - utc_rel_rate_ppm;
329 
330  if (dt > 10)
331  {
332  utc_rel_rate_error_integral = 0;
333  }
334  else
335  {
336  utc_rel_rate_error_integral += rel_rate_error * dt * utc_sync_params.rate_i;
337  utc_rel_rate_error_integral =
338  uavcan::max(utc_rel_rate_error_integral, -utc_sync_params.max_rate_correction_ppm);
339  utc_rel_rate_error_integral =
340  uavcan::min(utc_rel_rate_error_integral, utc_sync_params.max_rate_correction_ppm);
341  }
342 
343  /*
344  * Rate controller
345  */
346  float total_rate_correction_ppm = rel_rate_error + utc_rel_rate_error_integral;
347  total_rate_correction_ppm = uavcan::max(total_rate_correction_ppm, -utc_sync_params.max_rate_correction_ppm);
348  total_rate_correction_ppm = uavcan::min(total_rate_correction_ppm, utc_sync_params.max_rate_correction_ppm);
349 
350  utc_correction_nsec_per_overflow = uavcan::int32_t((USecPerOverflow * 1000) * (total_rate_correction_ppm / 1e6F));
351 
352 // syslog("$ adj=%f rel_rate=%f rel_rate_eint=%f tgt_rel_rate=%f ppm=%f\n",
353 // adj_usec, utc_rel_rate_ppm, utc_rel_rate_error_integral, target_rel_rate_ppm,
354 // total_rate_correction_ppm);
355 }
356 
357 void adjustUtc(uavcan::UtcDuration adjustment)
358 {
359  MutexLocker mlocker(mutex);
360  UAVCAN_ASSERT(initialized);
361 
362  if (adjustment.getAbs() > utc_sync_params.min_jump || !utc_set)
363  {
364  const uavcan::int64_t adj_usec = adjustment.toUSec();
365 
366  {
367  CriticalSectionLocker locker;
368  if ((adj_usec < 0) && uavcan::uint64_t(-adj_usec) > time_utc)
369  {
370  time_utc = 1;
371  }
372  else
373  {
374  time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + adj_usec);
375  }
376  }
377 
378  utc_set = true;
379  utc_locked = false;
380  utc_jump_cnt++;
381  utc_prev_adj = 0;
382  utc_rel_rate_ppm = 0;
383  }
384  else
385  {
386  updateRatePID(adjustment);
387 
388  if (!utc_locked)
389  {
390  utc_locked =
391  (std::abs(utc_rel_rate_ppm) < utc_sync_params.lock_thres_rate_ppm) &&
392  (std::abs(utc_prev_adj) < float(utc_sync_params.lock_thres_offset.toUSec()));
393  }
394  }
395 }
396 
397 float getUtcRateCorrectionPPM()
398 {
399  MutexLocker mlocker(mutex);
400  const float rate_correction_mult = float(utc_correction_nsec_per_overflow) / float(USecPerOverflow * 1000);
401  return 1e6F * rate_correction_mult;
402 }
403 
404 uavcan::uint32_t getUtcJumpCount()
405 {
406  MutexLocker mlocker(mutex);
407  return utc_jump_cnt;
408 }
409 
410 bool isUtcLocked()
411 {
412  MutexLocker mlocker(mutex);
413  return utc_locked;
414 }
415 
416 UtcSyncParams getUtcSyncParams()
417 {
418  MutexLocker mlocker(mutex);
419  return utc_sync_params;
420 }
421 
422 void setUtcSyncParams(const UtcSyncParams& params)
423 {
424  MutexLocker mlocker(mutex);
425  // Add some sanity check
426  utc_sync_params = params;
427 }
428 
429 } // namespace clock
430 
431 SystemClock& SystemClock::instance()
432 {
433  static union SystemClockStorage
434  {
435  uavcan::uint8_t buffer[sizeof(SystemClock)];
436  long long _aligner_1;
437  long double _aligner_2;
438  } storage;
439 
440  SystemClock* const ptr = reinterpret_cast<SystemClock*>(storage.buffer);
441 
442  if (!clock::initialized)
443  {
444  MutexLocker mlocker(clock::mutex);
445  clock::init();
446  new (ptr)SystemClock();
447  }
448  return *ptr;
449 }
450 
451 } // namespace uavcan_stm32
452 
453 
458 extern "C"
459 UAVCAN_STM32_IRQ_HANDLER(TIMX_IRQHandler)
460 {
461  UAVCAN_STM32_IRQ_PROLOGUE();
462 
463 # if UAVCAN_STM32_CHIBIOS || UAVCAN_STM32_BAREMETAL || UAVCAN_STM32_FREERTOS
464  TIMX->SR = 0;
465 # endif
466 # if UAVCAN_STM32_NUTTX
467  putreg16(0, TMR_REG(STM32_BTIM_SR_OFFSET));
468 # endif
469 
470  using namespace uavcan_stm32::clock;
471  UAVCAN_ASSERT(initialized);
472 
473  time_mono += USecPerOverflow;
474 
475  if (utc_set)
476  {
477  time_utc += USecPerOverflow;
478  utc_accumulated_correction_nsec += utc_correction_nsec_per_overflow;
479  if (std::abs(utc_accumulated_correction_nsec) >= 1000)
480  {
481  time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + utc_accumulated_correction_nsec / 1000);
482  utc_accumulated_correction_nsec %= 1000;
483  }
484 
485  // Correction decay - 1 nsec per 65536 usec
486  if (utc_correction_nsec_per_overflow > 0)
487  {
488  utc_correction_nsec_per_overflow--;
489  }
490  else if (utc_correction_nsec_per_overflow < 0)
491  {
492  utc_correction_nsec_per_overflow++;
493  }
494  else
495  {
496  ; // Zero
497  }
498  }
499 
500  UAVCAN_STM32_IRQ_EPILOGUE();
501 }
502 
503 #endif
uavcan_stm32::clock::getUtcJumpCount
uavcan::uint32_t getUtcJumpCount()
uavcan_stm32::clock::setUtc
void setUtc(uavcan::UtcTime time)
uavcan::uint64_t
std::uint64_t uint64_t
Definition: std.hpp:27
thread.hpp
uavcan::uint32_t
std::uint32_t uint32_t
Definition: std.hpp:26
uavcan::UtcTime
Implicitly convertible to/from uavcan.Timestamp.
Definition: time.hpp:191
uavcan::UtcDuration
Definition: time.hpp:189
uavcan_stm32
Definition: bxcan.hpp:22
internal.hpp
uavcan::DurationBase::getAbs
D getAbs() const
Definition: time.hpp:46
uavcan_stm32::clock::isUtcLocked
bool isUtcLocked()
uavcan::int64_t
std::int64_t int64_t
Definition: std.hpp:32
uavcan_stm32::clock::getUtcSyncParams
UtcSyncParams getUtcSyncParams()
uavcan_stm32::clock::setUtcSyncParams
void setUtcSyncParams(const UtcSyncParams &params)
uavcan_stm32::clock::getUtc
uavcan::UtcTime getUtc()
Definition: clock.cpp:97
uavcan_stm32::clock::init
void init()
Definition: clock.cpp:43
uavcan::int32_t
std::int32_t int32_t
Definition: std.hpp:31
uavcan::TimeBase< MonotonicTime, MonotonicDuration >::fromUSec
static MonotonicTime fromUSec(uint64_t us)
Definition: time.hpp:112
uavcan_stm32::clock::getUtcRateCorrectionPPM
float getUtcRateCorrectionPPM()
uavcan::uint8_t
std::uint8_t uint8_t
Definition: std.hpp:24
uavcan::max
const UAVCAN_EXPORT T & max(const T &a, const T &b)
Definition: templates.hpp:291
uavcan::min
const UAVCAN_EXPORT T & min(const T &a, const T &b)
Definition: templates.hpp:281
clock.hpp
uavcan_stm32::clock::getMonotonic
uavcan::MonotonicTime getMonotonic()
Definition: clock.cpp:83
UAVCAN_STM32_IRQ_EPILOGUE
#define UAVCAN_STM32_IRQ_EPILOGUE()
Definition: stm32/libuavcan/driver/src/internal.hpp:52
uavcan_stm32::clock::getUtcUSecFromCanInterrupt
uavcan::uint64_t getUtcUSecFromCanInterrupt()
Definition: clock.cpp:78
UAVCAN_STM32_IRQ_PROLOGUE
#define UAVCAN_STM32_IRQ_PROLOGUE()
Definition: stm32/libuavcan/driver/src/internal.hpp:51
NULL
#define NULL
Nullzeiger.
Definition: utils.h:64
uavcan_stm32::SystemClock::instance
static SystemClock & instance()
uavcan::MonotonicTime
Definition: time.hpp:184
ENABLE
@ ENABLE
Definition: lpc_types.h:68
UAVCAN_STM32_IRQ_HANDLER
#define UAVCAN_STM32_IRQ_HANDLER(id)
Definition: stm32/libuavcan/driver/src/internal.hpp:50
uavcan_stm32::clock::adjustUtc
void adjustUtc(uavcan::UtcDuration adjustment)
Definition: clock.cpp:117
uavcan::TimeBase::toUSec
uint64_t toUSec() const
Definition: time.hpp:120
uavcan::DurationBase::toUSec
int64_t toUSec() const
Definition: time.hpp:43
UAVCAN_ASSERT
#define UAVCAN_ASSERT(x)
Definition: libuavcan/libuavcan/include/uavcan/build_config.hpp:184
uavcan_stm32::clock
Definition: platform_specific_components/stm32/libuavcan/driver/include/uavcan_stm32/clock.hpp:13

uavcan_communicator
Author(s):
autogenerated on Fri Dec 13 2024 03:10:03