James Smith / Mbed 2 deprecated Filter

FIR Filter

Dependencies:   CMSIS_DSP_401 mbed

main.cpp@1:a2238b2df415, 2016-05-29 (annotated)

Committer:
jlsmith10
Date:
Sun May 29 20:16:11 2016 +0000
Revision:
1:a2238b2df415
Parent:
0:5cd703c0576c 
v2.0;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jlsmith10 1:a2238b2df415 1 /*******************************************************************************
jlsmith10 1:a2238b2df415 2 * main.cpp
jlsmith10 1:a2238b2df415 3 * by Jason Shen, James Smith and Luis Sanchez
jlsmith10 1:a2238b2df415 4 *
jlsmith10 1:a2238b2df415 5 * Acoustic Location System Filtering
jlsmith10 1:a2238b2df415 6 *
jlsmith10 1:a2238b2df415 7 * This C module implements the DSP filtering of an input signal for the
jlsmith10 1:a2238b2df415 8 * STM33F446RE Nucleo board.. The input signal will be passed in through
jlsmith10 1:a2238b2df415 9 * on board ADC connected to a transponder that receives accoustic waves
jlsmith10 1:a2238b2df415 10 * delivered by three transponders at 3 different frequencies. The coeffecients
jlsmith10 1:a2238b2df415 11 * used for the filter are defined in main.h and implement bandpass filters for
jlsmith10 1:a2238b2df415 12 * 35kHz, 40kHz and 45kHz with a pass bandwith of 500Hz.
jlsmith10 1:a2238b2df415 13 *
jlsmith10 1:a2238b2df415 14 * This program will output the time difference of arrival of the three
jlsmith10 1:a2238b2df415 15 * varying frequency signals. the default for for output is:
jlsmith10 1:a2238b2df415 16 *
jlsmith10 1:a2238b2df415 17 * [ A vs B time difference, B vs C time difference, A vs C time difference ]
jlsmith10 1:a2238b2df415 18 *
jlsmith10 1:a2238b2df415 19 * Currently the algorithm will first FIR filter the data for each of the 3
jlsmith10 1:a2238b2df415 20 * frequencies. Then it will use an in place moving average function to
jlsmith10 1:a2238b2df415 21 * smooth the data. Finally, it will run a peak detection function and find the
jlsmith10 1:a2238b2df415 22 * peak of each of the 3 filtered signals which will then be placed on an output
jlsmith10 1:a2238b2df415 23 * pin to be available for beagle bone on OpenROV.
jlsmith10 1:a2238b2df415 24 *
jlsmith10 1:a2238b2df415 25 *
jlsmith10 1:a2238b2df415 26 *
jlsmith10 1:a2238b2df415 27 *
jlsmith10 1:a2238b2df415 28 * TODO:
jlsmith10 1:a2238b2df415 29 * 1. Test on different location inputs from python script,
jlsmith10 1:a2238b2df415 30 * 2. Integrate ADC and filter code
jlsmith10 1:a2238b2df415 31 * 3. If current filtering and smoothing is innaccurate develop new algorithm
jlsmith10 1:a2238b2df415 32 * 4. Clean up code
jlsmith10 1:a2238b2df415 33 *******************************************************************************/
jlsmith10 1:a2238b2df415 34
jlsmith10 0:5cd703c0576c 35 /* Includes */
jlsmith10 0:5cd703c0576c 36 #include "main.h"
jlsmith10 0:5cd703c0576c 37
jlsmith10 0:5cd703c0576c 38
jlsmith10 1:a2238b2df415 39
jlsmith10 0:5cd703c0576c 40
jlsmith10 1:a2238b2df415 41 /* FIR filter variables */
jlsmith10 0:5cd703c0576c 42 static float firStateF[BLOCK_SIZE + NUM_TAPS - 1];
jlsmith10 0:5cd703c0576c 43 unsigned int blockSize = BLOCK_SIZE;
jlsmith10 0:5cd703c0576c 44 unsigned int numBlocks = NUM_SAMPLES/BLOCK_SIZE;
jlsmith10 0:5cd703c0576c 45
jlsmith10 0:5cd703c0576c 46
jlsmith10 0:5cd703c0576c 47 /* Function Prototypes */
jlsmith10 0:5cd703c0576c 48 float calculateSoundSpeedInWater(float temp, float salinity, float depth);
jlsmith10 0:5cd703c0576c 49
jlsmith10 1:a2238b2df415 50 int singleThresholdDetection(const float sampleData[], int startPosition);
jlsmith10 0:5cd703c0576c 51
jlsmith10 1:a2238b2df415 52 void filterAndSmooth(float signal[], float filterCoeffs[], int smoothingCoeff, float * filteredSignal);
jlsmith10 0:5cd703c0576c 53
jlsmith10 1:a2238b2df415 54 void movAvg(float signal[]);
jlsmith10 0:5cd703c0576c 55
jlsmith10 1:a2238b2df415 56 int findMax(float signal[]);
jlsmith10 0:5cd703c0576c 57
jlsmith10 1:a2238b2df415 58 /*------------------------------------
jlsmith10 1:a2238b2df415 59 * Hyperterminal configuration
jlsmith10 1:a2238b2df415 60 * 9600 bauds, 8-bit data, no parity
jlsmith10 1:a2238b2df415 61 ************************************/
jlsmith10 0:5cd703c0576c 62 Serial pc(SERIAL_TX, SERIAL_RX);
jlsmith10 0:5cd703c0576c 63
jlsmith10 1:a2238b2df415 64 DigitalOut myled(LED1); //Debug LED
jlsmith10 0:5cd703c0576c 65
jlsmith10 0:5cd703c0576c 66 int main(void)
jlsmith10 0:5cd703c0576c 67 {
jlsmith10 1:a2238b2df415 68 printf("---------------------------------------------------------------- ");
jlsmith10 1:a2238b2df415 69 printf(" ");
jlsmith10 1:a2238b2df415 70 printf(" Underwater Acoustic Location System DSP\n");
jlsmith10 1:a2238b2df415 71 printf(" ");
jlsmith10 1:a2238b2df415 72 printf("---------------------------------------------------------------- ");
jlsmith10 1:a2238b2df415 73 printf(" Running Filter... ");
jlsmith10 1:a2238b2df415 74 printf("---------------------------------------------------------------- ");
jlsmith10 1:a2238b2df415 75 printf(" Number of Samples: %d\n", NUM_SAMPLES);
jlsmith10 1:a2238b2df415 76 printf(" Moving Average window: %d samples\n", MOV_AVG_WIND);
jlsmith10 1:a2238b2df415 77
jlsmith10 0:5cd703c0576c 78 /*
jlsmith10 0:5cd703c0576c 79 float velocity = calculateSoundSpeedInWater(temp, salinity, depth);
jlsmith10 0:5cd703c0576c 80 pc.printf("The velocity is %lf", velocity);
jlsmith10 0:5cd703c0576c 81 */
jlsmith10 1:a2238b2df415 82
jlsmith10 1:a2238b2df415 83 // Fir instance variables
jlsmith10 1:a2238b2df415 84 arm_fir_instance_f32 S1, S2, S3;
jlsmith10 0:5cd703c0576c 85 arm_status status;
jlsmith10 0:5cd703c0576c 86 float32_t *input, *output;
jlsmith10 0:5cd703c0576c 87
jlsmith10 1:a2238b2df415 88 // Fir Coefficients
jlsmith10 0:5cd703c0576c 89 float coeffsA_f32[NUM_TAPS];
jlsmith10 1:a2238b2df415 90 float output_f32[NUM_SAMPLES];
jlsmith10 0:5cd703c0576c 91
jlsmith10 1:a2238b2df415 92 /* Input is the signal with all 3 frequencies present. */
jlsmith10 0:5cd703c0576c 93 input = &signalABC[0];
jlsmith10 1:a2238b2df415 94 output = &output_f32[0];
jlsmith10 1:a2238b2df415 95
jlsmith10 1:a2238b2df415 96 /* The array of time differences to place on output pin */
jlsmith10 1:a2238b2df415 97 int toReturn[3];
jlsmith10 1:a2238b2df415 98
jlsmith10 1:a2238b2df415 99 /***************************************************************************
jlsmith10 1:a2238b2df415 100 ****************** Filtering for f = 35kHz (from buoy A) *******************
jlsmith10 1:a2238b2df415 101 ***************************************************************************/
jlsmith10 1:a2238b2df415 102 if(UALS_DEBUG) {
jlsmith10 1:a2238b2df415 103 printf("Beginning filtering for f = 35kHz (buoy A)\n");
jlsmith10 0:5cd703c0576c 104 }
jlsmith10 1:a2238b2df415 105
jlsmith10 1:a2238b2df415 106 /* Initialize FIR instance structure */
jlsmith10 1:a2238b2df415 107 arm_fir_init_f32(&S1, NUM_TAPS, (float *) &coeffsA[0],
jlsmith10 1:a2238b2df415 108 &firStateF[0], blockSize);
jlsmith10 0:5cd703c0576c 109
jlsmith10 1:a2238b2df415 110 int i = 0;
jlsmith10 0:5cd703c0576c 111
jlsmith10 0:5cd703c0576c 112 /* FIR Filtering */
jlsmith10 0:5cd703c0576c 113 for( i = 0; i < numBlocks; i++) {
jlsmith10 1:a2238b2df415 114 arm_fir_f32(&S1, input + (i * blockSize),
jlsmith10 1:a2238b2df415 115 output + (i * blockSize), blockSize);
jlsmith10 0:5cd703c0576c 116 }
jlsmith10 0:5cd703c0576c 117
jlsmith10 0:5cd703c0576c 118 /* Take the absolute value of the filtered signal */
jlsmith10 0:5cd703c0576c 119 for(i = 0; i < NUM_SAMPLES; i++ ) {
jlsmith10 0:5cd703c0576c 120 if(output[i] < 0) {
jlsmith10 1:a2238b2df415 121 output[i] = -1 * output[i];
jlsmith10 1:a2238b2df415 122 }
jlsmith10 1:a2238b2df415 123 }
jlsmith10 1:a2238b2df415 124
jlsmith10 1:a2238b2df415 125 /* Print before moving average */
jlsmith10 1:a2238b2df415 126 printf("----------Before moving average-------------\n");
jlsmith10 1:a2238b2df415 127 if(UALS_DEBUG) {
jlsmith10 1:a2238b2df415 128 for(i = 0; i < NUM_SAMPLES; i++) {
jlsmith10 1:a2238b2df415 129 printf("%lf\n", output[i]);
jlsmith10 1:a2238b2df415 130 }
jlsmith10 1:a2238b2df415 131 }
jlsmith10 1:a2238b2df415 132
jlsmith10 1:a2238b2df415 133 float * avgSignal[NUM_SAMPLES];
jlsmith10 1:a2238b2df415 134 // Calculate moving average and do peak detection;
jlsmith10 1:a2238b2df415 135 movAvg(output);
jlsmith10 1:a2238b2df415 136
jlsmith10 1:a2238b2df415 137 /* Print signal after moving average */
jlsmith10 1:a2238b2df415 138 printf("----------After moving average-------------\n");
jlsmith10 1:a2238b2df415 139 if(UALS_DEBUG) {
jlsmith10 1:a2238b2df415 140 for(i = 0; i < NUM_SAMPLES; i++) {
jlsmith10 1:a2238b2df415 141 printf("%lf\n", output[i]);
jlsmith10 1:a2238b2df415 142 }
jlsmith10 1:a2238b2df415 143 }
jlsmith10 1:a2238b2df415 144
jlsmith10 1:a2238b2df415 145 /* Find the maximum value of the filtered signal */
jlsmith10 1:a2238b2df415 146 int maxA = findMax(output);
jlsmith10 1:a2238b2df415 147
jlsmith10 1:a2238b2df415 148 /***************************************************************************
jlsmith10 1:a2238b2df415 149 ********************* Filtering for f = 40kHz (from buoy B) ****************
jlsmith10 1:a2238b2df415 150 ***************************************************************************/
jlsmith10 1:a2238b2df415 151 if(UALS_DEBUG) {
jlsmith10 1:a2238b2df415 152 printf("Beginning filtering for f = 40kHz (buoy B)\n");
jlsmith10 1:a2238b2df415 153 }
jlsmith10 1:a2238b2df415 154 /* Initialize FIR instance structure */
jlsmith10 1:a2238b2df415 155 arm_fir_init_f32(&S2, NUM_TAPS, (float *) &coeffsB[0],
jlsmith10 1:a2238b2df415 156 &firStateF[0], blockSize);
jlsmith10 1:a2238b2df415 157
jlsmith10 1:a2238b2df415 158 /* FIR Filtering */
jlsmith10 1:a2238b2df415 159 for( i = 0; i < numBlocks; i++) {
jlsmith10 1:a2238b2df415 160 arm_fir_f32(&S2, input + (i * blockSize),
jlsmith10 1:a2238b2df415 161 output + (i * blockSize), blockSize);
jlsmith10 1:a2238b2df415 162 }
jlsmith10 1:a2238b2df415 163
jlsmith10 1:a2238b2df415 164 /* Take the absolute value of the filtered signal */
jlsmith10 1:a2238b2df415 165 for(i = 0; i < NUM_SAMPLES; i++ ) {
jlsmith10 1:a2238b2df415 166 if(output[i] < 0) {
jlsmith10 0:5cd703c0576c 167 output[i] = -1 * output[i];
jlsmith10 0:5cd703c0576c 168 }
jlsmith10 0:5cd703c0576c 169 }
jlsmith10 0:5cd703c0576c 170
jlsmith10 1:a2238b2df415 171 /* Print before moving average */
jlsmith10 1:a2238b2df415 172 if(UALS_DEBUG) {
jlsmith10 1:a2238b2df415 173 printf("----------Before moving average-------------\n");
jlsmith10 1:a2238b2df415 174 for(i = 0; i < NUM_SAMPLES; i++) {
jlsmith10 1:a2238b2df415 175 printf("%lf\n", output[i]);
jlsmith10 1:a2238b2df415 176 }
jlsmith10 1:a2238b2df415 177 }
jlsmith10 1:a2238b2df415 178
jlsmith10 1:a2238b2df415 179
jlsmith10 1:a2238b2df415 180 /* Perform moving average */
jlsmith10 1:a2238b2df415 181 movAvg(output);
jlsmith10 1:a2238b2df415 182
jlsmith10 1:a2238b2df415 183 /* Print signal after moving average */
jlsmith10 1:a2238b2df415 184 if(UALS_DEBUG) {
jlsmith10 1:a2238b2df415 185 printf("----------After moving average-------------\n");
jlsmith10 1:a2238b2df415 186 for(i = 0; i < NUM_SAMPLES; i++) {
jlsmith10 1:a2238b2df415 187 printf("%lf\n", output[i]);
jlsmith10 1:a2238b2df415 188 }
jlsmith10 1:a2238b2df415 189 }
jlsmith10 0:5cd703c0576c 190
jlsmith10 1:a2238b2df415 191 /* Find the maximum value of the filtered signal */
jlsmith10 1:a2238b2df415 192 int maxB = findMax(output);
jlsmith10 1:a2238b2df415 193
jlsmith10 1:a2238b2df415 194
jlsmith10 1:a2238b2df415 195 /***************************************************************************
jlsmith10 1:a2238b2df415 196 ******************** Filtering for f = 45kHz (from buoy C) *****************
jlsmith10 1:a2238b2df415 197 ***************************************************************************/
jlsmith10 1:a2238b2df415 198 if(UALS_DEBUG) {
jlsmith10 1:a2238b2df415 199 printf("Beginning filtering for f = 45kHz (buoy C)\n");
jlsmith10 1:a2238b2df415 200 }
jlsmith10 1:a2238b2df415 201 /* Initialize FIR instance structure */
jlsmith10 1:a2238b2df415 202 arm_fir_init_f32(&S3, NUM_TAPS, (float *) &coeffsC[0],
jlsmith10 1:a2238b2df415 203 &firStateF[0], blockSize);
jlsmith10 0:5cd703c0576c 204
jlsmith10 1:a2238b2df415 205 /* FIR Filtering */
jlsmith10 1:a2238b2df415 206 for( i = 0; i < numBlocks; i++) {
jlsmith10 1:a2238b2df415 207 arm_fir_f32(&S3, input + (i * blockSize),
jlsmith10 1:a2238b2df415 208 output + (i * blockSize), blockSize);
jlsmith10 1:a2238b2df415 209 }
jlsmith10 0:5cd703c0576c 210
jlsmith10 1:a2238b2df415 211 /* Take the absolute value of the filtered signal */
jlsmith10 1:a2238b2df415 212 for(i = 0; i < NUM_SAMPLES; i++ ) {
jlsmith10 1:a2238b2df415 213 if(output[i] < 0) {
jlsmith10 1:a2238b2df415 214 output[i] = -1 * output[i];
jlsmith10 1:a2238b2df415 215 }
jlsmith10 1:a2238b2df415 216 }
jlsmith10 1:a2238b2df415 217
jlsmith10 1:a2238b2df415 218 /* Print before moving average */
jlsmith10 1:a2238b2df415 219 if(UALS_DEBUG) {
jlsmith10 1:a2238b2df415 220 printf("----------Before moving average-------------\n");
jlsmith10 1:a2238b2df415 221 for(i = 0; i < NUM_SAMPLES; i++) {
jlsmith10 1:a2238b2df415 222 printf("%lf\n", output[i]);
jlsmith10 1:a2238b2df415 223 }
jlsmith10 0:5cd703c0576c 224 }
jlsmith10 1:a2238b2df415 225
jlsmith10 0:5cd703c0576c 226
jlsmith10 1:a2238b2df415 227 /* Perform moving average */
jlsmith10 1:a2238b2df415 228 movAvg(output);
jlsmith10 1:a2238b2df415 229
jlsmith10 1:a2238b2df415 230 /* Print signal after moving average */
jlsmith10 1:a2238b2df415 231 if(UALS_DEBUG) {
jlsmith10 1:a2238b2df415 232 printf("----------After moving average-------------\n");
jlsmith10 1:a2238b2df415 233 for(i = 0; i < NUM_SAMPLES; i++) {
jlsmith10 1:a2238b2df415 234 printf("%lf\n", output[i]);
jlsmith10 1:a2238b2df415 235 }
jlsmith10 0:5cd703c0576c 236 }
jlsmith10 1:a2238b2df415 237
jlsmith10 1:a2238b2df415 238 /* Find the maximum value of the filtered signal */
jlsmith10 1:a2238b2df415 239 int maxC = findMax(output);
jlsmith10 1:a2238b2df415 240
jlsmith10 1:a2238b2df415 241 /* The time differences to be returned */
jlsmith10 1:a2238b2df415 242 /* TODO: should we take the absolute value? What does TDOA take in */
jlsmith10 1:a2238b2df415 243 float tdAB = maxB - maxA;
jlsmith10 1:a2238b2df415 244 float tdBC = maxB - maxC;
jlsmith10 1:a2238b2df415 245 float tdAC = maxC - maxA;
jlsmith10 1:a2238b2df415 246
jlsmith10 1:a2238b2df415 247 printf("tdAB = %f\n", tdAB);
jlsmith10 1:a2238b2df415 248 printf("tdBC = %f\n", tdBC);
jlsmith10 1:a2238b2df415 249 printf("tdAC = %f\n", tdAC);
jlsmith10 1:a2238b2df415 250
jlsmith10 1:a2238b2df415 251 toReturn[0] = tdAB;
jlsmith10 1:a2238b2df415 252 toReturn[1] = tdBC;
jlsmith10 1:a2238b2df415 253 toReturn[2] = tdAC;
jlsmith10 0:5cd703c0576c 254 }
jlsmith10 0:5cd703c0576c 255
jlsmith10 0:5cd703c0576c 256 /*
jlsmith10 1:a2238b2df415 257 * Function Name: singleThresholdDetection()
jlsmith10 1:a2238b2df415 258 * Function Declaration: int singleThresholdDetection(const float sampleData[],
jlsmith10 1:a2238b2df415 259 int startPosition)
jlsmith10 1:a2238b2df415 260 * Function Description: Calculates the first occurrence of a value over the
jlsmith10 1:a2238b2df415 261 * threshold value and returns the position.
jlsmith10 0:5cd703c0576c 262 *
jlsmith10 0:5cd703c0576c 263 * Params:
jlsmith10 0:5cd703c0576c 264 * arg1: sampleData - sample data to find position of first peak
jlsmith10 0:5cd703c0576c 265 * arg2: startPosition - threshold value to search for
jlsmith10 0:5cd703c0576c 266 *
jlsmith10 0:5cd703c0576c 267 * Return Value: position where threshold is first reached
jlsmith10 0:5cd703c0576c 268 *
jlsmith10 0:5cd703c0576c 269 */
jlsmith10 1:a2238b2df415 270 int singleThresholdDetection(const float sampleData[], int startPosition)
jlsmith10 0:5cd703c0576c 271 {
jlsmith10 0:5cd703c0576c 272 int i;
jlsmith10 1:a2238b2df415 273
jlsmith10 0:5cd703c0576c 274 for(i = startPosition; i < NUM_SAMPLES; i++) {
jlsmith10 0:5cd703c0576c 275 if (sampleData[i] >= DETECT_THRESH) {
jlsmith10 0:5cd703c0576c 276 return i;
jlsmith10 0:5cd703c0576c 277 }
jlsmith10 0:5cd703c0576c 278 }
jlsmith10 0:5cd703c0576c 279
jlsmith10 0:5cd703c0576c 280 return -1;
jlsmith10 0:5cd703c0576c 281 }
jlsmith10 0:5cd703c0576c 282
jlsmith10 1:a2238b2df415 283
jlsmith10 1:a2238b2df415 284 /*
jlsmith10 1:a2238b2df415 285 * Function Name: movAvg();
jlsmith10 1:a2238b2df415 286 * Function Declaration: void movAvg(float signal[]);
jlsmith10 1:a2238b2df415 287 * Function Description: This function takes an input signal and does an in
jlsmith10 1:a2238b2df415 288 * place moving average algorithm. The window size is defined by MOV_AVG_WIND
jlsmith10 1:a2238b2df415 289 * in main.h.
jlsmith10 0:5cd703c0576c 290 *
jlsmith10 1:a2238b2df415 291 * This function serves a dual purpose of finding the position of the largest
jlsmith10 1:a2238b2df415 292 * peak and returning that position.
jlsmith10 1:a2238b2df415 293 *
jlsmith10 1:a2238b2df415 294 * Return Value: None
jlsmith10 0:5cd703c0576c 295 *
jlsmith10 0:5cd703c0576c 296 */
jlsmith10 1:a2238b2df415 297 void movAvg(float signal[]) {
jlsmith10 1:a2238b2df415 298 int i = 0;
jlsmith10 1:a2238b2df415 299
jlsmith10 1:a2238b2df415 300 int start = 0;
jlsmith10 1:a2238b2df415 301 int finish = 0;
jlsmith10 1:a2238b2df415 302 float total = 0;
jlsmith10 1:a2238b2df415 303
jlsmith10 1:a2238b2df415 304 // Buffer to hold the most recent samples
jlsmith10 1:a2238b2df415 305 float buffer[MOV_AVG_WIND*2];
jlsmith10 1:a2238b2df415 306
jlsmith10 1:a2238b2df415 307 // Go through signal array and calculate moving average
jlsmith10 1:a2238b2df415 308 for(i = 0; i < NUM_SAMPLES; i++) {
jlsmith10 1:a2238b2df415 309 start = i - MOV_AVG_WIND;
jlsmith10 1:a2238b2df415 310 finish = i + MOV_AVG_WIND;
jlsmith10 1:a2238b2df415 311
jlsmith10 1:a2238b2df415 312 if(start < 0) {
jlsmith10 1:a2238b2df415 313 start = 0;
jlsmith10 1:a2238b2df415 314 }
jlsmith10 0:5cd703c0576c 315
jlsmith10 1:a2238b2df415 316 if(finish > NUM_SAMPLES) {
jlsmith10 1:a2238b2df415 317 finish = NUM_SAMPLES;
jlsmith10 1:a2238b2df415 318 }
jlsmith10 1:a2238b2df415 319
jlsmith10 1:a2238b2df415 320 total = 0;
jlsmith10 1:a2238b2df415 321 for(int j = start; j < finish; j++) {
jlsmith10 1:a2238b2df415 322 total += signal[j];
jlsmith10 1:a2238b2df415 323 }
jlsmith10 1:a2238b2df415 324
jlsmith10 1:a2238b2df415 325 if(i > MOV_AVG_WIND*2 - 1) {
jlsmith10 1:a2238b2df415 326 signal[i-(MOV_AVG_WIND*2)] = buffer[i % (MOV_AVG_WIND*2)];
jlsmith10 1:a2238b2df415 327 }
jlsmith10 0:5cd703c0576c 328
jlsmith10 1:a2238b2df415 329 // Rotating buffer to save the avg
jlsmith10 1:a2238b2df415 330 buffer[i%(MOV_AVG_WIND*2)] = total / (finish - start);
jlsmith10 1:a2238b2df415 331
jlsmith10 1:a2238b2df415 332 }
jlsmith10 1:a2238b2df415 333 for(int i = NUM_SAMPLES-MOV_AVG_WIND*2; i < NUM_SAMPLES; i++) {
jlsmith10 1:a2238b2df415 334 signal[i] = buffer[i %(MOV_AVG_WIND*2)];
jlsmith10 1:a2238b2df415 335 }
jlsmith10 1:a2238b2df415 336
jlsmith10 1:a2238b2df415 337 if(UALS_DEBUG) {
jlsmith10 1:a2238b2df415 338 for(int i = 0; i < NUM_SAMPLES; i++) {
jlsmith10 1:a2238b2df415 339 printf("averagedSignal[i] = %f\n", signal[i]);
jlsmith10 1:a2238b2df415 340 }
jlsmith10 1:a2238b2df415 341 }
jlsmith10 0:5cd703c0576c 342 }
jlsmith10 0:5cd703c0576c 343
jlsmith10 0:5cd703c0576c 344
jlsmith10 1:a2238b2df415 345 /*
jlsmith10 1:a2238b2df415 346 * Function Name: findMax();
jlsmith10 1:a2238b2df415 347 * Function Declaration: int findMax(float signal[]);
jlsmith10 1:a2238b2df415 348 * Function Description: This function takes an input signal and returns the
jlsmith10 1:a2238b2df415 349 * position of the maximum valued sample.
jlsmith10 0:5cd703c0576c 350 *
jlsmith10 0:5cd703c0576c 351 */
jlsmith10 1:a2238b2df415 352 int findMax(float signal[]) {
jlsmith10 1:a2238b2df415 353 float maxVal = 0.0;
jlsmith10 1:a2238b2df415 354 int maxPosition = -1;
jlsmith10 1:a2238b2df415 355 int i;
jlsmith10 0:5cd703c0576c 356
jlsmith10 1:a2238b2df415 357 /* Go through each element searching for maximum */
jlsmith10 1:a2238b2df415 358 for(i = 0; i < NUM_SAMPLES; i++) {
jlsmith10 1:a2238b2df415 359 if(signal[i] > maxVal) {
jlsmith10 1:a2238b2df415 360 /* We've found a new max, replace */
jlsmith10 1:a2238b2df415 361 maxVal = signal[i];
jlsmith10 1:a2238b2df415 362 maxPosition = i;
jlsmith10 1:a2238b2df415 363 }
jlsmith10 1:a2238b2df415 364 }
jlsmith10 0:5cd703c0576c 365
jlsmith10 1:a2238b2df415 366 if(maxPosition > 0) {
jlsmith10 1:a2238b2df415 367 printf("Max position found at %d\n", maxPosition);
jlsmith10 1:a2238b2df415 368 return maxPosition;
jlsmith10 1:a2238b2df415 369 }
jlsmith10 1:a2238b2df415 370 else {
jlsmith10 1:a2238b2df415 371 printf("Error: no max found\n");
jlsmith10 1:a2238b2df415 372 return -1;
jlsmith10 1:a2238b2df415 373 }
jlsmith10 0:5cd703c0576c 374 }