File content as of revision 3:a85b742be262:

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

#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]; 

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



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