James Smith
FIR Filter
Dependencies: CMSIS_DSP_401 mbed
Diff: main.cpp
- Revision:
- 1:a2238b2df415
- Parent:
- 0:5cd703c0576c
--- a/main.cpp Sat May 28 21:26:55 2016 +0000
+++ b/main.cpp Sun May 29 20:16:11 2016 +0000
@@ -1,185 +1,264 @@
+/*******************************************************************************
+ * main.cpp
+ * by Jason Shen, James Smith and Luis Sanchez
+ *
+ * Acoustic Location System Filtering
+ *
+ * This C module implements the DSP filtering of an input signal for the
+ * STM33F446RE Nucleo board.. The input signal will be passed in through
+ * on board ADC connected to a transponder that receives accoustic waves
+ * delivered by three transponders at 3 different frequencies. The coeffecients
+ * used for the filter are defined in main.h and implement bandpass filters for
+ * 35kHz, 40kHz and 45kHz with a pass bandwith of 500Hz.
+ *
+ * This program will output the time difference of arrival of the three
+ * varying frequency signals. the default for for output is:
+ *
+ * [ A vs B time difference, B vs C time difference, A vs C time difference ]
+ *
+ * Currently the algorithm will first FIR filter the data for each of the 3
+ * frequencies. Then it will use an in place moving average function to
+ * smooth the data. Finally, it will run a peak detection function and find the
+ * peak of each of the 3 filtered signals which will then be placed on an output
+ * pin to be available for beagle bone on OpenROV.
+ *
+ *
+ *
+ *
+ * TODO:
+ * 1. Test on different location inputs from python script,
+ * 2. Integrate ADC and filter code
+ * 3. If current filtering and smoothing is innaccurate develop new algorithm
+ * 4. Clean up code
+*******************************************************************************/
+
/* Includes */
-#include <stdio.h> //fprintf, FILE
-#include <stdlib.h> //
-#include <math.h> //sqrt
-
-#include "mbed.h"
#include "main.h"
-//#include "arm_math.h"
-//#include "math_helper.h"
-#include "float.h"
+
-//#include "tiny_printf.c"
-
-#define BLOCK_SIZE (32)
-
+/* FIR filter variables */
static float firStateF[BLOCK_SIZE + NUM_TAPS - 1];
-
unsigned int blockSize = BLOCK_SIZE;
unsigned int numBlocks = NUM_SAMPLES/BLOCK_SIZE;
-/* Private macro */
-
-
-/* Private variables */
-//float signalA[NUM_SAMPLES];
-//float signalB[NUM_SAMPLES];
-//float signalC[NUM_SAMPLES];
-
-FILE* logFile;
-
-typedef struct coordinate {
- float x;
- float y;
-} coord;
-
/* Function Prototypes */
float calculateSoundSpeedInWater(float temp, float salinity, float depth);
-void createSignalsFromFile(void);
-
-void movingAverage(float signal[NUM_SAMPLES], float * averagedSignal[NUM_SAMPLES]);
-
-int singleThresholdDetection(const float sampleData[NUM_SAMPLES], int startPosition);
+int singleThresholdDetection(const float sampleData[], int startPosition);
-void getPositionFromSignal(
- float signal[NUM_SAMPLES], coord* projCoord,
- float velocity, float depth,
- float xa, float ya, float za,
- float xb, float yb, float zb,
- float xc, float yc, float zc);
+void filterAndSmooth(float signal[], float filterCoeffs[], int smoothingCoeff, float * filteredSignal);
-void filterAndSmooth(float signal[NUM_SAMPLES], float filterCoeffs[NUM_TAPS], int smoothingCoeff, float * filteredSignal);
+void movAvg(float signal[]);
-void calculateProjectedCoordinates(
- coord * projCoord,
- float depth, float velocity,
- float tsa, float tsb, float tsc,
- float xa, float ya, float za,
- float xb, float yb, float zb,
- float xc, float yc, float zc);
-
+int findMax(float signal[]);
-//------------------------------------
-// Hyperterminal configuration
-// 9600 bauds, 8-bit data, no parity
-//------------------------------------
-
+/*------------------------------------
+ * Hyperterminal configuration
+ * 9600 bauds, 8-bit data, no parity
+ ************************************/
Serial pc(SERIAL_TX, SERIAL_RX);
-DigitalOut myled(LED1);
+DigitalOut myled(LED1); //Debug LED
int main(void)
{
- int i = 0;
-
- /* TODO - Add your application code here */
- //FILE* fp = fopen("log.txt", "w");
- //if(fp == 0)
- // pc.printf("NULL\n");
- //fclose(fp);
-
- //createSignalsFromFile();
-
+ printf("---------------------------------------------------------------- ");
+ printf(" ");
+ printf(" Underwater Acoustic Location System DSP\n");
+ printf(" ");
+ printf("---------------------------------------------------------------- ");
+ printf(" Running Filter... ");
+ printf("---------------------------------------------------------------- ");
+ printf(" Number of Samples: %d\n", NUM_SAMPLES);
+ printf(" Moving Average window: %d samples\n", MOV_AVG_WIND);
+
/*
- float temp = TEMP;
- float depth = DEPTH;
- float salinity = SALINITY;
-
float velocity = calculateSoundSpeedInWater(temp, salinity, depth);
-
pc.printf("The velocity is %lf", velocity);
-
-
- int firstPeak = singleThresholdDetection(signalA, 0);
-
- pc.printf("First peak found at position %d\n", firstPeak);
- pc.printf("Time of first peak (in ms): %lf", firstPeak / 250.0);
*/
-
- arm_fir_instance_f32 S;
+
+ // Fir instance variables
+ arm_fir_instance_f32 S1, S2, S3;
arm_status status;
float32_t *input, *output;
+ // Fir Coefficients
float coeffsA_f32[NUM_TAPS];
+ float output_f32[NUM_SAMPLES];
+ /* Input is the signal with all 3 frequencies present. */
input = &signalABC[0];
- /*
- for(i = 0; i < NUM_TAPS; i++) {
- coeffsA_f32[i] = (float) coeffsA[i];
+ output = &output_f32[0];
+
+ /* The array of time differences to place on output pin */
+ int toReturn[3];
+
+ /***************************************************************************
+ ****************** Filtering for f = 35kHz (from buoy A) *******************
+ ***************************************************************************/
+ if(UALS_DEBUG) {
+ printf("Beginning filtering for f = 35kHz (buoy A)\n");
}
- */
+
+ /* Initialize FIR instance structure */
+ arm_fir_init_f32(&S1, NUM_TAPS, (float *) &coeffsA[0],
+ &firStateF[0], blockSize);
- /* Initialize FIR instance structure */
- arm_fir_init_f32(&S, NUM_TAPS, (float *) &coeffsA_f32[0], &firStateF[0], blockSize);
+ int i = 0;
/* FIR Filtering */
for( i = 0; i < numBlocks; i++) {
- arm_fir_f32(&S, input + (i * blockSize), output + (i * blockSize), blockSize);
+ arm_fir_f32(&S1, input + (i * blockSize),
+ output + (i * blockSize), blockSize);
}
/* Take the absolute value of the filtered signal */
for(i = 0; i < NUM_SAMPLES; i++ ) {
- //printf("Index: %d, Amplitude: %e\n", i, output[i]);
if(output[i] < 0) {
- //printf("Negative sample!\n");
+ output[i] = -1 * output[i];
+ }
+ }
+
+ /* Print before moving average */
+ printf("----------Before moving average-------------\n");
+ if(UALS_DEBUG) {
+ for(i = 0; i < NUM_SAMPLES; i++) {
+ printf("%lf\n", output[i]);
+ }
+ }
+
+ float * avgSignal[NUM_SAMPLES];
+ // Calculate moving average and do peak detection;
+ movAvg(output);
+
+ /* Print signal after moving average */
+ printf("----------After moving average-------------\n");
+ if(UALS_DEBUG) {
+ for(i = 0; i < NUM_SAMPLES; i++) {
+ printf("%lf\n", output[i]);
+ }
+ }
+
+ /* Find the maximum value of the filtered signal */
+ int maxA = findMax(output);
+
+ /***************************************************************************
+ ********************* Filtering for f = 40kHz (from buoy B) ****************
+ ***************************************************************************/
+ if(UALS_DEBUG) {
+ printf("Beginning filtering for f = 40kHz (buoy B)\n");
+ }
+ /* Initialize FIR instance structure */
+ arm_fir_init_f32(&S2, NUM_TAPS, (float *) &coeffsB[0],
+ &firStateF[0], blockSize);
+
+ /* FIR Filtering */
+ for( i = 0; i < numBlocks; i++) {
+ arm_fir_f32(&S2, input + (i * blockSize),
+ output + (i * blockSize), blockSize);
+ }
+
+ /* Take the absolute value of the filtered signal */
+ for(i = 0; i < NUM_SAMPLES; i++ ) {
+ if(output[i] < 0) {
output[i] = -1 * output[i];
}
}
- float * avgSignal[NUM_SAMPLES];
+ /* Print before moving average */
+ if(UALS_DEBUG) {
+ printf("----------Before moving average-------------\n");
+ for(i = 0; i < NUM_SAMPLES; i++) {
+ printf("%lf\n", output[i]);
+ }
+ }
+
+
+ /* Perform moving average */
+ movAvg(output);
+
+ /* Print signal after moving average */
+ if(UALS_DEBUG) {
+ printf("----------After moving average-------------\n");
+ for(i = 0; i < NUM_SAMPLES; i++) {
+ printf("%lf\n", output[i]);
+ }
+ }
- //movingAverage(output, avgSignal);
+ /* Find the maximum value of the filtered signal */
+ int maxB = findMax(output);
+
+
+ /***************************************************************************
+ ******************** Filtering for f = 45kHz (from buoy C) *****************
+ ***************************************************************************/
+ if(UALS_DEBUG) {
+ printf("Beginning filtering for f = 45kHz (buoy C)\n");
+ }
+ /* Initialize FIR instance structure */
+ arm_fir_init_f32(&S3, NUM_TAPS, (float *) &coeffsC[0],
+ &firStateF[0], blockSize);
- q15_t max = 0;
+ /* FIR Filtering */
+ for( i = 0; i < numBlocks; i++) {
+ arm_fir_f32(&S3, input + (i * blockSize),
+ output + (i * blockSize), blockSize);
+ }
- for(i = 0; i < NUM_SAMPLES; i++) {
- printf("%lf\n", output[i]);
+ /* Take the absolute value of the filtered signal */
+ for(i = 0; i < NUM_SAMPLES; i++ ) {
+ if(output[i] < 0) {
+ output[i] = -1 * output[i];
+ }
+ }
+
+ /* Print before moving average */
+ if(UALS_DEBUG) {
+ printf("----------Before moving average-------------\n");
+ for(i = 0; i < NUM_SAMPLES; i++) {
+ printf("%lf\n", output[i]);
+ }
}
- //printf("Max is at %d\n", max);
+
- /*
- for(i = 0; i < 20; i++) {
- printf("%d\n", coeffsA[i]);
+ /* Perform moving average */
+ movAvg(output);
+
+ /* Print signal after moving average */
+ if(UALS_DEBUG) {
+ printf("----------After moving average-------------\n");
+ for(i = 0; i < NUM_SAMPLES; i++) {
+ printf("%lf\n", output[i]);
+ }
}
- */
+
+ /* Find the maximum value of the filtered signal */
+ int maxC = findMax(output);
+
+ /* The time differences to be returned */
+ /* TODO: should we take the absolute value? What does TDOA take in */
+ float tdAB = maxB - maxA;
+ float tdBC = maxB - maxC;
+ float tdAC = maxC - maxA;
+
+ printf("tdAB = %f\n", tdAB);
+ printf("tdBC = %f\n", tdBC);
+ printf("tdAC = %f\n", tdAC);
+
+ toReturn[0] = tdAB;
+ toReturn[1] = tdBC;
+ toReturn[2] = tdAC;
}
-void movingAverage(float signal[NUM_SAMPLES], float ** averagedSignal) {
- int i = 0;
-
- int start = 0;
- int finish = 0;
- float total = 0;
-
- for(i = 0; i < NUM_SAMPLES; i++) {
- start = i - MOV_AVG_WIND;
- finish = i + MOV_AVG_WIND;
-
- if(start < 0) {
- start = 0;
- }
- if(finish > NUM_SAMPLES) {
- finish = NUM_SAMPLES;
- }
-
- total = 0;
- for(int j = start; j < finish; i++) {
- total += signal[j];
- }
-
- *averagedSignal[i] = total / (finish - start);
- }
-}
-
-
/*
- * Function: singleThresholdDetection()
- * Description: calculates the first occurrence of a value over the threshold
- * value and returns it.
+ * Function Name: singleThresholdDetection()
+ * Function Declaration: int singleThresholdDetection(const float sampleData[],
+ int startPosition)
+ * Function Description: Calculates the first occurrence of a value over the
+ * threshold value and returns the position.
*
* Params:
* arg1: sampleData - sample data to find position of first peak
@@ -188,10 +267,10 @@
* Return Value: position where threshold is first reached
*
*/
-int singleThresholdDetection(const float sampleData[NUM_SAMPLES], int startPosition)
+int singleThresholdDetection(const float sampleData[], int startPosition)
{
int i;
-
+
for(i = startPosition; i < NUM_SAMPLES; i++) {
if (sampleData[i] >= DETECT_THRESH) {
return i;
@@ -201,99 +280,95 @@
return -1;
}
-/*
- * Function: getPositionFromSignal()
- * Description: TODO
+
+/*
+ * Function Name: movAvg();
+ * Function Declaration: void movAvg(float signal[]);
+ * Function Description: This function takes an input signal and does an in
+ * place moving average algorithm. The window size is defined by MOV_AVG_WIND
+ * in main.h.
*
- * Params: TODO
- *
- *
- * Return Value: none
+ * This function serves a dual purpose of finding the position of the largest
+ * peak and returning that position.
+ *
+ * Return Value: None
*
*/
- /*
-void getPositionFromSignal(
- float signal[NUM_SAMPLES], coord* projCoord,
- float velocity, float depth,
- float xa, float ya, float za,
- float xb, float yb, float zb,
- float xc, float yc, float zc)
-{
-
- //TODO uncomment the following lines of code once filtering is working
- //TODO can be tuned
- //int smoothingCoeff = 20;
-
- //float extracted1[NUM_SAMPLES];
- //float extracted2[NUM_SAMPLES];
- //float extracted3[NUM_SAMPLES];
-
-
- //filterAndSmooth(signal, filter1Coeffs, smoothingCoeff, &extracted1);
- //filterAndSmooth(signal, filter2Coeffs, smoothingCoeff, &extracted2);
- //filterAndSmooth(signal, filter3Coeffs, smoothingCoeff, &extracted3);
+void movAvg(float signal[]) {
+ int i = 0;
+
+ int start = 0;
+ int finish = 0;
+ float total = 0;
+
+ // Buffer to hold the most recent samples
+ float buffer[MOV_AVG_WIND*2];
+
+ // Go through signal array and calculate moving average
+ for(i = 0; i < NUM_SAMPLES; i++) {
+ start = i - MOV_AVG_WIND;
+ finish = i + MOV_AVG_WIND;
+
+ if(start < 0) {
+ start = 0;
+ }
- //int tsa = singleThresholdDetection(extracted1, 0);
- //int tsb = singleThresholdDetection(extracted2, 0);
- //int tsc = singleThresholdDetection(extracted3, 0);
-
-
- int tsa = singleThresholdDetection(signalA, 0);
- int tsb = singleThresholdDetection(signalB, 0);
- int tsc = singleThresholdDetection(signalC, 0);
-
- if(tsa != -1 )
+ if(finish > NUM_SAMPLES) {
+ finish = NUM_SAMPLES;
+ }
+
+ total = 0;
+ for(int j = start; j < finish; j++) {
+ total += signal[j];
+ }
+
+ if(i > MOV_AVG_WIND*2 - 1) {
+ signal[i-(MOV_AVG_WIND*2)] = buffer[i % (MOV_AVG_WIND*2)];
+ }
- calculateProjectedCoordinates(projCoord, depth, velocity, tsa, tsb, tsc,
- xa, ya, za,
- xb, yb, zb,
- xc, yc, zc);
-}
-*/
-
-void filterAndSmooth(float signal[NUM_SAMPLES], float filterCoeffs[NUM_TAPS], int smoothingCoeff, float * filteredSignal)
-{
- //Low pass filter
- //arm_fir_instance_f32 * firInstance;
-
- //arm_fir_init_f32(firInstance, NUM_TAPS,
- //Moving average
-
- //return signal
-
+ // Rotating buffer to save the avg
+ buffer[i%(MOV_AVG_WIND*2)] = total / (finish - start);
+
+ }
+ for(int i = NUM_SAMPLES-MOV_AVG_WIND*2; i < NUM_SAMPLES; i++) {
+ signal[i] = buffer[i %(MOV_AVG_WIND*2)];
+ }
+
+ if(UALS_DEBUG) {
+ for(int i = 0; i < NUM_SAMPLES; i++) {
+ printf("averagedSignal[i] = %f\n", signal[i]);
+ }
+ }
}
-/*
- * Function: calculateProjectedCoordinates()
- * Description: TODO
- *
- * Params: TODO
- *
- *
- * Return Value: none
+/*
+ * Function Name: findMax();
+ * Function Declaration: int findMax(float signal[]);
+ * Function Description: This function takes an input signal and returns the
+ * position of the maximum valued sample.
*
*/
-void calculateProjectedCoordinates(
- coord * projCoord,
- float depth, float velocity,
- float tsa, float tsb, float tsc,
- float xa, float ya, float za,
- float xb, float yb, float zb,
- float xc, float yc, float zc)
-{
+int findMax(float signal[]) {
+ float maxVal = 0.0;
+ int maxPosition = -1;
+ int i;
- float dsa = velocity * tsa;
- float dsb = velocity * tsb;
- float dsc = velocity * tsc;
+ /* Go through each element searching for maximum */
+ for(i = 0; i < NUM_SAMPLES; i++) {
+ if(signal[i] > maxVal) {
+ /* We've found a new max, replace */
+ maxVal = signal[i];
+ maxPosition = i;
+ }
+ }
- float dsaProjected = sqrt(dsa * dsa - depth * depth);
- float dsbProjected = sqrt(dsb * dsb - depth * depth);
- float dscProjected = sqrt(dsc * dsc - depth * depth);
-
- float xCoord = (dsaProjected * dsaProjected - dsbProjected * dsbProjected + xb * xb)/(2 * xb);
- float yCoord = ((dsaProjected * dsaProjected - dscProjected * dscProjected + xc * xc + yc * yc)/(2 * yc)) - ((xc) / (yc)) * xCoord;
-
- projCoord->x = xCoord;
- projCoord->y = yCoord;
+ if(maxPosition > 0) {
+ printf("Max position found at %d\n", maxPosition);
+ return maxPosition;
+ }
+ else {
+ printf("Error: no max found\n");
+ return -1;
+ }
}