Tau ReSpeaker Setup V01
Dependencies: MbedJSONValue mbed
Fork of TAU_ReSpeaker_DSP_Test by
filters.h
- Committer:
- Arkadi
- Date:
- 2018-08-28
- Revision:
- 12:9d30df1529be
- Parent:
- 10:273127efdc6e
File content as of revision 12:9d30df1529be:
////////////////////// // Operation Modes // ////////////////////// ///////////////// // Variables: // ///////////////// // Filter mode - Regular / efficient //#define EFFICIENT_FILTER // second-order sections filter variables - upto 8 sections #define MAX_SECTION_NUMBER 5 // convertions #define ADC2Float (2.0f/4095.0f) // 0.0004884f// #define Float2ADC (4095.0f/2.0f) // 2047.5f // // Filter variables //SOS Matrix limited to order 10 int FilterSections=1; float SOSMat[5][6]= { // Predefined SOS Matrix (second order 5Khz filter 1, -2, 1, 1, -1.961, 0.9624, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; float Gscale = 0.9810f; float filterCurrInput[MAX_SECTION_NUMBER+1] = {0}; float filterLastInput[MAX_SECTION_NUMBER+1] = {0}; float filterLLastInput[MAX_SECTION_NUMBER+1] = {0}; // Trigger mode variables float signalGain = 1.0; // Signal Gain float trigTresh = 0.5; // threshold for trigger mode uint32_t trigDelaySet = 5000; // counter for pulse pass uint32_t trigDelay = trigDelaySet; // counter for pulse pass uint32_t trigPause = 10; // pause after trigger in microseconds // Buffer mode variables - Mid work uint32_t bufferSizeSet = 5000; uint32_t bufferSize = bufferSizeSet; uint32_t preBufferSizeSet = 1000; uint32_t bufferCountDown = bufferSizeSet - preBufferSizeSet; float bufferADC[5000] = {0}; // gains vector #define GAIN_VECTOR_SIZE 5 int gainsVectorSize = 5; float GainVector[] = {0.1 , 0.2 , 0.4 , 0.8 , 1.6}; /////////////////////////////// // available filters: // /////////////////////////////// // off mode, output vdd/2 inline void offMode(void); // passthrough function (output equals input) inline void passthrough(void); // highpass filter inline void highpass(void); // highpass filter + trigger mode inline void highpassTrig(void); // highpass filter + trigger mode + Gains vector inline void GainsTrig(void); // highpass filter + trigger mode + Delays vector inline void DelaysTrig(void); // highpass filter + trigger mode + FIR Filter (Convolution) inline void FIRTrig(void); // filter_implementation inline float HPF_Function(float ADCFloat); ////////////////////////////// // Filter Function: // ////////////////////////////// // filter_implementation inline float HPF_Function(float ADCFloat) { // filter local variable float ADCFloatFiltered; // filter mode: #ifdef EFFICIENT_FILTER // filter coeficients best performance if defined in loop float SOSMatHP[2][6] = { // 25khz cutoff at 920 kHz sample rate // closer to 30kHz when io toggle switched off. 1.000000, -2.000000, 1.000000, 1.000000, -1.706510, 0.731145, 1.000000, -2.000000, 1.000000, 1.000000, -1.852377, 0.879117 }; float GscaleHP = 0.801724; // num sections int NumSectionsHP = sizeof(SOSMatHP)/sizeof(float)/6; // filter stages variables static float CurrInput [MAX_SECTION_NUMBER+1]; static float LastInput [MAX_SECTION_NUMBER+1]; static float LLastInput [MAX_SECTION_NUMBER+1]; ////////////////// // Apply Filter // ////////////////// LLastInput[0] = LastInput[0]; LastInput[0] = CurrInput[0]; CurrInput[0] = ADCFloat; for (int ii=1; ii <= NumSectionsHP; ii++) { LLastInput[ii] = LastInput[ii]; LastInput[ii] = CurrInput[ii]; CurrInput[ii] = SOSMatHP[ii-1][0]*CurrInput[ii-1] + SOSMatHP[ii-1][1]*LastInput[ii-1] + SOSMatHP[ii-1][2]*LLastInput[ii-1] - SOSMatHP[ii-1][4]*LastInput[ii] - SOSMatHP[ii-1][5]*LLastInput[ii]; ADCFloatFiltered = CurrInput[ii]; } ADCFloatFiltered = ADCFloatFiltered * GscaleHP * signalGain; #else filterLLastInput[0] = filterLastInput[0]; filterLastInput[0] = filterCurrInput[0]; filterCurrInput[0] = ADCFloat; for (int ii=1; ii <= FilterSections; ii++) { filterLLastInput[ii] = filterLastInput[ii]; filterLastInput[ii] = filterCurrInput[ii]; filterCurrInput[ii] = SOSMat[ii-1][0]*filterCurrInput[ii-1] + SOSMat[ii-1][1]*filterLastInput[ii-1] + SOSMat[ii-1][2]*filterLLastInput[ii-1] - SOSMat[ii-1][4]*filterLastInput[ii] - SOSMat[ii-1][5]*filterLLastInput[ii]; } ADCFloatFiltered = filterCurrInput[FilterSections]; ADCFloatFiltered = ADCFloatFiltered * Gscale * signalGain; #endif return ADCFloatFiltered; } /////////////////////////////// // Operation Functions: // /////////////////////////////// // off mode, output vdd/2 inline void offMode(void) { // generate some delay (limit loop speeds, creates crashes to the Switch mcu // for (int ii=0; ii<10; ii++){ // __ASM volatile ("nop"); // one tick operation, Use to adjust frequency by slowing down the proccess // } uint16_t ADCValueOut; // set to vdd/2 ADCValueOut=(uint16_t)((0.0f +1.0f) * Float2ADC); // Output value using DAC *(__IO uint32_t *) Dac_Reg = ADCValueOut; }// end off mode // passthrough function (output equals input) inline void passthrough(void) { uint32_t ADCValue; // Read ADC input ADCValue = hadc1.Instance->DR; // Output value using DAC *(__IO uint32_t *) Dac_Reg = ADCValue; } // end passthrough // high pass filter inline void highpass(void) { // Dac output variable uint16_t ADCValueOut; // Read ADC input float ADCFloat=((uint16_t)(hadc1.Instance->DR) * ADC2Float)-1.0f; // Apply Filter float ADCFloatFiltered = HPF_Function( ADCFloat ); // Apply Saturation to Output if (ADCFloatFiltered < -1.0f) { ADCValueOut=0; // Min value } else if (ADCFloatFiltered > 1.0f) { ADCValueOut=0xFFF; // Max value } else { ADCValueOut=(uint16_t)((ADCFloatFiltered +1.0f) * Float2ADC); } // Output value using DAC // HAL_DAC_SetValue(&hdac1, DAC_CHANNEL_1, DAC_ALIGN_12B_R, ADCValueOut); *(__IO uint32_t *) Dac_Reg = ADCValueOut; } // end high pass filter // highpass filter + trigger mode inline void highpassTrig(void) { // trigger variables static bool trigFlag=0; // flag to indicate trigger event // Dac output variable uint16_t ADCValueOut; // Read ADC input float ADCFloat=((uint16_t)(hadc1.Instance->DR) * ADC2Float)-1.0f; // Apply Filter float ADCFloatFiltered = HPF_Function( ADCFloat ); /////////////////////////////////////////////// // Event detection // /////////////////////////////////////////////// if (trigFlag) { // event already detected trigDelay--; if (trigDelay == 0) { // pulse pass run out, perform delay and reset variables trigDelay = trigDelaySet; //reset counter for next iteration trigFlag=0; // reset filter ADCFloatFiltered = 0; // update dac ADCValueOut=(uint16_t)((ADCFloatFiltered +1.0f) * Float2ADC); *(__IO uint32_t *) Dac_Reg = ADCValueOut; //*(__IO uint32_t *) Dac_Reg = 0; // test triggered mode // delay for set time wait_ms(trigPause); } } else if (ADCFloatFiltered >= trigTresh) { trigFlag=1; } //////////////////////////////// // Apply Saturation to Output // //////////////////////////////// if (ADCFloatFiltered < -1.0f) { ADCValueOut=0; // Min value } else if (ADCFloatFiltered > 1.0f) { ADCValueOut=0xFFF; // Max value } else { ADCValueOut=(uint16_t)((ADCFloatFiltered +1.0f) * Float2ADC); } // Output value using DAC // HAL_DAC_SetValue(&hdac1, DAC_CHANNEL_1, DAC_ALIGN_12B_R, ADCValueOut); *(__IO uint32_t *) Dac_Reg = ADCValueOut; //wait_us(1); } // end high pass filter + trigger mode // highpass filter + trigger mode + Gains vector inline void GainsTrig(void) { // Gains variables: static int GainVectorIndex = 0; // trigger variables static bool trigFlag=0; // flag to indicate trigger event // Dac output variable uint16_t ADCValueOut; // Read ADC input float ADCFloat=((uint16_t)(hadc1.Instance->DR) * ADC2Float)-1.0f; // Apply Filter float ADCFloatFiltered = HPF_Function( ADCFloat ); /////////////////////////////////////////////// // Event detection // /////////////////////////////////////////////// if (trigFlag) { // event already detected trigDelay--; if (trigDelay == 0) { // pulse pass run out, perform delay and reset variables trigDelay = trigDelaySet; //reset counter for next iteration trigFlag=0; // reset filter ADCFloatFiltered = 0; // update dac ADCValueOut=(uint16_t)((ADCFloatFiltered +1.0f) * Float2ADC); *(__IO uint32_t *) Dac_Reg = ADCValueOut; //*(__IO uint32_t *) Dac_Reg = 0; // test triggered mode // delay for set time wait_ms(trigPause); // change gain settings GainVectorIndex++; GainVectorIndex = GainVectorIndex % gainsVectorSize; } } else if (ADCFloatFiltered >= trigTresh) { trigFlag=1; } ////////////////////////// // Apply Gain to Output // ////////////////////////// ADCFloatFiltered = ADCFloatFiltered * GainVector[GainVectorIndex]; if (ADCFloatFiltered < -1.0f) { ADCValueOut=0; // Min value } else if (ADCFloatFiltered > 1.0f) { ADCValueOut=0xFFF; // Max value } else { ADCValueOut=(uint16_t)((ADCFloatFiltered +1.0f) * Float2ADC); } // Output value using DAC // HAL_DAC_SetValue(&hdac1, DAC_CHANNEL_1, DAC_ALIGN_12B_R, ADCValueOut); *(__IO uint32_t *) Dac_Reg = ADCValueOut; //wait_us(1); } // end GainsTrig ////////////////////////////////////////////// /////////////////// Implement ////////////////////////////////////////////// // highpass filter + trigger mode + Delays vector inline void DelaysTrig(void) // - mid work feature. (not working yet) { /////////////////////////////// // Gains variables: // /////////////////////////////// #define DELAY_VECTOR_SIZE 5 uint32_t DelayVector[] = {100 , 200 , 300 , 400 , 500}; // millis static uint16_t DelayVectorIndex = 0; static uint32_t BufferIndex = 0; //uint32_t bufferSizeSet = 5000; //uint32_t preBufferSizeSet = 1000; //uint32_t bufferCountDown = bufferSizeSet - preBufferSizeSet; //float bufferADC[bufferSizeSet] = {0}; /////////////////////////////// // filter variables: // /////////////////////////////// // filter coeficients best performance if defined in loop float SOSMatHP[2][6] = { // 25khz cutoff at 920 kHz sample rate // closer to 30kHz when io toggle switched off. 1.000000, -2.000000, 1.000000, 1.000000, -1.706510, 0.731145, 1.000000, -2.000000, 1.000000, 1.000000, -1.852377, 0.879117 }; float GscaleHP = 0.801724; // num sections int NumSectionsHP = sizeof(SOSMatHP)/sizeof(float)/6; float ADCFloat; float ADCFloatFiltered; uint16_t ADCValueOut; // filter stages variables static float CurrInput [MAX_SECTION_NUMBER+1]; static float LastInput [MAX_SECTION_NUMBER+1]; static float LLastInput [MAX_SECTION_NUMBER+1]; // trigger variables static bool trigFlag=0; // flag to indicate trigger event //////////////////// // Read ADC input // //////////////////// ADCFloat=((uint16_t)(hadc1.Instance->DR) * ADC2Float)-1.0f; ////////////////////////////////////////////////////// // Apply Filter to input // ////////////////////////////////////////////////////// LLastInput[0] = LastInput[0]; LastInput[0] = CurrInput[0]; CurrInput[0] = ADCFloat; for (int ii=1; ii <= NumSectionsHP; ii++) { LLastInput[ii] = LastInput[ii]; LastInput[ii] = CurrInput[ii]; CurrInput[ii] = SOSMatHP[ii-1][0]*CurrInput[ii-1] + SOSMatHP[ii-1][1]*LastInput[ii-1] + SOSMatHP[ii-1][2]*LLastInput[ii-1] - SOSMatHP[ii-1][4]*LastInput[ii] - SOSMatHP[ii-1][5]*LLastInput[ii]; ADCFloatFiltered = CurrInput[ii]; } ADCFloatFiltered = ADCFloatFiltered * GscaleHP; //////////////////////////////// // Fill in buffer // //////////////////////////////// bufferADC[BufferIndex] = ADCFloatFiltered; BufferIndex++; BufferIndex = BufferIndex % bufferSize; /////////////////////////////////////////////// // Event detection // /////////////////////////////////////////////// if (trigFlag) { // event already detected bufferCountDown--; // count down if (bufferCountDown == 0) { // pulse pass run out, perform delay and reset variables bufferCountDown = bufferSizeSet - preBufferSizeSet; //reset counter for next iteration trigFlag=0; // reset filter for (int ii=1; ii <= NumSectionsHP; ii++) { LLastInput[ii] = 0; LastInput[ii] = 0; CurrInput[ii] = 0; ADCFloatFiltered = 0; } // generate delay // delay for set time wait_ms(DelayVector[DelayVectorIndex]); // change delay settings DelayVectorIndex++; DelayVectorIndex = DelayVectorIndex % DELAY_VECTOR_SIZE; // play out buffer for (int ii=0 ; ii<bufferSize ; ii++) { ADCFloatFiltered = bufferADC[BufferIndex]; BufferIndex++; BufferIndex = BufferIndex % bufferSize; //////////////////////////////// // Apply Saturation to Output // //////////////////////////////// if (ADCFloatFiltered < -1.0f) { ADCValueOut=0; // Min value } else if (ADCFloatFiltered > 1.0f) { ADCValueOut=0xFFF; // Max value } else { ADCValueOut=(uint16_t)((ADCFloatFiltered +1.0f) * Float2ADC); } // Output value using DAC // HAL_DAC_SetValue(&hdac1, DAC_CHANNEL_1, DAC_ALIGN_12B_R, ADCValueOut); *(__IO uint32_t *) Dac_Reg = ADCValueOut; //wait_us(1); } } } else if (ADCFloatFiltered >= trigTresh) { trigFlag=1; } } // end GainsTrig