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 Transmits data via the I2C bus as master.
* @details Number of receiving bytes must be 0 or more than 1 on STM32F1x.
* This is hardware restriction.
*
* @param[in] i2cp pointer to the @p I2CDriver object
* @param[in] addr slave device address
* @param[in] txbuf pointer to the transmit buffer
* @param[in] txbytes number of bytes to be transmitted
* @param[out] rxbuf pointer to the receive buffer
* @param[in] rxbytes number of bytes to be received
* @param[in] timeout the number of ticks before the operation timeouts,
* the following special values are allowed:
* - @a TIME_INFINITE no timeout.
* .
* @return The operation status.
* @retval RDY_OK if the function succeeded.
* @retval RDY_RESET if one or more I2C errors occurred, the errors can
* be retrieved using @p i2cGetErrors().
* @retval RDY_TIMEOUT if a timeout occurred before operation end. <b>After a
* timeout the driver must be stopped and restarted
* because the bus is in an uncertain state</b>.
*
* @notapi
*/
msg_t i2c_lld_master_transmit_timeout(I2CDriver *i2cp, i2caddr_t addr,
const uint8_t *txbuf, size_t txbytes,
uint8_t *rxbuf, size_t rxbytes,
systime_t timeout) {
I2C_TypeDef *dp = i2cp->i2c;
VirtualTimer vt;
#if defined(STM32F1XX_I2C)
chDbgCheck(((rxbytes == 0) || ((rxbytes > 1) && (rxbuf != NULL))),
"i2c_lld_master_transmit_timeout");
#endif
/* Global timeout for the whole operation.*/
if (timeout != TIME_INFINITE)
chVTSetI(&vt, timeout, i2c_lld_safety_timeout, (void *)i2cp);
/* Releases the lock from high level driver.*/
chSysUnlock();
/* Initializes driver fields, LSB = 0 -> write.*/
i2cp->addr = addr << 1;
i2cp->errors = 0;
/* TX DMA setup.*/
dmaStreamSetMemory0(i2cp->dmatx, txbuf);
dmaStreamSetTransactionSize(i2cp->dmatx, txbytes);
/* RX DMA setup.*/
dmaStreamSetMemory0(i2cp->dmarx, rxbuf);
dmaStreamSetTransactionSize(i2cp->dmarx, rxbytes);
/* Waits until BUSY flag is reset and the STOP from the previous operation
is completed, alternatively for a timeout condition.*/
while ((dp->SR2 & I2C_SR2_BUSY) || (dp->CR1 & I2C_CR1_STOP)) {
chSysLock();
if ((timeout != TIME_INFINITE) && !chVTIsArmedI(&vt))
return RDY_TIMEOUT;
chSysUnlock();
}
/* This lock will be released in high level driver.*/
chSysLock();
/* Atomic check on the timer in order to make sure that a timeout didn't
happen outside the critical zone.*/
if ((timeout != TIME_INFINITE) && !chVTIsArmedI(&vt))
return RDY_TIMEOUT;
/* Starts the operation.*/
dp->CR2 |= I2C_CR2_ITEVTEN;
dp->CR1 |= I2C_CR1_START;
/* Waits for the operation completion or a timeout.*/
i2cp->thread = chThdSelf();
chSchGoSleepS(THD_STATE_SUSPENDED);
if ((timeout != TIME_INFINITE) && chVTIsArmedI(&vt))
chVTResetI(&vt);
return chThdSelf()->p_u.rdymsg;
}
/**
* @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();
}
/*
* This callback is invoked when a transmission has physically completed.
*/
static void txend2(UARTDriver *uartp) {
(void)uartp;
palClearPad(GPIOE, GPIOE_LED3_RED);
chSysLockFromISR();
chVTResetI(&vt1);
chVTSetI(&vt1, MS2ST(5000), restart, 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();
}
static void adccallback(ADCDriver *adcp, adcsample_t *buffer, size_t n) {
(void)adcp;
(void)buffer;
(void)n;
chSysLockFromISR();
chVTSetI(&adcvt, MS2ST(10), tmo, (void *)"ADC timeout");
chSysUnlockFromISR();
}
/*
* This callback is invoked when a transmission has physically completed.
*/
static void txend2(UARTDriver *uartp) {
(void)uartp;
palClearPad(GPIOC, GPIOC_LED4);
chSysLockFromIsr();
if (chVTIsArmedI(&vt1))
chVTResetI(&vt1);
chVTSetI(&vt1, MS2ST(5000), restart, NULL);
chSysUnlockFromIsr();
}
/*
* This callback is invoked when a character is received but the application
* was not ready to receive it, the character is passed as parameter.
*/
static void rxchar(UARTDriver *uartp, uint16_t c) {
(void)uartp;
(void)c;
/* Flashing the LED each time a character is received.*/
palSetPad(GPIOD, GPIOD_LED4);
chSysLockFromISR();
chVTResetI(&vt4);
chVTSetI(&vt4, MS2ST(200), led4off, NULL);
chSysUnlockFromISR();
}
/**
* Init.
*/
void StorageInit(void){
chThdCreateStatic(SdThreadWA,
sizeof(SdThreadWA),
NORMALPRIO - 5,
SdThread,
NULL);
chSysLock();
chVTSetI(&sync_vt, MS2ST(SYNC_PERIOD), &sync_cb, NULL);
chSysUnlock();
}
/**
* @brief Polling monitor start.
*
* @param[in] p pointer to an object implementing @p BaseBlockDevice
*
* @notapi
*/
void sdc_tmr_init(void *p) {
chEvtInit(&sdc_inserted_event);
chEvtInit(&sdc_removed_event);
chEvtInit(&sdc_halt_event);
chEvtInit(&sdc_start_event);
chSysLock();
sdc_debounce_count = sdc_polling_interval;
chVTSetI(&sdc_tmr, MS2ST(sdc_polling_delay), sdc_tmrfunc, p);
chSysUnlock();
}
void App_t::Init() {
//Dose.Load();
// Uart.Printf("Dose = %u\r", Dose.Get());
PThd = chThdCreateStatic(waAppThread, sizeof(waAppThread), NORMALPRIO, (tfunc_t)AppThread, NULL);
// Timers init
chSysLock();
// chVTSetI(&ITmrDose, MS2ST(TM_DOSE_INCREASE_MS), TmrDoseCallback, nullptr);
// chVTSetI(&ITmrDoseSave, MS2ST(TM_DOSE_SAVE_MS), TmrDoseSaveCallback, nullptr);
chVTSetI(&ITmrPillCheck, MS2ST(TM_PILL_CHECK_MS), TmrPillCheckCallback, nullptr);
chSysUnlock();
}
/*
* This callback is invoked when a receive buffer has been completely written.
*/
static void rxend(UARTDriver *uartp) {
(void)uartp;
/* Flashing the LED each time a character is received.*/
palSetPad(GPIOD, GPIOD_LED3);
chSysLockFromISR();
chVTResetI(&vt3);
chVTSetI(&vt3, MS2ST(200), led3off, NULL);
chSysUnlockFromISR();
}
/**
* @brief Receives data via the I2C bus as master.
* @details Number of receiving bytes must be more than 1 because of stm32
* hardware restrictions.
*
* @param[in] i2cp pointer to the @p I2CDriver object
* @param[in] addr slave device address
* @param[out] rxbuf pointer to the receive buffer
* @param[in] rxbytes number of bytes to be received
* @param[in] timeout the number of ticks before the operation timeouts,
* the following special values are allowed:
* - @a TIME_INFINITE no timeout.
* .
* @return The operation status.
* @retval RDY_OK if the function succeeded.
* @retval RDY_RESET if one or more I2C errors occurred, the errors can
* be retrieved using @p i2cGetErrors().
* @retval RDY_TIMEOUT if a timeout occurred before operation end. <b>After a
* timeout the driver must be stopped and restarted
* because the bus is in an uncertain state</b>.
*
* @notapi
*/
msg_t i2c_lld_master_receive_timeout(I2CDriver *i2cp, i2caddr_t addr,
uint8_t *rxbuf, size_t rxbytes,
systime_t timeout) {
I2C_TypeDef *dp = i2cp->i2c;
VirtualTimer vt;
msg_t rdymsg;
chDbgCheck((rxbytes > 1), "i2c_lld_master_receive_timeout");
/* Global timeout for the whole operation.*/
chVTSetI(&vt, timeout, i2c_lld_safety_timeout, (void *)i2cp);
/* Releases the lock from high level driver.*/
chSysUnlock();
/* Initializes driver fields, LSB = 1 -> receive.*/
i2cp->addr = (addr << 1) | 0x01;
i2cp->errors = 0;
/* RX DMA setup.*/
dmaStreamSetMemory0(i2cp->dmarx, rxbuf);
dmaStreamSetTransactionSize(i2cp->dmarx, rxbytes);
/* Waits until BUSY flag is reset and the STOP from the previous operation
is completed, alternatively for a timeout condition.*/
while ((dp->SR2 & I2C_SR2_BUSY) || (dp->CR1 & I2C_CR1_STOP)) {
if (!chVTIsArmedI(&vt)) {
chSysLock();
return RDY_TIMEOUT;
}
}
/* This lock will be released in high level driver.*/
chSysLock();
/* Atomic check on the timer in order to make sure that a timeout didn't
happen outside the critical zone.*/
if (!chVTIsArmedI(&vt))
return RDY_TIMEOUT;
/* Starts the operation.*/
dp->CR2 |= I2C_CR2_ITEVTEN;
dp->CR1 |= I2C_CR1_START | I2C_CR1_ACK;
/* Waits for the operation completion or a timeout.*/
i2cp->thread = chThdSelf();
chSchGoSleepS(THD_STATE_SUSPENDED);
rdymsg = chThdSelf()->p_u.rdymsg;
if (rdymsg != RDY_TIMEOUT)
chVTResetI(&vt);
return rdymsg;
}
