Revision 3:a85b742be262, committed 2015-10-31

Comitter:

baxterja

Date:

Sat Oct 31 20:06:37 2015 +0000

Parent:

2:3771b3195c7b

Child:

4:9fd291254686

Commit message:

Sample Should be accessible, also most of the filter code is done.

Changed in this revision

--- a/Sample/dma.cpp	Thu Oct 29 17:15:20 2015 +0000
+++ b/Sample/dma.cpp	Sat Oct 31 20:06:37 2015 +0000
@@ -13,7 +13,7 @@
 int len = TOTAL_SAMPLES;
 uint16_t sample_array0[TOTAL_SAMPLES];
 uint16_t sample_array1[TOTAL_SAMPLES];
-uint16_t angle_array[TOTAL_SAMPLES+FILENAME_SIZE];
+uint16_t angle_array[TOTAL_SAMPLES+FILENAME_SIZE];//Change this to DAC Values
 bool dma_done = false;
 bool dma_half_done = false;
 
@@ -62,6 +62,8 @@
     DMA_TCD0_DOFF = 0x02; // Destination address offset of 1 bit per transaction
     DMA_TCD1_SOFF = 0x00; // Source address offset of 2 bits per transaction
     DMA_TCD1_DOFF = 0x02; // Destination address offset of 1 bit per transaction
+    
+    //DAC DMA Chang soff to 2, and DOFF to 0
     DMA_TCD2_SOFF = 0x00; // Source address offset of 2 bits per transaction
     DMA_TCD2_DOFF = 0x02; // Destination address offset of 1 bit per transaction
         
@@ -90,6 +92,8 @@
     DMA_TCD0_DLASTSGA = -len*2;  // Destination address adjustment
     DMA_TCD1_SLAST = 0;      // Source address adjustment
     DMA_TCD1_DLASTSGA = -len*2;  // Destination address adjustment
+    
+    //Change source and destination
     DMA_TCD2_SLAST = 0;      // Source address adjustment
     DMA_TCD2_DLASTSGA = -len*2;  // Destination address adjustment
     

--- a/SignalProcessing.cpp	Thu Oct 29 17:15:20 2015 +0000
+++ b/SignalProcessing.cpp	Sat Oct 31 20:06:37 2015 +0000
@@ -1,9 +1,13 @@
-
+#include "mbed.h"
+#include "SignalProcessing.h"
 
-#define pre_compute_length 4000
+#define pre_compute_length 2000
 #define DMA_PERIOD .00001
 #define DMA_FREQUENCY 100000
 #define CARRIERFREQUENCY 10000
+
+uint16_t phase_counter = 0;
+
 float i_mod_pre[pre_compute_length];
 float q_mod_pre[pre_compute_length];
 int out_val_pre[pre_compute_length]; 
@@ -11,13 +15,451 @@
 float twopi = 3.14159265359 * 2;
 
 void pre_compute_tables() {
-  /*
+  
   // This function will precompute the cos and sin tables used in the rest of the program
   for(int precompute_counter = 0; precompute_counter < pre_compute_length; precompute_counter++){
-    out_val_pre[precompute_counter] = (int) (cos(twopi * CarrierFrequency * DMA_PERIOD * precompute_counter) * 4965.0 + 49650.0);
-    i_mod_pre[precompute_counter] = (cos(twopi * CarrierFrequency * TimerInterruptInMicroSeconds * 1e-6 * precompute_counter));
-    q_mod_pre[precompute_counter] = (-sin(twopi * CarrierFrequency * TimerInterruptInMicroSeconds * 1e-6 * precompute_counter));
+    out_val_pre[precompute_counter] = (int) (cos(twopi * CARRIERFREQUENCY * DMA_PERIOD * precompute_counter) * 4965.0 + 49650.0);
+    i_mod_pre[precompute_counter] = (cos(twopi * CARRIERFREQUENCY * DMA_PERIOD * precompute_counter));
+    q_mod_pre[precompute_counter] = (-sin(twopi * CARRIERFREQUENCY * DMA_PERIOD * precompute_counter));
     
   }
-  */
-}
\ No newline at end of file
+}
+
+
+
+#define FIR_100_LENGTH 64
+float FIR100_Sample1_i[FIR_100_LENGTH];
+float FIR100_Sample1_q[FIR_100_LENGTH];
+float FIR100_Sample2_i[FIR_100_LENGTH];
+float FIR100_Sample2_q[FIR_100_LENGTH];
+uint8_t FIR100_Position = 0;
+//Fs = 100, Order = 63, Fpass = 1, Fstop = 5
+static float lp_100_coeff[FIR_100_LENGTH] = {
+-0.00122570885798289,
+-0.00188999637047550,
+-0.00153249020825747,
+-0.00274386935197498,
+-0.00291939338925560,
+-0.00386012904484287,
+-0.00422028369122005,
+-0.00487674304247136,
+-0.00511534666379814,
+-0.00534548809850574,
+-0.00517201272363819,
+-0.00477057905668482,
+-0.00391668442164137,
+-0.00268115305854952,
+-0.000941210196010094,
+0.00126714884107596,
+0.00399477044758532,
+0.00719945087788976,
+0.0108724813712926,
+0.0149475081564352,
+0.0193653158514034,
+0.0240295387918664,
+0.0288389643323217,
+0.0336754471496198,
+0.0384141305817155,
+0.0429241300038060,
+0.0470794715470055,
+0.0507566277450335,
+0.0538451475324508,
+0.0562499862639974,
+0.0578974160014292,
+0.0587336099426047,
+0.0587336099426047,
+0.0578974160014292,
+0.0562499862639974,
+0.0538451475324508,
+0.0507566277450335,
+0.0470794715470055,
+0.0429241300038060,
+0.0384141305817155,
+0.0336754471496198,
+0.0288389643323217,
+0.0240295387918664,
+0.0193653158514034,
+0.0149475081564352,
+0.0108724813712926,
+0.00719945087788976,
+0.00399477044758532,
+0.00126714884107596,
+-0.000941210196010094,
+-0.00268115305854952,
+-0.00391668442164137,
+-0.00477057905668482,
+-0.00517201272363819,
+-0.00534548809850574,
+-0.00511534666379814,
+-0.00487674304247136,
+-0.00422028369122005,
+-0.00386012904484287,
+-0.00291939338925560,
+-0.00274386935197498,
+-0.00153249020825747,
+-0.00188999637047550,
+-0.00122570885798289
+};
+#define NUMSAMPLESAVERAGE 100
+#define DecimationFactor_10K 10
+void filter100(float FIR100_Sample1_i_input, float FIR100_Sample1_q_input, float FIR100_Sample2_i_input, float FIR100_Sample2_q_input)
+{
+    static uint8_t finalAverageCounter = 0;
+    static uint8_t decimationCounter = 0;//used to keep track of how many samples you have currently decimated
+    static float FIR100_Sample1_i_DecimatedSum=0;
+    static float FIR100_Sample1_q_DecimatedSum=0;//when decimating sum up all 10 samples at a time have that be your output value
+    static float FIR100_Sample2_i_DecimatedSum=0;
+    static float FIR100_Sample2_q_DecimatedSum=0;
+    
+    static float Final_Average1_i=0;
+    static float Final_Average1_q=0;//when decimating sum up all 10 samples at a time have that be your output value
+    static float Final_Average2_i=0;
+    static float Final_Average2_q=0;
+    
+    FIR100_Sample1_i_DecimatedSum += FIR100_Sample1_i_input;//add sample to the sum of previous sample
+    FIR100_Sample1_q_DecimatedSum += FIR100_Sample1_q_input;
+    FIR100_Sample2_i_DecimatedSum += FIR100_Sample2_i_input;
+    FIR100_Sample2_q_DecimatedSum += FIR100_Sample2_q_input;
+    decimationCounter++;
+    if (decimationCounter >= DecimationFactor_10K)//once 10 samples have com
+    {
+        decimationCounter = 0;//reset decimation counter
+        //add sample to 10K filter
+        FIR100_Sample1_i[FIR100_Position] = FIR100_Sample1_i_DecimatedSum;
+        FIR100_Sample1_q[FIR100_Position] = FIR100_Sample1_q_DecimatedSum;
+        FIR100_Sample2_i[FIR100_Position] = FIR100_Sample2_i_DecimatedSum;
+        FIR100_Sample2_q[FIR100_Position] = FIR100_Sample2_q_DecimatedSum;
+        
+        FIR100_Sample1_i_DecimatedSum = 0;//reset decimated sum to 0 for next sample
+        FIR100_Sample1_q_DecimatedSum = 0;
+        FIR100_Sample2_i_DecimatedSum = 0;
+        FIR100_Sample2_q_DecimatedSum = 0;
+        
+        FIR100_Position++; //increment circular buffer
+        if (FIR100_Position >= FIR_100_LENGTH) //wrap around
+        {
+            FIR100_Position = 0;
+        }
+        
+        // Low pass filter of demodulated signal
+        float FIR100_Sample1_i_Output = FIR100_Sample1_i[FIR100_Position] * lp_100_coeff[0];//first multiply of convolution
+        float FIR100_Sample1_q_Output = FIR100_Sample1_q[FIR100_Position] * lp_100_coeff[0];
+        float FIR100_Sample2_i_Output = FIR100_Sample2_i[FIR100_Position] * lp_100_coeff[0];
+        float FIR100_Sample2_q_Output = FIR100_Sample2_q[FIR100_Position] * lp_100_coeff[0];
+        int fir_index;
+        for(int fir_counter = 1; fir_counter < FIR_100_LENGTH; fir_counter++)//the rest of the convolution
+        {
+            fir_index = FIR100_Position + fir_counter;
+            if (fir_index >= FIR_100_LENGTH) 
+            {
+                fir_index -= FIR_100_LENGTH;
+            }
+            FIR100_Sample1_i_Output += FIR100_Sample1_i[fir_index] * lp_100_coeff[fir_counter];//convolving
+            FIR100_Sample1_q_Output += FIR100_Sample1_q[fir_index] * lp_100_coeff[fir_counter];
+            FIR100_Sample2_i_Output += FIR100_Sample2_i[fir_index] * lp_100_coeff[fir_counter];
+            FIR100_Sample2_q_Output += FIR100_Sample2_q[fir_index] * lp_100_coeff[fir_counter];
+        }
+        
+        //float mag1 = sqrt(FIR100_Sample1_i_Output*FIR100_Sample1_i_Output+FIR100_Sample1_q_Output*FIR100_Sample1_q_Output);
+        //float mag2 = sqrt(FIR100_Sample2_i_Output*FIR100_Sample2_i_Output+FIR100_Sample2_q_Output*FIR100_Sample2_q_Output);
+        //printf("V1: %f\tV2: %f\n\r",mag1,mag2);  
+        
+        
+        Final_Average1_i+=FIR100_Sample1_i_Output;
+        Final_Average1_q+=FIR100_Sample1_q_Output;//when decimating sum up all 10 samples at a time have that be your output value
+        Final_Average2_i+=FIR100_Sample2_i_Output;
+        Final_Average2_q+=FIR100_Sample2_q_Output;
+        finalAverageCounter++;
+        if (finalAverageCounter>=NUMSAMPLESAVERAGE)
+        {
+            finalAverageCounter=0;
+            float mag1 = sqrt(Final_Average1_i*Final_Average1_i+Final_Average1_q*Final_Average1_q);
+            float mag2 = sqrt(Final_Average2_i*Final_Average2_i+Final_Average2_q*Final_Average2_q);
+            printf("V1: %f\tV2: %f\n\r",mag1,mag2);
+            Final_Average1_i=0;
+            Final_Average1_q=0;//when decimating sum up all 10 samples at a time have that be your output value
+            Final_Average2_i=0;
+            Final_Average2_q=0;
+        }
+        //float mag1 = sqrt(FIR100_Sample1_i_Output*FIR100_Sample1_i_Output+FIR100_Sample1_q_Output*FIR100_Sample1_q_Output);
+        //float mag2 = sqrt(FIR100_Sample2_i_Output*FIR100_Sample2_i_Output+FIR100_Sample2_q_Output*FIR100_Sample2_q_Output);
+        //printf("V1: %f\tV2: %f\n\r",mag1,mag2);  
+        //filter100(FIR100_Sample1_i_Output, FIR100_Sample1_q_Output, FIR100_Sample2_i_Output, FIR100_Sample2_q_Output);
+    
+    }
+}
+
+
+
+
+
+
+#define FIR_1K_LENGTH 32
+float FIR1K_Sample1_i[FIR_1K_LENGTH];
+float FIR1K_Sample1_q[FIR_1K_LENGTH];
+float FIR1K_Sample2_i[FIR_1K_LENGTH];
+float FIR1K_Sample2_q[FIR_1K_LENGTH];
+uint8_t FIR1K_Position = 0;
+//Fs = 1000, Order = 31, Fpass = 1, Fstop = 50
+static float lp_1K_coeff[FIR_1K_LENGTH] = {
+0.0108990071119901,
+0.00826963267129267,
+0.0110961530344968,
+0.0143019800886844,
+0.0178397268153335,
+0.0216326995075556,
+0.0255928087296069,
+0.0296287914936736,
+0.0336287768230528,
+0.0374693714591658,
+0.0410324028349472,
+0.0442148191339877,
+0.0468906440777966,
+0.0489881978583567,
+0.0504243479483288,
+0.0511581882807637,
+0.0511581882807637,
+0.0504243479483288,
+0.0489881978583567,
+0.0468906440777966,
+0.0442148191339877,
+0.0410324028349472,
+0.0374693714591658,
+0.0336287768230528,
+0.0296287914936736,
+0.0255928087296069,
+0.0216326995075556,
+0.0178397268153335,
+0.0143019800886844,
+0.0110961530344968,
+0.00826963267129267,
+0.0108990071119901
+};
+
+#define DecimationFactor_10K 10
+void filter1K(float FIR1K_Sample1_i_input, float FIR1K_Sample1_q_input, float FIR1K_Sample2_i_input, float FIR1K_Sample2_q_input)
+{
+    static uint8_t decimationCounter = 0;//used to keep track of how many samples you have currently decimated
+    static float FIR1K_Sample1_i_DecimatedSum=0;
+    static float FIR1K_Sample1_q_DecimatedSum=0;//when decimating sum up all 10 samples at a time have that be your output value
+    static float FIR1K_Sample2_i_DecimatedSum=0;
+    static float FIR1K_Sample2_q_DecimatedSum=0;
+    
+    FIR1K_Sample1_i_DecimatedSum += FIR1K_Sample1_i_input;//add sample to the sum of previous sample
+    FIR1K_Sample1_q_DecimatedSum += FIR1K_Sample1_q_input;
+    FIR1K_Sample2_i_DecimatedSum += FIR1K_Sample2_i_input;
+    FIR1K_Sample2_q_DecimatedSum += FIR1K_Sample2_q_input;
+    decimationCounter++;
+    if (decimationCounter >= DecimationFactor_10K)//once 10 samples have com
+    {
+        decimationCounter = 0;//reset decimation counter
+        //add sample to 10K filter
+        FIR1K_Sample1_i[FIR1K_Position] = FIR1K_Sample1_i_DecimatedSum;
+        FIR1K_Sample1_q[FIR1K_Position] = FIR1K_Sample1_q_DecimatedSum;
+        FIR1K_Sample2_i[FIR1K_Position] = FIR1K_Sample2_i_DecimatedSum;
+        FIR1K_Sample2_q[FIR1K_Position] = FIR1K_Sample2_q_DecimatedSum;
+        
+        FIR1K_Sample1_i_DecimatedSum = 0;//reset decimated sum to 0 for next sample
+        FIR1K_Sample1_q_DecimatedSum = 0;
+        FIR1K_Sample2_i_DecimatedSum = 0;
+        FIR1K_Sample2_q_DecimatedSum = 0;
+        
+        FIR1K_Position++; //increment circular buffer
+        if (FIR1K_Position >= FIR_1K_LENGTH) //wrap around
+        {
+            FIR1K_Position = 0;
+        }
+        
+        // Low pass filter of demodulated signal
+        float FIR1K_Sample1_i_Output = FIR1K_Sample1_i[FIR1K_Position] * lp_1K_coeff[0];//first multiply of convolution
+        float FIR1K_Sample1_q_Output = FIR1K_Sample1_q[FIR1K_Position] * lp_1K_coeff[0];
+        float FIR1K_Sample2_i_Output = FIR1K_Sample2_i[FIR1K_Position] * lp_1K_coeff[0];
+        float FIR1K_Sample2_q_Output = FIR1K_Sample2_q[FIR1K_Position] * lp_1K_coeff[0];
+        int fir_index;
+        for(int fir_counter = 1; fir_counter < FIR_1K_LENGTH; fir_counter++)//the rest of the convolution
+        {
+            fir_index = FIR1K_Position + fir_counter;
+            if (fir_index >= FIR_1K_LENGTH) 
+            {
+                fir_index -= FIR_1K_LENGTH;
+            }
+            FIR1K_Sample1_i_Output += FIR1K_Sample1_i[fir_index] * lp_1K_coeff[fir_counter];//convolving
+            FIR1K_Sample1_q_Output += FIR1K_Sample1_q[fir_index] * lp_1K_coeff[fir_counter];
+            FIR1K_Sample2_i_Output += FIR1K_Sample2_i[fir_index] * lp_1K_coeff[fir_counter];
+            FIR1K_Sample2_q_Output += FIR1K_Sample2_q[fir_index] * lp_1K_coeff[fir_counter];
+        }
+        //float mag1 = sqrt(FIR1K_Sample1_i_Output*FIR1K_Sample1_i_Output+FIR1K_Sample1_q_Output*FIR1K_Sample1_q_Output);
+        //float mag2 = sqrt(FIR1K_Sample2_i_Output*FIR1K_Sample2_i_Output+FIR1K_Sample2_q_Output*FIR1K_Sample2_q_Output);
+        //printf("V1: %f\tV2: %f\n\r",mag1,mag2);  
+        filter100(FIR1K_Sample1_i_Output, FIR1K_Sample1_q_Output, FIR1K_Sample2_i_Output, FIR1K_Sample2_q_Output);
+    }
+}
+
+
+
+#define FIR_10K_LENGTH 16
+float FIR10K_Sample1_i[FIR_10K_LENGTH];
+float FIR10K_Sample1_q[FIR_10K_LENGTH];
+float FIR10K_Sample2_i[FIR_10K_LENGTH];
+float FIR10K_Sample2_q[FIR_10K_LENGTH];
+uint8_t FIR10K_Position = 0;
+//Fs = 10000, Order = 15, Fpass = 10, Fstop = 900
+static float lp_10K_coeff[FIR_10K_LENGTH] = {
+0.0242947345184044,
+0.0283599756767857,
+0.0416004471421328,
+0.0557840684332377,
+0.0695867614174704,
+0.0816182734401924,
+0.0904748943926879,
+0.0951919735506062,
+0.0951919735506062,
+0.0904748943926879,
+0.0816182734401924,
+0.0695867614174704,
+0.0557840684332377,
+0.0416004471421328,
+0.0283599756767857,
+0.0242947345184044
+};
+
+#define DecimationFactor_100K 10
+void filter10K(float FIR10K_Sample1_i_input, float FIR10K_Sample1_q_input, float FIR10K_Sample2_i_input, float FIR10K_Sample2_q_input)
+{
+    static uint8_t decimationCounter = 0;//used to keep track of how many samples you have currently decimated
+    static float FIR10K_Sample1_i_DecimatedSum=0;
+    static float FIR10K_Sample1_q_DecimatedSum=0;//when decimating sum up all 10 samples at a time have that be your output value
+    static float FIR10K_Sample2_i_DecimatedSum=0;
+    static float FIR10K_Sample2_q_DecimatedSum=0;
+    
+    FIR10K_Sample1_i_DecimatedSum += FIR10K_Sample1_i_input;//add sample to the sum of previous sample
+    FIR10K_Sample1_q_DecimatedSum += FIR10K_Sample1_q_input;
+    FIR10K_Sample2_i_DecimatedSum += FIR10K_Sample2_i_input;
+    FIR10K_Sample2_q_DecimatedSum += FIR10K_Sample2_q_input;
+    decimationCounter++;
+    if (decimationCounter >= DecimationFactor_100K)//once 10 samples have com
+    {
+        decimationCounter = 0;//reset decimation counter
+        //add sample to 10K filter
+        FIR10K_Sample1_i[FIR10K_Position] = FIR10K_Sample1_i_DecimatedSum;
+        FIR10K_Sample1_q[FIR10K_Position] = FIR10K_Sample1_q_DecimatedSum;
+        FIR10K_Sample2_i[FIR10K_Position] = FIR10K_Sample2_i_DecimatedSum;
+        FIR10K_Sample2_q[FIR10K_Position] = FIR10K_Sample2_q_DecimatedSum;
+        
+        FIR10K_Sample1_i_DecimatedSum = 0;//reset decimated sum to 0 for next sample
+        FIR10K_Sample1_q_DecimatedSum = 0;
+        FIR10K_Sample2_i_DecimatedSum = 0;
+        FIR10K_Sample2_q_DecimatedSum = 0;
+        
+        FIR10K_Position++; //increment circular buffer
+        if (FIR10K_Position >= FIR_10K_LENGTH) //wrap around
+        {
+            FIR10K_Position = 0;
+        }
+        
+        // Low pass filter of demodulated signal
+        float FIR10K_Sample1_i_Output = FIR10K_Sample1_i[FIR10K_Position] * lp_10K_coeff[0];//first multiply of convolution
+        float FIR10K_Sample1_q_Output = FIR10K_Sample1_q[FIR10K_Position] * lp_10K_coeff[0];
+        float FIR10K_Sample2_i_Output = FIR10K_Sample2_i[FIR10K_Position] * lp_10K_coeff[0];
+        float FIR10K_Sample2_q_Output = FIR10K_Sample2_q[FIR10K_Position] * lp_10K_coeff[0];
+        int fir_index;
+        for(int fir_counter = 1; fir_counter < FIR_10K_LENGTH; fir_counter++)//the rest of the convolution
+        {
+            fir_index = FIR10K_Position + fir_counter;
+            if (fir_index >= FIR_10K_LENGTH) 
+            {
+                fir_index -= FIR_10K_LENGTH;
+            }
+            FIR10K_Sample1_i_Output += FIR10K_Sample1_i[fir_index] * lp_10K_coeff[fir_counter];//convolving
+            FIR10K_Sample1_q_Output += FIR10K_Sample1_q[fir_index] * lp_10K_coeff[fir_counter];
+            FIR10K_Sample2_i_Output += FIR10K_Sample2_i[fir_index] * lp_10K_coeff[fir_counter];
+            FIR10K_Sample2_q_Output += FIR10K_Sample2_q[fir_index] * lp_10K_coeff[fir_counter];
+        }
+        //float mag1 = sqrt(FIR10K_Sample1_i_Output*FIR10K_Sample1_i_Output+FIR10K_Sample1_q_Output*FIR10K_Sample1_q_Output);
+        //float mag2 = sqrt(FIR10K_Sample2_i_Output*FIR10K_Sample2_i_Output+FIR10K_Sample2_q_Output*FIR10K_Sample2_q_Output);
+        //printf("P1: %d\tV1: %f\tV2: %f\n\r",phase_counter,mag1,mag2);
+        //printf("V1: %f\tV2: %f\n\r",mag1,mag2);
+        filter1K(FIR10K_Sample1_i_Output, FIR10K_Sample1_q_Output, FIR10K_Sample2_i_Output, FIR10K_Sample2_q_Output);
+    }
+}
+
+
+
+
+
+#define FIR_100K_LENGTH 16
+float FIR100K_Sample1_i[FIR_100K_LENGTH];
+float FIR100K_Sample1_q[FIR_100K_LENGTH];
+float FIR100K_Sample2_i[FIR_100K_LENGTH];
+float FIR100K_Sample2_q[FIR_100K_LENGTH];
+
+uint8_t FIR100K_Position = 0;
+//Fs = 100000, Order = 15, Fpass = 100, Fstop = 9000
+static float lp_100K_coeff[FIR_100K_LENGTH] = {
+0.0242947345184044,
+0.0283599756767857,
+0.0416004471421328,
+0.0557840684332377,
+0.0695867614174704,
+0.0816182734401924,
+0.0904748943926879,
+0.0951919735506062,
+0.0951919735506062,
+0.0904748943926879,
+0.0816182734401924,
+0.0695867614174704,
+0.0557840684332377,
+0.0416004471421328,
+0.0283599756767857,
+0.0242947345184044
+};
+
+void filter100K(int sample1, int sample2)
+{
+    float current_i_mod = i_mod_pre[phase_counter];//sin and cos to demodulate signature
+    float current_q_mod = q_mod_pre[phase_counter];
+    phase_counter++;//increment the demodulating sinusoids by 1/Fs
+    
+    if (phase_counter>=pre_compute_length)//wrap around to beginning of sin wave
+    {
+        phase_counter = 0;
+    }
+    
+    FIR100K_Sample1_i[FIR100K_Position] = (sample1 * current_i_mod);//this centers the delta function at f =0 in z space
+    FIR100K_Sample1_q[FIR100K_Position] = (sample1 * current_q_mod);//this centers the delta function at f =0 in z space
+    FIR100K_Sample2_i[FIR100K_Position] = (sample2 * current_i_mod);//this centers the delta function at f =0 in z space
+    FIR100K_Sample2_q[FIR100K_Position] = (sample2 * current_q_mod);//this centers the delta function at f =0 in z space
+    
+    FIR100K_Position++;//increment position of circular buffer
+    if (FIR100K_Position >= FIR_100K_LENGTH)//wrap around at end of buffer 
+    {
+        FIR100K_Position = 0;
+    }
+    
+    // Low pass filter of demodulated signal
+    float FIR100K_Sample1_i_Output = FIR100K_Sample1_i[FIR100K_Position] * lp_100K_coeff[0];//first multiply in convolution
+    float FIR100K_Sample1_q_Output = FIR100K_Sample1_q[FIR100K_Position] * lp_100K_coeff[0];//first multiply in convolution
+    float FIR100K_Sample2_i_Output = FIR100K_Sample2_i[FIR100K_Position] * lp_100K_coeff[0];//first multiply in convolution
+    float FIR100K_Sample2_q_Output = FIR100K_Sample2_q[FIR100K_Position] * lp_100K_coeff[0];//first multiply in convolution
+    int fir_index;
+    for(int fir_counter = 1; fir_counter < FIR_100K_LENGTH; fir_counter++)//convolves the filter with the signal
+    {
+        fir_index = FIR100K_Position + fir_counter;
+        if (fir_index >= FIR_100K_LENGTH)//wrap around
+        {
+            fir_index -= FIR_100K_LENGTH;
+        }
+        FIR100K_Sample1_i_Output += FIR100K_Sample1_i[fir_index] * lp_100K_coeff[fir_counter];//convolution
+        FIR100K_Sample1_q_Output += FIR100K_Sample1_q[fir_index] * lp_100K_coeff[fir_counter];
+        FIR100K_Sample2_i_Output += FIR100K_Sample2_i[fir_index] * lp_100K_coeff[fir_counter];
+        FIR100K_Sample2_q_Output += FIR100K_Sample2_q[fir_index] * lp_100K_coeff[fir_counter];
+    }
+    //pass data to next filter
+    //float mag1 = sqrt(FIR100K_Sample1_i_Output*FIR100K_Sample1_i_Output+FIR100K_Sample1_q_Output*FIR100K_Sample1_q_Output);
+    //float mag2 = sqrt(FIR100K_Sample2_i_Output*FIR100K_Sample2_i_Output+FIR100K_Sample2_q_Output*FIR100K_Sample2_q_Output);
+    //printf("V1: %f\tV2: %f\n\r",mag1,mag2);
+    filter10K(FIR100K_Sample1_i_Output, FIR100K_Sample1_q_Output, FIR100K_Sample2_i_Output, FIR100K_Sample2_q_Output);
+}
+
+
+    

--- a/SignalProcessing.h	Thu Oct 29 17:15:20 2015 +0000
+++ b/SignalProcessing.h	Sat Oct 31 20:06:37 2015 +0000
@@ -0,0 +1,6 @@
+#ifndef SIGNAL_PROCESSING_H
+#define SIGNAL_PROCESSING_H
+
+void pre_compute_tables();
+void filter100K(int sample1, int sample2);
+#endif
\ No newline at end of file

--- a/main.cpp	Thu Oct 29 17:15:20 2015 +0000
+++ b/main.cpp	Sat Oct 31 20:06:37 2015 +0000
@@ -35,6 +35,7 @@
 #include "AngleEncoder.h"
 #include "MotorControl.h"
 
+#include "SignalProcessing.h"
 // for debug purposes
 Serial pc(USBTX, USBRX);
 DigitalOut led_red(LED_RED);
@@ -64,11 +65,25 @@
 Timer t1;
 using namespace std;
  
-int main() {
-    
+int main()
+{
     led_blue = 1;
     led_green = 1;
     led_red = 1;
+    pre_compute_tables();
+    printf("STARTING\n\r");
+    t1.reset();
+    for (int t  = 0; t<500000; t++)
+    {
+        int input1 = 5000*cos(3.14159265359 * 2*10000*t*.00001)+2500;
+        int input2 = 2500*sin(3.14159265359 * 2*10000*t*.00001)+2500;
+        t1.start();
+        filter100K(input1, input2);
+        t1.stop();
+    }
+    printf("FINAL TIME: %f\n\r",t1.read());
+ /*   
+    
     
     pc.baud(230400);
     pc.printf("Starting...\r\n");
@@ -121,6 +136,7 @@
             t1.reset();
         }
     }
+*/
  //   while(1) {
 //        if(pc.readable() > 0) {
 //            char temp = pc.getc();
@@ -307,6 +323,7 @@
 //            }
 //        }
 //    }
+
 }
 
 void output_data()