Tau ReSpeaker Setup V01
Dependencies: MbedJSONValue mbed
Fork of TAU_ReSpeaker_DSP_Test by
Revision 12:9d30df1529be, committed 2018-08-28
- Comitter:
- Arkadi
- Date:
- Tue Aug 28 08:20:15 2018 +0000
- Parent:
- 11:07981b0d28d3
- Commit message:
- Implemented Filter / rearranged code
Changed in this revision
filters.h | Show annotated file Show diff for this revision Revisions of this file |
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 07981b0d28d3 -r 9d30df1529be filters.h --- a/filters.h Mon Aug 27 11:22:46 2018 +0000 +++ b/filters.h Tue Aug 28 08:20:15 2018 +0000 @@ -1,32 +1,54 @@ - -// Filter variables: -/* -butter FILTER DESIGN, 23-Nov-2016 09:14:52, 2 sections, for sampling frequency 744000.0 Hz -Filter Order 4, Cut Off Frequency 20000 Hz -*/ +////////////////////// +// Operation Modes // +////////////////////// -/////////////////////////////// -// filter variables: // -/////////////////////////////// +///////////////// +// Variables: // +///////////////// -// filter coeficients, best performance to declare in function (constant values). -//float SOSMatHP[2][6] = { // 25khz cutoff at 920 kHz sample rate // 50kHz cutoff at 1.4 Mhz sample rate -// 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 mode - Regular / efficient +//#define EFFICIENT_FILTER // second-order sections filter variables - upto 8 sections -#define MAX_SECTION_NUMBER 8 +#define MAX_SECTION_NUMBER 5 // convertions #define ADC2Float (2.0f/4095.0f) // 0.0004884f// #define Float2ADC (4095.0f/2.0f) // 2047.5f // -//float ADC2Float=(2.0f/4095.0f); //ADCvalue*(2/0xFFF)-1.0f // 12 bits range -//float Float2ADC=(4095.0f/2.0f); //(ADCvalue+1.0f)*(0xFFF/2) // 12 bits range + +// 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: // @@ -45,11 +67,71 @@ 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; +} + /////////////////////////////// -// Filter Functions: // +// Operation Functions: // /////////////////////////////// - // off mode, output vdd/2 inline void offMode(void) { @@ -80,55 +162,16 @@ // high pass filter inline void highpass(void) { - /////////////////////////////// - // 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; + // Dac output variable 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]; - - //////////////////// - // Read ADC input // - //////////////////// - ADCFloat=((uint16_t)(hadc1.Instance->DR) * ADC2Float)-1.0f; - - ////////////////////////////////////////////////////// - // Apply Filter to input // - ////////////////////////////////////////////////////// + // Read ADC input + float ADCFloat=((uint16_t)(hadc1.Instance->DR) * ADC2Float)-1.0f; - 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]; - } + // Apply Filter + float ADCFloatFiltered = HPF_Function( ADCFloat ); - ADCFloatFiltered = ADCFloatFiltered * GscaleHP * signalGain; - - //////////////////////////////// - // Apply Saturation to Output // - //////////////////////////////// - + // Apply Saturation to Output if (ADCFloatFiltered < -1.0f) { ADCValueOut=0; // Min value } else if (ADCFloatFiltered > 1.0f) { @@ -146,53 +189,17 @@ // highpass filter + trigger mode inline void highpassTrig(void) { - /////////////////////////////// - // 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 // - ////////////////////////////////////////////////////// + // Dac output variable + uint16_t ADCValueOut; - 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]; - } + // Read ADC input + float ADCFloat=((uint16_t)(hadc1.Instance->DR) * ADC2Float)-1.0f; - ADCFloatFiltered = ADCFloatFiltered * GscaleHP * signalGain; + // Apply Filter + float ADCFloatFiltered = HPF_Function( ADCFloat ); /////////////////////////////////////////////// // Event detection // @@ -203,12 +210,7 @@ trigDelay = trigDelaySet; //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; - } + ADCFloatFiltered = 0; // update dac ADCValueOut=(uint16_t)((ADCFloatFiltered +1.0f) * Float2ADC); *(__IO uint32_t *) Dac_Reg = ADCValueOut; @@ -216,7 +218,7 @@ // delay for set time wait_ms(trigPause); } - } else if (ADCFloatFiltered >= trigTresh) { + } else if (ADCFloatFiltered >= trigTresh) { trigFlag=1; } //////////////////////////////// @@ -241,12 +243,86 @@ // 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 GAIN_VECTOR_SIZE 5 - float GainVector[] = {0.1 , 0.2 , 0.4 , 0.8 , 1.6}; - static uint16_t GainVectorIndex = 0; +#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: // /////////////////////////////// @@ -295,117 +371,6 @@ ADCFloatFiltered = ADCFloatFiltered * GscaleHP; - /////////////////////////////////////////////// - // 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 - for (int ii=1; ii <= NumSectionsHP; ii++) { - LLastInput[ii] = 0; - LastInput[ii] = 0; - CurrInput[ii] = 0; - 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 % GAIN_VECTOR_SIZE; - } - } 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 - -// 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 // //////////////////////////////// @@ -427,18 +392,18 @@ 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; @@ -458,10 +423,10 @@ // 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 \ No newline at end of file
diff -r 07981b0d28d3 -r 9d30df1529be main.cpp --- a/main.cpp Mon Aug 27 11:22:46 2018 +0000 +++ b/main.cpp Tue Aug 28 08:20:15 2018 +0000 @@ -53,13 +53,16 @@ /////////////// // #defines // /////////////// + +//#define TOGGLE_IO_MODE // loop frequency indicator + // UART debug modes: //#define DEBUG_MOD1 // json packet recognise -#define DEBUG_MOD2 // json parse -#define DEBUG_MOD3 // dsp handler -#define DEBUG_MOD10 // responsivity msges to gui -#define DEBUG_MOD11 // loop frequency indicator -// Sine generator +//#define DEBUG_MOD2 // json parse +//#define DEBUG_MOD3 // dsp handler +//#define DEBUG_MOD10 // responsivity msges to gui + +// PI variable #define PI_DOUBLE (3.141592653589793238462) #define PI_FLOAT (3.14159f) @@ -70,6 +73,7 @@ // Frequency counter #define TICKS2TOGGLE 1000000 + ///////////// // Objects // ///////////// @@ -105,6 +109,8 @@ uint32_t toggleCounter = 0; // toogle pin state bool toggleCounterState = 0; +// toogle pin state +bool toggelState=0; // json buffer char json[MSG_BUFFER_SIZE]; @@ -126,12 +132,6 @@ // Dac Register for direct method of setting DAC value`s __IO uint32_t Dac_Reg = 0; -// analog input -//uint16_t adc_in=0; - -// toogle pin state -bool toggelState=0; - // create function pointer typedef void(*functionPtr)(void); functionPtr FilterFunction; @@ -144,37 +144,6 @@ // 3 - hpf_trig - High Pass filter + Trigger mode // 4 - gains_trig - highpass filter + trigger mode + Gains vector - -// 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 - -// 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[6] = {0}; -float filterLastInput[6] = {0}; -float filterLLastInput[6] = {0}; - -// Buffer mode variables -uint32_t bufferSizeSet = 5000; -uint32_t bufferSize = bufferSizeSet; -uint32_t preBufferSizeSet = 1000; -uint32_t bufferCountDown = bufferSizeSet - preBufferSizeSet; -float bufferADC[5000] = {0}; - - /////////////// // Functions // /////////////// @@ -255,7 +224,7 @@ //highpass_filter(); // more elegant but much slower option: //FilterFunction(); -#ifdef DEBUG_MOD11 +#ifdef TOGGLE_IO_MODE // toggle pin, Loop frequency indicator toggelState=!toggelState; mytoggle.write(toggelState);