//-----------------------------------------------------------------------------
// polled read of a register
uint8_t
kbv_readRegister(VectorNavDriver * nvp, uint8_t reg, uint8_t size, uint8_t * buf)
{
uint8_t r[] = { 0x01, reg, 0x00, 0x00 };
chSysLock();
spiSelectI(nvp->spip);
// we ignore the first exchange's return
for ( int i = 0 ; i < 4 ; ++i )
spiPolledExchange(nvp->spip, r[i]);
spiUnselectI(nvp->spip);
gptPolledDelay(nvp->gpdp, 50);
spiSelectI(nvp->spip);
for ( int i = 0 ; i < 4 ; ++i )
r[i] = spiPolledExchange(nvp->spip, 0);
// was there an error?
if ( r[0] == 0x00 && r[1] == 0x01 && r[2] == reg && r[3] == 0x00 )
{
// all good!
for ( int i = 0 ; i < size ; ++i )
buf[i] = spiPolledExchange(nvp->spip, 0);
}
spiUnselectI(nvp->spip);
gptPolledDelay(nvp->gpdp, 50);
chSysUnlock();
return r[3];
}
void hipAdcCallback(adcsample_t value) {
if (state == WAITING_FOR_ADC_TO_SKIP) {
state = WAITING_FOR_RESULT_ADC;
} else if (state == WAITING_FOR_RESULT_ADC) {
engine->knockVolts = adcToVoltsDivided(value);
hipValueMax = maxF(engine->knockVolts, hipValueMax);
engine->knockLogic(engine->knockVolts);
float angleWindowWidth =
engineConfiguration->knockDetectionWindowEnd - engineConfiguration->knockDetectionWindowStart;
if (angleWindowWidth != currentAngleWindowWidth) {
currentAngleWindowWidth = angleWindowWidth;
prepareHip9011RpmLookup(currentAngleWindowWidth);
}
int integratorIndex = getIntegrationIndexByRpm(engine->rpmCalculator.rpmValue);
int gainIndex = getHip9011GainIndex(boardConfiguration->hip9011Gain);
int bandIndex = getBandIndex();
int prescalerIndex = engineConfiguration->hip9011PrescalerAndSDO;
if (currentGainIndex != gainIndex) {
currentGainIndex = gainIndex;
tx_buff[0] = SET_GAIN_CMD + gainIndex;
state = IS_SENDING_SPI_COMMAND;
spiSelectI(driver);
spiStartExchangeI(driver, 1, tx_buff, rx_buff);
} else if (currentIntergratorIndex != integratorIndex) {
currentIntergratorIndex = integratorIndex;
tx_buff[0] = SET_INTEGRATOR_CMD + integratorIndex;
state = IS_SENDING_SPI_COMMAND;
spiSelectI(driver);
spiStartExchangeI(driver, 1, tx_buff, rx_buff);
} else if (currentBandIndex != bandIndex) {
currentBandIndex = bandIndex;
tx_buff[0] = SET_BAND_PASS_CMD + bandIndex;
state = IS_SENDING_SPI_COMMAND;
spiSelectI(driver);
spiStartExchangeI(driver, 1, tx_buff, rx_buff);
} else if (currentPrescaler != prescalerIndex) {
currentPrescaler = prescalerIndex;
tx_buff[0] = SET_PRESCALER_CMD + prescalerIndex;
state = IS_SENDING_SPI_COMMAND;
spiSelectI(driver);
spiStartExchangeI(driver, 1, tx_buff, rx_buff);
} else {
state = READY_TO_INTEGRATE;
}
}
}
/*
* ADC end conversion callback.
* The PWM channels are reprogrammed using the latest ADC samples.
* The latest samples are transmitted into a single SPI transaction.
*/
void adccb(ADCDriver *adcp, adcsample_t *buffer, size_t n) {
(void) buffer; (void) n;
/* Note, only in the ADC_COMPLETE state because the ADC driver fires an
intermediate callback when the buffer is half full.*/
if (adcp->state == ADC_COMPLETE) {
adcsample_t avg_ch1, avg_ch2;
/* Calculates the average values from the ADC samples.*/
avg_ch1 = (samples[0] + samples[2] + samples[4] + samples[6]) / 4;
avg_ch2 = (samples[1] + samples[3] + samples[5] + samples[7]) / 4;
chSysLockFromIsr();
/* Changes the channels pulse width, the change will be effective
starting from the next cycle.*/
pwmEnableChannelI(&PWMD4, 0, PWM_FRACTION_TO_WIDTH(&PWMD4, 4096, avg_ch1));
pwmEnableChannelI(&PWMD4, 3, PWM_FRACTION_TO_WIDTH(&PWMD4, 4096, avg_ch2));
/* SPI slave selection and transmission start.*/
spiSelectI(&SPID2);
spiStartSendI(&SPID2, ADC_GRP1_NUM_CHANNELS * ADC_GRP1_BUF_DEPTH, samples);
chSysUnlockFromIsr();
}
}
//-----------------------------------------------------------------------------
static void
vectornav_gpt_end_cb(GPTDriver * gptp)
{
(void)gptp;
switch( async_vn_msg.state )
{
case VN_ASYNC_1ST_SLEEP:
chSysLockFromISR();
async_vn_msg.state = VN_ASYNC_2ND_SPI_CB;
memset(&async_vn_msg.buf, 0, sizeof(async_vn_msg.buf));
spiSelectI(VND1.spip);
spiStartReceiveI(VND1.spip, async_vn_msg.buf_size, async_vn_msg.buf);
chSysUnlockFromISR();
break;
case VN_ASYNC_2ND_SLEEP:
// finished, clear it all
memset(&async_vn_msg, 0, sizeof(async_vn_msg));
break;
case VN_ASYNC_INACTIVE:
case VN_ASYNC_1ST_SPI_CB:
case VN_ASYNC_2ND_SPI_CB:
default:
// @TODO: assert?
break;
}
}
void write_digit(int8_t num, uint8_t dig){
uint8_t out_bytes[1]; /*= {
(((num<10)&&(num>=0)) ? number_seg_bytes[num] : number_seg_bytes[10]),
};*/
out_bytes[0] =(((num<10)&&(num>=0)) ? number_seg_bytes[num] : number_seg_bytes[10]);
chSysLockFromIsr();
palClearPad(GPIOA,3);
/* SPI slave selection and transmission start.*/
spiSelectI(&SPID1);
//spiStartSendI(&SPID1, 1, out_bytes);
spiPolledExchange(&SPID1, out_bytes[0]);
uint8_t nd = NUM_DIGS;
while(nd--){
if(nd == dig){
palSetPad(GPIOC,nd);
} else {
palClearPad(GPIOC,nd);
}
}
spiUnselectI(&SPID1);
palSetPad(GPIOA, 3);
chSysUnlockFromIsr();
//while(SPI_done == FALSE);
chThdSleepMicroseconds(DIG_SWITCH_DELAY_US);
//chThdSleepMilliseconds(DIG_SWITCH_DELAY_MS);
}
/**
* @brief Asserts the slave select signal and prepares for transfers.
*
* @param[in] spip pointer to the @p SPIDriver object
*
* @api
*/
void spiSelect(SPIDriver *spip) {
osalDbgCheck(spip != NULL);
osalSysLock();
osalDbgAssert(spip->state == SPI_READY, "not ready");
spiSelectI(spip);
osalSysUnlock();
}
/**
* @brief Asserts the slave select signal and prepares for transfers.
* @pre ILI9341 is ready.
*
* @param[in] driverp pointer to the @p ILI9341Driver object
*
* @iclass
*/
void ili9341SelectI(ILI9341Driver *driverp) {
osalDbgCheckClassI();
osalDbgCheck(driverp != NULL);
osalDbgAssert(driverp->state == ILI9341_READY, "invalid state");
driverp->state = ILI9341_ACTIVE;
spiSelectI(driverp->config->spi);
}
/**
* @brief Asserts the slave select signal and prepares for transfers.
*
* @param[in] spip pointer to the @p SPIDriver object
*
* @api
*/
void spiSelect(SPIDriver *spip) {
chDbgCheck(spip != NULL, "spiSelect");
chSysLock();
chDbgAssert(spip->state == SPI_READY, "spiSelect(), #1", "not ready");
spiSelectI(spip);
chSysUnlock();
}
/**
* @brief Configures and activates the ILI9341 peripheral.
* @pre ILI9341 is stopped.
*
* @param[in] driverp pointer to the @p ILI9341Driver object
* @param[in] configp pointer to the @p ILI9341Config object
*
* @api
*/
void ili9341Start(ILI9341Driver *driverp, const ILI9341Config *configp) {
chSysLock();
osalDbgCheck(driverp != NULL);
osalDbgCheck(configp != NULL);
osalDbgCheck(configp->spi != NULL);
osalDbgAssert(driverp->state == ILI9341_STOP, "invalid state");
spiSelectI(configp->spi);
spiUnselectI(configp->spi);
driverp->config = configp;
driverp->state = ILI9341_READY;
chSysUnlock();
}
//-----------------------------------------------------------------------------
void
kbv_drIntExtiCB(EXTDriver *extp, expchannel_t channel)
{
(void)extp;
(void)channel;
chSysLockFromISR();
memset(&async_vn_msg, 0, sizeof(async_vn_msg));
async_vn_msg.reg = VN100_REG_YPR;
// ReadReg: [0x01, REG, 0x00, 0x00]
async_vn_msg.buf[0] = 1;
async_vn_msg.buf[1] = async_vn_msg.reg;
async_vn_msg.buf_size = VN100_REG_YPR_SIZE + VN100_HEADER_SIZE;
async_vn_msg.state = VN_ASYNC_1ST_SPI_CB;
spiSelectI(VND1.spip);
spiStartSendI(VND1.spip, 4, async_vn_msg.buf);
///**/kbed_dataReadyI();
chSysUnlockFromISR();
}
void
vexSpiSend()
{
int16_t i;
uint16_t *txbuf = (uint16_t *)vexSpiData.txdata.data;
uint16_t *rxbuf = (uint16_t *)vexSpiData.rxdata_t.data;
// configure team name if in configuration state
if(vexSpiData.txdata.pak.state == 0x03)
{
char *p = spiTeamName;
// Set team name data type
vexSpiData.txdata.pak.type = 0x55;
// Copy team name into data area
// This is the same area as occupied normally by motor data
for(i=0;i<8;i++)
{
if(*p != 0)
vexSpiData.txdata.data[6+i] = *p++;
else
vexSpiData.txdata.data[6+i] = ' ';
}
}
// Set handshake to indicate new spi message
palSetPad( VEX_SPI_ENABLE_PORT, VEX_SPI_ENABLE_PIN );
for(i=0;i<16;i++)
{
spiSelectI(&SPID1);
rxbuf[i] = spi_lld_polled_exchange( &SPID1, txbuf[i] );
//spiExchange( &SPID1, 1, &txbuf[i], &rxbuf[i]);
spiUnselectI(&SPID1);
if( ((i%4) == 3) && (i != 15) )
{
// long delay between each group of 4 words
vexSpiTickDelay(73);
// After 4 words negate handshake pin
palClearPad( VEX_SPI_ENABLE_PORT, VEX_SPI_ENABLE_PIN );
}
else
vexSpiTickDelay(8);
}
// increase id for next message
vexSpiData.txdata.pak.id++;
// check integrity of received data
if( (vexSpiData.rxdata_t.data[0] == 0x17 ) && (vexSpiData.rxdata_t.data[1] == 0xC9 ))
{
// copy temporary data
for(i=0;i<32;i++)
vexSpiData.rxdata.data[i] = vexSpiData.rxdata_t.data[i];
// Set online status if valid data status set
if( (vexSpiData.rxdata.pak.status & 0x0F) == 0x08 )
vexSpiData.online = 1;
// If in configuration initialize state (0x02 or 0x03)
if( (vexSpiData.txdata.pak.state & 0x0E) == 0x02 )
{
// check for configure request
if( (vexSpiData.rxdata.pak.status & 0x0F) == 0x02 )
vexSpiData.txdata.pak.state = 0x03;
// check for configure and acknowledge
if( (vexSpiData.rxdata.pak.status & 0x0F) == 0x03 )
{
vexSpiData.txdata.pak.state = 0x08;
vexSpiData.txdata.pak.type = 0;
}
// Either good or bad data force to normal transmission
// status will either be 0x04 or 0x08
if( (vexSpiData.rxdata.pak.status & 0x0C) != 0x00 )
{
vexSpiData.txdata.pak.state = 0x08;
vexSpiData.txdata.pak.type = 0;
}
}
}
else
vexSpiData.errors++;
}
void Hip9011Hardware::sendCommand(unsigned char command) {
tx_buff[0] = command;
spiSelectI(driver);
spiStartExchangeI(driver, 1, tx_buff, rx_buff);
}
