/* * 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); } }