// delay and micros for 2 MHz cpu speed
#include "Arduino.h"
/***************************************************************************************************/
#define TWO_MHZ 2000000
uint32_t micros_lp(void) {
    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();
    //systick_current = current;
    //systick_count = count;
    //systick_istatus = istatus & SCB_ICSR_PENDSTSET ? 1 : 0;
    if ((istatus & SCB_ICSR_PENDSTSET) && current > 50) count++;
    current = ((2000000 / 1000) - 1) - current;
    return count * 1000 + current / (TWO_MHZ / 1000000);
}
/***************************************************************************************************/
void delay_lp(uint32_t ms) {
    uint32_t start = micros();
    if (ms > 0) {
        while (1) {
            while ((micros() - start) >= 1000) {
                ms--;
                if (ms == 0) return;
                start += 1000;
            }
            yield();
        }
    }
}