My_Clock.h

#ifndef __CLOCK_H__
#define __CLOCK_H__
 
#include <Arduino.h>
#include <FreeRTOS.h>
#include <FreeRTOS/Source/include/task.h>


__STATIC_INLINE void DWT_Init(void) {
    CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk; // разрешаем использовать счётчик
    DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk; // запускаем счётчик
}
__STATIC_INLINE void DWT_delay_us(uint32_t us) {
    uint32_t us_count_tic = us * (SystemCoreClock / 1000000U);
    DWT->CYCCNT = 0U;
    while (DWT->CYCCNT < us_count_tic);
}
__STATIC_INLINE uint32_t DWT_micros(void) {
    return DWT->CYCCNT / (SystemCoreClock / 1000000U);
}
__STATIC_INLINE uint32_t DWT_value(void) {
    return DWT->CYCCNT ;
}
 
static void poly_delay(uint32_t ms) {
    if (xTaskGetSchedulerState() == taskSCHEDULER_NOT_STARTED) {
        delay(ms);
    } else {
        vTaskDelay(pdMS_TO_TICKS(ms));
    }
}
 
class Clock {
private:
    unsigned long m_startTime; 
public:

    Clock()
    :
    m_startTime(DWT_micros())
    {
    }


    float getElapsedTime()
    {
        unsigned long currentTime = DWT_value();
        float elapsedTimeInSeconds = (currentTime - m_startTime)/ (SystemCoreClock / 1000000U) / float(1e6);
        if(currentTime<m_startTime) {
            unsigned long dp=-m_startTime;
            //printf("%lu \n",dp);
            elapsedTimeInSeconds=(currentTime + dp)/ (SystemCoreClock / 1000000U) / float(1e6);
        }
        return elapsedTimeInSeconds;
    }


    float restart()
    {
        unsigned long currentTime = DWT_value();
        float elapsedTimeInSeconds = (currentTime - m_startTime)/ (SystemCoreClock / 1000000U) / float(1e6);
        if(currentTime<m_startTime) {
            unsigned long dp=-m_startTime;
            //printf("%lu %lu %lu \n",dp,-m_startTime,m_startTime);
            elapsedTimeInSeconds=(currentTime + dp)/ (SystemCoreClock / 1000000U) / float(1e6);
        }
        m_startTime = currentTime;
        return elapsedTimeInSeconds;
    }
};
 
 
#endif // __CLOCK_H__