File content as of revision 0:3927c6d8ad1f:

// TODO: 
#include "mbed.h"
#include <string>

#include <stdio.h>
#include "PeripheralNames.h"
#include "PeripheralPins.h"
#include "fsl_adc_hal.h"
#include "fsl_clock_manager.h"
#include "fsl_dspi_hal.h"



Serial pc(USBTX, USBRX);

/*
#if DEBUG
#define DEBUG_PRINT(x) pc.printf(x)
#else
#define DEBUG_PRINT(x) do {} while(0)
#endif
*/
DigitalOut led_red(LED_RED);
DigitalOut led_green(LED_GREEN);
DigitalOut led_blue(LED_BLUE);


#define MAX_FADC 6000000

#define TimerInterruptInMicroSeconds 25 // This is for 40 kHz


#define CarrierFrequency 190

#define ISR_counter_loop_reset 4 // 10 kHz sampling of the signal

int ISR_counter = 1;

#define MaxNumSamples 0  //set to 0 if want continuous sampling (i.e. run forever)
#define NumSamplesAverageInADC 32 //hardware averaging default is 2. values 0..32. Higher values reduce noise and maximum sample rate
#define NumADCbits 16

int CyclesInState = 0;
// While 32 is the minimum, it seems to take a few cycles to get things to work right. 32 + 18 + 18 + 18
#define PreCalculateCycles 256
#define NumCalculatedAverages 500

AnalogOut WaveOut(DAC0_OUT);
AnalogIn WaveIn1(A0);
AnalogIn WaveIn2(A2);


double previousTime = 0;


volatile unsigned long counter = 0; //keep track of how many times the timer interrupt executes


volatile int phase_counter = 0;
float twopi = 3.14159265359 * 2;

#define pre_compute_length 4000
float i_mod_pre[pre_compute_length];
float q_mod_pre[pre_compute_length];
int out_val_pre[pre_compute_length]; 

float current_i_mod;
float current_q_mod;


#define lp_fir_length 256
#define bp_fir_length 256

float x_0_i[lp_fir_length];
float x_0_q[lp_fir_length];
float x_1_i[lp_fir_length];
float x_1_q[lp_fir_length];

float real_ip = 0;
float real_ip_total = 0;
float imag_ip = 0;
float imag_ip_total = 0;
float y_0_squared = 0;
float y_1_squared = 0;

int max_0 = -1;
int min_0 = 65536;
int max_1 = -1;
int min_1 = 65536;


// Coefficients for FIR low-pass filter
// Order: 127
// Fs = 10000
// Fpass = 5
// Fstop = 50
// Density Factor 20
float lp_filter_coeff[lp_fir_length] = {
0.0861805974737537,
0.00194058236914933,
0.00196838475539374,
0.00198494885848217,
0.00201327315570587,
0.00202912020658817,
0.00205837690374429,
0.00207476466732652,
0.00210551867296513,
0.00212232982776793,
0.00215469918380619,
0.00217146660668663,
0.00220558455176705,
0.00222092552588268,
0.00225605241370255,
0.00226646783939495,
0.00230092287432329,
0.00229715073318613,
0.00232623464517315,
0.00225358157041652,
0.00237497949175132,
0.00240962754549243,
0.00239489319998042,
0.00243168327631056,
0.00244169878557599,
0.00247033152393311,
0.00248484040428462,
0.00250942587726843,
0.00252568320446658,
0.00254761155808375,
0.00256464167177514,
0.00258446154111681,
0.00260244294098803,
0.00262131473283837,
0.00264164776893963,
0.00266111980952651,
0.00268646307379128,
0.00270761719256829,
0.00274205375475237,
0.00274149898919641,
0.00274945418608348,
0.00279082346790514,
0.00279778186887203,
0.00282250662520772,
0.00283603752336853,
0.00285874465133605,
0.00287407059058428,
0.00289570516626933,
0.00291199026392523,
0.00293311428662787,
0.00295009345796840,
0.00297044279838009,
0.00298714691522817,
0.00300581268506110,
0.00302120747953588,
0.00303732312039544,
0.00305157040082656,
0.00306643736356742,
0.00308869428115324,
0.00311307778862543,
0.00311424353468359,
0.00313896920891965,
0.00315257957066846,
0.00317063676755790,
0.00318404202174892,
0.00320106537708682,
0.00321431574769385,
0.00323043068211672,
0.00324309082121968,
0.00325830171776266,
0.00327070521989578,
0.00328578417067009,
0.00329857943310240,
0.00331411607506732,
0.00332746621803916,
0.00334278731970893,
0.00335490587728655,
0.00336582204821038,
0.00337340614667260,
0.00339593158768486,
0.00340201380261714,
0.00341358117591052,
0.00342527970126076,
0.00343729081346655,
0.00344788640610150,
0.00345950626709297,
0.00346977560789619,
0.00348137861989829,
0.00349145780595203,
0.00350258155909523,
0.00351179661900564,
0.00352191431883533,
0.00352996025917487,
0.00353918710384162,
0.00354686935571502,
0.00355665353091388,
0.00356587002173744,
0.00357689031711855,
0.00357801462512765,
0.00359006856669697,
0.00359950458629560,
0.00360494297850754,
0.00361233039031660,
0.00361922292724758,
0.00362591596791954,
0.00363226321494882,
0.00363786374022990,
0.00364352288365460,
0.00364862849480178,
0.00365423915635453,
0.00365927328685460,
0.00366494409556548,
0.00366963573779647,
0.00367457381745451,
0.00367783024950588,
0.00368130382332767,
0.00368329710162579,
0.00369126347001870,
0.00369087449038130,
0.00369079790741907,
0.00369643982204526,
0.00369766453946281,
0.00369941489929772,
0.00370041177271180,
0.00370189241602639,
0.00370304548909528,
0.00370413802604405,
0.00370464038533780,
0.00370464038533780,
0.00370413802604405,
0.00370304548909528,
0.00370189241602639,
0.00370041177271180,
0.00369941489929772,
0.00369766453946281,
0.00369643982204526,
0.00369079790741907,
0.00369087449038130,
0.00369126347001870,
0.00368329710162579,
0.00368130382332767,
0.00367783024950588,
0.00367457381745451,
0.00366963573779647,
0.00366494409556548,
0.00365927328685460,
0.00365423915635453,
0.00364862849480178,
0.00364352288365460,
0.00363786374022990,
0.00363226321494882,
0.00362591596791954,
0.00361922292724758,
0.00361233039031660,
0.00360494297850754,
0.00359950458629560,
0.00359006856669697,
0.00357801462512765,
0.00357689031711855,
0.00356587002173744,
0.00355665353091388,
0.00354686935571502,
0.00353918710384162,
0.00352996025917487,
0.00352191431883533,
0.00351179661900564,
0.00350258155909523,
0.00349145780595203,
0.00348137861989829,
0.00346977560789619,
0.00345950626709297,
0.00344788640610150,
0.00343729081346655,
0.00342527970126076,
0.00341358117591052,
0.00340201380261714,
0.00339593158768486,
0.00337340614667260,
0.00336582204821038,
0.00335490587728655,
0.00334278731970893,
0.00332746621803916,
0.00331411607506732,
0.00329857943310240,
0.00328578417067009,
0.00327070521989578,
0.00325830171776266,
0.00324309082121968,
0.00323043068211672,
0.00321431574769385,
0.00320106537708682,
0.00318404202174892,
0.00317063676755790,
0.00315257957066846,
0.00313896920891965,
0.00311424353468359,
0.00311307778862543,
0.00308869428115324,
0.00306643736356742,
0.00305157040082656,
0.00303732312039544,
0.00302120747953588,
0.00300581268506110,
0.00298714691522817,
0.00297044279838009,
0.00295009345796840,
0.00293311428662787,
0.00291199026392523,
0.00289570516626933,
0.00287407059058428,
0.00285874465133605,
0.00283603752336853,
0.00282250662520772,
0.00279778186887203,
0.00279082346790514,
0.00274945418608348,
0.00274149898919641,
0.00274205375475237,
0.00270761719256829,
0.00268646307379128,
0.00266111980952651,
0.00264164776893963,
0.00262131473283837,
0.00260244294098803,
0.00258446154111681,
0.00256464167177514,
0.00254761155808375,
0.00252568320446658,
0.00250942587726843,
0.00248484040428462,
0.00247033152393311,
0.00244169878557599,
0.00243168327631056,
0.00239489319998042,
0.00240962754549243,
0.00237497949175132,
0.00225358157041652,
0.00232623464517315,
0.00229715073318613,
0.00230092287432329,
0.00226646783939495,
0.00225605241370255,
0.00222092552588268,
0.00220558455176705,
0.00217146660668663,
0.00215469918380619,
0.00212232982776793,
0.00210551867296513,
0.00207476466732652,
0.00205837690374429,
0.00202912020658817,
0.00201327315570587,
0.00198494885848217,
0.00196838475539374,
0.00194058236914933,
0.0861805974737537
};


// Coefficients for FIR band-pass filter (single-precision floating)
// Order: 255
// Fs = 10000
// Fstop1 = 140
// Fpass1 = 180
// Fpass2 = 200
// Fstop2 = 240  
// Density Factor 20
float bp_filter_coeff[bp_fir_length] = {
-0.0222569832875342,
-0.000668571820934435,
-0.000617098880070943,
-0.000526912832180624,
-0.000394083108191473,
-0.000224044823896433,
-1.40519139447993e-05,
0.000228753985399507,
0.000506043646368515,
0.000809081179935479,
0.00113886884349691,
0.00148482706293391,
0.00184749122790831,
0.00221412926605753,
0.00258556196728641,
0.00294623650522875,
0.00329828480589904,
0.00362103051636677,
0.00392129596654753,
0.00416037700790689,
0.00440774110765089,
0.00458742109894213,
0.00470432883601479,
0.00477604326343940,
0.00477932011891816,
0.00472369327143935,
0.00459673449069964,
0.00440550863345307,
0.00414276809544121,
0.00381610310764620,
0.00342194240814885,
0.00296924934443244,
0.00245734168541197,
0.00189704915840860,
0.00128969301635370,
0.000647875455120701,
-2.63604406483334e-05,
-0.000717680016887559,
-0.00142424098193147,
-0.00211871793213848,
-0.00280466259597701,
-0.00347302243192041,
-0.00410155649476450,
-0.00468849572012807,
-0.00521831221732055,
-0.00568473286208729,
-0.00607617428224791,
-0.00638642038264520,
-0.00660654522935380,
-0.00673201343716618,
-0.00675686946570070,
-0.00667909912380224,
-0.00649590629000032,
-0.00620853153050349,
-0.00581778091366643,
-0.00532861669082900,
-0.00474475510188909,
-0.00407398377208893,
-0.00332036080041792,
-0.00249639621821765,
-0.00161692422174727,
-0.000686114637835394,
0.000276969889091618,
0.00126158838172335,
0.00225109525801695,
0.00323185932244432,
0.00418763727591780,
0.00510440961298076,
0.00596653561986331,
0.00676059725642517,
0.00747245231692098,
0.00809037791844765,
0.00860240336852779,
0.00899909501543391,
0.00927138366550150,
0.00941337467689702,
0.00942035086127182,
0.00929102977283836,
0.00902403254438426,
0.00861862088073253,
0.00808277337301294,
0.00741973293721300,
0.00663808854880170,
0.00574764652047989,
0.00475963542671197,
0.00368791267821526,
0.00254692028134653,
0.00135318490817447,
0.000123381816987046,
-0.00112406829604574,
-0.00237116204428360,
-0.00359884130528209,
-0.00478919843371992,
-0.00592368092266592,
-0.00698521179296005,
-0.00795630134054653,
-0.00882180545791639,
-0.00956745282162880,
-0.0101826820881787,
-0.0106544412214730,
-0.0109757370305086,
-0.0111401872280476,
-0.0111439989937533,
-0.0109860306403343,
-0.0106669796725910,
-0.0101903953657629,
-0.00956214881730735,
-0.00879053537462268,
-0.00788592056463026,
-0.00686079789488663,
-0.00572962138717207,
-0.00450868146307788,
-0.00321565789521111,
-0.00186931333282453,
-0.000489113370880662,
0.000904730267919755,
0.00229132886837385,
0.00364982806960963,
0.00496072866543581,
0.00620453967164588,
0.00736222523931905,
0.00841655902006347,
0.00935134258076706,
0.0101530565640308,
0.0108094111402764,
0.0113105535457012,
0.0116487913595070,
0.0118191627771399,
0.0118191627771399,
0.0116487913595070,
0.0113105535457012,
0.0108094111402764,
0.0101530565640308,
0.00935134258076706,
0.00841655902006347,
0.00736222523931905,
0.00620453967164588,
0.00496072866543581,
0.00364982806960963,
0.00229132886837385,
0.000904730267919755,
-0.000489113370880662,
-0.00186931333282453,
-0.00321565789521111,
-0.00450868146307788,
-0.00572962138717207,
-0.00686079789488663,
-0.00788592056463026,
-0.00879053537462268,
-0.00956214881730735,
-0.0101903953657629,
-0.0106669796725910,
-0.0109860306403343,
-0.0111439989937533,
-0.0111401872280476,
-0.0109757370305086,
-0.0106544412214730,
-0.0101826820881787,
-0.00956745282162880,
-0.00882180545791639,
-0.00795630134054653,
-0.00698521179296005,
-0.00592368092266592,
-0.00478919843371992,
-0.00359884130528209,
-0.00237116204428360,
-0.00112406829604574,
0.000123381816987046,
0.00135318490817447,
0.00254692028134653,
0.00368791267821526,
0.00475963542671197,
0.00574764652047989,
0.00663808854880170,
0.00741973293721300,
0.00808277337301294,
0.00861862088073253,
0.00902403254438426,
0.00929102977283836,
0.00942035086127182,
0.00941337467689702,
0.00927138366550150,
0.00899909501543391,
0.00860240336852779,
0.00809037791844765,
0.00747245231692098,
0.00676059725642517,
0.00596653561986331,
0.00510440961298076,
0.00418763727591780,
0.00323185932244432,
0.00225109525801695,
0.00126158838172335,
0.000276969889091618,
-0.000686114637835394,
-0.00161692422174727,
-0.00249639621821765,
-0.00332036080041792,
-0.00407398377208893,
-0.00474475510188909,
-0.00532861669082900,
-0.00581778091366643,
-0.00620853153050349,
-0.00649590629000032,
-0.00667909912380224,
-0.00675686946570070,
-0.00673201343716618,
-0.00660654522935380,
-0.00638642038264520,
-0.00607617428224791,
-0.00568473286208729,
-0.00521831221732055,
-0.00468849572012807,
-0.00410155649476450,
-0.00347302243192041,
-0.00280466259597701,
-0.00211871793213848,
-0.00142424098193147,
-0.000717680016887559,
-2.63604406483334e-05,
0.000647875455120701,
0.00128969301635370,
0.00189704915840860,
0.00245734168541197,
0.00296924934443244,
0.00342194240814885,
0.00381610310764620,
0.00414276809544121,
0.00440550863345307,
0.00459673449069964,
0.00472369327143935,
0.00477932011891816,
0.00477604326343940,
0.00470432883601479,
0.00458742109894213,
0.00440774110765089,
0.00416037700790689,
0.00392129596654753,
0.00362103051636677,
0.00329828480589904,
0.00294623650522875,
0.00258556196728641,
0.00221412926605753,
0.00184749122790831,
0.00148482706293391,
0.00113886884349691,
0.000809081179935479,
0.000506043646368515,
0.000228753985399507,
-1.40519139447993e-05,
-0.000224044823896433,
-0.000394083108191473,
-0.000526912832180624,
-0.000617098880070943,
-0.000668571820934435,
-0.0222569832875342
};


int lp_fifo_num = 1;
int bp_fifo_num = 1;

float x_0[bp_fir_length];
float x_1[bp_fir_length];

int value[] = {0,0};

float mag = 0;
float calc_mag = 0;
double mag_total = 0;
float calc_phase = 0;
int NumInBlock = 0;

float mag_result = 0;
float phase_result = 0;
/*
int diff_0[3] = {0, 0, 0};
int diff_1[3] = {0, 0, 0};
*/
double final_mag = 0;
float final_phase = 0;

// Finite State Machine
int current_state = 0;

// Flag
bool ADC_reading = false;
bool isTransmitting = false;


//IntervalTimer timer0;
Ticker timer0;
Timer t1;

int adc_cal(void)
{
    ADC0_CFG1 |= (ADC_CFG1_MODE(3)  | // 16 bits mode
                  ADC_CFG1_ADICLK(1)| // Input Bus Clock divided by 2 (20-25 MHz out of reset (FEI mode) / 2)
                  ADC_CFG1_ADIV(2)) ; // Clock divide by 4 (2.5-3 MHz)
    
    ADC0_SC3 |= ADC_SC3_AVGE_MASK |   // Enable HW average
                ADC_SC3_AVGS(3)   |   // Set HW average of 32 samples
                ADC_SC3_CAL_MASK;     // Start calibration process
    
    while(ADC0_SC3 & ADC_SC3_CAL_MASK); // Wait for calibration to end
    
    if(ADC0_SC3 & ADC_SC3_CALF_MASK) return 1;  // Check for successful calibration
    
    uint16_t calib = 0; // calibration variable 
    calib += ADC0->CLPS + ADC0_CLP4 + ADC0_CLP3 + ADC0_CLP2 + ADC0_CLP1 + ADC0_CLP0;
    calib /= 2;
    calib |= 0x8000;    // Set MSB 
    ADC0_PG = calib;
    calib = 0;
    calib += ADC0_CLMS + ADC0_CLM4 + ADC0_CLM3 + ADC0_CLM2 + ADC0_CLM1 + ADC0_CLM0;
    calib /= 2;
    calib |= 0x8000;    // Set MSB
    ADC0_MG = calib;
    
    return 0;
}

void adc_init()
{
    // red, indicating not ready

    
    // Turn on the ADC0 and ADC1 clocks
    SIM_SCGC6 |= SIM_SCGC6_ADC0_MASK;
    SIM_SCGC3 |= SIM_SCGC3_ADC1_MASK;
    
    
     __disable_irq();
     adc_cal();
    //if(adc_cal()) {red = 0; green = 0;} // if calibration fails, display yellow
    __enable_irq();


    
    // Setup Configuration Register 1 
    ADC0_CFG1 = 0; // clear register
    ADC0_CFG1 |= ADC_CFG1_ADICLK(0);    // select bus clock
    ADC0_CFG1 |= ADC_CFG1_MODE(3);      // select 16-bit 2's complement output  
    ADC0_CFG1 |= ADC_CFG1_ADIV(0);      // clock divisor = 0.
    ADC0_CFG1 &= ~ADC_CFG1_ADLSMP_MASK; // select short sample time   //QUESTION
    ADC0_CFG1 &= ~ADC_CFG1_ADLPC_MASK;  // select normal power configuration
    ADC1_CFG1 = 0; // clear register
    ADC1_CFG1 |= ADC_CFG1_ADICLK(0);    // select bus clock
    ADC1_CFG1 |= ADC_CFG1_MODE(3);      // select 16-bit 2's complement output
    ADC1_CFG1 |= ADC_CFG1_ADIV(0);      // clock divisor = 0.
    ADC1_CFG1 &= ~ADC_CFG1_ADLSMP_MASK; // select short sample time
    ADC1_CFG1 &= ~ADC_CFG1_ADLPC_MASK;  // select normal power configuration
    
    // Setup Configuration Register 2 
    ADC0_CFG2 = 0; // clear register
    //ADC0_CFG2 |= ADC_CFG2_ADHSC_MASK ;  // select high-speed conversion
    ADC0_CFG2 |= ADC_CFG2_MUXSEL_MASK; // select a channels    //possibly change to channel b.
    ADC1_CFG2 = 0; // clear register
    //ADC1_CFG2 |= ADC_CFG2_ADHSC_MASK ;  // select high-speed conversion
    ADC1_CFG2 &= ~ADC_CFG2_MUXSEL_MASK; // select a channels    
    
    // Setup Status and Control Register 2 
    ADC0_SC2 = 0;                    // clear register
    ADC0_SC2 |= ADC_SC2_REFSEL(0);   // select external voltage reference   //QUESTION
    //ADC0_SC2 |= ADC_SC2_DMAEN_MASK;  // enable DMA
    //ADC0_SC2 &= ~ADC_SC2_ADTRG_MASK; // select hardware trigger  //QESTION
    ADC1_SC2 = 0;                    // clear register
    ADC1_SC2 |= ADC_SC2_REFSEL(0);   // select external voltage reference
    //ADC1_SC2 |= ADC_SC2_DMAEN_MASK;  // enable DMA
    //ADC1_SC2 &= ~ADC_SC2_ADTRG_MASK; // select hardware trigger
    
    // Setup Status and Control Register 3 now that calibration is complete
    ADC0_SC3 = 0; // Hardware Average set to 4 samples averaged
                  // Hardware Average Disabled
                  // select single conversion mode
    ADC1_SC3 = 0; // Hardware Average set to 4 samples averaged
                  // Hardware Average Disabled
                  // select single conversion mode
    //ADC1_SC3 = ADC_SC3_AVGS(0) |  ADC_SC3_AVGE_MASK;//JARED COME BACK AND LOOK AT THIS
    
    // Setup Status and Control Register 1A 
    ADC0_SC1A = 0; // clear register
    ADC0_SC1A &= ~ADC_SC1_DIFF_MASK; // select single-ended mode
    ADC0_SC1A |= ADC_SC1_AIEN_MASK;  // enable interrupt (for debugging)
    ADC0_SC1A |= ADC_SC1_ADCH(12);   // select channel 13
    ADC1_SC1A = 0; // clear register
    ADC1_SC1A &= ~ADC_SC1_DIFF_MASK; // select single-ended mode
    ADC1_SC1A |= ADC_SC1_AIEN_MASK;  // enable interrupt (for debugging)
    ADC1_SC1A |= ADC_SC1_ADCH(14);   // select channel 14
    
    
    // Check if ADC is finished initializing  TODO:  This part doesn't seem right, but I did it according to 871
    while( (ADC0_SC1B&ADC_SC1_COCO_MASK)) {}
    int gain = ADC0_RA; // read the register to clear SC1A[COCO]  //possibly a
    while( (ADC1_SC1A&ADC_SC1_COCO_MASK)) {}
    gain = ADC1_RA; // read the register to clear SC1A[COCO]
    
    
//QUESTION
    ADC0_SC3 &= ~ADC_SC3_ADCO_MASK;  // single conversion mode
    ADC1_SC3 &= ~ADC_SC3_ADCO_MASK;  // single conversion mode
    ADC0_SC2 &= ~ADC_SC2_ADTRG_MASK; // set ADC to software trigger
    ADC1_SC2 &= ~ADC_SC2_ADTRG_MASK; // set ADC to software trigger
    ADC0_SC1A |= ADC_SC1_ADCH(12);   // write to SC1B causing a trigger
    ADC1_SC1A |= ADC_SC1_ADCH(14);   // write to SC1A causing a trigger
}



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 * TimerInterruptInMicroSeconds * 1e-6 * precompute_counter) * 32500.0 + 33000.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));

  }
} //pre_compute_tables




void ISR_repeat() {//40 KHz
  // Calculation of loop for generating signal to output on DAC
  
  WaveOut.write_u16(out_val_pre[phase_counter]);  //creates a wave that bounces between 0 & 3.3 V

  // ISR counter reset and increment
  if (ISR_counter >= ISR_counter_loop_reset)//10KHz
  {
    ISR_counter = 1;
  //JARED ADD THESE 2 LINES BACK IN
    ADC0_SC1A |= ADC_SC1_ADCH(12);   // start ADC process Possibly change to B.
    ADC1_SC1A |= ADC_SC1_ADCH(14);   // start ADC process
    
    ADC_reading = true;
    current_i_mod = i_mod_pre[phase_counter];
    current_q_mod = q_mod_pre[phase_counter];
  }
  else {
    ISR_counter++;
  }
  
  phase_counter++;
  if (phase_counter >= pre_compute_length) phase_counter = 0;
  
} //ISR_repeat


void execute_DSP() {
bp_fifo_num++;  
  if (bp_fifo_num >= bp_fir_length) bp_fifo_num = 0;

   x_0[bp_fifo_num] = (float) (value[1] - value[0]);
   x_1[bp_fifo_num] = (float) (value[0] - 32768);

   // Band pass filter of incoming data
  float y_0 = x_0[bp_fifo_num] * bp_filter_coeff[0];
  float y_1 = x_1[bp_fifo_num] * bp_filter_coeff[0];
  
  for(int fir_counter = 1; fir_counter < bp_fir_length; fir_counter++){
    int fir_index = bp_fifo_num + fir_counter;
    if (fir_index >= bp_fir_length) {
      fir_index -= bp_fir_length;
    }
    y_0 += x_0[fir_index] * bp_filter_coeff[fir_counter];
    y_1 += x_1[fir_index] * bp_filter_coeff[fir_counter];
  }

  current_i_mod = i_mod_pre[phase_counter];
  current_q_mod = q_mod_pre[phase_counter];

  // Demodulation of the signal
  x_0_i[lp_fifo_num] = (y_0 * current_i_mod);
  x_0_q[lp_fifo_num] = (y_0 * current_q_mod);
  x_1_i[lp_fifo_num] = (y_1 * current_i_mod);
  x_1_q[lp_fifo_num] = (y_1 * current_q_mod); 

  lp_fifo_num++;  
  if (lp_fifo_num >= lp_fir_length) lp_fifo_num = 0;

  // Low pass filter of demodulated signal
  float y_0_i = x_0_i[lp_fifo_num] * lp_filter_coeff[0];
  float y_0_q = x_0_q[lp_fifo_num] * lp_filter_coeff[0];
  float y_1_i = x_1_i[lp_fifo_num] * lp_filter_coeff[0];
  float y_1_q = x_1_q[lp_fifo_num] * lp_filter_coeff[0];

  for(int fir_counter = 1; fir_counter < lp_fir_length; fir_counter++){
    int fir_index = lp_fifo_num + fir_counter;
    if (fir_index >= lp_fir_length) {
      fir_index -= lp_fir_length;
    }
    y_0_i += x_0_i[fir_index] * lp_filter_coeff[fir_counter];
    y_0_q += x_0_q[fir_index] * lp_filter_coeff[fir_counter];
    y_1_i += x_1_i[fir_index] * lp_filter_coeff[fir_counter];
    y_1_q += x_1_q[fir_index] * lp_filter_coeff[fir_counter];
  }  
   
  real_ip = y_0_i * y_1_i + y_0_q * y_1_q;
  imag_ip = y_0_q * y_1_i - y_0_i * y_1_q;
  y_0_squared = y_0_i * y_0_i + y_0_q * y_0_q; 
  y_1_squared = y_1_i * y_1_i + y_1_q * y_1_q;

//  float mag = sqrt((real_ip * real_ip + imag_ip * imag_ip) / y_0_squared;
  mag = sqrt(((float) real_ip * (float) real_ip + (float) imag_ip * (float) imag_ip)) / ((float) y_0_squared);
  mag_total += mag;
  calc_mag = mag_total / NumInBlock;

  real_ip_total += real_ip;
  imag_ip_total += imag_ip;
  calc_phase = -atan2(imag_ip_total, real_ip_total);
}



void setup() { 
  adc_init();
  pre_compute_tables();
  wait(1); 
} //setup

using namespace std;
 
int main() {
    led_blue = 1;
    led_green = 1;
    led_red = 0;
    
    setup();
    int count = 0;
    t1.start();
    
    pc.baud(115200);
    
    for(int i = 0; i < 86; i++) 
    {
        if(NVIC_GetPriority((IRQn_Type) i) == 0) NVIC_SetPriority((IRQn_Type) i, 2);
    }
    
    // Give hardware associated with 
    // sampling the highest priority
    NVIC_SetPriority(ADC1_IRQn,0);
    NVIC_SetPriority(ADC0_IRQn,0);
    NVIC_SetPriority(PDB0_IRQn,0);
    NVIC_SetPriority(DMA0_IRQn,0);
    NVIC_SetPriority(DMA1_IRQn,0);
    NVIC_SetPriority(DMA2_IRQn,0);
    
    NVIC_SetPriority(ENET_1588_Timer_IRQn,1);
    NVIC_SetPriority(ENET_Transmit_IRQn,1);
    NVIC_SetPriority(ENET_Receive_IRQn,1);
    NVIC_SetPriority(ENET_Error_IRQn,1);

    led_green = 1;
    led_red = 1;
    led_blue = 1;

    timer0.attach_us(&ISR_repeat, TimerInterruptInMicroSeconds);
    while(true) {
                
                count++;
                //printf("Loop");
                
                while (!ADC_reading)  
                {
                    
                led_blue = 0;
                }

            
              counter++;
              
              CyclesInState++;
              
              
              execute_DSP();
              
              if (CyclesInState <= PreCalculateCycles) {
                NumInBlock = 0;
                mag_total = 0;
                real_ip_total = 0;
                imag_ip_total = 0;
        
                max_0 = -1;
                max_1 = -1;
                min_0 = 65536;
                min_1 = 65536;
              }
              else {
                NumInBlock++;
                if (NumInBlock >= NumCalculatedAverages) {
                  mag_result = calc_mag;
                  phase_result = calc_phase;
                  final_mag = mag_result;
                  final_phase = phase_result;
                  NumInBlock = 0;
                  mag_total = 0;
                  real_ip_total = 0;
                  imag_ip_total = 0;
                  printf("Coeficient%f Phase%f\n\r", final_mag, final_phase);
                }
              }
              
                      
          value[0] = ADC0_RA;//Read adc
          value[1] = ADC1_RA;//Read adc
          //printf("%f %f\n\r", value[0], value[1]);
          ADC_reading = false;//lower flag

    }// end while(true) 
}// end main