/*
* ADXL345 (low-g accelerometer) main thread.
*/
msg_t adxl345_thread(void *arg)
{
(void)arg;
const SPIConfig spi_cfg = {
NULL,
ADXL345_SPI_CS_PORT,
ADXL345_SPI_CS_PIN,
SPI_CR1_BR_1 | SPI_CR1_BR_0 | SPI_CR1_CPOL | SPI_CR1_CPHA
};
int16_t accels[3], axis, g;
m2status_lg_accel_status(STATUS_WAIT);
chRegSetThreadName("ADXL345");
chBSemInit(&bs345, true);
spiStart(&ADXL345_SPID, &spi_cfg);
adxl3x5_init(&ADXL345_SPID, 4, &axis, &g);
log_i16(M2T_CH_CAL_LG_ACCEL, axis, g, 0, 0);
while(TRUE) {
adxl3x5_read_accel(&ADXL345_SPID, accels);
log_i16(M2T_CH_IMU_LG_ACCEL, accels[0], accels[1], accels[2], 0);
m2status_set_lga(accels[0], accels[1], accels[2]);
state_estimation_new_lg_accel(
adxl3x5_accels_to_axis(accels, axis, g));
/* Sleep until DRDY */
chSysLock();
chBSemWaitS(&bs345);
/*tp345 = chThdSelf();*/
/*chSchGoSleepTimeoutS(THD_STATE_SUSPENDED, 100);*/
/*tp345 = NULL;*/
chSysUnlock();
m2status_lg_accel_status(STATUS_OK);
}
}
msg_t BinarySemaphore::waitS(void) {
return chBSemWaitS(&bsem);
}
/****************** Thread main loop ***********************************/
msg_t analogue_thread(void *args)
{
(void)args;
chRegSetThreadName("Analogue");
chBSemObjectInit(&bsAnalogueInst, true);
chBSemObjectInit(&bsAnalogueFX, true);
adcInit();
adcStart(&ADCD1, NULL);
adcStart(&ADCD2, NULL);
adcStartConversion(&ADCD1, &adc_con_group_1, (adcsample_t*)buffer1,
INST_BUF_DEPTH);
adcStartConversion(&ADCD2, &adc_con_group_2, (adcsample_t*)fx_samples,
FX_BUF_DEPTH);
dacInit();
dacStart(&DACD1, &dac_cfg);
dacStartConversion(&DACD1, &dac_conv_grp, (dacsample_t*)buffer3,
INST_BUF_DEPTH);
// Enable DAC output buffer:
DACD1.params->dac->CR |= DAC_CR_BOFF1;
/* Start the GPT timers. They reload at after reaching 1 such that
* TRGO frequency equals timer frequency. */
gptStart(&GPTD3, &gpt_inst_config);
GPTD3.tim->CR2 |= STM32_TIM_CR2_MMS(2);
gptStartContinuous(&GPTD3, 2);
GPTD3.tim->DIER &= ~STM32_TIM_DIER_UIE;
gptStart(&GPTD8, &gpt_fx_config);
GPTD8.tim->CR2 |= STM32_TIM_CR2_MMS(2);
gptStartContinuous(&GPTD8, 2);
GPTD8.tim->DIER &= ~STM32_TIM_DIER_UIE;
state = 1;
// States:
// 1 - ADC:buf1, DSP:buf2, DAC:buf3
// 2 - DSP:buf1, DAC:buf2, ADC:buf3
// 3 - DAC:buf1, ADC:buf2, DSP:buf3
/* Wait until the ADC callback boops the semaphore. */
volatile uint16_t *dsp_buf;
while(true) {
chSysLock();
chBSemWaitS(&bsAnalogueInst);
chSysUnlock();
state += 1;
switch(state)
{
case 1:
dmaSetOtherMemory(ADCD1.dmastp, buffer1);
dsp_buf = buffer2;
dmaSetOtherMemory(DACD1.params->dma, buffer3);
break;
case 2:
dmaSetOtherMemory(ADCD1.dmastp, buffer3);
dsp_buf = buffer1;
dmaSetOtherMemory(DACD1.params->dma, buffer2);
break;
case 3:
dmaSetOtherMemory(ADCD1.dmastp, buffer2);
dsp_buf = buffer3;
dmaSetOtherMemory(DACD1.params->dma, buffer1);
break;
default:
state = 1;
dmaSetOtherMemory(ADCD1.dmastp, buffer1);
dsp_buf = buffer2;
dmaSetOtherMemory(DACD1.params->dma, buffer3);
}
dsp_stuff(dsp_buf);
}
}
