SHENG-HEN HSIEH
works fine on STM
Fork of
Sample_manerine_SPI_LSM9DS0
by 
SHENG-HEN HSIEH
Diff: main.cpp
- Revision:
- 6:c2efb0a3a543
- Parent:
- 5:2f0633d8fc20
- Child:
- 7:f53b52e23818
--- 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↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//