#define FILTER_TAP_NUM 49
static const int16_t coeffs[FILTER_TAP_NUM] = {
70, 37, -19, -140, -316, -505, -638, -646,
-485, -166, 232, 578, 727, 578, 126, -508,
-1098, -1369, -1084, -139, 1386, 3222, 4977,
6239, 6698, 6239, 4977, 3222, 1386, -139,
-1084, -1369, -1098, -508, 126, 578, 727,
578, 232, -166, -485, -646, -638, -505, -316,
-140, -19, 37, 70
};
#define ADC_BUFFER_LEN AUDIO_BLOCK_SAMPLES * 5
#define FIR_DELAY_LEN AUDIO_BLOCK_SAMPLES * 4 + FILTER_TAP_NUM
#define POLE(val) ((int16_t)(INT16_MAX * val))
DMAMEM static __attribute__((aligned(32))) int16_t adc_buffer[ADC_BUFFER_LEN];
DMAMEM static __attribute__((aligned(32))) int16_t buffer[AUDIO_BLOCK_SAMPLES * 4];
DMAMEM static int16_t fir_delay[FIR_DELAY_LEN];
static ADC *adc;
static DMAChannel dma(false);
static DMAChannel m2m(false);
static arm_fir_decimate_instance_q15 filter;
static uint32_t cycle = 0;
static void dcRemoval(int16_t *data, int blocksize)
{
static int xm = 0, ym = 0;
while(blocksize--){
//y(n) = x(n) - x(n-1) + POLE * y(n-1)
ym = *data - xm + ((ym * POLE(0.995)) >> 15);
xm = *data;
*data++ = ym;
}
}
static void cic_decimate(int16_t *indata, int16_t *oudata, int blocksize)
{
static int intg1 = 0, intg2 = 0, intg3 = 0, intg4 = 0;
static int comb1 = 0, comb2 = 0, comb3 = 0, comb4 = 0;
static int fir_del1 = 0, fir_del2 = 0;
// Cascaded Integrator Comb Filter
// N=4 R=4 M=1
while(blocksize--){
int r = 4;
while(r--){
// integrator section
intg1 += *indata++;
intg2 += intg1;
intg3 += intg2;
intg4 += intg3;
}
// comb section
int c1 = intg4 - comb1;
int c2 = c1 - comb2;
int c3 = c2 - comb3;
int c4 = (c3 - comb4)>>8; // result needs to be shifted by log2(M*N^R)
// comb delay
comb1 = intg4;
comb2 = c1;
comb3 = c2;
comb4 = c3;
// FIR compensation filter
*oudata++ = c4 + fir_del2 - fir_del1 * 3;
fir_del2 = fir_del1;
fir_del1 = c4;
}
}
void AudioInputAnalog_T4::init(uint8_t pin, ADC_CHN chn)
{
ARM_DEMCR |= ARM_DEMCR_TRCENA;
ARM_DWT_CTRL |= ARM_DWT_CTRL_CYCCNTENA;
arm_fir_decimate_init_q15(&filter, FILTER_TAP_NUM, 4, (int16_t*)&coeffs, fir_delay, AUDIO_BLOCK_SAMPLES * 4);
dma.begin();
dma.source((volatile uint16_t &)((chn == 1) ? ADC2_R0 : ADC1_R0));
dma.destinationBuffer(adc_buffer, sizeof(adc_buffer));
dma.triggerAtHardwareEvent((chn == 1) ? DMAMUX_SOURCE_ADC2 : DMAMUX_SOURCE_ADC1);
dma.enable();
m2m.begin();
m2m.sourceCircular(adc_buffer, sizeof(adc_buffer));
m2m.destinationBuffer(buffer, sizeof(buffer));
m2m.disableOnCompletion();
m2m.TCD->SOFF = -2;
adc = new ADC();
adc->adc[chn]->setResolution(12);
adc->adc[chn]->setAveraging(0);
adc->adc[chn]->startSingleRead(pin);
adc->adc[chn]->startQuadTimer(AUDIO_SAMPLE_RATE * 4);
adc->adc[chn]->enableDMA();
}
uint32_t AudioInputAnalog_T4::getCycleCount(){
return cycle;
}
void AudioInputAnalog_T4::update()
{
int16_t *pData = (int16_t*)dma.destinationAddress();
audio_block_t *output;
// TODO : to figure out why the hack m2m.complete doesnt return
// it works fine in a test project but it doesnt in update()
arm_dcache_flush_delete(adc_buffer, sizeof(adc_buffer));
m2m.TCD->SADDR = pData;
m2m.enable();
m2m.triggerContinuously();
while (!m2m.complete());
arm_dcache_delete(buffer, sizeof(buffer));
if ((output = allocate()) == NULL) return;
uint32_t start = ARM_DWT_CYCCNT;
#if 1
// CIC Filter is roughly 3 times faster then the 49 tap Fir
cic_decimate(buffer, output->data, AUDIO_BLOCK_SAMPLES);
#else
arm_fir_decimate_fast_q15(&filter, buffer, output->data, AUDIO_BLOCK_SAMPLES * 4);
#endif
cycle = ARM_DWT_CYCCNT - start;
dcRemoval(output->data, AUDIO_BLOCK_SAMPLES);
transmit(output);
release(output);
}
