File content as of revision 1:df1ae6d3fd03:

///////////////////////////////////////////////////////////////////////////
/** FS & SS EMULATOR CODE
 *
 * Version: 1.0
 * Author: Rachel Yang (Adapted by Patrick Doyle)
 * Last Date Modified: 2018.10.2
 *
 * Target Device: STM32F413
 *
 * Description:
 *      digital filter for SS (2nd order) and FS (main peak only)
 *
 * Notes:
 *      uses 2 external ADCs (e.g. LTC1992) for FS & SS inputs
 *      uses internal TIM2 to precisely control sampling rate (fs)
 *      uses internal DACs to output single-ended FS & SS outputs
 */
///////////////////////////////////////////////////////////////////////////

#include "mbed.h"

#ifdef __cplusplus
extern "C"
#endif

#define TIM_USR      TIM2
#define TIM_USR_IRQ  TIM2_IRQn

// initialize serial debug (UART)??
//Serial pc(PB_6, PB_8);  // USBTX, USBRX

// initialize SPI
SPI spi(NC, PA_12_ALT0, PB_0); // mosi, miso, sclk (corresponds to SPI5, default hardware connection)
//SPI spi(NC, PA_6, PB_3); // mosi, miso, sclk (corresponds to SPI1)
DigitalOut cs_fs(PB_1);   // FS chip select
DigitalOut cs_ss(PA_9);   // SS chip select

// initialize LED (used as indicator to confirm code uploaded)
DigitalOut myled1(PB_9);    // corresponds to LED1 (red on display emulator)

// pin declarations
//AnalogIn input_pin(PC_0);   // corresponds to A1 (A0 on F413ZH DISCO)
AnalogOut output_pin_n(PA_4); // HPZR SE output from MCU
AnalogOut output_pin_p(PA_5); // VPZR SE output from MCU

// for internal input read
int outputimpulse = 1;

// --- constants and global variables for SS ---

// constants for transfer function for SS
// note: when extracting constants, having more decimal places is more accurate
// H(z) =  a0 + a1*z^-1 + a2*z^-2 + a3*z^-3 + a4*z^-4 + a5*z^-5
//        ------------------------------------------------------
//         b0 + b1*z^-1 + b2*z^-2 + b3*z^-3 + b4*z^-4 + b5*z^-5

// for fs = 198 kHz: (samp_period = 20 with prescaler = 23)
//const float a0 = 2.347062E-05;
//const float a1 = 2.442144E-05;
//const float a2 = -4.408877E-05;
//const float a3 = -4.408877E-05;
//const float a4 = 2.442144E-05;
//const float a5 = 2.347062E-05;
//const float b0 = 1.0;
//const float b1 = -3.065112;
//const float b2 = 2.320246;
//const float b3 = 1.468017;
//const float b4 = -2.636283;
//const float b5 = 9.131404E-01;

const float gain_adj_ss=-.4; //set gain adjustment to lock mirrors

// for fs = 182 kHz: (samp_period = 21 with prescaler = 24)
float a0 = 2.790071E-5;
float a1 = 2.923599E-5;
float a2 = -5.179560E-5;
float a3 = -5.179560E-5;
float a4 = 2.923599E-5;
float a5 = 2.790071E-5;
float b0 = 1.0;
float b1 = -3.054730;
float b2 = 2.289613;
float b3 = 1.504730;
float b4 = -2.659329;
float b5 = 0.919726;

// filter's input and output arrays

int x_ss0 = 0;
int x_ss1 = 0;
int x_ss2 = 0;
int x_ss3 = 0;
int x_ss4 = 0;
int x_ss5 = 0;


float y_ss0 = 0;
float y_ss1 = 0;
float y_ss2 = 0;
float y_ss3 = 0;
float y_ss4 = 0;
float y_ss5 = 0;

int i2 =0;

// --- constants and global variables for FS ---

// constants for transfer function for FS
// note: when extracting constants, having more decimal places is more accurate
// H(z) =  c0 + c1*z^-1 + c2*z^-2
//        ------------------------
//         d0 + d1*z^-1 + d2*z^-2

// for fs = 198 kHz prewarped & scaled: (samp_period = 20 with prescaler = 23)
//const float c0 = 1.216980E-02;
//const float c1 = 0;
//const float c2 = -1.216980E-02;
//const float d0 = 1.0;
//const float d1 = -1.366137;
//const float d2 = 9.997566E-01;


//198kHz, Q = 1000
//float c0 = 3.650050e-02;
//float c1 = 0;
//float c2 = -3.650050e-02;
//float d0 = 1;
//float d1 = -1.365804e+00;
//float d2 = 9.992700e-01;

//198kHz, Q = 500
//float c0 = 7.297437e-02;
//float c1 = 0;
//float c2 = -7.297437e-02;
//float d0 = 1;
//float d1 = -1.365306e+00;
//float d2 = 9.985405e-01;

//198kHz, Q = 100
//float c0 = 3.638099e-01;
//float c1 = 0;
//float c2 = -3.638099e-01;
//float d0 = 1;
//float d1 = -1.361332e+00;
//float d2 = 9.927238e-01;

//198kHz, Q = 10
//float c0 = 3.522754e+00;
//float c1 = 0;
//float c2 = -3.522754e+00;
//float d0 = 1;
//float d1 = -1.318172e+00;
//float d2 = 9.295449e-01;


//265kHz, Q = 3000
//float c0 = 9.570427e-03;
//float c1 = 0;
//float c2 = -9.570427e-03;
//float d0 = 1;
//float d1 = -1.637160e+00;
//float d2 = 9.998086e-01;

//265kHz, Q = 1000
//float c0 = 2.870579e-02;
//float c1 = 0;
//float c2 = -2.870579e-02;
//float d0 = 1;
//float d1 = -1.636847e+00;
//float d2 = 9.994259e-01;

//265kHz, Q = 500
//float c0 = 5.739510e-02;
//float c1 = 0;
//float c2 = -5.739510e-02;
//float d0 = 1;
//float d1 = -1.636377e+00;
//float d2 = 9.988521e-01;

//Elena's
// fr=2.8kHz, Q=1000, fs=277,778 z
//float c0 = 0.08273001473594155;
//
//float c1 = 0.16546002947188265;
//
//float c2 = 0.08273001473594155;
//
//float d0 = 1.0;
//
//float d1 = -1.6685290763800154;
//
//float d2 = 0.9994491353237815;


//@277,778 Hz Fs
//float c0 = 0.08274520854011191;
//
//float c1 = 0.16549041708022338;
//
//float c2 = 0.08274520854011225;
//
//float d0 = 1.0;
//
//float d1 = -1.6688355105577437;
//
//float d2 = 0.999816344718191;

float gain_adj_fs= 2*2*(.25*(.5*(.5*(.25*.29))))/5;// (1/atteunation relative to 10x)(multiplier)(starting point)  ///.9*.029; //set final gain adjustment to lock mirrors. Targets In/Out ratio of .82 @26kHz (to match Prototype)
//most stable prior to hpzr attenuator addition 2*(.25*(.5*(.5*(.25*.29))))/5;
//most stable in general, w/ attenuator 2*2*(.25*(.5*(.5*(.25*.29))))/5;

//float gain_adj_fs=.002;

// for fs = 182 kHz, Q=3000 prewarped & scaled: (samp_period = 21 with prescaler = 24)
//float c0 = 0.0129569;
//float c1 = 0;
//float c2 = -0.0129569;
//float d0 = 1.0;
//float d1 = -1.257566;
//float d2 = 0.999741;


// for fs = 182000, Q=100
float c0 = 3.872520e-01;
float c1 = 0;
float c2 = -3.872520e-01;
float d0 = 1;
float d1 = -1.252858e+00;
float d2 = 9.922550e-01;


// filter's input and output arrays
int x_fs0 = 0;
int x_fs1 = 0;
int x_fs2 = 0;
int x_fs3 = 0;

float y_fs0 = 0;
float y_fs1 = 0;
float y_fs2 = 0;
float y_fs3 = 0;

// --- FUNCTIONS ---

// --- functions for handling TIM2 initialization to control sampling rate ---

//void SysTick_Handler(void)
//{
//    HAL_IncTick();
//    HAL_SYSTICK_IRQHandler();
//}
//
//// --- end functions for TIM2 ---



/** reads external ADC from SPI
 *      @param cs (chip select pin)
 *      @returns ADC input value (range from -2048 to 2047)
 */

int input = 0;
int read_adc(DigitalOut cs){
    input = 0;
    
    // select external adc
    cs = 0; 
    
    // send dummy byte to receive ADC read
    uint16_t adc_read = spi.write(0x0);
//    pc.printf("ADC = %X ", adc_read);     // for debug

    // deselect external adc
    cs = 1; 

    // bit shift adc_input for 12-bit resolution
    uint16_t input_temp = adc_read >> 3;
    
    // convert uint16_t to int using two's complement for Differential ADC (not used anymore)
//    if ((input_temp & 0x800) == 0x800) {    // check if sign bit is raised (i.e. neg value)
//        input_temp = ~(input_temp-1);       // convert from two's complement form
//        input = -1*(input_temp^0xF000);     // mask input_temp for 12 bits & add sign back in
//    } else {
//        input = input_temp; // no action if sign bit is not raised (i.e. pos value)
//    }


 //Assuming non-differential ADC (AD7046)
    input = input_temp; // no action if sign bit is not raised (i.e. pos value)
    
    return input;
}

/** computes and writes SS output to DAC based on SS difference eqn using ADC input
 *      @returns none
 */
void compute_ss_output(){

    // shift inputs for new time step
    x_ss5 = x_ss4;    // x[n-4] --> x[n-5]
    x_ss4 = x_ss3;    // x[n-3] --> x[n-4]
    x_ss3 = x_ss2;    // x[n-2] --> x[n-3]
    x_ss2 = x_ss1;    // x[n-1] --> x[n-2]
    x_ss1 = x_ss0;    // x[n]   --> x[n-1]

    //shift outputs for new time step    
    y_ss5 = y_ss4;    // y[n-4] --> y[n-5]
    y_ss4 = y_ss3;    // y[n-3] --> y[n-4]
    y_ss3 = y_ss2;    // y[n-2] --> y[n-3]
    y_ss2 = y_ss1;    // y[n-1] --> y[n-2]
    y_ss1 = y_ss0;    // y[n]   --> y[n-1]

    // read input x[n] from SS DRV P
    x_ss0 = read_adc(cs_ss);     // value from 0 to 4095 for 12-bit ADC
//    pc.printf("ADC = %d ", x_ss[0]);    // for debug
    
    // for verifying impulse response
    // "read" input x[n] (internal input read)
//    if (outputimpulse > 0) {
//        x_ss[0] = 2047;
//        outputimpulse = outputimpulse - 1;
//    } else {
//        x_ss[0] = 0;
//    }
    
    // calculate output y[n] (need intermediary computations to prevent overflow)
    float xterms = a0*x_ss0+a1*x_ss1+a2*x_ss2+a3*x_ss3+a4*x_ss4+a5*x_ss5;
    float yterms = b1*y_ss1+b2*y_ss2+b3*y_ss3+b4*y_ss4+b5*y_ss5;
    y_ss0 =  (xterms - yterms); // *1/b0
    
//    pc.printf("xterms = %f ", xterms);
//    pc.printf("yterms = %f ", yterms);
//    pc.printf("y[0] = %f ", y[0]);
    
    // --- for writing DAC output directly to register ---
    
    // scale y[n], then bias y[n] at VCC/2 = 1.25V and scale gain
    uint16_t output = (uint16_t) (gain_adj_ss*y_ss0+2048);
//    pc.printf(" %d ",output);
    
    // set DAC output by writing directly to register (pin PA_5)
    // note: DAC register takes in unsigned 12-bit (i.e. 0 to 4096)
    DAC->DHR12R2 = output;
    


    // - end -
 // myled1 = !myled1;
}

/** computes and writes FS output to DAC based on FS difference eqn using ADC input
 *      @returns none
 */

 uint16_t output=2048;
void compute_fs_output(){
    //myled1 = !myled1;
    DAC->DHR12R1 = output;
  //  myled1 = !myled1;
    // shift inputs for new time step
    x_fs2 = x_fs1;  // x[n-1] --> x[n-2]
    x_fs1 = x_fs0;  // x[n]   --> x[n-1]

    //shift outputs for new time step  
    y_fs2 = y_fs1;  // y[n-1] --> y[n-2]
    y_fs1 = y_fs0;  // y[n]   --> y[n-1]

    // read input x[n] - ONLY READ FROM OUTPUT_P (not N)
    x_fs0 = read_adc(cs_fs);     // value from -2048 to 2047 for 12-bit ADC
//    pc.printf("ADC = %d ", x_fs[0]);    // for debug
    //DAC->DHR12R1 = (uint16_t) (x_fs[0]); //debug
    

    
    // calculate output y[n] (need intermediary computations to prevent overflow)
//    float xterms = c0*x_fs[0]+c1*x_fs[1]+c2*x_fs[2];  // explicit c1 = 0
    float xterms = c0*x_fs0+c2*x_fs2;     // implicit c1 = 0
    float yterms = d1*y_fs1+d2*y_fs2;
    y_fs0 = (xterms - yterms); // *1/d0

    


    // --- for writing DAC output directly to register ---
    
    // scale y[n], then bias y[n] at VCC/2 = 1.25V and scale gain
   output = (uint16_t) (gain_adj_fs*y_fs0+2048);
//    pc.printf(" %d ",output);
    

    // set DAC output by writing directly to register (pin PA_4)
    // note: DAC register takes in unsigned 12-bit (i.e. 0 to 4096)
        //DAC->DHR12R1 = output;
    
    
}




TIM_HandleTypeDef mTimUserHandle;
 int iteration1=0;
 int iteration2=0;
extern "C"
void M_TIM_USR_Handler(void) {
  //myled1 = 0;//!myled1;

  if (__HAL_TIM_GET_FLAG(&mTimUserHandle, TIM_FLAG_UPDATE) == SET) {

    myled1 = 1;//!myled1;
    __HAL_TIM_CLEAR_FLAG(&mTimUserHandle, TIM_FLAG_UPDATE);
    
    //for(int i = 0; i < 17; i++){
    //    myled1 = !myled1;


   // }

   compute_fs_output();
   compute_ss_output();
   myled1=0;

    //if (__HAL_TIM_GET_FLAG(&mTimUserHandle, TIM_FLAG_UPDATE) == SET) {
//        myled1 = !myled1;
//        }
  }
}


int main()
{
    // set serial debug baud rate
   // pc.baud(9600);

    // deselect external adcs
    cs_ss = 1;
    cs_fs = 1;

    // set up spi
    spi.format(16,3);
    spi.frequency(50000000);    // 50MHz (fastest possible with ADC)

     // LED sequence to indicate code uploaded
    for(int i = 0; i < 10; i++){
        myled1 = i & 0x1;
        wait(0.1);
    }
    
    
    // initialize timer TIM2
    __HAL_RCC_TIM2_CLK_ENABLE();

  mTimUserHandle.Instance               = TIM_USR;
  mTimUserHandle.Init.Prescaler         = 0;
  mTimUserHandle.Init.CounterMode       = TIM_COUNTERMODE_UP;
  mTimUserHandle.Init.Period            = 549; //549 FOR 182KHz //504 for 198kHz, 359 for 277khz
  mTimUserHandle.Init.ClockDivision     = TIM_CLOCKDIVISION_DIV1;
  HAL_TIM_Base_Init(&mTimUserHandle);
  HAL_TIM_Base_Start_IT(&mTimUserHandle);

  NVIC_SetVector(TIM_USR_IRQ, (uint32_t)M_TIM_USR_Handler);
  NVIC_EnableIRQ(TIM_USR_IRQ);


    //pc.printf("\nstarting ss_rlc\n");

    // to read system clock (max at 100MHz)
//    SystemCoreClock = 125000000;  // set CPU clk (can overclock to 125MHz max)
    //pc.printf("SystemCoreClock = %d", SystemCoreClock);
//
//uint16_t dac_out=0;
//uint16_t output = (uint16_t) (4000);
    while(1) {
    myled1 = 1;
    myled1 = 0;



           //myled1=!myled1;
         //       DAC->DHR12R1 = output;


        
        
        
    }
    
}