Yossi_Students
Tau ReSpeaker Setup V01
Dependencies: MbedJSONValue mbed
Fork of
TAU_ReSpeaker_DSP_Test
by 
Yossi_Students
Diff: filters.h
- Revision:
- 12:9d30df1529be
- Parent:
- 10:273127efdc6e
--- 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