/**
* @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;
}
static msg_t lsm303_acc_update_thread(void *p) {
I2CDriver *i2cp = (I2CDriver *)p;
while (TRUE) {
msg_t msg;
/* Waiting for the IRQ to happen.*/
chSysLock();
acctp = chThdSelf();
chSchGoSleepS(THD_STATE_SUSPENDED);
msg = chThdSelf()->p_u.rdymsg;
chSysUnlock();
/* If data ready, update all axis.*/
if (msg == LSM303_ACC_DATA_READY) {
lsm303_acc_update(i2cp);
while (status_a != 0x00) {
lsm303_acc_update(i2cp);
}
// chprintf((BaseChannel *)&SERIAL_DRIVER, "ACC T: %d X: %d Y: %d Z: %d\r\n",
// acc_update_time, acc_data.x, acc_data.y, acc_data.z);
}
}
return RDY_OK;
}
static msg_t lsm303_mag_update_thread(void *p) {
I2CDriver *i2cp = (I2CDriver *)p;
while (TRUE) {
msg_t msg;
/* Waiting for the IRQ to happen.*/
chSysLock();
magtp = chThdSelf();
chSchGoSleepS(THD_STATE_SUSPENDED);
msg = chThdSelf()->p_u.rdymsg;
chSysUnlock();
/* If data ready, update all axis.*/
if (msg == LSM303_MAG_DATA_READY) {
lsm303_mag_update(i2cp);
// if (status_m != 0x00) {
// lsm303_mag_update(i2cp);
// }
}
}
return RDY_OK;
}
static msg_t thread1(BaseSequentialStream *chp) {
systime_t time = chTimeNow();
/*
* Thread-specific parameters
*/
chRegSetThreadName("t1");
static int d = 300;
static int w = 75000;
volatile uint32_t i, n;
while(1) {
if(chThdShouldTerminate())
return 0;
palSetPad(GPIOD, GPIOD_LED3);
/*
* Deadline for current execution of this task
*/
time += d;
chprintf(chp, "%s N %d %d %d %d\r\n", chRegGetThreadName(chThdSelf()), chThdGetPriority(),
chTimeNow(), time, chThdGetTicks(chThdSelf()));
/*
* Do some "work"
*/
for(i = 0; i < w; i ++) {
n = i / 3;
}
chprintf(chp, "%s X %d %d %d %d\r\n", chRegGetThreadName(chThdSelf()), chThdGetPriority(),
chTimeNow(), time, chThdGetTicks(chThdSelf()));
palClearPad(GPIOD, GPIOD_LED3);
/*
* Yield control of CPU until the deadline (which is also beginning of next period)
*/
chThdSleepUntil(time);
}
return 0;
}
int ThreadWait::sleep() {
chSysLock();
thread_to_wake = chThdSelf();
chSchGoSleepS(THD_STATE_SUSPENDED);
const auto result = chThdSelf()->p_u.rdymsg;
chSysUnlock();
return result;
}
uint8_t i2c_t::CmdWriteRead(uint8_t Addr,
uint8_t *WPtr, uint8_t WLength,
uint8_t *RPtr, uint8_t RLength) {
if(IBusyWait() != OK) return FAILURE;
// Clear flags
ii2c->SR1 = 0;
while(RxIsNotEmpty()) (void)ii2c->DR; // Read DR until it empty
ClearAddrFlag();
// Start transmission
SendStart();
if(WaitEv5() != OK) return FAILURE;
SendAddrWithWrite(Addr);
if(WaitEv6() != OK) { SendStop(); return FAILURE; }
// Start TX DMA if needed
if(WLength != 0) {
if(WaitEv8() != OK) return FAILURE;
dmaStreamSetMemory0(PDmaTx, WPtr);
dmaStreamSetTransactionSize(PDmaTx, WLength);
PRequestingThread = chThdSelf();
dmaStreamEnable(PDmaTx);
chSysLock();
chSchGoSleepS(THD_STATE_SUSPENDED); // Sleep until end
chSysUnlock();
dmaStreamDisable(PDmaTx);
}
// Read if needed
if(RLength != 0) {
if(WaitEv8() != OK) return FAILURE;
// Send repeated start
SendStart();
if(WaitEv5() != OK) return FAILURE;
SendAddrWithRead(Addr);
if(WaitEv6() != OK) { SendStop(); return FAILURE; }
// If number of data to be read is 1, then DMA cannot be used
if(RLength == 1) {
AckDisable();
SendStop();
if(WaitRx() != OK) return FAILURE;
*RPtr = ReceiveData();
if(WaitStop() != OK) return FAILURE;
return OK;
}
else { // more than 1 byte, use DMA
AckEnable();
dmaStreamSetMemory0(PDmaRx, RPtr);
dmaStreamSetTransactionSize(PDmaRx, RLength);
DmaLastTransferSet(); // Inform DMA that this is last transfer => do not ACK last byte
PRequestingThread = chThdSelf();
dmaStreamEnable(PDmaRx);
chSysLock();
chSchGoSleepS(THD_STATE_SUSPENDED); // Sleep until end
chSysUnlock();
dmaStreamDisable(PDmaRx);
} // if lng==1
} // if != 0
SendStop();
return OK;
}
/**
* @brief Transmits data via the I2C bus as master.
* @details When performing reading through write you can not write more than
* 3 bytes of data to I2C slave. This is SAM7 platform limitation.
*
* @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 this value is ignored on SAM7 platform.
* .
* @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().
*
* @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) {
(void)timeout;
/* SAM7 specific check */
chDbgCheck(((rxbytes == 0) || ((txbytes > 0) && (txbytes < 4) && (rxbuf != NULL))),
"i2c_lld_master_transmit_timeout");
/* prepare to read through write operation */
if (rxbytes > 0){
AT91C_BASE_TWI->TWI_MMR |= txbytes << 8;
/* store internal slave address in TWI_IADR registers */
AT91C_BASE_TWI->TWI_IADR = 0;
while (txbytes > 0){
AT91C_BASE_TWI->TWI_IADR = (AT91C_BASE_TWI->TWI_IADR << 8);
AT91C_BASE_TWI->TWI_IADR |= *(txbuf++);
txbytes--;
}
/* Internal address of I2C slave was set in special Atmel registers.
* Now we must call read function. The I2C cell automatically sends
* bytes from IADR register to bus and issues repeated start. */
return i2c_lld_master_receive_timeout(i2cp, addr, rxbuf, rxbytes, timeout);
}
else{
if (txbytes == 1){
/* In single data byte master read or write, the START and STOP
* must both be set. */
AT91C_BASE_TWI->TWI_CR |= AT91C_TWI_STOP;
}
AT91C_BASE_TWI->TWI_MMR = 0;
AT91C_BASE_TWI->TWI_MMR |= addr << 16;
/* enable just needed interrupts */
AT91C_BASE_TWI->TWI_IER = AT91C_TWI_TXRDY | AT91C_TWI_NACK;
/* correct size and pointer because first byte will be written
* for issue start condition */
i2cp->txbuf = txbuf + 1;
i2cp->txbytes = txbytes - 1;
/* According to datasheet there is no need to set START manually
* we just need to write first byte in THR */
AT91C_BASE_TWI->TWI_THR = txbuf[0];
/* Waits for the operation completion.*/
i2cp->thread = chThdSelf();
chSchGoSleepS(THD_STATE_SUSPENDED);
return chThdSelf()->p_u.rdymsg;
}
}
uint8_t i2c_t::CmdWriteRead(uint8_t Addr,
uint8_t *WPtr, uint8_t WLength,
uint8_t *RPtr, uint8_t RLength) {
if(IBusyWait() != OK) return FAILURE;
// Clear flags
ii2c->SR1 = 0;
while(RxIsNotEmpty()) (void)ii2c->DR; // Read DR until it empty
ClearAddrFlag();
// Start transmission
SendStart();
if(WaitEv5() != OK) return FAILURE;
SendAddrWithWrite(Addr);
if(WaitEv6() != OK) { SendStop(); return FAILURE; }
ClearAddrFlag();
// Start TX DMA if needed
if(WLength != 0) {
if(WaitEv8() != OK) return FAILURE;
dmaStreamSetMemory0(PDmaTx, WPtr);
dmaStreamSetMode (PDmaTx, I2C_DMATX_MODE);
dmaStreamSetTransactionSize(PDmaTx, WLength);
chSysLock();
PRequestingThread = chThdSelf();
dmaStreamEnable(PDmaTx);
chSchGoSleepS(THD_STATE_SUSPENDED); // Sleep until end
chSysUnlock();
dmaStreamDisable(PDmaTx);
}
// Read if needed
if(RLength != 0) {
if(WaitEv8() != OK) return FAILURE;
// Send repeated start
SendStart();
if(WaitEv5() != OK) return FAILURE;
SendAddrWithRead(Addr);
if(WaitEv6() != OK) { SendStop(); return FAILURE; }
// If single byte is to be received, disable ACK before clearing ADDR flag
if(RLength == 1) AckDisable();
else AckEnable();
ClearAddrFlag();
dmaStreamSetMemory0(PDmaRx, RPtr);
dmaStreamSetMode (PDmaRx, I2C_DMARX_MODE);
dmaStreamSetTransactionSize(PDmaRx, RLength);
DmaLastTransferSet(); // Inform DMA that this is last transfer => do not ACK last byte
chSysLock();
PRequestingThread = chThdSelf();
dmaStreamEnable(PDmaRx);
chSchGoSleepS(THD_STATE_SUSPENDED); // Sleep until end
chSysUnlock();
dmaStreamDisable(PDmaRx);
} // if != 0
else WaitBTF(); // if nothing to read, just stop
SendStop();
return OK;
}
/**
* @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;
}
/**
* @brief Receives data via the I2C bus as master.
* @details Number of receiving bytes must be 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[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) {
(void)i2cp;
(void)addr;
(void)rxbuf;
(void)rxbytes;
(void)timeout;
uint32_t status;
uint32_t stop_sent;
i2cdef_t i2c = i2cp->i2c;
/* Set read mode, slave address and 3 internal address byte lengths */
i2c->TWI_MMR = 0;
i2c->TWI_MMR = TWI_MMR_MREAD | TWI_MMR_DADR(addr->chip) | ((addr->len << TWI_MMR_IADRSZ_Pos) & TWI_MMR_IADRSZ_Msk);
/* Set internal address for remote chip */
i2c->TWI_IADR = 0;
i2c->TWI_IADR = twi_mk_addr(addr->addr, addr->len);
/* Send a START condition */
if (rxbytes = 1) {
i2c->TWI_CR = TWI_CR_START | TWI_CR_STOP;
while (!(i2c->TWI_SR & TWI_SR_RXRDY));
*rxbuf = i2c->TWI_RHR;
} else {
i2c->TWI_PTCR = TWI_PTCR_RXTDIS | TWI_PTCR_TXTDIS;
i2c->TWI_IER = TWI_IER_ENDRX | TWI_IER_NACK | TWI_IER_OVRE;
chSysLock();
i2cp->thread = chThdSelf();
i2c->TWI_RNPR = 0;
i2c->TWI_RNCR = 0;
i2c->TWI_RPR = (uint32_t) rxbuf;
i2c->TWI_RCR = rxbytes - 2;
i2c->TWI_PTCR = TWI_PTCR_RXTEN;
i2cp->curbuf = rxbuf + (rxbytes - 2);
i2c->TWI_CR = TWI_CR_START;
chSchGoSleepS(THD_STATE_SUSPENDED);
return chThdSelf()->p_u.rdymsg;
}
return RDY_OK;
}
/**
* @brief CPU pulse.
* @note The current implementation is not totally reliable.
*
* @param[in] duration CPU pulse duration in milliseconds
*/
void test_cpu_pulse(unsigned duration) {
systime_t start, end, now;
start = chThdSelf()->p_time;
end = start + MS2ST(duration);
do {
now = chThdSelf()->p_time;
#if defined(SIMULATOR)
ChkIntSources();
#endif
}
while (end > start ? (now >= start) && (now < end) :
(now >= start) || (now < end));
}
/**
* @brief Performs an ADC conversion.
* @details Performs a synchronous conversion operation.
* @note The buffer is organized as a matrix of M*N elements where M is the
* channels number configured into the conversion group and N is the
* buffer depth. The samples are sequentially written into the buffer
* with no gaps.
*
* @param[in] adcp pointer to the @p ADCDriver object
* @param[in] grpp pointer to a @p ADCConversionGroup object
* @param[out] samples pointer to the samples buffer
* @param[in] depth buffer depth (matrix rows number). The buffer depth
* must be one or an even number.
* @return The operation result.
* @retval RDY_OK Conversion finished.
* @retval RDY_RESET The conversion has been stopped using
* @p acdStopConversion() or @p acdStopConversionI(),
* the result buffer may contain incorrect data.
* @retval RDY_TIMEOUT The conversion has been stopped because an hardware
* error.
*
* @api
*/
msg_t adcConvert(ADCDriver *adcp,
const ADCConversionGroup *grpp,
adcsample_t *samples,
size_t depth) {
msg_t msg;
chSysLock();
chDbgAssert(adcp->thread == NULL, "adcConvert(), #1", "already waiting");
adcStartConversionI(adcp, grpp, samples, depth);
adcp->thread = chThdSelf();
chSchGoSleepS(THD_STATE_SUSPENDED);
msg = chThdSelf()->p_u.rdymsg;
chSysUnlock();
return msg;
}
/**
* @brief Master transmission.
*
* @param[in] i2cp pointer to the @p I2CDriver object
* @param[in] addr slave device address (7 bits) without R/W bit
* @param[in] txbuf transmit data buffer pointer
* @param[in] txbytes number of bytes to be transmitted
* @param[out] rxbuf receive data buffer pointer
* @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.
* .
*
* @notapi
*/
msg_t i2c_lld_master_transmit_timeout(I2CDriver *i2cp, i2caddr_t addr,
const uint8_t *txbuf, size_t txbytes,
uint8_t *rxbuf, const uint8_t rxbytes,
systime_t timeout) {
VirtualTimer vt;
/* Global timeout for the whole operation.*/
if (timeout != TIME_INFINITE)
chVTSetI(&vt, timeout, i2c_lld_safety_timeout, (void *)i2cp);
i2cp->addr = addr;
i2cp->txbuf = txbuf;
i2cp->txbytes = txbytes;
i2cp->txidx = 0;
i2cp->rxbuf = rxbuf;
i2cp->rxbytes = rxbytes;
i2cp->rxidx = 0;
bscdevice_t *device = i2cp->device;
device->slaveAddress = addr;
device->dataLength = txbytes;
device->status = CLEAR_STATUS;
/* Enable Interrupts and start transfer.*/
device->control |= (BSC_INTT | BSC_INTD | START_WRITE);
/* Is this really needed? there is an outer lock already */
chSysLock();
i2cp->thread = chThdSelf();
chSchGoSleepS(THD_STATE_SUSPENDED);
if ((timeout != TIME_INFINITE) && chVTIsArmedI(&vt))
chVTResetI(&vt);
chSysUnlock();
msg_t status = chThdSelf()->p_u.rdymsg;
if (status == RDY_OK && rxbytes > 0) {
/* The TIMEOUT_INFINITE prevents receive from setting up it's own timer.*/
status = i2c_lld_master_receive_timeout(i2cp, addr, rxbuf,
rxbytes, TIME_INFINITE);
if ((timeout != TIME_INFINITE) && chVTIsArmedI(&vt))
chVTResetI(&vt);
}
return status;
}
static void pwmpcb_fast(PWMDriver *pwmp) {
efiAssertVoid(getRemainingStack(chThdSelf())> 32, "lwStAdcFast");
#if EFI_INTERNAL_ADC
(void) pwmp;
/*
* Starts an asynchronous ADC conversion operation, the conversion
* will be executed in parallel to the current PWM cycle and will
* terminate before the next PWM cycle.
*/
chSysLockFromIsr()
;
if (ADC_FAST_DEVICE.state != ADC_READY &&
ADC_FAST_DEVICE.state != ADC_COMPLETE &&
ADC_FAST_DEVICE.state != ADC_ERROR) {
fastAdc.errorsCount++;
// todo: when? why? firmwareError("ADC fast not ready?");
chSysUnlockFromIsr()
;
return;
}
adcStartConversionI(&ADC_FAST_DEVICE, &adcgrpcfg_fast, fastAdc.samples, ADC_BUF_DEPTH_FAST);
chSysUnlockFromIsr()
;
fastAdc.conversionCount++;
#endif
}
static void pwmpcb_slow(PWMDriver *pwmp) {
efiAssertVoid(getRemainingStack(chThdSelf())> 32, "lwStAdcSlow");
#if EFI_INTERNAL_ADC
(void) pwmp;
/* Starts an asynchronous ADC conversion operation, the conversion
will be executed in parallel to the current PWM cycle and will
terminate before the next PWM cycle.*/
slowAdc.conversionCount++;
chSysLockFromIsr()
;
if (ADC_SLOW_DEVICE.state != ADC_READY &&
ADC_SLOW_DEVICE.state != ADC_COMPLETE &&
ADC_SLOW_DEVICE.state != ADC_ERROR) {
// todo: why and when does this happen? firmwareError("ADC slow not ready?");
slowAdc.errorsCount++;
chSysUnlockFromIsr()
;
return;
}
adcStartConversionI(&ADC_SLOW_DEVICE, &adcgrpcfgSlow, slowAdc.samples, ADC_BUF_DEPTH_SLOW);
chSysUnlockFromIsr()
;
#endif
}
void appendPrintf(Logging *logging, const char *fmt, ...) {
efiAssertVoid(getRemainingStack(chThdSelf()) > 128, "lowstck#4");
va_list ap;
va_start(ap, fmt);
vappendPrintf(logging, fmt, ap);
va_end(ap);
}
void vappendPrintf(Logging *logging, const char *fmt, va_list arg) {
efiAssertVoid(getRemainingStack(chThdSelf()) > 16, "stack#5b");
if (!intermediateLoggingBufferInited) {
firmwareError("intermediateLoggingBufferInited not inited!");
return;
}
int is_locked = isLocked();
int icsr_vectactive = isIsrContext();
if (is_locked) {
vappendPrintfI(logging, fmt, arg);
} else {
if (icsr_vectactive == 0) {
chSysLock()
;
vappendPrintfI(logging, fmt, arg);
chSysUnlock()
;
} else {
chSysLockFromIsr()
;
vappendPrintfI(logging, fmt, arg);
chSysUnlockFromIsr()
;
}
}
}
void readFromFlash(void) {
efiAssertVoid(getRemainingStack(chThdSelf()) > 256, "read f");
printMsg(logger, "readFromFlash()");
flashRead(FLASH_ADDR, (char *) &persistentState, PERSISTENT_SIZE);
persisted_configuration_state_e result;
if (!isValidCrc(&persistentState)) {
result = CRC_FAILED;
resetConfigurationExt(logger, DEFAULT_ENGINE_TYPE PASS_ENGINE_PARAMETER);
} else if (persistentState.version != FLASH_DATA_VERSION || persistentState.size != PERSISTENT_SIZE) {
result = INCOMPATIBLE_VERSION;
resetConfigurationExt(logger, engineConfiguration->engineType PASS_ENGINE_PARAMETER);
} else {
/**
* At this point we know that CRC and version number is what we expect. Safe to assume it's a valid configuration.
*/
result = OK;
applyNonPersistentConfiguration(logger PASS_ENGINE_PARAMETER);
}
// we can only change the state after the CRC check
engineConfiguration->firmwareVersion = getRusEfiVersion();
if (result == CRC_FAILED) {
printMsg(logger, "Need to reset flash to default due to CRC");
} else if (result == INCOMPATIBLE_VERSION) {
printMsg(logger, "Resetting but saving engine type [%d]", engineConfiguration->engineType);
} else {
printMsg(logger, "Got valid configuration from flash!");
}
}
static msg_t detect_thread(void *arg) {
(void)arg;
chRegSetThreadName("Detect");
detect_tp = chThdSelf();
for(;;) {
chEvtWaitAny((eventmask_t) 1);
if (!conf_general_detect_motor_param(detect_current, detect_min_rpm,
detect_low_duty, &detect_cycle_int_limit, &detect_coupling_k,
detect_hall_table, &detect_hall_res)) {
detect_cycle_int_limit = 0.0;
detect_coupling_k = 0.0;
}
int32_t ind = 0;
send_buffer[ind++] = COMM_DETECT_MOTOR_PARAM;
buffer_append_int32(send_buffer, (int32_t)(detect_cycle_int_limit * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(detect_coupling_k * 1000.0), &ind);
memcpy(send_buffer + ind, detect_hall_table, 8);
ind += 8;
send_buffer[ind++] = detect_hall_res;
commands_send_packet(send_buffer, ind);
}
return 0;
}
static ALWAYS_INLINE void handleFuel(bool limitedFuel, uint32_t eventIndex, int rpm DECLARE_ENGINE_PARAMETER_S) {
if (!isInjectionEnabled(engineConfiguration))
return;
efiAssertVoid(getRemainingStack(chThdSelf()) > 128, "lowstck#3");
efiAssertVoid(eventIndex < ENGINE(triggerShape.getLength()), "handleFuel/event index");
/**
* Ignition events are defined by addFuelEvents() according to selected
* fueling strategy
*/
FuelSchedule *fs = ENGINE(engineConfiguration2)->injectionEvents;
InjectionEventList *source = &fs->injectionEvents;
if (!fs->hasEvents[eventIndex])
return;
ENGINE(tpsAccelEnrichment.onNewValue(getTPS(PASS_ENGINE_PARAMETER_F) PASS_ENGINE_PARAMETER));
ENGINE(engineLoadAccelEnrichment.onEngineCycle(PASS_ENGINE_PARAMETER_F));
ENGINE(fuelMs) = getFuelMs(rpm PASS_ENGINE_PARAMETER) * CONFIG(globalFuelCorrection);
for (int i = 0; i < source->size; i++) {
InjectionEvent *event = &source->elements[i];
if (event->injectionStart.eventIndex != eventIndex)
continue;
handleFuelInjectionEvent(i, limitedFuel, event, rpm PASS_ENGINE_PARAMETER);
}
}
/*
* Application entry point.
*/
int main(void) {
halInit();
chSysInit();
/*
* Serial port initialization.
*/
sdStart(&SD1, NULL);
chprintf((BaseSequentialStream *)&SD1, "BCM2835 GPIO Demonstration\r\n");
ioportid_t ledPort = ONBOARD_LED_PORT;
uint32_t ledPad = ONBOARD_LED_PAD;
palSetPadMode(ledPort, ledPad, PAL_MODE_OUTPUT);
palSetPad(ledPort, ledPad);
palSetPadMode(GPIO4_PORT, GPIO4_PAD, PAL_MODE_INPUT_PULLUP);
for (;;) {
uint32_t button_state = palReadPad(GPIO4_PORT, GPIO4_PAD);
if (button_state) {
palSetPad(ledPort, ledPad);
}
else {
palClearPad(ledPort, ledPad);
}
}
/*
* Events servicing loop.
*/
chThdWait(chThdSelf());
return 0;
}
/* Our module initialiser */
void _gosInit(void) {
/* Don't initialise if the user already has */
if (!chThdSelf()) {
halInit();
chSysInit();
}
}
static void vappendPrintfI(Logging *logging, const char *fmt, va_list arg) {
intermediateLoggingBuffer.eos = 0; // reset
efiAssertVoid(getRemainingStack(chThdSelf()) > 128, "lowstck#1b");
chvprintf((BaseSequentialStream *) &intermediateLoggingBuffer, fmt, arg);
intermediateLoggingBuffer.buffer[intermediateLoggingBuffer.eos] = 0; // need to terminate explicitly
append(logging, (char *) intermediateLoggingBufferData);
}
void chDbgPanic(const char *msg1) {
#if CH_USE_REGISTRY
Uart.PrintfNow("\r%S @ %S\r", msg1, chThdSelf()->p_name);
#else
Uart.PrintfNow("\r%S\r", msg1);
#endif
}
static ALWAYS_INLINE void handleFuel(uint32_t eventIndex, int rpm DECLARE_ENGINE_PARAMETER_S) {
if (!isInjectionEnabled(engine->engineConfiguration))
return;
efiAssertVoid(getRemainingStack(chThdSelf()) > 128, "lowstck#3");
efiAssertVoid(eventIndex < engine->triggerShape.getLength(), "handleFuel/event index");
/**
* Ignition events are defined by addFuelEvents() according to selected
* fueling strategy
*/
FuelSchedule *fs =
isCrankingR(rpm) ?
&ENGINE(engineConfiguration2)->crankingInjectionEvents : &engine->engineConfiguration2->injectionEvents;
InjectionEventList *source = &fs->events;
if (!fs->hasEvents[eventIndex])
return;
engine->tpsAccelEnrichment.onEngineCycleTps(PASS_ENGINE_PARAMETER_F);
engine->mapAccelEnrichment.onEngineCycle(PASS_ENGINE_PARAMETER_F);
ENGINE(fuelMs) = getFuelMs(rpm PASS_ENGINE_PARAMETER) * engineConfiguration->globalFuelCorrection;
for (int i = 0; i < source->size; i++) {
InjectionEvent *event = &source->elements[i];
if (event->injectionStart.eventIndex != eventIndex)
continue;
handleFuelInjectionEvent(event, rpm PASS_ENGINE_PARAMETER);
}
}
static void init() {
i2s::i2s0::configure(
audio::i2s0_config_tx,
audio::i2s0_config_rx,
audio::i2s0_config_dma
);
audio::dma::init();
audio::dma::configure();
audio::dma::enable();
i2s::i2s0::tx_start();
i2s::i2s0::rx_start();
LPC_CREG->DMAMUX = portapack::gpdma_mux;
gpdma::controller.enable();
nvicEnableVector(DMA_IRQn, CORTEX_PRIORITY_MASK(LPC_DMA_IRQ_PRIORITY));
baseband::dma::init();
rf::rssi::init();
touch::dma::init();
thread_main = chThdSelf();
thread_rssi = chThdCreateStatic(rssi_thread_wa, sizeof(rssi_thread_wa),
rssi_thread_priority, rssi_fn,
nullptr
);
chThdCreateStatic(baseband_thread_wa, sizeof(baseband_thread_wa),
baseband_thread_priority, baseband_fn,
nullptr
);
}
static __attribute__((noreturn)) msg_t baseband_fn(void *arg) {
(void)arg;
chRegSetThreadName("baseband");
BasebandStatsCollector stats {
chSysGetIdleThread(),
thread_main,
thread_rssi,
chThdSelf()
};
while(true) {
// TODO: Place correct sampling rate into buffer returned here:
const auto buffer_tmp = baseband::dma::wait_for_rx_buffer();
const buffer_c8_t buffer {
buffer_tmp.p, buffer_tmp.count, baseband_configuration.sampling_rate
};
if( baseband_processor ) {
baseband_processor->execute(buffer);
}
stats.process(buffer,
[](const BasebandStatistics statistics) {
BasebandStatisticsMessage message;
message.statistics = statistics;
shared_memory.application_queue.push(message);
}
);
}
}
/*-----------------------------------------------------------------------------*/
void
StopTask(tfunc_t pf)
{
int16_t i;
Thread *tp;
for(i=0;i<RC_TASKS;i++)
{
if( rcTasks[i].pf == pf )
{
if( rcTasks[i].tp != NULL )
{
tp = rcTasks[i].tp;
// We cannot stop ourself
if( tp == chThdSelf() )
return;
// Set terminate flag - the other thread has to see this and exit
chThdTerminate( tp );
// this will cause a higher priority task to run immeadiately
chThdResume( tp );
// wait for termination
chThdWait( tp );
// may already be NULL for high priority task
rcTasks[i].tp = NULL;
rcTasks[i].pf = NULL;
}
}
}
}
static void msg1_execute(void) {
msg_t msg;
/*
* Testing the whole messages loop.
*/
threads[0] = chThdCreateStatic(wa[0], WA_SIZE, chThdGetPriority() + 1,
thread, chThdSelf());
chMsgRelease(msg = chMsgWait());
test_emit_token(msg);
chMsgRelease(msg = chMsgWait());
test_emit_token(msg);
chMsgRelease(msg = chMsgWait());
test_emit_token(msg);
test_assert_sequence(1, "ABC");
/*
* Testing message fetch using chMsgGet().
* Note, the following is valid because the sender has higher priority than
* the receiver.
*/
msg = chMsgGet();
test_assert(1, msg != 0, "no message");
chMsgRelease(0);
test_assert(2, msg == 'D', "wrong message");
/*
* Must not have pending messages.
*/
msg = chMsgGet();
test_assert(3, msg == 0, "unknown message");
}
int main() {
// ==== Setup clock ====
Clk.UpdateFreqValues();
uint8_t ClkResult = FAILURE;
Clk.SetupFlashLatency(12); // Setup Flash Latency for clock in MHz
// 12 MHz/6 = 2; 2*192 = 384; 384/8 = 48 (preAHB divider); 384/8 = 48 (USB clock)
Clk.SetupPLLDividers(6, 192, pllSysDiv8, 8);
// 48/4 = 12 MHz core clock. APB1 & APB2 clock derive on AHB clock
Clk.SetupBusDividers(ahbDiv4, apbDiv1, apbDiv1);
if((ClkResult = Clk.SwitchToPLL()) == 0) Clk.HSIDisable();
Clk.UpdateFreqValues();
// ==== Init OS ====
halInit();
chSysInit();
// ==== Init Hard & Soft ====
App.PThd = chThdSelf();
Uart.Init(115200);
Uart.Printf("\rLockNFC3 F205 AHB freq=%uMHz", Clk.AHBFreqHz/1000000);
// Report problem with clock if any
if(ClkResult) Uart.Printf("Clock failure\r");
LedService.Init();
LedService.StartSequence(lsqBlinkGreenX2);
Led.Init();
Led.StartSequence(lsqDoorClose);
Sensors.Init();
Pn.Init();
SD.Init(); // SD-card init
App.IDStore.Load(); // Init Srorage of IDs
SndList.Init();
App.ReadConfig(); // Read config from SD-card
Sound.Init();
Sound.SetVolume(250);
Sound.RegisterAppThd(chThdSelf());
Sound.Play("alive.wav");
#if USB_ENABLED
MassStorage.Init(); // Init USB MassStorage device
#endif
// ==== Main cycle ====
App.ITask();
}
