SHENG-HEN HSIEH
/
LSM9DS0_STM32compatible
works fine on STM
Fork of Sample_manerine_SPI_LSM9DS0 by
Diff: main.cpp
- Revision:
- 6:c2efb0a3a543
- Parent:
- 5:2f0633d8fc20
- Child:
- 7:f53b52e23818
diff -r 2f0633d8fc20 -r c2efb0a3a543 main.cpp --- a/main.cpp Sat Sep 17 13:24:17 2016 +0000 +++ b/main.cpp Sun Sep 25 06:04:57 2016 +0000 @@ -8,6 +8,11 @@ #define dt 0.005f #define NN 200 +#define Kp 3.6f +#define Ki 5.0f +#define Kd 0.12f +#define Kcon 0.00f + #define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓GPIO registor↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// @@ -109,6 +114,9 @@ float Gdx = 0.0; float Gdy = 0.0; float Gdz = 0.0; +float Adx = 0.0; +float Ady = 0.0; +float Adz = 0.0; float Ele_phy = 0.0; //estimation of top plate attitide float Til_phy = 0.0; float Ele_phy_int = 0.0; @@ -124,7 +132,7 @@ void TT_main(); //timebase function rated by TT void init_IO(); //initialize IO state void init_IMU(); //initialize IMU -void init_Gdrift(); //read Gdrift at start up +void init_GXdrift(); //read GXdrift at start up void read_IMU(); //read IMU data give raw data void state_update(); //estimation of new attitude @@ -145,7 +153,7 @@ init_IO(); //initialized value init_IMU(); //initialize IMU - init_Gdrift(); //read Gdrift at start up + init_GXdrift(); //read GXdrift at start up wait_ms(1000); init_TIMER(); //start TT_main @@ -192,9 +200,9 @@ //for Serial-Oscilloscope // pc.printf("%.3f\r", Bet); -// pc.printf("%.3f,%.3f\r", Ele_phy, Til_phy); + pc.printf("%.3f,%.3f\r", Ele_phy, Til_phy); // pc.printf("%.2f,%.2f\r", VEC[0][4], VEC[0][5]); - pc.printf("%.2f,%.2f,%.2f\r", Ax, Ay, Az); +// pc.printf("%.2f,%.2f,%.2f\r", Ax, Ay, Az); // pc.printf("%.3f,%.3f,%.3f\r", gDIR[0][0], gDIR[0][1], gDIR[0][2]); } //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of Timebase funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// @@ -217,7 +225,7 @@ //gloable config SPI_CSXM = 1; //high as init for disable SPI SPI_CSG = 1; - spi.format(8, 0); //byte width, spi mode + spi.format(8, 3); //byte width, spi mode spi.frequency(4000000); //8MHz //for GYRO config @@ -246,11 +254,11 @@ -//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓init_Gdrift funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// -void init_Gdrift(void) //initialize +//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓init_G/Xdrift funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// +void init_GXdrift(void) //initialize { for(int i=0; i<1000; i++) { - read_IMU(); //note Gdrift = 0 at this moment + read_IMU(); //note GXdrift = 0 at this moment gDIR[0][0] = gDIR[0][0] - Wx; gDIR[0][1] = gDIR[0][1] - Wy; gDIR[0][2] = gDIR[0][2] - Wz; @@ -259,12 +267,23 @@ Gdx = gDIR[0][0] /1000.0f; Gdy = gDIR[0][1] /1000.0f; Gdz = gDIR[0][2] /1000.0f; + + for(int i=0; i<1000; i++) { + read_IMU(); //note GXdrift = 0 at this moment + gDIR[0][0] = gDIR[0][0] - Ax; + gDIR[0][1] = gDIR[0][1] - Ay; + gDIR[0][2] = gDIR[0][2] - Az; + wait_ms(2); + } + Adx = gDIR[0][0] /1000.0f; + Ady = gDIR[0][1] /1000.0f; + Adz = gDIR[0][2] /1000.0f - 1.0f; // pc.printf("%.4f,%.4f,%.4f\r", Gdx, Gdy, Gdz); gDIR[0][0] = 0; gDIR[0][1] = 0; gDIR[0][2] = -1; } -//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of init_Gdrift funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// +//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of init_GXdrift funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// @@ -279,7 +298,7 @@ Buff = high_byte << 8 |low_byte; SPI_CSG = 1; //end spi talking // Wx = Buff * Gpx + Gdx; - Wx = lpf(Buff * Gpx + Gdx, Wx, 25.0f); + Wx = lpf(Buff * Gpx + Gdx, Wx, 48.0f); //Wy SPI_CSG = 0; //start spi talking Wx spi.write(0xEA); //read B11101010 read/multi/address @@ -288,7 +307,7 @@ Buff = high_byte << 8 |low_byte; SPI_CSG = 1; //end spi talking // Wy = Buff * Gpy + Gdy; - Wy = lpf(Buff * Gpy + Gdy, Wy, 25.0f); + Wy = lpf(Buff * Gpy + Gdy, Wy, 48.0f); //Wz SPI_CSG = 0; //start spi talking Wx spi.write(0xEC); //read B11101100 read/multi/address @@ -297,7 +316,7 @@ Buff = high_byte << 8 |low_byte; SPI_CSG = 1; //end spi talking // Wz = Buff * Gpz + Gdz; - Wz = lpf(Buff * Gpz + Gdz, Wz, 25.0f); + Wz = lpf(Buff * Gpz + Gdz, Wz, 48.0f); //Ax SPI_CSXM = 0; //start spi talking Ax spi.write(0xE8); //read B11101000 read/multi/address @@ -305,7 +324,7 @@ high_byte = spi.write(0); Buff = high_byte << 8 |low_byte; SPI_CSXM = 1; //end spi talking - Ax = lpf(Buff*Apx + Adx, Ax, 15.0f); + Ax = lpf(Buff*Apx + Adx, Ax, 13.0f); //Ay SPI_CSXM = 0; //start spi talking Ax spi.write(0xEA); //read B11101010 read/multi/address @@ -313,7 +332,7 @@ high_byte = spi.write(0); Buff = high_byte << 8 |low_byte; SPI_CSXM = 1; //end spi talking - Ay = lpf(Buff*Apy + Ady, Ay, 15.0f); + Ay = lpf(Buff*Apy + Ady, Ay, 13.0f); //Az SPI_CSXM = 0; //start spi talking Ax spi.write(0xEC); //read B11101100 read/multi/address @@ -321,7 +340,7 @@ high_byte = spi.write(0); Buff = high_byte << 8 |low_byte; SPI_CSXM = 1; //end spi talking - Az = lpf(Buff*Apz + Adz, Az, 15.0f); + Az = lpf(Buff*Apz + Adz, Az, 13.0f); } //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of read_IMU funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// @@ -363,17 +382,17 @@ { Ele_phy_int = Ele_phy_int + Ele_phy*dt; Til_phy_int = Til_phy_int + Til_phy*dt; - Ele_phy_int = constrain(Ele_phy_int,-0.006f,0.006f); - Til_phy_int = constrain(Til_phy_int,-0.006f,0.006f); + Ele_phy_int = constrain(Ele_phy_int,-0.090f,0.090f); + Til_phy_int = constrain(Til_phy_int,-0.090f,0.090f); - Ele_control = -3.5f*Ele_phy - 0.10f*Wy - 20.0f*Ele_phy_int; - Til_control = -3.5f*Til_phy - 0.10f*Wx - 20.0f*Til_phy_int; + Ele_control = -Kp*Ele_phy - Kd*Wy - Ki*Ele_phy_int - Kcon*Wx; + Til_control = -Kp*Til_phy - Kd*Wx - Ki*Til_phy_int - Kcon*Wy; Ele_control = constrain(Ele_control,-0.35f,0.35f); Til_control = constrain(Til_control,-0.35f,0.35f); - VEC[0][4] = lpf(Ele_control, VEC[0][4], 20.0f); - VEC[0][5] = lpf(Til_control, VEC[0][5], 20.0f); + VEC[0][4] = lpf(Ele_control, VEC[0][4], 21.0f); + VEC[0][5] = lpf(Til_control, VEC[0][5], 21.0f); VEC[0][2] = Z0 -Z_dis; } //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of RR funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//