Diff: main.cpp

Revision:

3:502b83f7761c

Parent:

2:0d90c0436797

Child:

4:b9dd320947ff

--- a/main.cpp	Thu Sep 01 17:14:29 2016 +0000
+++ b/main.cpp	Mon Sep 05 14:43:43 2016 +0000
@@ -1,6 +1,9 @@
 #include "mbed.h"
 #include "LSM9DS0_SH.h"
-#define pi 3.141592
+#define pi  3.141592f
+#define d2r 0.01745329f
+#define Rms 5000
+#define dt  0.005f
 #define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
 
 
@@ -26,25 +29,30 @@
 int     count = 0;                  //one second counter for extrenal led blink
 
 //~~~IMU_SPI~~~//
-short low_byte = 0x00;               //buffer
+short low_byte = 0x00;              //buffer
 short high_byte = 0x00;
-
-short Wx = 0x00;
-short Wy = 0x00;
-short Wz = 0x00;
-
-short Ax = 0x00;
-short Ay = 0x00;
-short Az = 0x00;
+short Buff = 0x00;
+float Wx = 0.0;
+float Wy = 0.0;
+float Wz = 0.0;
+float Ax = 0.0;
+float Ay = 0.0;
+float Az = 0.0;
+float gDIR[1][3] = {                //g vector's direction , required unitconstrain
+    {0,0,-1},                       //X Y Z
+};
+float gUnity = 0;
 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of Varible registor↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
 
 
 
 //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓Function registor↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
+void init_TIMER();          //set TT_main() rate
+void TT_main();             //timebase function rated by TT
 void init_IO();             //initialize IO state
 void init_IMU();            //initialize IMU
-void init_TIMER();          //set TT_main() rate
-void TT_main();             //timebase function rated by TT
+void read_IMU();            //read IMU data give raw data
+void state_update();        //estimation of new attitude
 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of Function registor↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
 
 
@@ -69,25 +77,47 @@
 
 
 
-//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓init_IO funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
+//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓Timebase funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
+void init_TIMER()                   //set TT_main{} rate
+{
+    TT.attach_us(&TT_main, Rms);
+}
+void TT_main()                      //interrupt function by TT
+{
+    TT_ext = !TT_ext;               //indicate TT_main() function working
+    count = count+1;                //one second counter
+
+    read_IMU();                     //read IMU data give raw data
+    state_update();                 //estimation of new attitude
+
+    pc.printf("%.2f\t%.2f\t%.2f\n", gDIR[0][0], gDIR[0][1], gDIR[0][2]);
+//    pc.printf("%.2f\t%.2f\n", Sele, Srol);
+//    pc.printf("%.2f\n", Sx);
+}
+//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of Timebase funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
+
+
+
+//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓init_IO funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
 void init_IO(void)                  //initialize
 {
     TT_ext = 0;
     led = 1;
 }
-//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of init_IO funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
+//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of init_IO funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
 
 
 
-//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓init_IMU funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
-void init_IMU(void)                  //initialize
+//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓init_IMU funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
+void init_IMU(void)                 //initialize
 {
+    //gloable config
     SPI_CSXM = 1;                   //high as init for disable SPI
     SPI_CSG = 1;
     spi.format(8, 0);               //byte width, spi mode
     spi.frequency(4000000);         //8MHz
 
-//for GYRO config
+    //for GYRO config
     SPI_CSG = 0;                    //start spi talking
     spi.write(CTRL_REG1_G);
     spi.write(0x9F);                //data rate 380 Hz/ cut off 25 Hz
@@ -98,7 +128,7 @@
     spi.write(0x10);                //Scle 500dps
     SPI_CSG = 1;                    //end spi talking
 
-//for ACC config
+    //for ACC config
     SPI_CSXM = 0;                   //start spi talking
     spi.write(CTRL_REG1_XM);
     spi.write(0x87);                //data rate 400 Hz/ Enable
@@ -106,38 +136,84 @@
 
     SPI_CSXM = 0;                   //start spi talking
     spi.write(CTRL_REG2_XM);
-    spi.write(0xC0);                //cut off 50 Hz/ Scale +-2g
+    spi.write(0xC8);                //cut off 50 Hz/ Scale +-4g
     SPI_CSXM = 1;                   //end spi talking
 }
-//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of init_IMU funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
+//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of init_IMU funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
 
 
 
-//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓Timebase funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
-void init_TIMER()                   //set TT_main{} rate
+//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓read_IMU funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
+void read_IMU(void)                 //read IMU data give raw data
 {
-    TT.attach_us(&TT_main, 5000);
-}
-void TT_main()                      //interrupt function by TT
-{
-    TT_ext = !TT_ext;               //indicate TT_main() function working
-    count = count+1;                //one second counter
-
-    SPI_CSG = 0;                    //start spi talking
+    //Wx
+    SPI_CSG = 0;                    //start spi talking Wx
+    spi.write(0xE8);                //read B11101000 read/multi/address
+    low_byte = spi.write(0);
+    high_byte = spi.write(0);
+    Buff = high_byte << 8 |low_byte;
+    SPI_CSG = 1;                    //end spi talking
+    Wx = Buff * 2.663e-4; //1.526e-2 * d2r =
+    //Wy
+    SPI_CSG = 0;                    //start spi talking Wx
+    spi.write(0xEA);                //read B11101010 read/multi/address
+    low_byte = spi.write(0);
+    high_byte = spi.write(0);
+    Buff = high_byte << 8 |low_byte;
+    SPI_CSG = 1;                    //end spi talking
+    Wy = Buff * 2.663e-4; //1.526e-2 * d2r =
+    //Wz
+    SPI_CSG = 0;                    //start spi talking Wx
+    spi.write(0xEC);                //read B11101100 read/multi/address
+    low_byte = spi.write(0);
+    high_byte = spi.write(0);
+    Buff = high_byte << 8 |low_byte;
+    SPI_CSG = 1;                    //end spi talking
+    Wz = Buff * 2.663e-4; //1.526e-2 * d2r =
+    //Ax
+    SPI_CSXM = 0;                   //start spi talking Ax
     spi.write(0xE8);                //read B11101000 read/multi/address
     low_byte = spi.write(0);
     high_byte = spi.write(0);
-    Wx = high_byte << 8 |low_byte;
-    SPI_CSG = 1;                    //end spi talking
+    Buff = high_byte << 8 |low_byte;
+    SPI_CSXM = 1;                   //end spi talking
+    Ax = Buff * 1.22e-4;
+    //Ay
+    SPI_CSXM = 0;                   //start spi talking Ax
+    spi.write(0xEA);                //read B11101010 read/multi/address
+    low_byte = spi.write(0);
+    high_byte = spi.write(0);
+    Buff = high_byte << 8 |low_byte;
+    SPI_CSXM = 1;                   //end spi talking
+    Ay = Buff * 1.22e-4;
+    //Az
+    SPI_CSXM = 0;                   //start spi talking Ax
+    spi.write(0xEC);                //read B11101100 read/multi/address
+    low_byte = spi.write(0);
+    high_byte = spi.write(0);
+    Buff = high_byte << 8 |low_byte;
+    SPI_CSXM = 1;                   //end spi talking
+    Az = Buff * 1.22e-4;
+}
+//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of read_IMU funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
 
 
-    SPI_CSXM = 0;                   //start spi talking
-    spi.write(0xE8);                //read B11101000 read/multi/address
-    low_byte = spi.write(0);
-    high_byte = spi.write(0);
-    Ax = high_byte << 8 |low_byte;
-    SPI_CSXM = 1;                   //end spi talking
 
-    pc.printf("%d\t%d\n", Wx, Ax);
+//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓state_update funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
+void state_update(void)         //estimation of new attitude
+{
+    //pridict
+    gDIR[0][0] = gDIR[0][0] - (Wy*gDIR[0][2] - Wz*gDIR[0][1])*dt;
+    gDIR[0][1] = gDIR[0][1] - (Wz*gDIR[0][0] - Wx*gDIR[0][2])*dt;
+    gDIR[0][2] = gDIR[0][2] - (Wx*gDIR[0][1] - Wy*gDIR[0][0])*dt;
+    
+    //update
+    
+    
+    //nutralizing
+    gUnity = gDIR[0][0]*gDIR[0][0] + gDIR[0][1]*gDIR[0][1] + gDIR[0][2]*gDIR[0][2];
+    for(int i=0; i<3; i++) {
+        gDIR[0][i] =  gDIR[0][i] / gUnity;
+    }
 }
-//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of Timebase funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
\ No newline at end of file
+//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of state_update funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
\ No newline at end of file