File content as of revision 7:af255a90505e:

#include "mbed.h"
#include "SignalProcessing.h"
#include "Sensor.h"

#define pre_compute_length 2000
#define DMA_PERIOD .00001
#define DMA_FREQUENCY 100000
#define CARRIERFREQUENCY 1000

uint16_t phase_counter = 0;

float i_mod_pre[pre_compute_length];
float q_mod_pre[pre_compute_length];
//uint16_t out_val_pre[pre_compute_length]; 

#define 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 * DMA_PERIOD * precompute_counter));
    q_mod_pre[precompute_counter] = (-sin(twopi * CARRIERFREQUENCY * DMA_PERIOD * precompute_counter));
    
  }
}




#define FIR_33_LENGTH 128
float FIR33_Sample1_i[FIR_33_LENGTH];
float FIR33_Sample1_q[FIR_33_LENGTH];
float FIR33_Sample2_i[FIR_33_LENGTH];
float FIR33_Sample2_q[FIR_33_LENGTH];
uint8_t FIR33_Position = 0;
//Fs = 33, Order = 63, Fpass = 1, Fstop = 5
static float lp_33_coeff[FIR_33_LENGTH] = {
0.00112064914891618,
0.00122265940667415,
0.00134320266606308,
0.00148248497505270,
0.00163752208873233,
0.00180917436717814,
0.00199487440500326,
0.00219591677377202,
0.00240942435359224,
0.00263715016478107,
0.00287620469595143,
0.00312896642110311,
0.00339115146671554,
0.00366703367280862,
0.00395013103364271,
0.00424959421024652,
0.00454749937640992,
0.00486587065943205,
0.00519015600428925,
0.00551644394778870,
0.00585208754960567,
0.00619545680874284,
0.00654599064376495,
0.00690046871786865,
0.00725861046415282,
0.00761931288503866,
0.00798229719629724,
0.00834687323478277,
0.00871239637379653,
0.00907848882392502,
0.00944349817409369,
0.00980681579778287,
0.0101666025128362,
0.0105235376400116,
0.0108740956379341,
0.0112200337619850,
0.0115576132398033,
0.0118892792678013,
0.0122101053096016,
0.0125220143316710,
0.0128250887436567,
0.0131145746752160,
0.0133925589239844,
0.0136577821847874,
0.0139097056313474,
0.0141461938484364,
0.0143672111207676,
0.0145723586994284,
0.0147612534972782,
0.0149330287990815,
0.0150868196634048,
0.0152228115376150,
0.0153400576626228,
0.0154384553803622,
0.0155165567546008,
0.0155766985839343,
0.0156161625679816,
0.0156360898266519,
0.0156360898266519,
0.0156161625679816,
0.0155766985839343,
0.0155165567546008,
0.0154384553803622,
0.0153400576626228,
0.0152228115376150,
0.0150868196634048,
0.0149330287990815,
0.0147612534972782,
0.0145723586994284,
0.0143672111207676,
0.0141461938484364,
0.0139097056313474,
0.0136577821847874,
0.0133925589239844,
0.0131145746752160,
0.0128250887436567,
0.0125220143316710,
0.0122101053096016,
0.0118892792678013,
0.0115576132398033,
0.0112200337619850,
0.0108740956379341,
0.0105235376400116,
0.0101666025128362,
0.00980681579778287,
0.00944349817409369,
0.00907848882392502,
0.00871239637379653,
0.00834687323478277,
0.00798229719629724,
0.00761931288503866,
0.00725861046415282,
0.00690046871786865,
0.00654599064376495,
0.00619545680874284,
0.00585208754960567,
0.00551644394778870,
0.00519015600428925,
0.00486587065943205,
0.00454749937640992,
0.00424959421024652,
0.00395013103364271,
0.00366703367280862,
0.00339115146671554,
0.00312896642110311,
0.00287620469595143,
0.00263715016478107,
0.00240942435359224,
0.00219591677377202,
0.00199487440500326,
0.00180917436717814,
0.00163752208873233,
0.00148248497505270,
0.00134320266606308,
0.00122265940667415,
0.00112064914891618,
0.00104032814636243,
0.000982028128151230,
0.000950473399969370,
0.000946711675637481,
0.000976800083269572,
-0.00572883284452795
};
#define NUMSAMPLESAVERAGE 100
#define DecimationFactor_33 3

void filter33(float FIR33_Sample1_i_input, float FIR33_Sample1_q_input, float FIR33_Sample2_i_input, float FIR33_Sample2_q_input)
{
    //printf("f");
    static uint8_t finalAverageCounter = 0;
    static uint8_t decimationCounter = 0;//used to keep track of how many samples you have currently decimated
    static float FIR33_Sample1_i_DecimatedSum=0;
    static float FIR33_Sample1_q_DecimatedSum=0;//when decimating sum up all 10 samples at a time have that be your output value
    static float FIR33_Sample2_i_DecimatedSum=0;
    static float FIR33_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;
    
    FIR33_Sample1_i_DecimatedSum += FIR33_Sample1_i_input;//add sample to the sum of previous sample
    FIR33_Sample1_q_DecimatedSum += FIR33_Sample1_q_input;
    FIR33_Sample2_i_DecimatedSum += FIR33_Sample2_i_input;
    FIR33_Sample2_q_DecimatedSum += FIR33_Sample2_q_input;
    decimationCounter++;
    if (decimationCounter >= DecimationFactor_33)//once 10 samples have com
    {
        decimationCounter = 0;//reset decimation counter
        //add sample to 10K filter
        FIR33_Sample1_i[FIR33_Position] = FIR33_Sample1_i_DecimatedSum;
        FIR33_Sample1_q[FIR33_Position] = FIR33_Sample1_q_DecimatedSum;
        FIR33_Sample2_i[FIR33_Position] = FIR33_Sample2_i_DecimatedSum;
        FIR33_Sample2_q[FIR33_Position] = FIR33_Sample2_q_DecimatedSum;
        
        FIR33_Sample1_i_DecimatedSum = 0;//reset decimated sum to 0 for next sample
        FIR33_Sample1_q_DecimatedSum = 0;
        FIR33_Sample2_i_DecimatedSum = 0;
        FIR33_Sample2_q_DecimatedSum = 0;
        
        FIR33_Position++; //increment circular buffer
        if (FIR33_Position >= FIR_33_LENGTH) //wrap around
        {
            FIR33_Position = 0;
        }
        
        // Low pass filter of demodulated signal
        float FIR33_Sample1_i_Output = FIR33_Sample1_i[FIR33_Position] * lp_33_coeff[0];//first multiply of convolution
        float FIR33_Sample1_q_Output = FIR33_Sample1_q[FIR33_Position] * lp_33_coeff[0];
        float FIR33_Sample2_i_Output = FIR33_Sample2_i[FIR33_Position] * lp_33_coeff[0];
        float FIR33_Sample2_q_Output = FIR33_Sample2_q[FIR33_Position] * lp_33_coeff[0];
        int fir_index;
        for(int fir_counter = 1; fir_counter < FIR_33_LENGTH; fir_counter++)//the rest of the convolution
        {
            fir_index = FIR33_Position + fir_counter;
            if (fir_index >= FIR_33_LENGTH) 
            {
                fir_index -= FIR_33_LENGTH;
            }
            FIR33_Sample1_i_Output += FIR33_Sample1_i[fir_index] * lp_33_coeff[fir_counter];//convolving
            FIR33_Sample1_q_Output += FIR33_Sample1_q[fir_index] * lp_33_coeff[fir_counter];
            FIR33_Sample2_i_Output += FIR33_Sample2_i[fir_index] * lp_33_coeff[fir_counter];
            FIR33_Sample2_q_Output += FIR33_Sample2_q[fir_index] * lp_33_coeff[fir_counter];
        }
        
        //float mag1 = sqrt(FIR33_Sample1_i_Output*FIR33_Sample1_i_Output+FIR33_Sample1_q_Output*FIR33_Sample1_q_Output);
        //float mag2 = sqrt(FIR33_Sample2_i_Output*FIR33_Sample2_i_Output+FIR33_Sample2_q_Output*FIR33_Sample2_q_Output);
        //printf("V1: %f\tV2: %f\n\r",mag1,mag2);  
        
        
        Final_Average1_i+=FIR33_Sample1_i_Output;
        Final_Average1_q+=FIR33_Sample1_q_Output;//when decimating sum up all 10 samples at a time have that be your output value
        Final_Average2_i+=FIR33_Sample2_i_Output;
        Final_Average2_q+=FIR33_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\tRatio: %f\n\r",mag1,mag2,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(FIR33_Sample1_i_Output*FIR33_Sample1_i_Output+FIR33_Sample1_q_Output*FIR33_Sample1_q_Output);
        //float mag2 = sqrt(FIR33_Sample2_i_Output*FIR33_Sample2_i_Output+FIR33_Sample2_q_Output*FIR33_Sample2_q_Output);
        //printf("V1: %f\tV2: %f\n\r",mag1,mag2);  
        //filter33(FIR33_Sample1_i_Output, FIR33_Sample1_q_Output, FIR33_Sample2_i_Output, FIR33_Sample2_q_Output);
    
    }
}






#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.0127764520494401,
0.000293288299851251,
0.000495191328182707,
0.000838157407497840,
0.00131525363039988,
0.00192793120776209,
0.00267432434442746,
0.00355353493845821,
0.00456036887752939,
0.00569272050003285,
0.00694090445590618,
0.00830083540850880,
0.00975882121858377,
0.0113079414406056,
0.0129307225563250,
0.0146171626466673,
0.0163450416131430,
0.0181032221508212,
0.0198618118533429,
0.0216165682984801,
0.0233125759526720,
0.0249849393161916,
0.0265739031713570,
0.0280649931338029,
0.0294518285092336,
0.0307008224951110,
0.0318087914746566,
0.0327529952249549,
0.0335263522468559,
0.0341150932603310,
0.0345128562450896,
0.0347130017511486,
0.0347130017511486,
0.0345128562450896,
0.0341150932603310,
0.0335263522468559,
0.0327529952249549,
0.0318087914746566,
0.0307008224951110,
0.0294518285092336,
0.0280649931338029,
0.0265739031713570,
0.0249849393161916,
0.0233125759526720,
0.0216165682984801,
0.0198618118533429,
0.0181032221508212,
0.0163450416131430,
0.0146171626466673,
0.0129307225563250,
0.0113079414406056,
0.00975882121858377,
0.00830083540850880,
0.00694090445590618,
0.00569272050003285,
0.00456036887752939,
0.00355353493845821,
0.00267432434442746,
0.00192793120776209,
0.00131525363039988,
0.000838157407497840,
0.000495191328182707,
0.000293288299851251,
-0.0127764520494401
};

//#define NUMSAMPLESAVERAGE 100
#define DecimationFactor_1K 10
void filter100(float FIR100_Sample1_i_input, float FIR100_Sample1_q_input, float FIR100_Sample2_i_input, float FIR100_Sample2_q_input)
{
    //printf("f");
    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_1K)//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));
            setFiltered(mag1);
            setFilteredRef(mag2);
            printf("V1: %f\tV2: %f\tRatio: %f\n\r",mag1,mag2,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);  
        //filter33(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 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;
//    static int finalAverageCounter = 0;
//    
    
    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];
        }
        
        //Final_Average1_i+=FIR1K_Sample1_i_Output;
//        Final_Average1_q+=FIR1K_Sample1_q_Output;//when decimating sum up all 10 samples at a time have that be your output value
//        Final_Average2_i+=FIR1K_Sample2_i_Output;
//        Final_Average2_q+=FIR1K_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\tRatio: %f\n\r",mag1,mag2,mag2/mag1);
//            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(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.0102922869776514,
0.0196357484226367,
0.0346068275004222,
0.0528964828542786,
0.0725363140207878,
0.0908817555678698,
0.105122693390187,
0.112904816559370,
0.112904816559370,
0.105122693390187,
0.0908817555678698,
0.0725363140207878,
0.0528964828542786,
0.0346068275004222,
0.0196357484226367,
0.0102922869776514
};

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);
}