ReSpeaker DSP V02
Dependencies: mbed MbedJSONValue
filters.h@10:273127efdc6e, 2018-08-27 (annotated)
- Committer:
- Arkadi
- Date:
- Mon Aug 27 10:32:08 2018 +0000
- Revision:
- 10:273127efdc6e
- Parent:
- 8:8a3e6241c104
- Child:
- 12:9d30df1529be
Added filter and communication supports
Who changed what in which revision?
User | Revision | Line number | New 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 | 7:25c81cb23e42 | 42 | // highpass filter + trigger mode + Gains vector |
Arkadi | 7:25c81cb23e42 | 43 | inline void GainsTrig(void); |
Arkadi | 7:25c81cb23e42 | 44 | // highpass filter + trigger mode + Delays vector |
Arkadi | 7:25c81cb23e42 | 45 | inline void DelaysTrig(void); |
Arkadi | 7:25c81cb23e42 | 46 | // highpass filter + trigger mode + FIR Filter (Convolution) |
Arkadi | 7:25c81cb23e42 | 47 | inline void FIRTrig(void); |
Arkadi | 1:574b54755983 | 48 | |
Arkadi | 1:574b54755983 | 49 | /////////////////////////////// |
Arkadi | 1:574b54755983 | 50 | // Filter Functions: // |
Arkadi | 1:574b54755983 | 51 | /////////////////////////////// |
Arkadi | 4:59319802012b | 52 | |
Arkadi | 4:59319802012b | 53 | // off mode, output vdd/2 |
Arkadi | 5:ec6f2323a263 | 54 | inline void offMode(void) |
Arkadi | 5:ec6f2323a263 | 55 | { |
Arkadi | 10:273127efdc6e | 56 | // generate some delay (limit loop speeds, creates crashes to the Switch mcu |
Arkadi | 10:273127efdc6e | 57 | // for (int ii=0; ii<10; ii++){ |
Arkadi | 10:273127efdc6e | 58 | // __ASM volatile ("nop"); // one tick operation, Use to adjust frequency by slowing down the proccess |
Arkadi | 10:273127efdc6e | 59 | // } |
Arkadi | 4:59319802012b | 60 | uint16_t ADCValueOut; |
Arkadi | 4:59319802012b | 61 | // set to vdd/2 |
Arkadi | 4:59319802012b | 62 | ADCValueOut=(uint16_t)((0.0f +1.0f) * Float2ADC); |
Arkadi | 4:59319802012b | 63 | // Output value using DAC |
Arkadi | 5:ec6f2323a263 | 64 | *(__IO uint32_t *) Dac_Reg = ADCValueOut; |
Arkadi | 4:59319802012b | 65 | }// end off mode |
Arkadi | 4:59319802012b | 66 | |
Arkadi | 4:59319802012b | 67 | // passthrough function (output equals input) |
Arkadi | 4:59319802012b | 68 | inline void passthrough(void) |
Arkadi | 1:574b54755983 | 69 | { |
Arkadi | 1:574b54755983 | 70 | uint32_t ADCValue; |
Arkadi | 1:574b54755983 | 71 | |
Arkadi | 1:574b54755983 | 72 | // Read ADC input |
Arkadi | 1:574b54755983 | 73 | ADCValue = hadc1.Instance->DR; |
Arkadi | 1:574b54755983 | 74 | // Output value using DAC |
Arkadi | 1:574b54755983 | 75 | *(__IO uint32_t *) Dac_Reg = ADCValue; |
Arkadi | 1:574b54755983 | 76 | |
Arkadi | 4:59319802012b | 77 | } // end passthrough |
Arkadi | 1:574b54755983 | 78 | |
Arkadi | 3:48258b86e182 | 79 | |
Arkadi | 1:574b54755983 | 80 | // high pass filter |
Arkadi | 4:59319802012b | 81 | inline void highpass(void) |
Arkadi | 1:574b54755983 | 82 | { |
Arkadi | 4:59319802012b | 83 | /////////////////////////////// |
Arkadi | 4:59319802012b | 84 | // filter variables: // |
Arkadi | 4:59319802012b | 85 | /////////////////////////////// |
Arkadi | 4:59319802012b | 86 | // filter coeficients best performance if defined in loop |
Arkadi | 4:59319802012b | 87 | float SOSMatHP[2][6] = { // 25khz cutoff at 920 kHz sample rate // closer to 30kHz when io toggle switched off. |
Arkadi | 4:59319802012b | 88 | 1.000000, -2.000000, 1.000000, 1.000000, -1.706510, 0.731145, |
Arkadi | 4:59319802012b | 89 | 1.000000, -2.000000, 1.000000, 1.000000, -1.852377, 0.879117 |
Arkadi | 4:59319802012b | 90 | }; |
Arkadi | 4:59319802012b | 91 | float GscaleHP = 0.801724; |
Arkadi | 4:59319802012b | 92 | |
Arkadi | 4:59319802012b | 93 | // num sections |
Arkadi | 4:59319802012b | 94 | int NumSectionsHP = sizeof(SOSMatHP)/sizeof(float)/6; |
Arkadi | 4:59319802012b | 95 | |
Arkadi | 1:574b54755983 | 96 | float ADCFloat; |
Arkadi | 1:574b54755983 | 97 | float ADCFloatFiltered; |
Arkadi | 1:574b54755983 | 98 | uint16_t ADCValueOut; |
Arkadi | 1:574b54755983 | 99 | |
Arkadi | 1:574b54755983 | 100 | // filter stages variables |
Arkadi | 1:574b54755983 | 101 | static float CurrInput [MAX_SECTION_NUMBER+1]; |
Arkadi | 1:574b54755983 | 102 | static float LastInput [MAX_SECTION_NUMBER+1]; |
Arkadi | 1:574b54755983 | 103 | static float LLastInput [MAX_SECTION_NUMBER+1]; |
Arkadi | 5:ec6f2323a263 | 104 | |
Arkadi | 4:59319802012b | 105 | //////////////////// |
Arkadi | 4:59319802012b | 106 | // Read ADC input // |
Arkadi | 4:59319802012b | 107 | //////////////////// |
Arkadi | 1:574b54755983 | 108 | ADCFloat=((uint16_t)(hadc1.Instance->DR) * ADC2Float)-1.0f; |
Arkadi | 1:574b54755983 | 109 | |
Arkadi | 1:574b54755983 | 110 | ////////////////////////////////////////////////////// |
Arkadi | 1:574b54755983 | 111 | // Apply Filter to input // |
Arkadi | 1:574b54755983 | 112 | ////////////////////////////////////////////////////// |
Arkadi | 1:574b54755983 | 113 | |
Arkadi | 1:574b54755983 | 114 | LLastInput[0] = LastInput[0]; |
Arkadi | 1:574b54755983 | 115 | LastInput[0] = CurrInput[0]; |
Arkadi | 1:574b54755983 | 116 | CurrInput[0] = ADCFloat; |
Arkadi | 1:574b54755983 | 117 | for (int ii=1; ii <= NumSectionsHP; ii++) { |
Arkadi | 1:574b54755983 | 118 | LLastInput[ii] = LastInput[ii]; |
Arkadi | 1:574b54755983 | 119 | LastInput[ii] = CurrInput[ii]; |
Arkadi | 1:574b54755983 | 120 | CurrInput[ii] = SOSMatHP[ii-1][0]*CurrInput[ii-1] + SOSMatHP[ii-1][1]*LastInput[ii-1] + |
Arkadi | 1:574b54755983 | 121 | SOSMatHP[ii-1][2]*LLastInput[ii-1] - |
Arkadi | 1:574b54755983 | 122 | SOSMatHP[ii-1][4]*LastInput[ii] - SOSMatHP[ii-1][5]*LLastInput[ii]; |
Arkadi | 1:574b54755983 | 123 | ADCFloatFiltered = CurrInput[ii]; |
Arkadi | 1:574b54755983 | 124 | } |
Arkadi | 1:574b54755983 | 125 | |
Arkadi | 8:8a3e6241c104 | 126 | ADCFloatFiltered = ADCFloatFiltered * GscaleHP * signalGain; |
Arkadi | 1:574b54755983 | 127 | |
Arkadi | 7:25c81cb23e42 | 128 | //////////////////////////////// |
Arkadi | 7:25c81cb23e42 | 129 | // Apply Saturation to Output // |
Arkadi | 7:25c81cb23e42 | 130 | //////////////////////////////// |
Arkadi | 1:574b54755983 | 131 | |
Arkadi | 1:574b54755983 | 132 | if (ADCFloatFiltered < -1.0f) { |
Arkadi | 1:574b54755983 | 133 | ADCValueOut=0; // Min value |
Arkadi | 1:574b54755983 | 134 | } else if (ADCFloatFiltered > 1.0f) { |
Arkadi | 1:574b54755983 | 135 | ADCValueOut=0xFFF; // Max value |
Arkadi | 1:574b54755983 | 136 | } else { |
Arkadi | 1:574b54755983 | 137 | ADCValueOut=(uint16_t)((ADCFloatFiltered +1.0f) * Float2ADC); |
Arkadi | 1:574b54755983 | 138 | } |
Arkadi | 1:574b54755983 | 139 | |
Arkadi | 1:574b54755983 | 140 | // Output value using DAC |
Arkadi | 1:574b54755983 | 141 | // HAL_DAC_SetValue(&hdac1, DAC_CHANNEL_1, DAC_ALIGN_12B_R, ADCValueOut); |
Arkadi | 1:574b54755983 | 142 | *(__IO uint32_t *) Dac_Reg = ADCValueOut; |
Arkadi | 4:59319802012b | 143 | |
Arkadi | 4:59319802012b | 144 | } // end high pass filter |
Arkadi | 4:59319802012b | 145 | |
Arkadi | 4:59319802012b | 146 | // highpass filter + trigger mode |
Arkadi | 5:ec6f2323a263 | 147 | inline void highpassTrig(void) |
Arkadi | 5:ec6f2323a263 | 148 | { |
Arkadi | 4:59319802012b | 149 | /////////////////////////////// |
Arkadi | 4:59319802012b | 150 | // filter variables: // |
Arkadi | 4:59319802012b | 151 | /////////////////////////////// |
Arkadi | 4:59319802012b | 152 | // filter coeficients best performance if defined in loop |
Arkadi | 4:59319802012b | 153 | float SOSMatHP[2][6] = { // 25khz cutoff at 920 kHz sample rate // closer to 30kHz when io toggle switched off. |
Arkadi | 4:59319802012b | 154 | 1.000000, -2.000000, 1.000000, 1.000000, -1.706510, 0.731145, |
Arkadi | 4:59319802012b | 155 | 1.000000, -2.000000, 1.000000, 1.000000, -1.852377, 0.879117 |
Arkadi | 4:59319802012b | 156 | }; |
Arkadi | 4:59319802012b | 157 | float GscaleHP = 0.801724; |
Arkadi | 4:59319802012b | 158 | |
Arkadi | 4:59319802012b | 159 | // num sections |
Arkadi | 4:59319802012b | 160 | int NumSectionsHP = sizeof(SOSMatHP)/sizeof(float)/6; |
Arkadi | 4:59319802012b | 161 | |
Arkadi | 4:59319802012b | 162 | float ADCFloat; |
Arkadi | 4:59319802012b | 163 | float ADCFloatFiltered; |
Arkadi | 4:59319802012b | 164 | uint16_t ADCValueOut; |
Arkadi | 4:59319802012b | 165 | |
Arkadi | 4:59319802012b | 166 | // filter stages variables |
Arkadi | 4:59319802012b | 167 | static float CurrInput [MAX_SECTION_NUMBER+1]; |
Arkadi | 4:59319802012b | 168 | static float LastInput [MAX_SECTION_NUMBER+1]; |
Arkadi | 4:59319802012b | 169 | static float LLastInput [MAX_SECTION_NUMBER+1]; |
Arkadi | 5:ec6f2323a263 | 170 | |
Arkadi | 5:ec6f2323a263 | 171 | // trigger variables |
Arkadi | 5:ec6f2323a263 | 172 | static bool trigFlag=0; // flag to indicate trigger event |
Arkadi | 5:ec6f2323a263 | 173 | |
Arkadi | 4:59319802012b | 174 | //////////////////// |
Arkadi | 4:59319802012b | 175 | // Read ADC input // |
Arkadi | 4:59319802012b | 176 | //////////////////// |
Arkadi | 4:59319802012b | 177 | ADCFloat=((uint16_t)(hadc1.Instance->DR) * ADC2Float)-1.0f; |
Arkadi | 1:574b54755983 | 178 | |
Arkadi | 4:59319802012b | 179 | ////////////////////////////////////////////////////// |
Arkadi | 4:59319802012b | 180 | // Apply Filter to input // |
Arkadi | 4:59319802012b | 181 | ////////////////////////////////////////////////////// |
Arkadi | 4:59319802012b | 182 | |
Arkadi | 4:59319802012b | 183 | LLastInput[0] = LastInput[0]; |
Arkadi | 4:59319802012b | 184 | LastInput[0] = CurrInput[0]; |
Arkadi | 4:59319802012b | 185 | CurrInput[0] = ADCFloat; |
Arkadi | 4:59319802012b | 186 | for (int ii=1; ii <= NumSectionsHP; ii++) { |
Arkadi | 4:59319802012b | 187 | LLastInput[ii] = LastInput[ii]; |
Arkadi | 4:59319802012b | 188 | LastInput[ii] = CurrInput[ii]; |
Arkadi | 4:59319802012b | 189 | CurrInput[ii] = SOSMatHP[ii-1][0]*CurrInput[ii-1] + SOSMatHP[ii-1][1]*LastInput[ii-1] + |
Arkadi | 4:59319802012b | 190 | SOSMatHP[ii-1][2]*LLastInput[ii-1] - |
Arkadi | 4:59319802012b | 191 | SOSMatHP[ii-1][4]*LastInput[ii] - SOSMatHP[ii-1][5]*LLastInput[ii]; |
Arkadi | 4:59319802012b | 192 | ADCFloatFiltered = CurrInput[ii]; |
Arkadi | 4:59319802012b | 193 | } |
Arkadi | 4:59319802012b | 194 | |
Arkadi | 8:8a3e6241c104 | 195 | ADCFloatFiltered = ADCFloatFiltered * GscaleHP * signalGain; |
Arkadi | 4:59319802012b | 196 | |
Arkadi | 5:ec6f2323a263 | 197 | /////////////////////////////////////////////// |
Arkadi | 5:ec6f2323a263 | 198 | // Event detection // |
Arkadi | 5:ec6f2323a263 | 199 | /////////////////////////////////////////////// |
Arkadi | 5:ec6f2323a263 | 200 | if (trigFlag) { // event already detected |
Arkadi | 5:ec6f2323a263 | 201 | trigDelay--; |
Arkadi | 5:ec6f2323a263 | 202 | if (trigDelay == 0) { // pulse pass run out, perform delay and reset variables |
Arkadi | 5:ec6f2323a263 | 203 | trigDelay = trigDelaySet; //reset counter for next iteration |
Arkadi | 5:ec6f2323a263 | 204 | trigFlag=0; |
Arkadi | 5:ec6f2323a263 | 205 | // reset filter |
Arkadi | 5:ec6f2323a263 | 206 | for (int ii=1; ii <= NumSectionsHP; ii++) { |
Arkadi | 5:ec6f2323a263 | 207 | LLastInput[ii] = 0; |
Arkadi | 5:ec6f2323a263 | 208 | LastInput[ii] = 0; |
Arkadi | 5:ec6f2323a263 | 209 | CurrInput[ii] = 0; |
Arkadi | 5:ec6f2323a263 | 210 | ADCFloatFiltered = 0; |
Arkadi | 5:ec6f2323a263 | 211 | } |
Arkadi | 5:ec6f2323a263 | 212 | // update dac |
Arkadi | 5:ec6f2323a263 | 213 | ADCValueOut=(uint16_t)((ADCFloatFiltered +1.0f) * Float2ADC); |
Arkadi | 5:ec6f2323a263 | 214 | *(__IO uint32_t *) Dac_Reg = ADCValueOut; |
Arkadi | 6:e8b4ca41c691 | 215 | //*(__IO uint32_t *) Dac_Reg = 0; // test triggered mode |
Arkadi | 5:ec6f2323a263 | 216 | // delay for set time |
Arkadi | 8:8a3e6241c104 | 217 | wait_ms(trigPause); |
Arkadi | 5:ec6f2323a263 | 218 | } |
Arkadi | 8:8a3e6241c104 | 219 | } else if (ADCFloatFiltered >= trigTresh) { |
Arkadi | 5:ec6f2323a263 | 220 | trigFlag=1; |
Arkadi | 5:ec6f2323a263 | 221 | } |
Arkadi | 7:25c81cb23e42 | 222 | //////////////////////////////// |
Arkadi | 7:25c81cb23e42 | 223 | // Apply Saturation to Output // |
Arkadi | 7:25c81cb23e42 | 224 | //////////////////////////////// |
Arkadi | 4:59319802012b | 225 | |
Arkadi | 4:59319802012b | 226 | if (ADCFloatFiltered < -1.0f) { |
Arkadi | 4:59319802012b | 227 | ADCValueOut=0; // Min value |
Arkadi | 4:59319802012b | 228 | } else if (ADCFloatFiltered > 1.0f) { |
Arkadi | 4:59319802012b | 229 | ADCValueOut=0xFFF; // Max value |
Arkadi | 4:59319802012b | 230 | } else { |
Arkadi | 4:59319802012b | 231 | ADCValueOut=(uint16_t)((ADCFloatFiltered +1.0f) * Float2ADC); |
Arkadi | 4:59319802012b | 232 | } |
Arkadi | 4:59319802012b | 233 | |
Arkadi | 4:59319802012b | 234 | // Output value using DAC |
Arkadi | 4:59319802012b | 235 | // HAL_DAC_SetValue(&hdac1, DAC_CHANNEL_1, DAC_ALIGN_12B_R, ADCValueOut); |
Arkadi | 6:e8b4ca41c691 | 236 | *(__IO uint32_t *) Dac_Reg = ADCValueOut; |
Arkadi | 4:59319802012b | 237 | //wait_us(1); |
Arkadi | 7:25c81cb23e42 | 238 | } // end high pass filter + trigger mode |
Arkadi | 7:25c81cb23e42 | 239 | |
Arkadi | 7:25c81cb23e42 | 240 | |
Arkadi | 7:25c81cb23e42 | 241 | // highpass filter + trigger mode + Gains vector |
Arkadi | 7:25c81cb23e42 | 242 | inline void GainsTrig(void) |
Arkadi | 7:25c81cb23e42 | 243 | { |
Arkadi | 7:25c81cb23e42 | 244 | /////////////////////////////// |
Arkadi | 7:25c81cb23e42 | 245 | // Gains variables: // |
Arkadi | 7:25c81cb23e42 | 246 | /////////////////////////////// |
Arkadi | 7:25c81cb23e42 | 247 | #define GAIN_VECTOR_SIZE 5 |
Arkadi | 7:25c81cb23e42 | 248 | float GainVector[] = {0.1 , 0.2 , 0.4 , 0.8 , 1.6}; |
Arkadi | 7:25c81cb23e42 | 249 | static uint16_t GainVectorIndex = 0; |
Arkadi | 7:25c81cb23e42 | 250 | /////////////////////////////// |
Arkadi | 7:25c81cb23e42 | 251 | // filter variables: // |
Arkadi | 7:25c81cb23e42 | 252 | /////////////////////////////// |
Arkadi | 7:25c81cb23e42 | 253 | // filter coeficients best performance if defined in loop |
Arkadi | 7:25c81cb23e42 | 254 | float SOSMatHP[2][6] = { // 25khz cutoff at 920 kHz sample rate // closer to 30kHz when io toggle switched off. |
Arkadi | 7:25c81cb23e42 | 255 | 1.000000, -2.000000, 1.000000, 1.000000, -1.706510, 0.731145, |
Arkadi | 7:25c81cb23e42 | 256 | 1.000000, -2.000000, 1.000000, 1.000000, -1.852377, 0.879117 |
Arkadi | 7:25c81cb23e42 | 257 | }; |
Arkadi | 7:25c81cb23e42 | 258 | float GscaleHP = 0.801724; |
Arkadi | 7:25c81cb23e42 | 259 | |
Arkadi | 7:25c81cb23e42 | 260 | // num sections |
Arkadi | 7:25c81cb23e42 | 261 | int NumSectionsHP = sizeof(SOSMatHP)/sizeof(float)/6; |
Arkadi | 7:25c81cb23e42 | 262 | |
Arkadi | 7:25c81cb23e42 | 263 | float ADCFloat; |
Arkadi | 7:25c81cb23e42 | 264 | float ADCFloatFiltered; |
Arkadi | 7:25c81cb23e42 | 265 | uint16_t ADCValueOut; |
Arkadi | 7:25c81cb23e42 | 266 | |
Arkadi | 7:25c81cb23e42 | 267 | // filter stages variables |
Arkadi | 7:25c81cb23e42 | 268 | static float CurrInput [MAX_SECTION_NUMBER+1]; |
Arkadi | 7:25c81cb23e42 | 269 | static float LastInput [MAX_SECTION_NUMBER+1]; |
Arkadi | 7:25c81cb23e42 | 270 | static float LLastInput [MAX_SECTION_NUMBER+1]; |
Arkadi | 7:25c81cb23e42 | 271 | |
Arkadi | 7:25c81cb23e42 | 272 | // trigger variables |
Arkadi | 7:25c81cb23e42 | 273 | static bool trigFlag=0; // flag to indicate trigger event |
Arkadi | 7:25c81cb23e42 | 274 | |
Arkadi | 7:25c81cb23e42 | 275 | //////////////////// |
Arkadi | 7:25c81cb23e42 | 276 | // Read ADC input // |
Arkadi | 7:25c81cb23e42 | 277 | //////////////////// |
Arkadi | 7:25c81cb23e42 | 278 | ADCFloat=((uint16_t)(hadc1.Instance->DR) * ADC2Float)-1.0f; |
Arkadi | 7:25c81cb23e42 | 279 | |
Arkadi | 7:25c81cb23e42 | 280 | ////////////////////////////////////////////////////// |
Arkadi | 7:25c81cb23e42 | 281 | // Apply Filter to input // |
Arkadi | 7:25c81cb23e42 | 282 | ////////////////////////////////////////////////////// |
Arkadi | 7:25c81cb23e42 | 283 | |
Arkadi | 7:25c81cb23e42 | 284 | LLastInput[0] = LastInput[0]; |
Arkadi | 7:25c81cb23e42 | 285 | LastInput[0] = CurrInput[0]; |
Arkadi | 7:25c81cb23e42 | 286 | CurrInput[0] = ADCFloat; |
Arkadi | 7:25c81cb23e42 | 287 | for (int ii=1; ii <= NumSectionsHP; ii++) { |
Arkadi | 7:25c81cb23e42 | 288 | LLastInput[ii] = LastInput[ii]; |
Arkadi | 7:25c81cb23e42 | 289 | LastInput[ii] = CurrInput[ii]; |
Arkadi | 7:25c81cb23e42 | 290 | CurrInput[ii] = SOSMatHP[ii-1][0]*CurrInput[ii-1] + SOSMatHP[ii-1][1]*LastInput[ii-1] + |
Arkadi | 7:25c81cb23e42 | 291 | SOSMatHP[ii-1][2]*LLastInput[ii-1] - |
Arkadi | 7:25c81cb23e42 | 292 | SOSMatHP[ii-1][4]*LastInput[ii] - SOSMatHP[ii-1][5]*LLastInput[ii]; |
Arkadi | 7:25c81cb23e42 | 293 | ADCFloatFiltered = CurrInput[ii]; |
Arkadi | 7:25c81cb23e42 | 294 | } |
Arkadi | 7:25c81cb23e42 | 295 | |
Arkadi | 7:25c81cb23e42 | 296 | ADCFloatFiltered = ADCFloatFiltered * GscaleHP; |
Arkadi | 7:25c81cb23e42 | 297 | |
Arkadi | 7:25c81cb23e42 | 298 | /////////////////////////////////////////////// |
Arkadi | 7:25c81cb23e42 | 299 | // Event detection // |
Arkadi | 7:25c81cb23e42 | 300 | /////////////////////////////////////////////// |
Arkadi | 7:25c81cb23e42 | 301 | if (trigFlag) { // event already detected |
Arkadi | 7:25c81cb23e42 | 302 | trigDelay--; |
Arkadi | 7:25c81cb23e42 | 303 | if (trigDelay == 0) { // pulse pass run out, perform delay and reset variables |
Arkadi | 7:25c81cb23e42 | 304 | trigDelay = trigDelaySet; //reset counter for next iteration |
Arkadi | 7:25c81cb23e42 | 305 | trigFlag=0; |
Arkadi | 7:25c81cb23e42 | 306 | // reset filter |
Arkadi | 7:25c81cb23e42 | 307 | for (int ii=1; ii <= NumSectionsHP; ii++) { |
Arkadi | 7:25c81cb23e42 | 308 | LLastInput[ii] = 0; |
Arkadi | 7:25c81cb23e42 | 309 | LastInput[ii] = 0; |
Arkadi | 7:25c81cb23e42 | 310 | CurrInput[ii] = 0; |
Arkadi | 7:25c81cb23e42 | 311 | ADCFloatFiltered = 0; |
Arkadi | 7:25c81cb23e42 | 312 | } |
Arkadi | 7:25c81cb23e42 | 313 | // update dac |
Arkadi | 7:25c81cb23e42 | 314 | ADCValueOut=(uint16_t)((ADCFloatFiltered +1.0f) * Float2ADC); |
Arkadi | 7:25c81cb23e42 | 315 | *(__IO uint32_t *) Dac_Reg = ADCValueOut; |
Arkadi | 7:25c81cb23e42 | 316 | //*(__IO uint32_t *) Dac_Reg = 0; // test triggered mode |
Arkadi | 7:25c81cb23e42 | 317 | // delay for set time |
Arkadi | 8:8a3e6241c104 | 318 | wait_ms(trigPause); |
Arkadi | 7:25c81cb23e42 | 319 | |
Arkadi | 7:25c81cb23e42 | 320 | // change gain settings |
Arkadi | 7:25c81cb23e42 | 321 | GainVectorIndex++; |
Arkadi | 7:25c81cb23e42 | 322 | GainVectorIndex = GainVectorIndex % GAIN_VECTOR_SIZE; |
Arkadi | 7:25c81cb23e42 | 323 | } |
Arkadi | 7:25c81cb23e42 | 324 | } else if (ADCFloatFiltered >= trigTresh) { |
Arkadi | 7:25c81cb23e42 | 325 | trigFlag=1; |
Arkadi | 7:25c81cb23e42 | 326 | } |
Arkadi | 7:25c81cb23e42 | 327 | ////////////////////////// |
Arkadi | 7:25c81cb23e42 | 328 | // Apply Gain to Output // |
Arkadi | 7:25c81cb23e42 | 329 | ////////////////////////// |
Arkadi | 7:25c81cb23e42 | 330 | ADCFloatFiltered = ADCFloatFiltered * GainVector[GainVectorIndex]; |
Arkadi | 7:25c81cb23e42 | 331 | |
Arkadi | 7:25c81cb23e42 | 332 | if (ADCFloatFiltered < -1.0f) { |
Arkadi | 7:25c81cb23e42 | 333 | ADCValueOut=0; // Min value |
Arkadi | 7:25c81cb23e42 | 334 | } else if (ADCFloatFiltered > 1.0f) { |
Arkadi | 7:25c81cb23e42 | 335 | ADCValueOut=0xFFF; // Max value |
Arkadi | 7:25c81cb23e42 | 336 | } else { |
Arkadi | 7:25c81cb23e42 | 337 | ADCValueOut=(uint16_t)((ADCFloatFiltered +1.0f) * Float2ADC); |
Arkadi | 7:25c81cb23e42 | 338 | } |
Arkadi | 7:25c81cb23e42 | 339 | |
Arkadi | 7:25c81cb23e42 | 340 | // Output value using DAC |
Arkadi | 7:25c81cb23e42 | 341 | // HAL_DAC_SetValue(&hdac1, DAC_CHANNEL_1, DAC_ALIGN_12B_R, ADCValueOut); |
Arkadi | 7:25c81cb23e42 | 342 | *(__IO uint32_t *) Dac_Reg = ADCValueOut; |
Arkadi | 7:25c81cb23e42 | 343 | //wait_us(1); |
Arkadi | 8:8a3e6241c104 | 344 | } // end GainsTrig |
Arkadi | 8:8a3e6241c104 | 345 | |
Arkadi | 8:8a3e6241c104 | 346 | // highpass filter + trigger mode + Delays vector |
Arkadi | 8:8a3e6241c104 | 347 | inline void DelaysTrig(void) // - mid work feature. (not working yet) |
Arkadi | 8:8a3e6241c104 | 348 | { |
Arkadi | 8:8a3e6241c104 | 349 | /////////////////////////////// |
Arkadi | 8:8a3e6241c104 | 350 | // Gains variables: // |
Arkadi | 8:8a3e6241c104 | 351 | /////////////////////////////// |
Arkadi | 8:8a3e6241c104 | 352 | #define DELAY_VECTOR_SIZE 5 |
Arkadi | 8:8a3e6241c104 | 353 | uint32_t DelayVector[] = {100 , 200 , 300 , 400 , 500}; // millis |
Arkadi | 8:8a3e6241c104 | 354 | static uint16_t DelayVectorIndex = 0; |
Arkadi | 8:8a3e6241c104 | 355 | static uint32_t BufferIndex = 0; |
Arkadi | 8:8a3e6241c104 | 356 | |
Arkadi | 8:8a3e6241c104 | 357 | //uint32_t bufferSizeSet = 5000; |
Arkadi | 8:8a3e6241c104 | 358 | //uint32_t preBufferSizeSet = 1000; |
Arkadi | 8:8a3e6241c104 | 359 | //uint32_t bufferCountDown = bufferSizeSet - preBufferSizeSet; |
Arkadi | 8:8a3e6241c104 | 360 | //float bufferADC[bufferSizeSet] = {0}; |
Arkadi | 8:8a3e6241c104 | 361 | /////////////////////////////// |
Arkadi | 8:8a3e6241c104 | 362 | // filter variables: // |
Arkadi | 8:8a3e6241c104 | 363 | /////////////////////////////// |
Arkadi | 8:8a3e6241c104 | 364 | // filter coeficients best performance if defined in loop |
Arkadi | 8:8a3e6241c104 | 365 | float SOSMatHP[2][6] = { // 25khz cutoff at 920 kHz sample rate // closer to 30kHz when io toggle switched off. |
Arkadi | 8:8a3e6241c104 | 366 | 1.000000, -2.000000, 1.000000, 1.000000, -1.706510, 0.731145, |
Arkadi | 8:8a3e6241c104 | 367 | 1.000000, -2.000000, 1.000000, 1.000000, -1.852377, 0.879117 |
Arkadi | 8:8a3e6241c104 | 368 | }; |
Arkadi | 8:8a3e6241c104 | 369 | float GscaleHP = 0.801724; |
Arkadi | 8:8a3e6241c104 | 370 | |
Arkadi | 8:8a3e6241c104 | 371 | // num sections |
Arkadi | 8:8a3e6241c104 | 372 | int NumSectionsHP = sizeof(SOSMatHP)/sizeof(float)/6; |
Arkadi | 8:8a3e6241c104 | 373 | |
Arkadi | 8:8a3e6241c104 | 374 | float ADCFloat; |
Arkadi | 8:8a3e6241c104 | 375 | float ADCFloatFiltered; |
Arkadi | 8:8a3e6241c104 | 376 | uint16_t ADCValueOut; |
Arkadi | 8:8a3e6241c104 | 377 | |
Arkadi | 8:8a3e6241c104 | 378 | // filter stages variables |
Arkadi | 8:8a3e6241c104 | 379 | static float CurrInput [MAX_SECTION_NUMBER+1]; |
Arkadi | 8:8a3e6241c104 | 380 | static float LastInput [MAX_SECTION_NUMBER+1]; |
Arkadi | 8:8a3e6241c104 | 381 | static float LLastInput [MAX_SECTION_NUMBER+1]; |
Arkadi | 8:8a3e6241c104 | 382 | |
Arkadi | 8:8a3e6241c104 | 383 | // trigger variables |
Arkadi | 8:8a3e6241c104 | 384 | static bool trigFlag=0; // flag to indicate trigger event |
Arkadi | 8:8a3e6241c104 | 385 | |
Arkadi | 8:8a3e6241c104 | 386 | //////////////////// |
Arkadi | 8:8a3e6241c104 | 387 | // Read ADC input // |
Arkadi | 8:8a3e6241c104 | 388 | //////////////////// |
Arkadi | 8:8a3e6241c104 | 389 | ADCFloat=((uint16_t)(hadc1.Instance->DR) * ADC2Float)-1.0f; |
Arkadi | 8:8a3e6241c104 | 390 | |
Arkadi | 8:8a3e6241c104 | 391 | ////////////////////////////////////////////////////// |
Arkadi | 8:8a3e6241c104 | 392 | // Apply Filter to input // |
Arkadi | 8:8a3e6241c104 | 393 | ////////////////////////////////////////////////////// |
Arkadi | 8:8a3e6241c104 | 394 | |
Arkadi | 8:8a3e6241c104 | 395 | LLastInput[0] = LastInput[0]; |
Arkadi | 8:8a3e6241c104 | 396 | LastInput[0] = CurrInput[0]; |
Arkadi | 8:8a3e6241c104 | 397 | CurrInput[0] = ADCFloat; |
Arkadi | 8:8a3e6241c104 | 398 | for (int ii=1; ii <= NumSectionsHP; ii++) { |
Arkadi | 8:8a3e6241c104 | 399 | LLastInput[ii] = LastInput[ii]; |
Arkadi | 8:8a3e6241c104 | 400 | LastInput[ii] = CurrInput[ii]; |
Arkadi | 8:8a3e6241c104 | 401 | CurrInput[ii] = SOSMatHP[ii-1][0]*CurrInput[ii-1] + SOSMatHP[ii-1][1]*LastInput[ii-1] + |
Arkadi | 8:8a3e6241c104 | 402 | SOSMatHP[ii-1][2]*LLastInput[ii-1] - |
Arkadi | 8:8a3e6241c104 | 403 | SOSMatHP[ii-1][4]*LastInput[ii] - SOSMatHP[ii-1][5]*LLastInput[ii]; |
Arkadi | 8:8a3e6241c104 | 404 | ADCFloatFiltered = CurrInput[ii]; |
Arkadi | 8:8a3e6241c104 | 405 | } |
Arkadi | 8:8a3e6241c104 | 406 | |
Arkadi | 8:8a3e6241c104 | 407 | ADCFloatFiltered = ADCFloatFiltered * GscaleHP; |
Arkadi | 8:8a3e6241c104 | 408 | |
Arkadi | 8:8a3e6241c104 | 409 | //////////////////////////////// |
Arkadi | 8:8a3e6241c104 | 410 | // Fill in buffer // |
Arkadi | 8:8a3e6241c104 | 411 | //////////////////////////////// |
Arkadi | 8:8a3e6241c104 | 412 | bufferADC[BufferIndex] = ADCFloatFiltered; |
Arkadi | 8:8a3e6241c104 | 413 | BufferIndex++; |
Arkadi | 8:8a3e6241c104 | 414 | BufferIndex = BufferIndex % bufferSize; |
Arkadi | 8:8a3e6241c104 | 415 | /////////////////////////////////////////////// |
Arkadi | 8:8a3e6241c104 | 416 | // Event detection // |
Arkadi | 8:8a3e6241c104 | 417 | /////////////////////////////////////////////// |
Arkadi | 8:8a3e6241c104 | 418 | if (trigFlag) { // event already detected |
Arkadi | 8:8a3e6241c104 | 419 | bufferCountDown--; // count down |
Arkadi | 8:8a3e6241c104 | 420 | if (bufferCountDown == 0) { // pulse pass run out, perform delay and reset variables |
Arkadi | 8:8a3e6241c104 | 421 | bufferCountDown = bufferSizeSet - preBufferSizeSet; //reset counter for next iteration |
Arkadi | 8:8a3e6241c104 | 422 | trigFlag=0; |
Arkadi | 8:8a3e6241c104 | 423 | // reset filter |
Arkadi | 8:8a3e6241c104 | 424 | for (int ii=1; ii <= NumSectionsHP; ii++) { |
Arkadi | 8:8a3e6241c104 | 425 | LLastInput[ii] = 0; |
Arkadi | 8:8a3e6241c104 | 426 | LastInput[ii] = 0; |
Arkadi | 8:8a3e6241c104 | 427 | CurrInput[ii] = 0; |
Arkadi | 8:8a3e6241c104 | 428 | ADCFloatFiltered = 0; |
Arkadi | 8:8a3e6241c104 | 429 | } |
Arkadi | 8:8a3e6241c104 | 430 | |
Arkadi | 8:8a3e6241c104 | 431 | // generate delay |
Arkadi | 8:8a3e6241c104 | 432 | // delay for set time |
Arkadi | 8:8a3e6241c104 | 433 | wait_ms(DelayVector[DelayVectorIndex]); |
Arkadi | 8:8a3e6241c104 | 434 | // change delay settings |
Arkadi | 8:8a3e6241c104 | 435 | DelayVectorIndex++; |
Arkadi | 8:8a3e6241c104 | 436 | DelayVectorIndex = DelayVectorIndex % DELAY_VECTOR_SIZE; |
Arkadi | 8:8a3e6241c104 | 437 | |
Arkadi | 8:8a3e6241c104 | 438 | |
Arkadi | 8:8a3e6241c104 | 439 | // play out buffer |
Arkadi | 8:8a3e6241c104 | 440 | for (int ii=0 ; ii<bufferSize ; ii++) { |
Arkadi | 8:8a3e6241c104 | 441 | |
Arkadi | 8:8a3e6241c104 | 442 | ADCFloatFiltered = bufferADC[BufferIndex]; |
Arkadi | 8:8a3e6241c104 | 443 | BufferIndex++; |
Arkadi | 8:8a3e6241c104 | 444 | BufferIndex = BufferIndex % bufferSize; |
Arkadi | 8:8a3e6241c104 | 445 | //////////////////////////////// |
Arkadi | 8:8a3e6241c104 | 446 | // Apply Saturation to Output // |
Arkadi | 8:8a3e6241c104 | 447 | //////////////////////////////// |
Arkadi | 8:8a3e6241c104 | 448 | |
Arkadi | 8:8a3e6241c104 | 449 | if (ADCFloatFiltered < -1.0f) { |
Arkadi | 8:8a3e6241c104 | 450 | ADCValueOut=0; // Min value |
Arkadi | 8:8a3e6241c104 | 451 | } else if (ADCFloatFiltered > 1.0f) { |
Arkadi | 8:8a3e6241c104 | 452 | ADCValueOut=0xFFF; // Max value |
Arkadi | 8:8a3e6241c104 | 453 | } else { |
Arkadi | 8:8a3e6241c104 | 454 | ADCValueOut=(uint16_t)((ADCFloatFiltered +1.0f) * Float2ADC); |
Arkadi | 8:8a3e6241c104 | 455 | } |
Arkadi | 8:8a3e6241c104 | 456 | |
Arkadi | 8:8a3e6241c104 | 457 | // Output value using DAC |
Arkadi | 8:8a3e6241c104 | 458 | // HAL_DAC_SetValue(&hdac1, DAC_CHANNEL_1, DAC_ALIGN_12B_R, ADCValueOut); |
Arkadi | 8:8a3e6241c104 | 459 | *(__IO uint32_t *) Dac_Reg = ADCValueOut; |
Arkadi | 8:8a3e6241c104 | 460 | //wait_us(1); |
Arkadi | 10:273127efdc6e | 461 | } |
Arkadi | 8:8a3e6241c104 | 462 | } |
Arkadi | 8:8a3e6241c104 | 463 | } else if (ADCFloatFiltered >= trigTresh) { |
Arkadi | 8:8a3e6241c104 | 464 | trigFlag=1; |
Arkadi | 8:8a3e6241c104 | 465 | } |
Arkadi | 8:8a3e6241c104 | 466 | |
Arkadi | 7:25c81cb23e42 | 467 | } // end GainsTrig |