Arkadi Rafalovich / Mbed 2 deprecated TAU_ReSpeaker_DSP_V02

ReSpeaker DSP V02

Dependencies:   mbed MbedJSONValue

filters.h@14:8a4699aa69b5, 2019-06-20 (annotated)

Committer:
Arkadi
Date:
Thu Jun 20 09:06:46 2019 +0000
Revision:
14:8a4699aa69b5
Parent:
12:9d30df1529be 
experiments version

Who changed what in which revision?

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