void LedRGB_t::IStartSequenceI(const LedChunk_t *PLedChunk) {
    // Reset timer
    if(chVTIsArmedI(&ITmr)) chVTResetI(&ITmr);
    // Process the sequence
    while(PLedChunk != nullptr) {
//        Uart.Printf("\rCh %u", PLedChunk->ChunkSort);
        switch(PLedChunk->ChunkSort) {
            case csSetColor:
                if(ICurrColor != PLedChunk->Color) {
                    if(PLedChunk->SmoothVar == 0) {   // If smooth time is zero,
                        SetColor(PLedChunk->Color); // set color now,
                        PLedChunk++;                // and goto next chunk
                    }
                    else {
                        // Adjust color
                        ICurrColor.Adjust(&PLedChunk->Color);
                        ISetCurrent();
                        // Check if completed now
                        if(ICurrColor == PLedChunk->Color) PLedChunk++;
                        else { // Not completed
                            // Calculate time to next adjustment
                            uint32_t DelayR = (ICurrColor.Red   == PLedChunk->Color.Red  )? 0 : ICalcDelay(ICurrColor.Red,   PLedChunk->SmoothVar);
                            uint32_t DelayG = (ICurrColor.Green == PLedChunk->Color.Green)? 0 : ICalcDelay(ICurrColor.Green, PLedChunk->SmoothVar);
                            uint32_t DelayB = (ICurrColor.Blue  == PLedChunk->Color.Blue )? 0 : ICalcDelay(ICurrColor.Blue,  PLedChunk->SmoothVar);
                            uint32_t Delay = DelayR;
                            if(DelayG > Delay) Delay = DelayG;
                            if(DelayB > Delay) Delay = DelayB;
                            chVTSetI(&ITmr, MS2ST(Delay), LedTmrCallback, (void*)PLedChunk);
                            return;
                        } // Not completed
                    } // if time > 256
                } // if color is different
                else PLedChunk++; // Color is the same, goto next chunk
                break;

            case csWait: // Start timer, pointing to next chunk
                chVTSetI(&ITmr, MS2ST(PLedChunk->Time_ms), LedTmrCallback, (void*)(PLedChunk+1));
                return;
                break;

            case csJump:
                PLedChunk = IPStartChunk + PLedChunk->ChunkToJumpTo;
                break;

            case csEnd:
                IPStartChunk = nullptr;
                return;
                break;
        } // switch
    } // while
}

static void adc_end_cb(ADCDriver *adcp, adcsample_t *buffer, size_t n) {

  (void)adcp;
  (void)n;

  /*
   * The bandgap value represents the ADC reading for 1.0V
   */
  uint16_t sensor = buffer[0];
  uint16_t bandgap = buffer[1];

  /*
   * The v25 value is the voltage reading at 25C, it comes from the ADC
   * electricals table in the processor manual. V25 is in millivolts.
   */
  int32_t v25 = 716;

  /*
   * The m value is slope of the temperature sensor values, again from
   * the ADC electricals table in the processor manual.
   * M in microvolts per degree.
   */
  int32_t m = 1620;

  /*
   * Divide the temperature sensor reading by the bandgap to get
   * the voltage for the ambient temperature in millivolts.
   */
  int32_t vamb = (sensor * 1000) / bandgap;

  /*
   * This formula comes from the reference manual.
   * Temperature is in millidegrees C.
   */
  int32_t delta = (((vamb - v25) * 1000000) / m);
  int32_t temp = 25000 - delta;

  palSetPad(TEENSY_PIN13_IOPORT, TEENSY_PIN13);
  chSysLockFromISR();
  chVTResetI(&vt);
  if (temp < 19000) {
    chVTSetI(&vt, MS2ST(10), ledoff, NULL);
  } else if (temp > 28000) {
    chVTSetI(&vt, MS2ST(20), ledoff, NULL);
  } else {
    chVTSetI(&vt, MS2ST(40), ledoff, NULL);
  }
  chSysUnlockFromISR();
}

/**
 * @brief   Default data transmitted callback.
 * @details The application must use this function as callback for the IN
 *          data endpoint.
 *
 * @param[in] usbp      pointer to the @p USBDriver object
 * @param[in] ep        endpoint number
 */
void hidDebugDataTransmitted(USBDriver *usbp, usbep_t ep) {
  HIDDebugDriver *hiddp = usbp->in_params[ep - 1U];
  size_t n;

  if(hiddp == NULL) {
    return;
  }

  osalSysLockFromISR();

  /* rearm the flush timer */
  chVTSetI(&hid_debug_flush_timer, MS2ST(DEBUG_TX_FLUSH_MS), hid_debug_flush_cb, hiddp);

  /* see if we've transmitted everything */
  if((n = oqGetFullI(&hiddp->oqueue)) == 0) {
    chnAddFlagsI(hiddp, CHN_OUTPUT_EMPTY);
  }

  /* Check if there's enough data in the queue to send again */
  if(n >= DEBUG_TX_SIZE) {
    /* The endpoint cannot be busy, we are in the context of the callback,
     * so it is safe to transmit without a check.*/
    osalSysUnlockFromISR();

    usbPrepareQueuedTransmit(usbp, ep, &hiddp->oqueue, DEBUG_TX_SIZE);

    osalSysLockFromISR();
    (void)usbStartTransmitI(usbp, ep);
  }

  osalSysUnlockFromISR();
}

void usb_debug_flush_output(HIDDebugDriver *hiddp) {
  size_t n;

  /* we'll sleep for a moment to finish any transfers that may be pending already */
  /* there's a race condition somewhere, maybe because we have 2x buffer */
  chThdSleepMilliseconds(2);
  osalSysLock();
  /* check that the states of things are as they're supposed to */
  if((usbGetDriverStateI(hiddp->config->usbp) != USB_ACTIVE) ||
     (hiddp->state != HIDDEBUG_READY)) {
    osalSysUnlock();
    return;
  }

  /* rearm the timer */
  chVTSetI(&hid_debug_flush_timer, MS2ST(DEBUG_TX_FLUSH_MS), hid_debug_flush_cb, hiddp);

  /* don't do anything if the queue is empty */
  if((n = oqGetFullI(&hiddp->oqueue)) == 0) {
    osalSysUnlock();
    return;
  }

  osalSysUnlock();
  /* if we don't have enough bytes in the queue, fill with zeroes */
  while(n++ < DEBUG_TX_SIZE) {
    oqPut(&hiddp->oqueue, 0);
  }
  /* will transmit automatically because of the onotify callback */
  /* which transmits as soon as the queue has enough */
}

static void doScheduleForLater(scheduling_s *scheduling, int delayUs, schfunc_t callback, void *param) {
	int delaySt = MY_US2ST(delayUs);
	if (delaySt <= 0) {
		/**
		 * in case of zero delay, we should invoke the callback
		 */
		callback(param);
		return;
	}

	bool alreadyLocked = lockAnyContext();
	scheduling->callback = callback;
	scheduling->param = param;
	int isArmed = chVTIsArmedI(&scheduling->timer);
	if (isArmed) {
		/**
		 * timer reuse is normal for example in case of sudden RPM increase
		 */
		chVTResetI(&scheduling->timer);
	}

#if EFI_SIMULATOR
	if (callback == (schfunc_t)&seTurnPinLow) {
		printf("setTime cb=seTurnPinLow p=%d\r\n", (int)param);
	} else {
//		printf("setTime cb=%d p=%d\r\n", (int)callback, (int)param);
	}
#endif /* EFI_SIMULATOR */

	chVTSetI(&scheduling->timer, delaySt, (vtfunc_t)timerCallback, scheduling);
	if (!alreadyLocked) {
		unlockAnyContext();
	}
}

/**
 * @brief   Insertion monitor timer callback function.
 *
 * @param[in] p         pointer to the @p BaseBlockDevice object
 *
 * @notapi
 */
static void tmrfunc(void *p) {
  BaseBlockDevice *bbdp = p;

  /* The presence check is performed only while the driver is not in a
     transfer state because it is often performed by changing the mode of
     the pin connected to the CS/D3 contact of the card, this could disturb
     the transfer.*/
  blkstate_t state = blkGetDriverState(bbdp);
  chSysLockFromIsr();
  if ((state != BLK_READING) && (state != BLK_WRITING)) {
    /* Safe to perform the check.*/
    if (cnt > 0) {
      if (blkIsInserted(bbdp)) {
        if (--cnt == 0) {
          chEvtBroadcastI(&inserted_event);
        }
      }
      else
        cnt = POLLING_INTERVAL;
    }
    else {
      if (!blkIsInserted(bbdp)) {
        cnt = POLLING_INTERVAL;
        chEvtBroadcastI(&removed_event);
      }
    }
  }
  chVTSetI(&tmr, MS2ST(POLLING_DELAY), tmrfunc, bbdp);
  chSysUnlockFromIsr();
}

void App_t::LedBlink(uint32_t Duration_ms) {
    PinSet(LED_GPIO, LED_PIN);
    chSysLock()
    if(chVTIsArmedI(&TmrLed)) chVTResetI(&TmrLed);
    chVTSetI(&TmrLed, MS2ST(Duration_ms), LedTmrCallback, nullptr);
    chSysUnlock();
}

/*
 * Insertion monitor timer callback function.
 *
 * pointer to the p BaseBlockDevice object
 *
 */
static void tmrfunc( void *p ) 
{
    BaseBlockDevice *bbdp = p;
    
    chSysLockFromISR();
    if( cnt > 0 ) 
    {
        if( blkIsInserted( bbdp ) ) 
        {
            if( --cnt == 0 ) 
            {
                chEvtBroadcastI( &inserted_event );
            }
        }
        else
        {
            cnt = POLLING_INTERVAL;
        }
    }
    else if( ! blkIsInserted( bbdp ) ) 
    {
        cnt = POLLING_INTERVAL;
        chEvtBroadcastI( &removed_event );
    }
    
    chVTSetI( &tmr, MS2ST( POLLING_DELAY ), tmrfunc, bbdp );
    chSysUnlockFromISR();
}

static msg_t spi_thread(void *p) {
  unsigned i;
  SPIDriver *spip = (SPIDriver *)p;
  VirtualTimer vt;
  uint8_t txbuf[256];
  uint8_t rxbuf[256];

  /* Prepare transmit pattern.*/
  for (i = 0; i < sizeof(txbuf); i++)
    txbuf[i] = (uint8_t)i;

  /* Continuous transmission.*/
  while (TRUE) {
    /* Starts a VT working as watchdog to catch a malfunction in the SPI
       driver.*/
    chSysLock();
    chVTSetI(&vt, MS2ST(10), tmo, NULL);
    chSysUnlock();

    spiExchange(spip, sizeof(txbuf), txbuf, rxbuf);

    /* Stops the watchdog.*/
    chSysLock();
    if (chVTIsArmedI(&vt))
      chVTResetI(&vt);
    chSysUnlock();
  }
}

static void sync_cb(void *par){
  (void)par;
  chSysLockFromIsr();
  chVTSetI(&sync_vt, MS2ST(SYNC_PERIOD), &sync_cb, NULL);
  sync_tmo = TRUE;
  chSysUnlockFromIsr();
}

/**
 * @brief   Starts the test timer.
 *
 * @param[in] ms        time in milliseconds
 */
void test_start_timer(unsigned ms) {

  systime_t duration = MS2ST(ms);
  test_timer_done = FALSE;
  chSysLock();
  chVTSetI(&vt, duration, tmr, NULL);
  chSysUnlock();
}

// Polling monitor start.
static void tmr_init(void *p) {
  chEvtInit(&inserted_event);
  chEvtInit(&removed_event);
  chSysLock();
  cnt = POLLING_INTERVAL;
  chVTSetI(&tmr, MS2ST(POLLING_DELAY), tmrfunc, p);
  chSysUnlock();
}

static void tmrcb(void *p) {
  EvTimer *etp = p;

  chSysLockFromIsr();
  chEvtBroadcastI(&etp->et_es);
  chVTSetI(&etp->et_vt, etp->et_interval, tmrcb, etp);
  chSysUnlockFromIsr();
}

/**
 * @brief Starts the timer
 * @details If the timer was already running then the function has no effect.
 *
 * @param etp pointer to an initialized @p EvTimer structure.
 */
void evtStart(EvTimer *etp) {

  chSysLock();

  if (!chVTIsArmedI(&etp->et_vt))
    chVTSetI(&etp->et_vt, etp->et_interval, tmrcb, etp);

  chSysUnlock();
}

/* Triggered when the button is pressed or released. The LED is set to ON.*/
static void extcb1(EXTDriver *extp, expchannel_t channel) {

  (void)extp;
  (void)channel;
  palClearPad(GPIOC, GPIOC_LED);
  chSysLockFromISR();
  chVTSetI(&vt, MS2ST(200), ledoff, NULL);
  chSysUnlockFromISR();
}

/**
 * @brief   Polling monitor start.
 *
 * @param[in] sdcp      pointer to the @p SDCDriver object
 *
 * @notapi
 */
static void tmr_init(SDCDriver *sdcp) {

  chEvtInit(&inserted_event);
  chEvtInit(&removed_event);
  chSysLock();
  cnt = SDC_POLLING_INTERVAL;
  chVTSetI(&tmr, MS2ST(SDC_POLLING_DELAY), tmrfunc, sdcp);
  chSysUnlock();
}

/*
 * This callback is invoked when a transmission has physically completed.
 */
static void txend2(UARTDriver *uartp) {

  (void)uartp;
  palSetPad(GPIOD, GPIOD_LED5);
  chSysLockFromISR();
  chVTResetI(&vt5);
  chVTSetI(&vt5, MS2ST(200), led5off, NULL);
  chSysUnlockFromISR();
}

/*
 * Set from bootloader, here for reference.
static const WDGConfig wdgcfg = {
  STM32_IWDG_PR_64,
  STM32_IWDG_RL(250), // 250ms
  STM32_IWDG_WIN_DISABLED
};
*/
CCM_FUNC void freqinVTHandler(void *arg)
{
  (void)arg;

  reEnableInputCapture(&TIMCAPD3);

  chSysLockFromISR();
  chVTSetI(&vt_freqin, FREQIN_INTERVAL, freqinVTHandler, NULL);
  chSysUnlockFromISR();
}

void vMBMasterPortTimersRespondTimeoutEnable()
{
	//chprintf((BaseSequentialStream *)&itm_port, "%s\n", "TimeOut Enable");
	//palSetPad(GPIOC, GPIOC_PIN10);
    /* Set current timer mode, don't change it.*/
    chSysLockFromISR();
	chVTResetI(&vtout);
	chVTSetI(&vtout, MS2ST((uint32_t)MB_MASTER_TIMEOUT_MS_RESPOND), timer_timeout_ind, NULL);
	chSysUnlockFromISR();
}

void vMBMasterPortTimersConvertDelayEnable()
{
    /* Set current timer mode, don't change it.*/
    vMBMasterSetCurTimerMode(MB_TMODE_CONVERT_DELAY);

    chSysLockFromISR();
	chVTResetI(&vtdelay);
	chVTSetI(&vtdelay, MS2ST((uint32_t)MB_MASTER_DELAY_MS_CONVERT), timer_timeout_ind, NULL);
	chSysUnlockFromISR();
}