Arkadi Rafalovich / Mbed 2 deprecated TAU_ReSpeaker_DSP_V02

ReSpeaker DSP V02

Dependencies:   mbed MbedJSONValue

filters.h@8:8a3e6241c104, 2018-08-22 (annotated)

Committer:
Arkadi
Date:
Wed Aug 22 12:57:13 2018 +0000
Revision:
8:8a3e6241c104
Parent:
7:25c81cb23e42
Child:
10:273127efdc6e
Added parameters to modify through gui

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