diff options
Diffstat (limited to 'platforms/chibios/timer.c')
| -rw-r--r-- | platforms/chibios/timer.c | 110 | 
1 files changed, 85 insertions, 25 deletions
| diff --git a/platforms/chibios/timer.c b/platforms/chibios/timer.c index 9f664e1f79..e3bdfdcc37 100644 --- a/platforms/chibios/timer.c +++ b/platforms/chibios/timer.c @@ -2,46 +2,106 @@  #include "timer.h" -static uint32_t reset_point = 0; +static uint32_t ticks_offset = 0; +static uint32_t last_ticks   = 0; +static uint32_t ms_offset    = 0;  #if CH_CFG_ST_RESOLUTION < 32  static uint32_t last_systime = 0;  static uint32_t overflow     = 0;  #endif -void timer_init(void) { timer_clear(); } +// Get the current system time in ticks as a 32-bit number. +// This function must be called from within a system lock zone (so that it can safely use and update the static data). +static inline uint32_t get_system_time_ticks(void) { +    uint32_t systime = (uint32_t)chVTGetSystemTimeX(); -void timer_clear(void) { -    reset_point = (uint32_t)chVTGetSystemTime();  #if CH_CFG_ST_RESOLUTION < 32 -    last_systime = reset_point; -    overflow     = 0; -#endif -} - -uint16_t timer_read(void) { return (uint16_t)timer_read32(); } - -uint32_t timer_read32(void) { -    uint32_t systime = (uint32_t)chVTGetSystemTime(); - -#if CH_CFG_ST_RESOLUTION < 32 -    // If/when we need to support 64-bit chips, this may need to be modified to match the native bit-ness of the MCU. -    // At this point, the only SysTick resolution allowed other than 32 is 16 bit. -    // In the 16-bit case, at: +    // If the real system timer resolution is less than 32 bits, provide the missing bits by checking for the counter +    // overflow.  For this to work, this function must be called at least once for every overflow of the system timer. +    // In the 16-bit case, the corresponding times are:      //    - CH_CFG_ST_FREQUENCY = 100000, overflow will occur every ~0.65 seconds      //    - CH_CFG_ST_FREQUENCY = 10000, overflow will occur every ~6.5 seconds      //    - CH_CFG_ST_FREQUENCY = 1000, overflow will occur every ~65 seconds -    // With this implementation, as long as we ensure a timer read happens at least once during the overflow period, timing should be accurate.      if (systime < last_systime) {          overflow += ((uint32_t)1) << CH_CFG_ST_RESOLUTION;      } -      last_systime = systime; -    return (uint32_t)TIME_I2MS(systime - reset_point + overflow); -#else -    return (uint32_t)TIME_I2MS(systime - reset_point); +    systime += overflow; +#endif + +    return systime; +} + +#if CH_CFG_ST_RESOLUTION < 32 +static virtual_timer_t update_timer; + +// Update the system tick counter every half of the timer overflow period; this should keep the tick counter correct +// even if something blocks timer interrupts for 1/2 of the timer overflow period. +#    define UPDATE_INTERVAL (((sysinterval_t)1) << (CH_CFG_ST_RESOLUTION - 1)) + +// VT callback function to keep the overflow bits of the system tick counter updated. +static void update_fn(void *arg) { +    (void)arg; +    chSysLockFromISR(); +    get_system_time_ticks(); +    chVTSetI(&update_timer, UPDATE_INTERVAL, update_fn, NULL); +    chSysUnlockFromISR(); +} +#endif + +// The highest multiple of CH_CFG_ST_FREQUENCY that fits into uint32_t.  This number of ticks will necessarily +// correspond to some integer number of seconds. +#define OVERFLOW_ADJUST_TICKS ((uint32_t)((UINT32_MAX / CH_CFG_ST_FREQUENCY) * CH_CFG_ST_FREQUENCY)) + +// The time in milliseconds which corresponds to OVERFLOW_ADJUST_TICKS ticks (this is a precise conversion, because +// OVERFLOW_ADJUST_TICKS corresponds to an integer number of seconds). +#define OVERFLOW_ADJUST_MS (TIME_I2MS(OVERFLOW_ADJUST_TICKS)) + +void timer_init(void) { +    timer_clear(); +#if CH_CFG_ST_RESOLUTION < 32 +    chVTObjectInit(&update_timer); +    chVTSet(&update_timer, UPDATE_INTERVAL, update_fn, NULL);  #endif  } -uint16_t timer_elapsed(uint16_t last) { return TIMER_DIFF_16(timer_read(), last); } +void timer_clear(void) { +    chSysLock(); +    ticks_offset = get_system_time_ticks(); +    last_ticks   = 0; +    ms_offset    = 0; +    chSysUnlock(); +} + +uint16_t timer_read(void) { +    return (uint16_t)timer_read32(); +} -uint32_t timer_elapsed32(uint32_t last) { return TIMER_DIFF_32(timer_read32(), last); } +uint32_t timer_read32(void) { +    chSysLock(); +    uint32_t ticks = get_system_time_ticks() - ticks_offset; +    if (ticks < last_ticks) { +        // The 32-bit tick counter overflowed and wrapped around.  We cannot just extend the counter to 64 bits here, +        // because TIME_I2MS() may encounter overflows when handling a 64-bit argument; therefore the solution here is +        // to subtract a reasonably large number of ticks from the tick counter to bring its value below the 32-bit +        // limit again, and then add the equivalent number of milliseconds to the converted value.  (Adjusting just the +        // converted value to account for 2**32 ticks is not possible in general, because 2**32 ticks may not correspond +        // to an integer number of milliseconds). +        ticks -= OVERFLOW_ADJUST_TICKS; +        ticks_offset += OVERFLOW_ADJUST_TICKS; +        ms_offset += OVERFLOW_ADJUST_MS; +    } +    last_ticks              = ticks; +    uint32_t ms_offset_copy = ms_offset; // read while still holding the lock to ensure a consistent value +    chSysUnlock(); + +    return (uint32_t)TIME_I2MS(ticks) + ms_offset_copy; +} + +uint16_t timer_elapsed(uint16_t last) { +    return TIMER_DIFF_16(timer_read(), last); +} + +uint32_t timer_elapsed32(uint32_t last) { +    return TIMER_DIFF_32(timer_read32(), last); +} | 
