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@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?

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 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