Implementing a high resolution Teensy clock - TeensyUser/doc GitHub Wiki

uint32_t oldLow = ARM_DWT_CYCCNT;
uint32_t curHigh = 0;

uint64_t getCnt()
{
    uint32_t curLow = ARM_DWT_CYCCNT;
    if (curLow < oldLow) // we had a roll over
    {
        curHigh++;
    }
    oldLow = curLow;
    uint64_t curVal = ((uint64_t)curHigh << 32) | curLow;

    return curVal;
}

// disable periodic interrupt
SNVS_HPCR &= ~SNVS_HPCR_PI_EN;
while ((SNVS_HPCR & SNVS_HPCR_PI_EN)){}   // spin until PI_EN is reset...

// set interrupt frequency to 1Hz
SNVS_HPCR = SNVS_HPCR_PI_FREQ(0b1111);

// enable periodic interrupt
SNVS_HPCR |= SNVS_HPCR_PI_EN;
while (!(SNVS_HPCR & SNVS_HPCR_PI_EN)){}  // spin until PI_EN is set...

// attach a callback
attachInterruptVector(IRQ_SNVS_IRQ, SNVS_isr);
NVIC_SET_PRIORITY(IRQ_SNVS_IRQ, 255);     // lowest priority
NVIC_ENABLE_IRQ(IRQ_SNVS_IRQ);

void SNVS_isr(void)
{
   SNVS_HPSR |= 0b11; // reset interrupt flag
   getCnt()           // dummy call to check for overflow of the cycle counter
   asm("dsb");        // wait until flag is synced over the busses to prevent double calls of the isr
}

struct teensy_clock
{
    // required typdefs:
    using duration = std::chrono::duration<uint64_t, std::ratio<1, F_CPU>>;  // use a uint64_t representation with a time step of 1/F_CPU (=1.667ns @600MHz)
    using rep = duration::rep;                                               // uint64_t
    using period = duration::period;                                         // std::ratio<1,600>
    using time_point = std::chrono::time_point<teensy_clock, duration>;

    static constexpr bool is_steady = false;                                 // can not be guaranteed to be steady (could be readjusted by syncToRTC)

    static time_point now()
    {
        duration t = duration(t0 + cycles64::get());                         // adds the current 64bit cycle counter to an offset set by syncToRTC() (default: t0=0)
        return time_point(t);                                                // ... and returns the correspoinging time point.
    }

    static void begin(bool sync = true);                                     // starts the 64bit cycle counter update interrupt. Sync=true sycns the clock to the RTC
    static void syncToRTC();                                                 // Sync to RTC whenever needed (e.g. after adjusting the RTC)

    //Map to C API
    static std::time_t to_time_t(const time_point& t);                       // returns the time_t value (seconds since 1.1.1970) to be used with standard C-API functions
    static time_point from_time_t(std::time_t t);                            // converts a time_t value to a time_point

 private:
    static uint64_t t0;                                                      // offset to adjust time (seconds from 1.1.1970 to now).
};

void teensy_clock::syncToRTC()
{
    t0 = ((uint64_t)rtc_get()) * F_CPU - cycles64::get();
}

#include "teensy_clock.h"
using namespace std::chrono;

typedef teensy_clock::time_point timePoint;               // just for the sake of less typing
typedef duration<float, std::micro> micros_f;             // float based microseconds (predefined type 'microseconds' is integer based)

void setup()
{
    teensy_clock::begin();                                // t_0 the clock and sync to rtc (works with and without battery)
}

void loop()
{
    // demonstrate clock resolution --------------------------------------------------
    unsigned delay_us = random(10, 5000);
    Serial.printf("delayMicroseconds(%u)\n", delay_us);

    timePoint t_0 = teensy_clock::now();                  // get two timepoints 'delay_us' apart
    delayMicroseconds(delay_us);
    timePoint t_1 = teensy_clock::now();

    auto dt = duration_cast<micros_f>(t_1 - t_0);         // cast delta to microseconds (float)
    Serial.printf("dt = t_1-t_0: %7.2f µs\n", dt.count());

    // convert to C-API ---------------------------------------------------------------
    timePoint currentTime = teensy_clock::now();          // get current time
    time_t ct = teensy_clock::to_time_t(currentTime);     // convert C-API time_t
    Serial.printf("Current Time: %s\n", ctime(&ct));      // pretty print date/time

    delay(1000);
}