Long time monotonic microsecond clock for UAVCAN using systick timer

[Jollerprutt]

Hi,

This is my first post on this forum although I have been playing with Teensys for quite some time now

I'm using UAVCAN (libuavcan) on a teensy 3.2 and I'm trying to implement a proper clock. Next step is time synchronization.

So I don't really have an issue, but I would like to implement a good utc clock and think I should have a proper monotonic clock first.

Actually for my purposes just using micros() or elapsedMicros would be perfect if it did not overflow after 71 minutes.
A modified micros that outputs an uint64 would also be great if systick_millis_count did not overflow after 49 days.

In my old code I use elapsedMicros and if it passes 4000000000 i wind it back and save what i remove to a separate variable.
My new code is basically a modified micros() function, but I'm not sure if it's good

Old code:

static uavcan::uint64_t sampleTime()
{
  if(usecElapsed >= 4000000000)
  {
  uavcan::uint32_t usecTemp = usecElapsed;
  usecElapsed -= usecTemp;
  time_mono += usecTemp;
  }
  return time_mono + usecElapsed;
}

uavcan::MonotonicTime getMonotonic()
{
  uavcan::uint64_t usec = 0;
  {
    usec = sampleTime();
  }

  return uavcan::MonotonicTime::fromUSec(usec);
}

New code:

uavcan::MonotonicTime getMonotonic()
{
  uavcan::uint64_t usec = 0;
  uavcan::uint32_t count, current, istatus;

  __disable_irq();
  current = SYST_CVR;
  count = systick_millis_count;
  istatus = SCB_ICSR; // bit 26 indicates if systick exception pending
  __enable_irq();

  if ((istatus & SCB_ICSR_PENDSTSET) && current > 50)
    count++;
  current = ((F_CPU / 1000) - 1) - current;
#if defined(KINETISL) && F_CPU == 48000000
  return count * 1000 + ((current * (uint32_t)87381) >> 22);
#elif defined(KINETISL) && F_CPU == 24000000
  return count * 1000 + ((current * (uint32_t)174763) >> 22);
#endif
  if (last_count > count)
  {
    overflows++;
  }
  usec = overflows * 0xFFFFFFFF + count * 1000 + current / (F_CPU / 1000000);
  last_count = count;

  return uavcan::MonotonicTime::fromUSec(usec);
}

So, for my actual questions:
Is my new code an improvement?
Considering the new code does large number multiplication while the old code multiplied small (fits in uint32) and just did addition for the large numbers. Maybe my new approach is slower?
Should I move "overflows * 0xFFFFFFFF" into the if statement and save into a large variable that I add to the total time? I guess that would slow the clock slightly once every 49 days? but result in a generally more accurate clock?
Is "istatus = SCB_ICSR;" correct or should it be "istatus = SCB_ICSR_PENDSTSET;"? is there some bit addition logic that makes this correct anyway?
Does anyone know when SCB_ICSR_PENDSTSET changes to 1? Is it some amount of ticks from 0?

Any comments and suggestions are highly appreciated

Cheers,
Carl