James Smith / Mbed 2 deprecated Filter

FIR Filter

Dependencies:   CMSIS_DSP_401 mbed

main.cpp@0:5cd703c0576c, 2016-05-28 (annotated)

Committer:
jlsmith10
Date:
Sat May 28 21:26:55 2016 +0000
Revision:
0:5cd703c0576c
Child:
1:a2238b2df415
filter;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jlsmith10 0:5cd703c0576c 1 /* Includes */
jlsmith10 0:5cd703c0576c 2 #include <stdio.h> //fprintf, FILE
jlsmith10 0:5cd703c0576c 3 #include <stdlib.h> //
jlsmith10 0:5cd703c0576c 4 #include <math.h> //sqrt
jlsmith10 0:5cd703c0576c 5
jlsmith10 0:5cd703c0576c 6 #include "mbed.h"
jlsmith10 0:5cd703c0576c 7 #include "main.h"
jlsmith10 0:5cd703c0576c 8
jlsmith10 0:5cd703c0576c 9 //#include "arm_math.h"
jlsmith10 0:5cd703c0576c 10 //#include "math_helper.h"
jlsmith10 0:5cd703c0576c 11
jlsmith10 0:5cd703c0576c 12 #include "float.h"
jlsmith10 0:5cd703c0576c 13
jlsmith10 0:5cd703c0576c 14 //#include "tiny_printf.c"
jlsmith10 0:5cd703c0576c 15
jlsmith10 0:5cd703c0576c 16 #define BLOCK_SIZE (32)
jlsmith10 0:5cd703c0576c 17
jlsmith10 0:5cd703c0576c 18 static float firStateF[BLOCK_SIZE + NUM_TAPS - 1];
jlsmith10 0:5cd703c0576c 19
jlsmith10 0:5cd703c0576c 20 unsigned int blockSize = BLOCK_SIZE;
jlsmith10 0:5cd703c0576c 21 unsigned int numBlocks = NUM_SAMPLES/BLOCK_SIZE;
jlsmith10 0:5cd703c0576c 22
jlsmith10 0:5cd703c0576c 23
jlsmith10 0:5cd703c0576c 24 /* Private macro */
jlsmith10 0:5cd703c0576c 25
jlsmith10 0:5cd703c0576c 26
jlsmith10 0:5cd703c0576c 27 /* Private variables */
jlsmith10 0:5cd703c0576c 28 //float signalA[NUM_SAMPLES];
jlsmith10 0:5cd703c0576c 29 //float signalB[NUM_SAMPLES];
jlsmith10 0:5cd703c0576c 30 //float signalC[NUM_SAMPLES];
jlsmith10 0:5cd703c0576c 31
jlsmith10 0:5cd703c0576c 32 FILE* logFile;
jlsmith10 0:5cd703c0576c 33
jlsmith10 0:5cd703c0576c 34 typedef struct coordinate {
jlsmith10 0:5cd703c0576c 35 float x;
jlsmith10 0:5cd703c0576c 36 float y;
jlsmith10 0:5cd703c0576c 37 } coord;
jlsmith10 0:5cd703c0576c 38
jlsmith10 0:5cd703c0576c 39 /* Function Prototypes */
jlsmith10 0:5cd703c0576c 40 float calculateSoundSpeedInWater(float temp, float salinity, float depth);
jlsmith10 0:5cd703c0576c 41
jlsmith10 0:5cd703c0576c 42 void createSignalsFromFile(void);
jlsmith10 0:5cd703c0576c 43
jlsmith10 0:5cd703c0576c 44 void movingAverage(float signal[NUM_SAMPLES], float * averagedSignal[NUM_SAMPLES]);
jlsmith10 0:5cd703c0576c 45
jlsmith10 0:5cd703c0576c 46 int singleThresholdDetection(const float sampleData[NUM_SAMPLES], int startPosition);
jlsmith10 0:5cd703c0576c 47
jlsmith10 0:5cd703c0576c 48 void getPositionFromSignal(
jlsmith10 0:5cd703c0576c 49 float signal[NUM_SAMPLES], coord* projCoord,
jlsmith10 0:5cd703c0576c 50 float velocity, float depth,
jlsmith10 0:5cd703c0576c 51 float xa, float ya, float za,
jlsmith10 0:5cd703c0576c 52 float xb, float yb, float zb,
jlsmith10 0:5cd703c0576c 53 float xc, float yc, float zc);
jlsmith10 0:5cd703c0576c 54
jlsmith10 0:5cd703c0576c 55 void filterAndSmooth(float signal[NUM_SAMPLES], float filterCoeffs[NUM_TAPS], int smoothingCoeff, float * filteredSignal);
jlsmith10 0:5cd703c0576c 56
jlsmith10 0:5cd703c0576c 57 void calculateProjectedCoordinates(
jlsmith10 0:5cd703c0576c 58 coord * projCoord,
jlsmith10 0:5cd703c0576c 59 float depth, float velocity,
jlsmith10 0:5cd703c0576c 60 float tsa, float tsb, float tsc,
jlsmith10 0:5cd703c0576c 61 float xa, float ya, float za,
jlsmith10 0:5cd703c0576c 62 float xb, float yb, float zb,
jlsmith10 0:5cd703c0576c 63 float xc, float yc, float zc);
jlsmith10 0:5cd703c0576c 64
jlsmith10 0:5cd703c0576c 65
jlsmith10 0:5cd703c0576c 66 //------------------------------------
jlsmith10 0:5cd703c0576c 67 // Hyperterminal configuration
jlsmith10 0:5cd703c0576c 68 // 9600 bauds, 8-bit data, no parity
jlsmith10 0:5cd703c0576c 69 //------------------------------------
jlsmith10 0:5cd703c0576c 70
jlsmith10 0:5cd703c0576c 71 Serial pc(SERIAL_TX, SERIAL_RX);
jlsmith10 0:5cd703c0576c 72
jlsmith10 0:5cd703c0576c 73 DigitalOut myled(LED1);
jlsmith10 0:5cd703c0576c 74
jlsmith10 0:5cd703c0576c 75 int main(void)
jlsmith10 0:5cd703c0576c 76 {
jlsmith10 0:5cd703c0576c 77 int i = 0;
jlsmith10 0:5cd703c0576c 78
jlsmith10 0:5cd703c0576c 79 /* TODO - Add your application code here */
jlsmith10 0:5cd703c0576c 80 //FILE* fp = fopen("log.txt", "w");
jlsmith10 0:5cd703c0576c 81 //if(fp == 0)
jlsmith10 0:5cd703c0576c 82 // pc.printf("NULL\n");
jlsmith10 0:5cd703c0576c 83 //fclose(fp);
jlsmith10 0:5cd703c0576c 84
jlsmith10 0:5cd703c0576c 85 //createSignalsFromFile();
jlsmith10 0:5cd703c0576c 86
jlsmith10 0:5cd703c0576c 87 /*
jlsmith10 0:5cd703c0576c 88 float temp = TEMP;
jlsmith10 0:5cd703c0576c 89 float depth = DEPTH;
jlsmith10 0:5cd703c0576c 90 float salinity = SALINITY;
jlsmith10 0:5cd703c0576c 91
jlsmith10 0:5cd703c0576c 92 float velocity = calculateSoundSpeedInWater(temp, salinity, depth);
jlsmith10 0:5cd703c0576c 93
jlsmith10 0:5cd703c0576c 94 pc.printf("The velocity is %lf", velocity);
jlsmith10 0:5cd703c0576c 95
jlsmith10 0:5cd703c0576c 96
jlsmith10 0:5cd703c0576c 97 int firstPeak = singleThresholdDetection(signalA, 0);
jlsmith10 0:5cd703c0576c 98
jlsmith10 0:5cd703c0576c 99 pc.printf("First peak found at position %d\n", firstPeak);
jlsmith10 0:5cd703c0576c 100 pc.printf("Time of first peak (in ms): %lf", firstPeak / 250.0);
jlsmith10 0:5cd703c0576c 101 */
jlsmith10 0:5cd703c0576c 102
jlsmith10 0:5cd703c0576c 103 arm_fir_instance_f32 S;
jlsmith10 0:5cd703c0576c 104 arm_status status;
jlsmith10 0:5cd703c0576c 105 float32_t *input, *output;
jlsmith10 0:5cd703c0576c 106
jlsmith10 0:5cd703c0576c 107 float coeffsA_f32[NUM_TAPS];
jlsmith10 0:5cd703c0576c 108
jlsmith10 0:5cd703c0576c 109 input = &signalABC[0];
jlsmith10 0:5cd703c0576c 110 /*
jlsmith10 0:5cd703c0576c 111 for(i = 0; i < NUM_TAPS; i++) {
jlsmith10 0:5cd703c0576c 112 coeffsA_f32[i] = (float) coeffsA[i];
jlsmith10 0:5cd703c0576c 113 }
jlsmith10 0:5cd703c0576c 114 */
jlsmith10 0:5cd703c0576c 115
jlsmith10 0:5cd703c0576c 116 /* Initialize FIR instance structure */
jlsmith10 0:5cd703c0576c 117 arm_fir_init_f32(&S, NUM_TAPS, (float *) &coeffsA_f32[0], &firStateF[0], blockSize);
jlsmith10 0:5cd703c0576c 118
jlsmith10 0:5cd703c0576c 119 /* FIR Filtering */
jlsmith10 0:5cd703c0576c 120 for( i = 0; i < numBlocks; i++) {
jlsmith10 0:5cd703c0576c 121 arm_fir_f32(&S, input + (i * blockSize), output + (i * blockSize), blockSize);
jlsmith10 0:5cd703c0576c 122 }
jlsmith10 0:5cd703c0576c 123
jlsmith10 0:5cd703c0576c 124 /* Take the absolute value of the filtered signal */
jlsmith10 0:5cd703c0576c 125 for(i = 0; i < NUM_SAMPLES; i++ ) {
jlsmith10 0:5cd703c0576c 126 //printf("Index: %d, Amplitude: %e\n", i, output[i]);
jlsmith10 0:5cd703c0576c 127 if(output[i] < 0) {
jlsmith10 0:5cd703c0576c 128 //printf("Negative sample!\n");
jlsmith10 0:5cd703c0576c 129 output[i] = -1 * output[i];
jlsmith10 0:5cd703c0576c 130 }
jlsmith10 0:5cd703c0576c 131 }
jlsmith10 0:5cd703c0576c 132
jlsmith10 0:5cd703c0576c 133 float * avgSignal[NUM_SAMPLES];
jlsmith10 0:5cd703c0576c 134
jlsmith10 0:5cd703c0576c 135 //movingAverage(output, avgSignal);
jlsmith10 0:5cd703c0576c 136
jlsmith10 0:5cd703c0576c 137 q15_t max = 0;
jlsmith10 0:5cd703c0576c 138
jlsmith10 0:5cd703c0576c 139 for(i = 0; i < NUM_SAMPLES; i++) {
jlsmith10 0:5cd703c0576c 140 printf("%lf\n", output[i]);
jlsmith10 0:5cd703c0576c 141 }
jlsmith10 0:5cd703c0576c 142 //printf("Max is at %d\n", max);
jlsmith10 0:5cd703c0576c 143
jlsmith10 0:5cd703c0576c 144 /*
jlsmith10 0:5cd703c0576c 145 for(i = 0; i < 20; i++) {
jlsmith10 0:5cd703c0576c 146 printf("%d\n", coeffsA[i]);
jlsmith10 0:5cd703c0576c 147 }
jlsmith10 0:5cd703c0576c 148 */
jlsmith10 0:5cd703c0576c 149 }
jlsmith10 0:5cd703c0576c 150
jlsmith10 0:5cd703c0576c 151 void movingAverage(float signal[NUM_SAMPLES], float ** averagedSignal) {
jlsmith10 0:5cd703c0576c 152 int i = 0;
jlsmith10 0:5cd703c0576c 153
jlsmith10 0:5cd703c0576c 154 int start = 0;
jlsmith10 0:5cd703c0576c 155 int finish = 0;
jlsmith10 0:5cd703c0576c 156 float total = 0;
jlsmith10 0:5cd703c0576c 157
jlsmith10 0:5cd703c0576c 158 for(i = 0; i < NUM_SAMPLES; i++) {
jlsmith10 0:5cd703c0576c 159 start = i - MOV_AVG_WIND;
jlsmith10 0:5cd703c0576c 160 finish = i + MOV_AVG_WIND;
jlsmith10 0:5cd703c0576c 161
jlsmith10 0:5cd703c0576c 162 if(start < 0) {
jlsmith10 0:5cd703c0576c 163 start = 0;
jlsmith10 0:5cd703c0576c 164 }
jlsmith10 0:5cd703c0576c 165 if(finish > NUM_SAMPLES) {
jlsmith10 0:5cd703c0576c 166 finish = NUM_SAMPLES;
jlsmith10 0:5cd703c0576c 167 }
jlsmith10 0:5cd703c0576c 168
jlsmith10 0:5cd703c0576c 169 total = 0;
jlsmith10 0:5cd703c0576c 170 for(int j = start; j < finish; i++) {
jlsmith10 0:5cd703c0576c 171 total += signal[j];
jlsmith10 0:5cd703c0576c 172 }
jlsmith10 0:5cd703c0576c 173
jlsmith10 0:5cd703c0576c 174 *averagedSignal[i] = total / (finish - start);
jlsmith10 0:5cd703c0576c 175 }
jlsmith10 0:5cd703c0576c 176 }
jlsmith10 0:5cd703c0576c 177
jlsmith10 0:5cd703c0576c 178
jlsmith10 0:5cd703c0576c 179 /*
jlsmith10 0:5cd703c0576c 180 * Function: singleThresholdDetection()
jlsmith10 0:5cd703c0576c 181 * Description: calculates the first occurrence of a value over the threshold
jlsmith10 0:5cd703c0576c 182 * value and returns it.
jlsmith10 0:5cd703c0576c 183 *
jlsmith10 0:5cd703c0576c 184 * Params:
jlsmith10 0:5cd703c0576c 185 * arg1: sampleData - sample data to find position of first peak
jlsmith10 0:5cd703c0576c 186 * arg2: startPosition - threshold value to search for
jlsmith10 0:5cd703c0576c 187 *
jlsmith10 0:5cd703c0576c 188 * Return Value: position where threshold is first reached
jlsmith10 0:5cd703c0576c 189 *
jlsmith10 0:5cd703c0576c 190 */
jlsmith10 0:5cd703c0576c 191 int singleThresholdDetection(const float sampleData[NUM_SAMPLES], int startPosition)
jlsmith10 0:5cd703c0576c 192 {
jlsmith10 0:5cd703c0576c 193 int i;
jlsmith10 0:5cd703c0576c 194
jlsmith10 0:5cd703c0576c 195 for(i = startPosition; i < NUM_SAMPLES; i++) {
jlsmith10 0:5cd703c0576c 196 if (sampleData[i] >= DETECT_THRESH) {
jlsmith10 0:5cd703c0576c 197 return i;
jlsmith10 0:5cd703c0576c 198 }
jlsmith10 0:5cd703c0576c 199 }
jlsmith10 0:5cd703c0576c 200
jlsmith10 0:5cd703c0576c 201 return -1;
jlsmith10 0:5cd703c0576c 202 }
jlsmith10 0:5cd703c0576c 203
jlsmith10 0:5cd703c0576c 204 /*
jlsmith10 0:5cd703c0576c 205 * Function: getPositionFromSignal()
jlsmith10 0:5cd703c0576c 206 * Description: TODO
jlsmith10 0:5cd703c0576c 207 *
jlsmith10 0:5cd703c0576c 208 * Params: TODO
jlsmith10 0:5cd703c0576c 209 *
jlsmith10 0:5cd703c0576c 210 *
jlsmith10 0:5cd703c0576c 211 * Return Value: none
jlsmith10 0:5cd703c0576c 212 *
jlsmith10 0:5cd703c0576c 213 */
jlsmith10 0:5cd703c0576c 214 /*
jlsmith10 0:5cd703c0576c 215 void getPositionFromSignal(
jlsmith10 0:5cd703c0576c 216 float signal[NUM_SAMPLES], coord* projCoord,
jlsmith10 0:5cd703c0576c 217 float velocity, float depth,
jlsmith10 0:5cd703c0576c 218 float xa, float ya, float za,
jlsmith10 0:5cd703c0576c 219 float xb, float yb, float zb,
jlsmith10 0:5cd703c0576c 220 float xc, float yc, float zc)
jlsmith10 0:5cd703c0576c 221 {
jlsmith10 0:5cd703c0576c 222
jlsmith10 0:5cd703c0576c 223 //TODO uncomment the following lines of code once filtering is working
jlsmith10 0:5cd703c0576c 224 //TODO can be tuned
jlsmith10 0:5cd703c0576c 225 //int smoothingCoeff = 20;
jlsmith10 0:5cd703c0576c 226
jlsmith10 0:5cd703c0576c 227 //float extracted1[NUM_SAMPLES];
jlsmith10 0:5cd703c0576c 228 //float extracted2[NUM_SAMPLES];
jlsmith10 0:5cd703c0576c 229 //float extracted3[NUM_SAMPLES];
jlsmith10 0:5cd703c0576c 230
jlsmith10 0:5cd703c0576c 231
jlsmith10 0:5cd703c0576c 232 //filterAndSmooth(signal, filter1Coeffs, smoothingCoeff, &extracted1);
jlsmith10 0:5cd703c0576c 233 //filterAndSmooth(signal, filter2Coeffs, smoothingCoeff, &extracted2);
jlsmith10 0:5cd703c0576c 234 //filterAndSmooth(signal, filter3Coeffs, smoothingCoeff, &extracted3);
jlsmith10 0:5cd703c0576c 235
jlsmith10 0:5cd703c0576c 236 //int tsa = singleThresholdDetection(extracted1, 0);
jlsmith10 0:5cd703c0576c 237 //int tsb = singleThresholdDetection(extracted2, 0);
jlsmith10 0:5cd703c0576c 238 //int tsc = singleThresholdDetection(extracted3, 0);
jlsmith10 0:5cd703c0576c 239
jlsmith10 0:5cd703c0576c 240
jlsmith10 0:5cd703c0576c 241 int tsa = singleThresholdDetection(signalA, 0);
jlsmith10 0:5cd703c0576c 242 int tsb = singleThresholdDetection(signalB, 0);
jlsmith10 0:5cd703c0576c 243 int tsc = singleThresholdDetection(signalC, 0);
jlsmith10 0:5cd703c0576c 244
jlsmith10 0:5cd703c0576c 245 if(tsa != -1 )
jlsmith10 0:5cd703c0576c 246
jlsmith10 0:5cd703c0576c 247 calculateProjectedCoordinates(projCoord, depth, velocity, tsa, tsb, tsc,
jlsmith10 0:5cd703c0576c 248 xa, ya, za,
jlsmith10 0:5cd703c0576c 249 xb, yb, zb,
jlsmith10 0:5cd703c0576c 250 xc, yc, zc);
jlsmith10 0:5cd703c0576c 251 }
jlsmith10 0:5cd703c0576c 252 */
jlsmith10 0:5cd703c0576c 253
jlsmith10 0:5cd703c0576c 254 void filterAndSmooth(float signal[NUM_SAMPLES], float filterCoeffs[NUM_TAPS], int smoothingCoeff, float * filteredSignal)
jlsmith10 0:5cd703c0576c 255 {
jlsmith10 0:5cd703c0576c 256 //Low pass filter
jlsmith10 0:5cd703c0576c 257 //arm_fir_instance_f32 * firInstance;
jlsmith10 0:5cd703c0576c 258
jlsmith10 0:5cd703c0576c 259 //arm_fir_init_f32(firInstance, NUM_TAPS,
jlsmith10 0:5cd703c0576c 260 //Moving average
jlsmith10 0:5cd703c0576c 261
jlsmith10 0:5cd703c0576c 262 //return signal
jlsmith10 0:5cd703c0576c 263
jlsmith10 0:5cd703c0576c 264 }
jlsmith10 0:5cd703c0576c 265
jlsmith10 0:5cd703c0576c 266
jlsmith10 0:5cd703c0576c 267 /*
jlsmith10 0:5cd703c0576c 268 * Function: calculateProjectedCoordinates()
jlsmith10 0:5cd703c0576c 269 * Description: TODO
jlsmith10 0:5cd703c0576c 270 *
jlsmith10 0:5cd703c0576c 271 * Params: TODO
jlsmith10 0:5cd703c0576c 272 *
jlsmith10 0:5cd703c0576c 273 *
jlsmith10 0:5cd703c0576c 274 * Return Value: none
jlsmith10 0:5cd703c0576c 275 *
jlsmith10 0:5cd703c0576c 276 */
jlsmith10 0:5cd703c0576c 277 void calculateProjectedCoordinates(
jlsmith10 0:5cd703c0576c 278 coord * projCoord,
jlsmith10 0:5cd703c0576c 279 float depth, float velocity,
jlsmith10 0:5cd703c0576c 280 float tsa, float tsb, float tsc,
jlsmith10 0:5cd703c0576c 281 float xa, float ya, float za,
jlsmith10 0:5cd703c0576c 282 float xb, float yb, float zb,
jlsmith10 0:5cd703c0576c 283 float xc, float yc, float zc)
jlsmith10 0:5cd703c0576c 284 {
jlsmith10 0:5cd703c0576c 285
jlsmith10 0:5cd703c0576c 286 float dsa = velocity * tsa;
jlsmith10 0:5cd703c0576c 287 float dsb = velocity * tsb;
jlsmith10 0:5cd703c0576c 288 float dsc = velocity * tsc;
jlsmith10 0:5cd703c0576c 289
jlsmith10 0:5cd703c0576c 290 float dsaProjected = sqrt(dsa * dsa - depth * depth);
jlsmith10 0:5cd703c0576c 291 float dsbProjected = sqrt(dsb * dsb - depth * depth);
jlsmith10 0:5cd703c0576c 292 float dscProjected = sqrt(dsc * dsc - depth * depth);
jlsmith10 0:5cd703c0576c 293
jlsmith10 0:5cd703c0576c 294 float xCoord = (dsaProjected * dsaProjected - dsbProjected * dsbProjected + xb * xb)/(2 * xb);
jlsmith10 0:5cd703c0576c 295 float yCoord = ((dsaProjected * dsaProjected - dscProjected * dscProjected + xc * xc + yc * yc)/(2 * yc)) - ((xc) / (yc)) * xCoord;
jlsmith10 0:5cd703c0576c 296
jlsmith10 0:5cd703c0576c 297 projCoord->x = xCoord;
jlsmith10 0:5cd703c0576c 298 projCoord->y = yCoord;
jlsmith10 0:5cd703c0576c 299 }