Arkadi Rafalovich / Mbed 2 deprecated TAU_ReSpeaker_DSP_V02

ReSpeaker DSP V02

Dependencies:   mbed MbedJSONValue

filters.h@4:59319802012b, 2018-02-19 (annotated)

Committer:
Arkadi
Date:
Mon Feb 19 15:14:44 2018 +0000
Revision:
4:59319802012b
Parent:
3:48258b86e182
Child:
5:ec6f2323a263
Updated operation modes, code optimisations,

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 4:59319802012b 50 inline void offMode(void){
Arkadi 4:59319802012b 51 uint16_t ADCValueOut;
Arkadi 4:59319802012b 52 // set to vdd/2
Arkadi 4:59319802012b 53 ADCValueOut=(uint16_t)((0.0f +1.0f) * Float2ADC);
Arkadi 4:59319802012b 54 // Output value using DAC
Arkadi 4:59319802012b 55 *(__IO uint32_t *) Dac_Reg = ADCValueOut;
Arkadi 4:59319802012b 56 }// end off mode
Arkadi 4:59319802012b 57
Arkadi 4:59319802012b 58 // passthrough function (output equals input)
Arkadi 4:59319802012b 59 inline void passthrough(void)
Arkadi 1:574b54755983 60 {
Arkadi 1:574b54755983 61 uint32_t ADCValue;
Arkadi 1:574b54755983 62
Arkadi 1:574b54755983 63 // Read ADC input
Arkadi 1:574b54755983 64 ADCValue = hadc1.Instance->DR;
Arkadi 1:574b54755983 65 // Output value using DAC
Arkadi 1:574b54755983 66 *(__IO uint32_t *) Dac_Reg = ADCValue;
Arkadi 1:574b54755983 67
Arkadi 4:59319802012b 68 } // end passthrough
Arkadi 1:574b54755983 69
Arkadi 3:48258b86e182 70
Arkadi 1:574b54755983 71 // high pass filter
Arkadi 4:59319802012b 72 inline void highpass(void)
Arkadi 1:574b54755983 73 {
Arkadi 4:59319802012b 74 ///////////////////////////////
Arkadi 4:59319802012b 75 // filter variables: //
Arkadi 4:59319802012b 76 ///////////////////////////////
Arkadi 4:59319802012b 77 // filter coeficients best performance if defined in loop
Arkadi 4:59319802012b 78 float SOSMatHP[2][6] = { // 25khz cutoff at 920 kHz sample rate // closer to 30kHz when io toggle switched off.
Arkadi 4:59319802012b 79 1.000000, -2.000000, 1.000000, 1.000000, -1.706510, 0.731145,
Arkadi 4:59319802012b 80 1.000000, -2.000000, 1.000000, 1.000000, -1.852377, 0.879117
Arkadi 4:59319802012b 81 };
Arkadi 4:59319802012b 82 float GscaleHP = 0.801724;
Arkadi 4:59319802012b 83
Arkadi 4:59319802012b 84 // num sections
Arkadi 4:59319802012b 85 int NumSectionsHP = sizeof(SOSMatHP)/sizeof(float)/6;
Arkadi 4:59319802012b 86
Arkadi 1:574b54755983 87 float ADCFloat;
Arkadi 1:574b54755983 88 float ADCFloatFiltered;
Arkadi 1:574b54755983 89 uint16_t ADCValueOut;
Arkadi 1:574b54755983 90
Arkadi 1:574b54755983 91 // filter stages variables
Arkadi 1:574b54755983 92 static float CurrInput [MAX_SECTION_NUMBER+1];
Arkadi 1:574b54755983 93 static float LastInput [MAX_SECTION_NUMBER+1];
Arkadi 1:574b54755983 94 static float LLastInput [MAX_SECTION_NUMBER+1];
Arkadi 4:59319802012b 95
Arkadi 4:59319802012b 96 ////////////////////
Arkadi 4:59319802012b 97 // Read ADC input //
Arkadi 4:59319802012b 98 ////////////////////
Arkadi 1:574b54755983 99 ADCFloat=((uint16_t)(hadc1.Instance->DR) * ADC2Float)-1.0f;
Arkadi 1:574b54755983 100
Arkadi 1:574b54755983 101 //////////////////////////////////////////////////////
Arkadi 1:574b54755983 102 // Apply Filter to input //
Arkadi 1:574b54755983 103 //////////////////////////////////////////////////////
Arkadi 1:574b54755983 104
Arkadi 1:574b54755983 105 LLastInput[0] = LastInput[0];
Arkadi 1:574b54755983 106 LastInput[0] = CurrInput[0];
Arkadi 1:574b54755983 107 CurrInput[0] = ADCFloat;
Arkadi 1:574b54755983 108 for (int ii=1; ii <= NumSectionsHP; ii++) {
Arkadi 1:574b54755983 109 LLastInput[ii] = LastInput[ii];
Arkadi 1:574b54755983 110 LastInput[ii] = CurrInput[ii];
Arkadi 1:574b54755983 111 CurrInput[ii] = SOSMatHP[ii-1][0]*CurrInput[ii-1] + SOSMatHP[ii-1][1]*LastInput[ii-1] +
Arkadi 1:574b54755983 112 SOSMatHP[ii-1][2]*LLastInput[ii-1] -
Arkadi 1:574b54755983 113 SOSMatHP[ii-1][4]*LastInput[ii] - SOSMatHP[ii-1][5]*LLastInput[ii];
Arkadi 1:574b54755983 114 ADCFloatFiltered = CurrInput[ii];
Arkadi 1:574b54755983 115 }
Arkadi 1:574b54755983 116
Arkadi 1:574b54755983 117 ADCFloatFiltered = ADCFloatFiltered * GscaleHP;
Arkadi 1:574b54755983 118
Arkadi 1:574b54755983 119 ////////////////////////////
Arkadi 1:574b54755983 120 // Apply Filter to Output //
Arkadi 1:574b54755983 121 ////////////////////////////
Arkadi 1:574b54755983 122
Arkadi 1:574b54755983 123 if (ADCFloatFiltered < -1.0f) {
Arkadi 1:574b54755983 124 ADCValueOut=0; // Min value
Arkadi 1:574b54755983 125 } else if (ADCFloatFiltered > 1.0f) {
Arkadi 1:574b54755983 126 ADCValueOut=0xFFF; // Max value
Arkadi 1:574b54755983 127 } else {
Arkadi 1:574b54755983 128 ADCValueOut=(uint16_t)((ADCFloatFiltered +1.0f) * Float2ADC);
Arkadi 1:574b54755983 129 }
Arkadi 1:574b54755983 130
Arkadi 1:574b54755983 131 // Output value using DAC
Arkadi 1:574b54755983 132 // HAL_DAC_SetValue(&hdac1, DAC_CHANNEL_1, DAC_ALIGN_12B_R, ADCValueOut);
Arkadi 1:574b54755983 133 *(__IO uint32_t *) Dac_Reg = ADCValueOut;
Arkadi 4:59319802012b 134
Arkadi 4:59319802012b 135 } // end high pass filter
Arkadi 4:59319802012b 136
Arkadi 4:59319802012b 137 // highpass filter + trigger mode
Arkadi 4:59319802012b 138 inline void highpassTrig(void){
Arkadi 4:59319802012b 139 ///////////////////////////////
Arkadi 4:59319802012b 140 // filter variables: //
Arkadi 4:59319802012b 141 ///////////////////////////////
Arkadi 4:59319802012b 142 // filter coeficients best performance if defined in loop
Arkadi 4:59319802012b 143 float SOSMatHP[2][6] = { // 25khz cutoff at 920 kHz sample rate // closer to 30kHz when io toggle switched off.
Arkadi 4:59319802012b 144 1.000000, -2.000000, 1.000000, 1.000000, -1.706510, 0.731145,
Arkadi 4:59319802012b 145 1.000000, -2.000000, 1.000000, 1.000000, -1.852377, 0.879117
Arkadi 4:59319802012b 146 };
Arkadi 4:59319802012b 147 float GscaleHP = 0.801724;
Arkadi 4:59319802012b 148
Arkadi 4:59319802012b 149 // num sections
Arkadi 4:59319802012b 150 int NumSectionsHP = sizeof(SOSMatHP)/sizeof(float)/6;
Arkadi 4:59319802012b 151
Arkadi 4:59319802012b 152 float ADCFloat;
Arkadi 4:59319802012b 153 float ADCFloatFiltered;
Arkadi 4:59319802012b 154 uint16_t ADCValueOut;
Arkadi 4:59319802012b 155
Arkadi 4:59319802012b 156 // filter stages variables
Arkadi 4:59319802012b 157 static float CurrInput [MAX_SECTION_NUMBER+1];
Arkadi 4:59319802012b 158 static float LastInput [MAX_SECTION_NUMBER+1];
Arkadi 4:59319802012b 159 static float LLastInput [MAX_SECTION_NUMBER+1];
Arkadi 4:59319802012b 160
Arkadi 4:59319802012b 161 ////////////////////
Arkadi 4:59319802012b 162 // Read ADC input //
Arkadi 4:59319802012b 163 ////////////////////
Arkadi 4:59319802012b 164 ADCFloat=((uint16_t)(hadc1.Instance->DR) * ADC2Float)-1.0f;
Arkadi 1:574b54755983 165
Arkadi 4:59319802012b 166 //////////////////////////////////////////////////////
Arkadi 4:59319802012b 167 // Apply Filter to input //
Arkadi 4:59319802012b 168 //////////////////////////////////////////////////////
Arkadi 4:59319802012b 169
Arkadi 4:59319802012b 170 LLastInput[0] = LastInput[0];
Arkadi 4:59319802012b 171 LastInput[0] = CurrInput[0];
Arkadi 4:59319802012b 172 CurrInput[0] = ADCFloat;
Arkadi 4:59319802012b 173 for (int ii=1; ii <= NumSectionsHP; ii++) {
Arkadi 4:59319802012b 174 LLastInput[ii] = LastInput[ii];
Arkadi 4:59319802012b 175 LastInput[ii] = CurrInput[ii];
Arkadi 4:59319802012b 176 CurrInput[ii] = SOSMatHP[ii-1][0]*CurrInput[ii-1] + SOSMatHP[ii-1][1]*LastInput[ii-1] +
Arkadi 4:59319802012b 177 SOSMatHP[ii-1][2]*LLastInput[ii-1] -
Arkadi 4:59319802012b 178 SOSMatHP[ii-1][4]*LastInput[ii] - SOSMatHP[ii-1][5]*LLastInput[ii];
Arkadi 4:59319802012b 179 ADCFloatFiltered = CurrInput[ii];
Arkadi 4:59319802012b 180 }
Arkadi 4:59319802012b 181
Arkadi 4:59319802012b 182 ADCFloatFiltered = ADCFloatFiltered * GscaleHP;
Arkadi 4:59319802012b 183
Arkadi 4:59319802012b 184 ////////////////////////////
Arkadi 4:59319802012b 185 // Apply Filter to Output //
Arkadi 4:59319802012b 186 ////////////////////////////
Arkadi 4:59319802012b 187
Arkadi 4:59319802012b 188 if (ADCFloatFiltered < -1.0f) {
Arkadi 4:59319802012b 189 ADCValueOut=0; // Min value
Arkadi 4:59319802012b 190 } else if (ADCFloatFiltered > 1.0f) {
Arkadi 4:59319802012b 191 ADCValueOut=0xFFF; // Max value
Arkadi 4:59319802012b 192 } else {
Arkadi 4:59319802012b 193 ADCValueOut=(uint16_t)((ADCFloatFiltered +1.0f) * Float2ADC);
Arkadi 4:59319802012b 194 }
Arkadi 4:59319802012b 195
Arkadi 4:59319802012b 196 // Output value using DAC
Arkadi 4:59319802012b 197 // HAL_DAC_SetValue(&hdac1, DAC_CHANNEL_1, DAC_ALIGN_12B_R, ADCValueOut);
Arkadi 4:59319802012b 198 *(__IO uint32_t *) Dac_Reg = ADCValueOut/2;
Arkadi 4:59319802012b 199 //wait_us(1);
Arkadi 4:59319802012b 200 } // end high pass filter + trigger mode