Руслан Урядинский / libuavcan

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependents:   UAVCAN UAVCAN_Subscriber

libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp@0:dfe6edabb8ec, 2018-04-14 (annotated)

Committer:
RuslanUrya
Date:
Sat Apr 14 10:25:32 2018 +0000
Revision:
0:dfe6edabb8ec
Initial commit

Who changed what in which revision?

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