main.cpp

00001 // Copyright (C) 2018 Analog Devices, Inc. - All Rights Reserved
00002 #include "mbed.h"
00003 #include "arm_math.h"
00004 
00005 I2C i2c(D4, D5); // I2C i2c(D14, D15);
00006 DigitalIn gpio0(D9);
00007 Serial serial(PA_2, PA_15, 115200); //Serial serial(D0, D1, 115200);
00008 
00009 #define W 5
00010 #define NTAPS 25
00011 
00012 volatile float32_t x_YT[W], x_YB[W], x_XL[W], x_XR[W];
00013 
00014 float32_t coefs[10]={0.310235, 0, -0.310235, -0.602633, -0.379530, 0.310235, 0, -0.310235, -0.602633, -0.379530};
00015 float32_t state_vars[8];
00016 arm_biquad_casd_df1_inst_f32 filter;
00017 
00018 float32_t Q_taps[NTAPS]={1.000000, -0.409069, -0.665326,  0.953396, -0.114683, -0.859570,  0.817929,  0.190391,
00019        -0.973695,  0.606225,  0.477720, -0.997066,  0.338017,  0.720522, -0.927502,  0.038303,
00020         0.896166, -0.771489, -0.264982,  0.988280, -0.543568, -0.543568,  0.988280, -0.264982,
00021        -0.771489};
00022 
00023 float32_t I_taps[NTAPS]={0.000000,  0.912504, -0.746553, -0.301721,  0.993402, -0.511019, -0.575319,  0.981708,
00024        -0.227854, -0.795293,  0.878512,  0.076549, -0.941140,  0.693433,  0.373817, -0.999266,
00025         0.443720,  0.636242, -0.964253,  0.152649,  0.839365, -0.839365, -0.152649,  0.964253,
00026        -0.636242};
00027 
00028 uint16_t ADPD_Read_Register(uint8_t register_address)
00029 {
00030     char data[1] = {register_address};
00031     char buffer[2];
00032     i2c.write(0xC8, data, 1);
00033     i2c.read(0xC9, buffer, 2);
00034     uint16_t register_value = buffer[0]*256 + buffer[1];
00035     return(register_value);
00036 }
00037 
00038 void ADPD_Write_Register(uint8_t register_address, uint16_t register_value)
00039 {
00040     char data[3];
00041     data[0] = register_address;
00042     data[1] = register_value>>8;
00043     data[2] = register_value;
00044     i2c.write(0xC8, data, 3);
00045 }
00046 
00047 void ADPD_Load_Config(void)
00048 {
00049     ADPD_Write_Register(0x10, 0x0001);
00050     ADPD_Write_Register(0x00, 0x80FF);
00051     ADPD_Write_Register(0x01, 0x01BF);
00052     ADPD_Write_Register(0x02, 0x0005);
00053     ADPD_Write_Register(0x06, 0x0000);
00054     ADPD_Write_Register(0x11, 0x3120);
00055     ADPD_Write_Register(0x12, 0x000D);
00056     ADPD_Write_Register(0x14, 0x0550);
00057     ADPD_Write_Register(0x15, 0x0000);
00058     ADPD_Write_Register(0x1E, 0x1F00);
00059     ADPD_Write_Register(0x1F, 0x1F00);
00060     ADPD_Write_Register(0x20, 0x1F00);
00061     ADPD_Write_Register(0x21, 0x1F00);
00062     ADPD_Write_Register(0x34, 0x0000);
00063     ADPD_Write_Register(0x35, 0x0210);
00064     ADPD_Write_Register(0x36, 0x5E0E);
00065     ADPD_Write_Register(0x3B, 0x20F4);
00066     ADPD_Write_Register(0x3C, 0x3006);
00067     ADPD_Write_Register(0x44, 0x1C37);
00068     ADPD_Write_Register(0x45, 0xADA5);
00069     ADPD_Write_Register(0x4B, 0x2695);
00070     ADPD_Write_Register(0x54, 0x0AA0);
00071     
00072     ADPD_Write_Register(0x58, 0x0820);
00073     ADPD_Write_Register(0x1D, 0x0006);
00074 }
00075 
00076 void get_window(void)
00077 {
00078     uint8_t r = 0;
00079     
00080     ADPD_Write_Register(0x10, 0x0002);
00081     while(r<W+1) {
00082         if (gpio0 == 0) {
00083             if (r==0) {
00084                 ADPD_Read_Register(0x68);
00085                 ADPD_Read_Register(0x69);   
00086                 ADPD_Read_Register(0x6A);   
00087                 ADPD_Read_Register(0x6B);
00088                 ADPD_Write_Register(0x00, 0x80FF);
00089             } else {
00090                 x_YT[r-1] = ADPD_Read_Register(0x68);
00091                 x_YB[r-1] = ADPD_Read_Register(0x69);
00092                 x_XL[r-1] = ADPD_Read_Register(0x6A);
00093                 x_XR[r-1] = ADPD_Read_Register(0x6B);
00094                 ADPD_Write_Register(0x00, 0x80FF);
00095             }
00096             r++;
00097         }
00098     }
00099     ADPD_Write_Register(0x10, 0x0000);
00100 }
00101 
00102 
00103 float32_t demod(float32_t* x)
00104 {
00105     float32_t min_val, max_val, temp;
00106     uint32_t jnk;
00107     arm_max_f32(x, W, &max_val, &jnk);
00108     arm_min_f32(x, W, &min_val, &jnk);
00109     temp = max_val - min_val;
00110 
00111     return temp;
00112 }
00113 
00114 
00115 int main() {
00116     float32_t d_YT = 0, d_YB = 0, d_XL = 0,d_XR = 0;
00117     float32_t Xangle = 0, Yangle = 0;
00118     arm_biquad_cascade_df1_init_f32(&filter,2,coefs,state_vars);
00119     i2c.frequency(400000);    
00120     ADPD_Load_Config();
00121     int decimate = 4;
00122     int i = 0;
00123     while(1) {
00124         d_YT = 0;
00125         d_YB = 0;
00126         d_XL = 0;
00127         d_XR = 0;
00128         for(i=0;i<decimate;i++) {
00129             get_window();
00130             d_YT += demod( (float32_t*) x_YT);
00131             d_YB += demod( (float32_t*) x_YB);
00132             d_XL += demod( (float32_t*) x_XL);
00133             d_XR += demod( (float32_t*) x_XR);
00134         }
00135         Xangle = (d_XL - d_XR)/(d_XL + d_XR) / 0.005;
00136         Yangle = (d_YT - d_YB)/(d_YT + d_YB) / 0.005;
00137         serial.printf("# %2.5f %2.5f\n\r", Xangle, Yangle);
00138     }
00139 }