|
123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429 |
-
-
-
- #include <Arduino.h>
- #include "input_adc.h"
- #include "utility/dspinst.h"
-
- #if defined(KINETISK)
-
- #include "utility/pdb.h"
-
- #define COEF_HPF_DCBLOCK (1048300<<10)
-
- DMAMEM __attribute__((aligned(32))) static uint16_t analog_rx_buffer[AUDIO_BLOCK_SAMPLES];
- audio_block_t * AudioInputAnalog::block_left = NULL;
- uint16_t AudioInputAnalog::block_offset = 0;
- int32_t AudioInputAnalog::hpf_y1 = 0;
- int32_t AudioInputAnalog::hpf_x1 = 0;
-
- bool AudioInputAnalog::update_responsibility = false;
- DMAChannel AudioInputAnalog::dma(false);
-
- void AudioInputAnalog::init(uint8_t pin)
- {
- int32_t tmp;
-
-
-
-
- analogReadRes(16);
- analogReference(INTERNAL);
- #if F_BUS == 96000000 || F_BUS == 48000000 || F_BUS == 24000000
- analogReadAveraging(8);
- #else
- analogReadAveraging(4);
- #endif
-
-
-
- tmp = (uint16_t) analogRead(pin);
- tmp = ( ((int32_t) tmp) << 14);
- hpf_x1 = tmp;
- hpf_y1 = 0;
-
-
- if (!(SIM_SCGC6 & SIM_SCGC6_PDB)
- || (PDB0_SC & PDB_CONFIG) != PDB_CONFIG
- || PDB0_MOD != PDB_PERIOD
- || PDB0_IDLY != 1
- || PDB0_CH0C1 != 0x0101) {
- SIM_SCGC6 |= SIM_SCGC6_PDB;
- PDB0_IDLY = 1;
- PDB0_MOD = PDB_PERIOD;
- PDB0_SC = PDB_CONFIG | PDB_SC_LDOK;
- PDB0_SC = PDB_CONFIG | PDB_SC_SWTRIG;
- PDB0_CH0C1 = 0x0101;
- }
-
- ADC0_SC2 |= ADC_SC2_ADTRG | ADC_SC2_DMAEN;
-
-
- dma.begin(true);
- dma.TCD->SADDR = &ADC0_RA;
- dma.TCD->SOFF = 0;
- dma.TCD->ATTR = DMA_TCD_ATTR_SSIZE(1) | DMA_TCD_ATTR_DSIZE(1);
- dma.TCD->NBYTES_MLNO = 2;
- dma.TCD->SLAST = 0;
- dma.TCD->DADDR = analog_rx_buffer;
- dma.TCD->DOFF = 2;
- dma.TCD->CITER_ELINKNO = sizeof(analog_rx_buffer) / 2;
- dma.TCD->DLASTSGA = -sizeof(analog_rx_buffer);
- dma.TCD->BITER_ELINKNO = sizeof(analog_rx_buffer) / 2;
- dma.TCD->CSR = DMA_TCD_CSR_INTHALF | DMA_TCD_CSR_INTMAJOR;
- dma.triggerAtHardwareEvent(DMAMUX_SOURCE_ADC0);
- update_responsibility = update_setup();
- dma.enable();
- dma.attachInterrupt(isr);
- }
-
-
- void AudioInputAnalog::isr(void)
- {
- uint32_t daddr, offset;
- const uint16_t *src, *end;
- uint16_t *dest_left;
- audio_block_t *left;
-
- daddr = (uint32_t)(dma.TCD->DADDR);
- dma.clearInterrupt();
-
- if (daddr < (uint32_t)analog_rx_buffer + sizeof(analog_rx_buffer) / 2) {
-
-
- src = (uint16_t *)&analog_rx_buffer[AUDIO_BLOCK_SAMPLES/2];
- end = (uint16_t *)&analog_rx_buffer[AUDIO_BLOCK_SAMPLES];
- if (update_responsibility) AudioStream::update_all();
- } else {
-
-
- src = (uint16_t *)&analog_rx_buffer[0];
- end = (uint16_t *)&analog_rx_buffer[AUDIO_BLOCK_SAMPLES/2];
- }
- left = block_left;
- if (left != NULL) {
- offset = block_offset;
- if (offset > AUDIO_BLOCK_SAMPLES/2) offset = AUDIO_BLOCK_SAMPLES/2;
- dest_left = (uint16_t *)&(left->data[offset]);
- block_offset = offset + AUDIO_BLOCK_SAMPLES/2;
- do {
- *dest_left++ = *src++;
- } while (src < end);
- }
- }
-
- void AudioInputAnalog::update(void)
- {
- audio_block_t *new_left=NULL, *out_left=NULL;
- uint32_t offset;
- int32_t tmp;
- int16_t s, *p, *end;
-
-
-
-
- new_left = allocate();
-
- __disable_irq();
- offset = block_offset;
- if (offset < AUDIO_BLOCK_SAMPLES) {
-
- if (new_left != NULL) {
-
- if (block_left == NULL) {
-
-
- block_left = new_left;
- block_offset = 0;
- __enable_irq();
-
- } else {
-
- __enable_irq();
- release(new_left);
-
-
- }
- } else {
-
-
-
- __enable_irq();
-
- }
- return;
- }
-
-
- out_left = block_left;
- block_left = new_left;
- block_offset = 0;
- __enable_irq();
-
-
-
-
-
-
-
-
-
- p = out_left->data;
- end = p + AUDIO_BLOCK_SAMPLES;
- do {
- tmp = (uint16_t)(*p);
- tmp = ( ((int32_t) tmp) << 14);
- int32_t acc = hpf_y1 - hpf_x1;
- acc += tmp;
- hpf_y1 = FRACMUL_SHL(acc, COEF_HPF_DCBLOCK, 1);
- hpf_x1 = tmp;
- s = signed_saturate_rshift(hpf_y1, 16, 14);
- *p++ = s;
- } while (p < end);
-
-
- transmit(out_left);
- release(out_left);
- }
- #endif
-
-
-
- #if defined(__IMXRT1062__)
-
- #include <Arduino.h>
- #include "input_adc.h"
-
- extern "C" void xbar_connect(unsigned int input, unsigned int output);
-
- #define FILTERLEN 15
-
- DMAChannel AudioInputAnalog::dma(false);
-
- static __attribute__((aligned(32))) uint16_t adc_buffer[AUDIO_BLOCK_SAMPLES*4+200];
- static int16_t capture_buffer[AUDIO_BLOCK_SAMPLES*4+FILTERLEN];
-
-
- PROGMEM static const uint8_t adc2_pin_to_channel[] = {
- 7,
- 8,
- 12,
- 11,
- 6,
- 5,
- 15,
- 0,
- 13,
- 14,
- 255,
- 255,
- 3,
- 4,
- 7,
- 8,
- 12,
- 11,
- 6,
- 5,
- 15,
- 0,
- 13,
- 14,
- 255,
- 255,
- 3,
- 4,
- #ifdef ARDUINO_TEENSY41
- 255,
- 255,
- 255,
- 255,
- 255,
- 255,
- 255,
- 255,
- 255,
- 255,
- 1,
- 2,
- 9,
- 10,
- #endif
- };
-
- static const int16_t filter[FILTERLEN] = {
- 1449,
- 3676,
- 6137,
- 9966,
- 13387,
- 16896,
- 18951,
- 19957,
- 18951,
- 16896,
- 13387,
- 9966,
- 6137,
- 3676,
- 1449
- };
-
-
- void AudioInputAnalog::init(uint8_t pin)
- {
- if (pin >= sizeof(adc2_pin_to_channel)) return;
- const uint8_t adc_channel = adc2_pin_to_channel[pin];
- if (adc_channel == 255) return;
-
-
-
-
-
- const int comp1 = ((float)F_BUS_ACTUAL) / (AUDIO_SAMPLE_RATE_EXACT * 4.0f) / 2.0f + 0.5f;
- TMR4_ENBL &= ~(1<<3);
- TMR4_SCTRL3 = TMR_SCTRL_OEN | TMR_SCTRL_FORCE;
- TMR4_CSCTRL3 = TMR_CSCTRL_CL1(1) | TMR_CSCTRL_TCF1EN;
- TMR4_CNTR3 = 0;
- TMR4_LOAD3 = 0;
- TMR4_COMP13 = comp1;
- TMR4_CMPLD13 = comp1;
- TMR4_CTRL3 = TMR_CTRL_CM(1) | TMR_CTRL_PCS(8) | TMR_CTRL_LENGTH | TMR_CTRL_OUTMODE(3);
- TMR4_DMA3 = TMR_DMA_CMPLD1DE;
- TMR4_CNTR3 = 0;
- TMR4_ENBL |= (1<<3);
-
-
- const int trigger = 4;
- CCM_CCGR2 |= CCM_CCGR2_XBAR1(CCM_CCGR_ON);
- xbar_connect(XBARA1_IN_QTIMER4_TIMER3, XBARA1_OUT_ADC_ETC_TRIG00 + trigger);
-
-
- if (ADC_ETC_CTRL & (ADC_ETC_CTRL_SOFTRST | ADC_ETC_CTRL_TSC_BYPASS)) {
- ADC_ETC_CTRL = 0;
- ADC_ETC_CTRL = 0;
- }
- ADC_ETC_CTRL |= ADC_ETC_CTRL_TRIG_ENABLE(1 << trigger) | ADC_ETC_CTRL_DMA_MODE_SEL;
- ADC_ETC_DMA_CTRL |= ADC_ETC_DMA_CTRL_TRIQ_ENABLE(trigger);
-
-
- const int len = 1;
- IMXRT_ADC_ETC.TRIG[trigger].CTRL = ADC_ETC_TRIG_CTRL_TRIG_CHAIN(len - 1) |
- ADC_ETC_TRIG_CTRL_TRIG_PRIORITY(7);
- IMXRT_ADC_ETC.TRIG[trigger].CHAIN_1_0 = ADC_ETC_TRIG_CHAIN_HWTS0(1) |
- ADC_ETC_TRIG_CHAIN_CSEL0(adc2_pin_to_channel[pin]) | ADC_ETC_TRIG_CHAIN_B2B0;
-
-
- Serial.printf("ADC2_CFG = %08X\n", ADC2_CFG);
- ADC2_CFG |= ADC_CFG_ADTRG;
- ADC2_CFG = ADC_CFG_MODE(2) | ADC_CFG_ADSTS(3) | ADC_CFG_ADLSMP | ADC_CFG_ADTRG |
- ADC_CFG_ADICLK(1) | ADC_CFG_ADIV(0) ;
- ADC2_GC &= ~ADC_GC_AVGE;
- ADC2_HC0 = ADC_HC_ADCH(16);
-
-
- dma.begin();
- dma.TCD->SADDR = &(IMXRT_ADC_ETC.TRIG[4].RESULT_1_0);
- dma.TCD->SOFF = 0;
- dma.TCD->ATTR = DMA_TCD_ATTR_SSIZE(1) | DMA_TCD_ATTR_DSIZE(1);
- dma.TCD->NBYTES_MLNO = 2;
- dma.TCD->SLAST = 0;
- dma.TCD->DADDR = adc_buffer;
- dma.TCD->DOFF = 2;
- dma.TCD->CITER_ELINKNO = sizeof(adc_buffer) / 2;
- dma.TCD->DLASTSGA = -sizeof(adc_buffer);
- dma.TCD->BITER_ELINKNO = sizeof(adc_buffer) / 2;
- dma.TCD->CSR = 0;
- dma.triggerAtHardwareEvent(DMAMUX_SOURCE_ADC_ETC);
- dma.enable();
-
-
- }
-
- static int16_t fir(const int16_t *data, const int16_t *impulse, int len)
- {
- int64_t sum=0;
-
- while (len > 0) {
- sum += *data++ * *impulse++;
- len --;
- }
- sum = sum >> 15;
- if (sum > 32767) return 32767;
- if (sum < -32768) return -32768;
- return sum;
- }
-
- void AudioInputAnalog::update(void)
- {
- audio_block_t *output=NULL;
- output = allocate();
- if (output == NULL) return;
-
- uint16_t *p = (uint16_t *)dma.TCD->DADDR;
-
-
-
-
-
-
- const unsigned int capture_len = sizeof(capture_buffer) / 2;
- for (unsigned int i=0; i < capture_len; i++) {
-
- if (--p < adc_buffer) p = adc_buffer + (sizeof(adc_buffer) / 2 - 1);
-
-
-
- int dc_offset = 550;
-
- int n = (int)*p - dc_offset;
- if (n > 4095) n = 4095;
- if (n < -4095) n = -4095;
-
- capture_buffer[i] = n;
- }
-
-
-
- int16_t *dest = output->data + AUDIO_BLOCK_SAMPLES - 1;
- for (int i=0; i < AUDIO_BLOCK_SAMPLES; i++) {
- #if 1
-
- *dest-- = fir(capture_buffer + i * 4, filter, sizeof(filter)/2);
- #else
-
- *dest-- = capture_buffer[i * 4] + capture_buffer[i * 4 + 1]
- + capture_buffer[i * 4 + 2] + capture_buffer[i * 4 + 3];
- #endif
- }
- transmit(output);
- release(output);
- }
-
-
-
- #endif
|