SHENG-HEN HSIEH
/
LSM9DS0_STM32compatible
works fine on STM
Fork of Sample_manerine_SPI_LSM9DS0 by
main.cpp
- Committer:
- open4416
- Date:
- 2017-02-13
- Revision:
- 7:f53b52e23818
- Parent:
- 6:c2efb0a3a543
File content as of revision 7:f53b52e23818:
#include "mbed.h" #include "LSM9DS0_SH.h" #define pi 3.141592f #define d2r 0.01745329f #define Rms 5000 //TT rate #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↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// //~~~structure~~~// DigitalOut led(D13); //detection DigitalOut TT_ext(D12); //~~~IMU_SPI~~~// DigitalOut SPI_CSG(D7,1); //low for GYRO enable DigitalOut SPI_CSXM(D6,1); //low for ACC/MAG enable SPI spi(D4, D5, D3); //MOSI MISO SCLK //~~~Serial~~~// Serial pc(D1, D0); //Serial reg(TX RX) //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of GPIO registor↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓Varible registor↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// //~~~globle~~~// Ticker TT; //call a timer int count = 0; //one second counter for extrenal led blink //~~~IMU_SPI~~~// short low_byte = 0x00; //buffer short high_byte = 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; //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑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 read_IMU(); //read IMU data give raw data float lpf(float input, float output_old, float frequency); //lpf discrete //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of Function registor↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓main funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// int main() { pc.baud(115200); //set baud rate init_IO(); //initialized value init_IMU(); //initialize IMU init_TIMER(); //start TT_main while(1) { //main() loop if(count >= NN) { //check if main working count=0; led = !led; } } } //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of main 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 //for Serial-Oscilloscope pc.printf("%.2f,%.2f,%.2f\r", Ax, Ay, Az); } //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of Timebase funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓init_IO funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// void init_IO(void) //initialize { TT_ext = 0; led = 1; } //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of init_IO funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓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, 3); //byte width, spi mode spi.frequency(4000000); //8MHz //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 SPI_CSG = 1; //end spi talking SPI_CSG = 0; //start spi talking spi.write(CTRL_REG4_G); spi.write(0x10); //Scle 500dps SPI_CSG = 1; //end spi talking //for ACC config SPI_CSXM = 0; //start spi talking spi.write(CTRL_REG1_XM); spi.write(0x87); //data rate 400 Hz/ Enable SPI_CSXM = 1; //end spi talking SPI_CSXM = 0; //start spi talking spi.write(CTRL_REG2_XM); spi.write(0xC8); //cut off 50 Hz/ Scale +-4g SPI_CSXM = 1; //end spi talking } //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of init_IMU funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓read_IMU funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// void read_IMU(void) //read IMU data give raw data { //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 * Gpx + Gdx; Wx = lpf(Buff * Gpx, Wx, 48.0f); //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 * Gpy + Gdy; Wy = lpf(Buff * Gpy, Wy, 48.0f); //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 * Gpz + Gdz; Wz = lpf(Buff * Gpz, Wz, 48.0f); //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); Buff = high_byte << 8 |low_byte; SPI_CSXM = 1; //end spi talking Ax = lpf(Buff * Apx, Ax, 13.0f); //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 = lpf(Buff * Apy, Ay, 13.0f); //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 = lpf(Buff * Apz, Az, 13.0f); } //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of read_IMU funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓lpf funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// float lpf(float input, float output_old, float frequency) { float output = 0; output = (output_old + frequency*dt*input) / (1 + frequency*dt); return output; } //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of lpf funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//