SHENG-HEN HSIEH
/
LSM9DS0_STM32compatible
works fine on STM
Fork of Sample_manerine_SPI_LSM9DS0 by
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