Yossi_Students / Mbed 2 deprecated TAU_ReSpeaker_DSP_V01

Tau ReSpeaker Setup V01

Dependencies:   MbedJSONValue mbed

Fork of TAU_ReSpeaker_DSP_Test by Yossi_Students

filters.h@5:ec6f2323a263, 2018-07-12 (annotated)

Committer:
Arkadi
Date:
Thu Jul 12 10:11:20 2018 +0000
Revision:
5:ec6f2323a263
Parent:
4:59319802012b
Child:
6:e8b4ca41c691
minor changes

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Arkadi 1:574b54755983 1
Arkadi 1:574b54755983 2 // Filter variables:
Arkadi 1:574b54755983 3 /*
Arkadi 1:574b54755983 4 butter FILTER DESIGN, 23-Nov-2016 09:14:52, 2 sections, for sampling frequency 744000.0 Hz
Arkadi 1:574b54755983 5 Filter Order 4, Cut Off Frequency 20000 Hz
Arkadi 1:574b54755983 6 */
Arkadi 1:574b54755983 7
Arkadi 1:574b54755983 8 ///////////////////////////////
Arkadi 1:574b54755983 9 // filter variables: //
Arkadi 1:574b54755983 10 ///////////////////////////////
Arkadi 1:574b54755983 11
Arkadi 4:59319802012b 12 // filter coeficients, best performance to declare in function (constant values).
Arkadi 4:59319802012b 13 //float SOSMatHP[2][6] = { // 25khz cutoff at 920 kHz sample rate // 50kHz cutoff at 1.4 Mhz sample rate
Arkadi 4:59319802012b 14 // 1.000000, -2.000000, 1.000000, 1.000000, -1.706510, 0.731145,
Arkadi 4:59319802012b 15 // 1.000000, -2.000000, 1.000000, 1.000000, -1.852377, 0.879117
Arkadi 4:59319802012b 16 //};
Arkadi 4:59319802012b 17 //float GscaleHP = 0.801724;
Arkadi 4:59319802012b 18 //
Arkadi 4:59319802012b 19 //// num sections
Arkadi 4:59319802012b 20 //int NumSectionsHP = sizeof(SOSMatHP)/sizeof(float)/6;
Arkadi 1:574b54755983 21
Arkadi 1:574b54755983 22 // second-order sections filter variables - upto 8 sections
Arkadi 1:574b54755983 23 #define MAX_SECTION_NUMBER 8
Arkadi 1:574b54755983 24
Arkadi 1:574b54755983 25 // convertions
Arkadi 4:59319802012b 26 #define ADC2Float (2.0f/4095.0f) // 0.0004884f//
Arkadi 4:59319802012b 27 #define Float2ADC (4095.0f/2.0f) // 2047.5f //
Arkadi 4:59319802012b 28 //float ADC2Float=(2.0f/4095.0f); //ADCvalue*(2/0xFFF)-1.0f // 12 bits range
Arkadi 4:59319802012b 29 //float Float2ADC=(4095.0f/2.0f); //(ADCvalue+1.0f)*(0xFFF/2) // 12 bits range
Arkadi 1:574b54755983 30
Arkadi 1:574b54755983 31 ///////////////////////////////
Arkadi 1:574b54755983 32 // available filters: //
Arkadi 1:574b54755983 33 ///////////////////////////////
Arkadi 4:59319802012b 34 // off mode, output vdd/2
Arkadi 4:59319802012b 35 inline void offMode(void);
Arkadi 4:59319802012b 36 // passthrough function (output equals input)
Arkadi 4:59319802012b 37 inline void passthrough(void);
Arkadi 4:59319802012b 38 // highpass filter
Arkadi 4:59319802012b 39 inline void highpass(void);
Arkadi 4:59319802012b 40 // highpass filter + trigger mode
Arkadi 4:59319802012b 41 inline void highpassTrig(void);
Arkadi 1:574b54755983 42
Arkadi 1:574b54755983 43
Arkadi 1:574b54755983 44
Arkadi 1:574b54755983 45 ///////////////////////////////
Arkadi 1:574b54755983 46 // Filter Functions: //
Arkadi 1:574b54755983 47 ///////////////////////////////
Arkadi 4:59319802012b 48
Arkadi 4:59319802012b 49 // off mode, output vdd/2
Arkadi 5:ec6f2323a263 50 inline void offMode(void)
Arkadi 5:ec6f2323a263 51 {
Arkadi 4:59319802012b 52 uint16_t ADCValueOut;
Arkadi 4:59319802012b 53 // set to vdd/2
Arkadi 4:59319802012b 54 ADCValueOut=(uint16_t)((0.0f +1.0f) * Float2ADC);
Arkadi 4:59319802012b 55 // Output value using DAC
Arkadi 5:ec6f2323a263 56 *(__IO uint32_t *) Dac_Reg = ADCValueOut;
Arkadi 4:59319802012b 57 }// end off mode
Arkadi 4:59319802012b 58
Arkadi 4:59319802012b 59 // passthrough function (output equals input)
Arkadi 4:59319802012b 60 inline void passthrough(void)
Arkadi 1:574b54755983 61 {
Arkadi 1:574b54755983 62 uint32_t ADCValue;
Arkadi 1:574b54755983 63
Arkadi 1:574b54755983 64 // Read ADC input
Arkadi 1:574b54755983 65 ADCValue = hadc1.Instance->DR;
Arkadi 1:574b54755983 66 // Output value using DAC
Arkadi 1:574b54755983 67 *(__IO uint32_t *) Dac_Reg = ADCValue;
Arkadi 1:574b54755983 68
Arkadi 4:59319802012b 69 } // end passthrough
Arkadi 1:574b54755983 70
Arkadi 3:48258b86e182 71
Arkadi 1:574b54755983 72 // high pass filter
Arkadi 4:59319802012b 73 inline void highpass(void)
Arkadi 1:574b54755983 74 {
Arkadi 4:59319802012b 75 ///////////////////////////////
Arkadi 4:59319802012b 76 // filter variables: //
Arkadi 4:59319802012b 77 ///////////////////////////////
Arkadi 4:59319802012b 78 // filter coeficients best performance if defined in loop
Arkadi 4:59319802012b 79 float SOSMatHP[2][6] = { // 25khz cutoff at 920 kHz sample rate // closer to 30kHz when io toggle switched off.
Arkadi 4:59319802012b 80 1.000000, -2.000000, 1.000000, 1.000000, -1.706510, 0.731145,
Arkadi 4:59319802012b 81 1.000000, -2.000000, 1.000000, 1.000000, -1.852377, 0.879117
Arkadi 4:59319802012b 82 };
Arkadi 4:59319802012b 83 float GscaleHP = 0.801724;
Arkadi 4:59319802012b 84
Arkadi 4:59319802012b 85 // num sections
Arkadi 4:59319802012b 86 int NumSectionsHP = sizeof(SOSMatHP)/sizeof(float)/6;
Arkadi 4:59319802012b 87
Arkadi 1:574b54755983 88 float ADCFloat;
Arkadi 1:574b54755983 89 float ADCFloatFiltered;
Arkadi 1:574b54755983 90 uint16_t ADCValueOut;
Arkadi 1:574b54755983 91
Arkadi 1:574b54755983 92 // filter stages variables
Arkadi 1:574b54755983 93 static float CurrInput [MAX_SECTION_NUMBER+1];
Arkadi 1:574b54755983 94 static float LastInput [MAX_SECTION_NUMBER+1];
Arkadi 1:574b54755983 95 static float LLastInput [MAX_SECTION_NUMBER+1];
Arkadi 5:ec6f2323a263 96
Arkadi 4:59319802012b 97 ////////////////////
Arkadi 4:59319802012b 98 // Read ADC input //
Arkadi 4:59319802012b 99 ////////////////////
Arkadi 1:574b54755983 100 ADCFloat=((uint16_t)(hadc1.Instance->DR) * ADC2Float)-1.0f;
Arkadi 1:574b54755983 101
Arkadi 1:574b54755983 102 //////////////////////////////////////////////////////
Arkadi 1:574b54755983 103 // Apply Filter to input //
Arkadi 1:574b54755983 104 //////////////////////////////////////////////////////
Arkadi 1:574b54755983 105
Arkadi 1:574b54755983 106 LLastInput[0] = LastInput[0];
Arkadi 1:574b54755983 107 LastInput[0] = CurrInput[0];
Arkadi 1:574b54755983 108 CurrInput[0] = ADCFloat;
Arkadi 1:574b54755983 109 for (int ii=1; ii <= NumSectionsHP; ii++) {
Arkadi 1:574b54755983 110 LLastInput[ii] = LastInput[ii];
Arkadi 1:574b54755983 111 LastInput[ii] = CurrInput[ii];
Arkadi 1:574b54755983 112 CurrInput[ii] = SOSMatHP[ii-1][0]*CurrInput[ii-1] + SOSMatHP[ii-1][1]*LastInput[ii-1] +
Arkadi 1:574b54755983 113 SOSMatHP[ii-1][2]*LLastInput[ii-1] -
Arkadi 1:574b54755983 114 SOSMatHP[ii-1][4]*LastInput[ii] - SOSMatHP[ii-1][5]*LLastInput[ii];
Arkadi 1:574b54755983 115 ADCFloatFiltered = CurrInput[ii];
Arkadi 1:574b54755983 116 }
Arkadi 1:574b54755983 117
Arkadi 1:574b54755983 118 ADCFloatFiltered = ADCFloatFiltered * GscaleHP;
Arkadi 1:574b54755983 119
Arkadi 1:574b54755983 120 ////////////////////////////
Arkadi 1:574b54755983 121 // Apply Filter to Output //
Arkadi 1:574b54755983 122 ////////////////////////////
Arkadi 1:574b54755983 123
Arkadi 1:574b54755983 124 if (ADCFloatFiltered < -1.0f) {
Arkadi 1:574b54755983 125 ADCValueOut=0; // Min value
Arkadi 1:574b54755983 126 } else if (ADCFloatFiltered > 1.0f) {
Arkadi 1:574b54755983 127 ADCValueOut=0xFFF; // Max value
Arkadi 1:574b54755983 128 } else {
Arkadi 1:574b54755983 129 ADCValueOut=(uint16_t)((ADCFloatFiltered +1.0f) * Float2ADC);
Arkadi 1:574b54755983 130 }
Arkadi 1:574b54755983 131
Arkadi 1:574b54755983 132 // Output value using DAC
Arkadi 1:574b54755983 133 // HAL_DAC_SetValue(&hdac1, DAC_CHANNEL_1, DAC_ALIGN_12B_R, ADCValueOut);
Arkadi 1:574b54755983 134 *(__IO uint32_t *) Dac_Reg = ADCValueOut;
Arkadi 4:59319802012b 135
Arkadi 4:59319802012b 136 } // end high pass filter
Arkadi 4:59319802012b 137
Arkadi 4:59319802012b 138 // highpass filter + trigger mode
Arkadi 5:ec6f2323a263 139 inline void highpassTrig(void)
Arkadi 5:ec6f2323a263 140 {
Arkadi 4:59319802012b 141 ///////////////////////////////
Arkadi 4:59319802012b 142 // filter variables: //
Arkadi 4:59319802012b 143 ///////////////////////////////
Arkadi 4:59319802012b 144 // filter coeficients best performance if defined in loop
Arkadi 4:59319802012b 145 float SOSMatHP[2][6] = { // 25khz cutoff at 920 kHz sample rate // closer to 30kHz when io toggle switched off.
Arkadi 4:59319802012b 146 1.000000, -2.000000, 1.000000, 1.000000, -1.706510, 0.731145,
Arkadi 4:59319802012b 147 1.000000, -2.000000, 1.000000, 1.000000, -1.852377, 0.879117
Arkadi 4:59319802012b 148 };
Arkadi 4:59319802012b 149 float GscaleHP = 0.801724;
Arkadi 4:59319802012b 150
Arkadi 4:59319802012b 151 // num sections
Arkadi 4:59319802012b 152 int NumSectionsHP = sizeof(SOSMatHP)/sizeof(float)/6;
Arkadi 4:59319802012b 153
Arkadi 4:59319802012b 154 float ADCFloat;
Arkadi 4:59319802012b 155 float ADCFloatFiltered;
Arkadi 4:59319802012b 156 uint16_t ADCValueOut;
Arkadi 4:59319802012b 157
Arkadi 4:59319802012b 158 // filter stages variables
Arkadi 4:59319802012b 159 static float CurrInput [MAX_SECTION_NUMBER+1];
Arkadi 4:59319802012b 160 static float LastInput [MAX_SECTION_NUMBER+1];
Arkadi 4:59319802012b 161 static float LLastInput [MAX_SECTION_NUMBER+1];
Arkadi 5:ec6f2323a263 162
Arkadi 5:ec6f2323a263 163 // trigger variables
Arkadi 5:ec6f2323a263 164 static bool trigFlag=0; // flag to indicate trigger event
Arkadi 5:ec6f2323a263 165
Arkadi 4:59319802012b 166 ////////////////////
Arkadi 4:59319802012b 167 // Read ADC input //
Arkadi 4:59319802012b 168 ////////////////////
Arkadi 4:59319802012b 169 ADCFloat=((uint16_t)(hadc1.Instance->DR) * ADC2Float)-1.0f;
Arkadi 1:574b54755983 170
Arkadi 4:59319802012b 171 //////////////////////////////////////////////////////
Arkadi 4:59319802012b 172 // Apply Filter to input //
Arkadi 4:59319802012b 173 //////////////////////////////////////////////////////
Arkadi 4:59319802012b 174
Arkadi 4:59319802012b 175 LLastInput[0] = LastInput[0];
Arkadi 4:59319802012b 176 LastInput[0] = CurrInput[0];
Arkadi 4:59319802012b 177 CurrInput[0] = ADCFloat;
Arkadi 4:59319802012b 178 for (int ii=1; ii <= NumSectionsHP; ii++) {
Arkadi 4:59319802012b 179 LLastInput[ii] = LastInput[ii];
Arkadi 4:59319802012b 180 LastInput[ii] = CurrInput[ii];
Arkadi 4:59319802012b 181 CurrInput[ii] = SOSMatHP[ii-1][0]*CurrInput[ii-1] + SOSMatHP[ii-1][1]*LastInput[ii-1] +
Arkadi 4:59319802012b 182 SOSMatHP[ii-1][2]*LLastInput[ii-1] -
Arkadi 4:59319802012b 183 SOSMatHP[ii-1][4]*LastInput[ii] - SOSMatHP[ii-1][5]*LLastInput[ii];
Arkadi 4:59319802012b 184 ADCFloatFiltered = CurrInput[ii];
Arkadi 4:59319802012b 185 }
Arkadi 4:59319802012b 186
Arkadi 4:59319802012b 187 ADCFloatFiltered = ADCFloatFiltered * GscaleHP;
Arkadi 4:59319802012b 188
Arkadi 5:ec6f2323a263 189 ///////////////////////////////////////////////
Arkadi 5:ec6f2323a263 190 // Event detection //
Arkadi 5:ec6f2323a263 191 ///////////////////////////////////////////////
Arkadi 5:ec6f2323a263 192 if (trigFlag) { // event already detected
Arkadi 5:ec6f2323a263 193 trigDelay--;
Arkadi 5:ec6f2323a263 194 if (trigDelay == 0) { // pulse pass run out, perform delay and reset variables
Arkadi 5:ec6f2323a263 195 trigDelay = trigDelaySet; //reset counter for next iteration
Arkadi 5:ec6f2323a263 196 trigFlag=0;
Arkadi 5:ec6f2323a263 197 // reset filter
Arkadi 5:ec6f2323a263 198 for (int ii=1; ii <= NumSectionsHP; ii++) {
Arkadi 5:ec6f2323a263 199 LLastInput[ii] = 0;
Arkadi 5:ec6f2323a263 200 LastInput[ii] = 0;
Arkadi 5:ec6f2323a263 201 CurrInput[ii] = 0;
Arkadi 5:ec6f2323a263 202 ADCFloatFiltered = 0;
Arkadi 5:ec6f2323a263 203 }
Arkadi 5:ec6f2323a263 204 // update dac
Arkadi 5:ec6f2323a263 205 ADCValueOut=(uint16_t)((ADCFloatFiltered +1.0f) * Float2ADC);
Arkadi 5:ec6f2323a263 206 *(__IO uint32_t *) Dac_Reg = ADCValueOut;
Arkadi 5:ec6f2323a263 207 // delay for set time
Arkadi 5:ec6f2323a263 208 wait_us(trigPause);
Arkadi 5:ec6f2323a263 209 }
Arkadi 5:ec6f2323a263 210 } else if (ADCFloatFiltered >= trigTresh) {
Arkadi 5:ec6f2323a263 211 trigFlag=1;
Arkadi 5:ec6f2323a263 212 }
Arkadi 4:59319802012b 213 ////////////////////////////
Arkadi 4:59319802012b 214 // Apply Filter to Output //
Arkadi 4:59319802012b 215 ////////////////////////////
Arkadi 4:59319802012b 216
Arkadi 4:59319802012b 217 if (ADCFloatFiltered < -1.0f) {
Arkadi 4:59319802012b 218 ADCValueOut=0; // Min value
Arkadi 4:59319802012b 219 } else if (ADCFloatFiltered > 1.0f) {
Arkadi 4:59319802012b 220 ADCValueOut=0xFFF; // Max value
Arkadi 4:59319802012b 221 } else {
Arkadi 4:59319802012b 222 ADCValueOut=(uint16_t)((ADCFloatFiltered +1.0f) * Float2ADC);
Arkadi 4:59319802012b 223 }
Arkadi 4:59319802012b 224
Arkadi 4:59319802012b 225 // Output value using DAC
Arkadi 4:59319802012b 226 // HAL_DAC_SetValue(&hdac1, DAC_CHANNEL_1, DAC_ALIGN_12B_R, ADCValueOut);
Arkadi 4:59319802012b 227 *(__IO uint32_t *) Dac_Reg = ADCValueOut/2;
Arkadi 4:59319802012b 228 //wait_us(1);
Arkadi 4:59319802012b 229 } // end high pass filter + trigger mode