Diff: main.cpp

Revision:

1:df1ae6d3fd03

Parent:

0:15495bc2362a

--- a/main.cpp	Tue Nov 13 01:28:09 2018 +0000
+++ b/main.cpp	Tue Nov 13 19:27:58 2018 +0000
@@ -1,31 +1,488 @@
-#include "mbed.h"
-
-AnalogIn analog_value(A0);
-
-DigitalOut led(LED1);
-
-int main()
-{
-    float meas_r;
-    float meas_v;
-    
-    printf("\nAnalogIn example\n");
-
-    while(1) {
-
-        meas_r = analog_value.read(); // Read the analog input value (value from 0.0 to 1.0 = full ADC conversion range)
-        meas_v = meas_r * 3300; // Converts value in the 0V-3.3V range
-        
-        // Display values
-        printf("measure = %f = %.0f mV\n", meas_r, meas_v);
-
-        // LED is ON is the value is below 1V
-        if (meas_v < 1000) {
-            led = 1; // LED ON
-        } else {
-            led = 0; // LED OFF
-        }
-
-        wait(1.0); // 1 second
-    }
-}
+///////////////////////////////////////////////////////////////////////////
+/** 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;
+
+
+        
+        
+        
+    }
+    
+}