Diff: hydro_dsp.cpp

Revision:

0:2381a319fc35

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/hydro_dsp.cpp	Fri Aug 02 02:23:36 2013 +0000
@@ -0,0 +1,48 @@
+#include "hydro_dsp.h"
+
+arm_iir_lattice_instance_q15 iir_filter[NUM_CHANNELS];
+
+// TODO: need an A and B for each possible frequency
+// our pinger: 27k
+// dave's pinger(s): TODO: ask about this
+q15_t A_27k0[FILTER_ORDER] = {};
+q15_t B_27k0[FILTER_ORDER+1] = {};
+
+// Set these variables to select which set of coefficients (taps) we use
+q15_t* A_select = A_27k0;
+q15_t* B_select = B_27k0;
+
+q15_t state_arr[NUM_CHANNELS][FILTER_ORDER + BLOCK_SIZE];
+
+q15_t filtered[BLOCK_SIZE];
+
+void setup_dsp()
+{
+    memset(state_arr, 0, 2*NUM_CHANNELS*(FILTER_ORDER + BLOCK_SIZE));
+    for (int i = 0; i < NUM_CHANNELS; i++) {
+        arm_iir_lattice_init_q15(iir_filter+i, FILTER_ORDER, A_27k0, B_27k0, state_arr[i], BLOCK_SIZE);
+    }
+}
+
+// Figure out where the wave starts in the filtered output
+// TODO: does this work?
+// Returns the index of the first "high" in the signal or -1 if there is none
+int find_timestamp_tdoa(q15_t* filtered)
+{
+    // Uncomment the num_hits stuff if our filtering sucks
+    //char num_hits = 0;
+    for (int j = 0; j < BLOCK_SIZE; j++)
+    {
+        if (filtered[j] > TDOA_THRESH || filtered[j] < -1 * TDOA_THRESH)
+        {
+            return j;
+            /*num_hits++;
+            if (num_hits > TDOA_MIN_NUM_HITS)
+            {
+                return j;            
+            }*/
+        }
+    }
+    
+    return -1;
+}