SHENG-HEN HSIEH / Mbed 2 deprecated LSM9DS0_STM32compatible

works fine on STM

Dependencies:   mbed

Fork of Sample_manerine_SPI_LSM9DS0 by SHENG-HEN HSIEH

main.cpp@3:502b83f7761c, 2016-09-05 (annotated)

Committer:
open4416
Date:
Mon Sep 05 14:43:43 2016 +0000
Revision:
3:502b83f7761c
Parent:
2:0d90c0436797
Child:
4:b9dd320947ff
estimation only(gyro)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
open4416 0:d68e088dfbcd 1 #include "mbed.h"
open4416 1:b42c3522a50a 2 #include "LSM9DS0_SH.h"
open4416 3:502b83f7761c 3 #define pi 3.141592f
open4416 3:502b83f7761c 4 #define d2r 0.01745329f
open4416 3:502b83f7761c 5 #define Rms 5000
open4416 3:502b83f7761c 6 #define dt 0.005f
open4416 1:b42c3522a50a 7 #define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
open4416 0:d68e088dfbcd 8
open4416 0:d68e088dfbcd 9
open4416 1:b42c3522a50a 10 //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓GPIO registor↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
open4416 1:b42c3522a50a 11 //~~~structure~~~//
open4416 1:b42c3522a50a 12 DigitalOut led(D13); //detection
open4416 1:b42c3522a50a 13 DigitalOut TT_ext(D12);
open4416 1:b42c3522a50a 14
open4416 2:0d90c0436797 15 //~~~IMU_SPI~~~//
open4416 1:b42c3522a50a 16 DigitalOut SPI_CSG(D7,1); //low for GYRO enable
open4416 1:b42c3522a50a 17 DigitalOut SPI_CSXM(D6,1); //low for ACC/MAG enable
open4416 1:b42c3522a50a 18 SPI spi(D4, D5, D3); //MOSI MISO SCLK
open4416 1:b42c3522a50a 19
open4416 1:b42c3522a50a 20 //~~~Serial~~~//
open4416 1:b42c3522a50a 21 Serial pc(D1, D0); //Serial reg(TX RX)
open4416 1:b42c3522a50a 22 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of GPIO registor↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
open4416 1:b42c3522a50a 23
open4416 0:d68e088dfbcd 24
open4416 0:d68e088dfbcd 25
open4416 1:b42c3522a50a 26 //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓Varible registor↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
open4416 1:b42c3522a50a 27 //~~~globle~~~//
open4416 1:b42c3522a50a 28 Ticker TT; //call a timer
open4416 1:b42c3522a50a 29 int count = 0; //one second counter for extrenal led blink
open4416 2:0d90c0436797 30
open4416 2:0d90c0436797 31 //~~~IMU_SPI~~~//
open4416 3:502b83f7761c 32 short low_byte = 0x00; //buffer
open4416 2:0d90c0436797 33 short high_byte = 0x00;
open4416 3:502b83f7761c 34 short Buff = 0x00;
open4416 3:502b83f7761c 35 float Wx = 0.0;
open4416 3:502b83f7761c 36 float Wy = 0.0;
open4416 3:502b83f7761c 37 float Wz = 0.0;
open4416 3:502b83f7761c 38 float Ax = 0.0;
open4416 3:502b83f7761c 39 float Ay = 0.0;
open4416 3:502b83f7761c 40 float Az = 0.0;
open4416 3:502b83f7761c 41 float gDIR[1][3] = { //g vector's direction , required unitconstrain
open4416 3:502b83f7761c 42 {0,0,-1}, //X Y Z
open4416 3:502b83f7761c 43 };
open4416 3:502b83f7761c 44 float gUnity = 0;
open4416 1:b42c3522a50a 45 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of Varible registor↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
open4416 1:b42c3522a50a 46
open4416 0:d68e088dfbcd 47
open4416 0:d68e088dfbcd 48
open4416 1:b42c3522a50a 49 //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓Function registor↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
open4416 3:502b83f7761c 50 void init_TIMER(); //set TT_main() rate
open4416 3:502b83f7761c 51 void TT_main(); //timebase function rated by TT
open4416 1:b42c3522a50a 52 void init_IO(); //initialize IO state
open4416 1:b42c3522a50a 53 void init_IMU(); //initialize IMU
open4416 3:502b83f7761c 54 void read_IMU(); //read IMU data give raw data
open4416 3:502b83f7761c 55 void state_update(); //estimation of new attitude
open4416 1:b42c3522a50a 56 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of Function registor↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
open4416 1:b42c3522a50a 57
open4416 1:b42c3522a50a 58
open4416 1:b42c3522a50a 59
open4416 1:b42c3522a50a 60 //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓main funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
open4416 0:d68e088dfbcd 61 int main()
open4416 0:d68e088dfbcd 62 {
open4416 1:b42c3522a50a 63 init_IO(); //initialized value
open4416 1:b42c3522a50a 64 init_IMU(); //initialize IMU
open4416 1:b42c3522a50a 65 init_TIMER(); //start TT_main
open4416 1:b42c3522a50a 66 pc.baud(115200); //set baud rate
open4416 0:d68e088dfbcd 67
open4416 1:b42c3522a50a 68 while(1) { //main() loop
open4416 1:b42c3522a50a 69 if(count >= 200) { //check if main working
open4416 0:d68e088dfbcd 70 count=0;
open4416 0:d68e088dfbcd 71 led = !led;
open4416 0:d68e088dfbcd 72 }
open4416 1:b42c3522a50a 73 }
open4416 0:d68e088dfbcd 74
open4416 0:d68e088dfbcd 75 }
open4416 1:b42c3522a50a 76 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of main funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
open4416 0:d68e088dfbcd 77
open4416 0:d68e088dfbcd 78
open4416 0:d68e088dfbcd 79
open4416 3:502b83f7761c 80 //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓Timebase funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
open4416 3:502b83f7761c 81 void init_TIMER() //set TT_main{} rate
open4416 3:502b83f7761c 82 {
open4416 3:502b83f7761c 83 TT.attach_us(&TT_main, Rms);
open4416 3:502b83f7761c 84 }
open4416 3:502b83f7761c 85 void TT_main() //interrupt function by TT
open4416 3:502b83f7761c 86 {
open4416 3:502b83f7761c 87 TT_ext = !TT_ext; //indicate TT_main() function working
open4416 3:502b83f7761c 88 count = count+1; //one second counter
open4416 3:502b83f7761c 89
open4416 3:502b83f7761c 90 read_IMU(); //read IMU data give raw data
open4416 3:502b83f7761c 91 state_update(); //estimation of new attitude
open4416 3:502b83f7761c 92
open4416 3:502b83f7761c 93 pc.printf("%.2f\t%.2f\t%.2f\n", gDIR[0][0], gDIR[0][1], gDIR[0][2]);
open4416 3:502b83f7761c 94 // pc.printf("%.2f\t%.2f\n", Sele, Srol);
open4416 3:502b83f7761c 95 // pc.printf("%.2f\n", Sx);
open4416 3:502b83f7761c 96 }
open4416 3:502b83f7761c 97 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of Timebase funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
open4416 3:502b83f7761c 98
open4416 3:502b83f7761c 99
open4416 3:502b83f7761c 100
open4416 3:502b83f7761c 101 //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓init_IO funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
open4416 1:b42c3522a50a 102 void init_IO(void) //initialize
open4416 1:b42c3522a50a 103 {
open4416 1:b42c3522a50a 104 TT_ext = 0;
open4416 1:b42c3522a50a 105 led = 1;
open4416 1:b42c3522a50a 106 }
open4416 3:502b83f7761c 107 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of init_IO funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
open4416 0:d68e088dfbcd 108
open4416 0:d68e088dfbcd 109
open4416 0:d68e088dfbcd 110
open4416 3:502b83f7761c 111 //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓init_IMU funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
open4416 3:502b83f7761c 112 void init_IMU(void) //initialize
open4416 0:d68e088dfbcd 113 {
open4416 3:502b83f7761c 114 //gloable config
open4416 1:b42c3522a50a 115 SPI_CSXM = 1; //high as init for disable SPI
open4416 1:b42c3522a50a 116 SPI_CSG = 1;
open4416 1:b42c3522a50a 117 spi.format(8, 0); //byte width, spi mode
open4416 1:b42c3522a50a 118 spi.frequency(4000000); //8MHz
open4416 1:b42c3522a50a 119
open4416 3:502b83f7761c 120 //for GYRO config
open4416 1:b42c3522a50a 121 SPI_CSG = 0; //start spi talking
open4416 1:b42c3522a50a 122 spi.write(CTRL_REG1_G);
open4416 1:b42c3522a50a 123 spi.write(0x9F); //data rate 380 Hz/ cut off 25 Hz
open4416 1:b42c3522a50a 124 SPI_CSG = 1; //end spi talking
open4416 1:b42c3522a50a 125
open4416 1:b42c3522a50a 126 SPI_CSG = 0; //start spi talking
open4416 1:b42c3522a50a 127 spi.write(CTRL_REG4_G);
open4416 1:b42c3522a50a 128 spi.write(0x10); //Scle 500dps
open4416 1:b42c3522a50a 129 SPI_CSG = 1; //end spi talking
open4416 1:b42c3522a50a 130
open4416 3:502b83f7761c 131 //for ACC config
open4416 1:b42c3522a50a 132 SPI_CSXM = 0; //start spi talking
open4416 1:b42c3522a50a 133 spi.write(CTRL_REG1_XM);
open4416 1:b42c3522a50a 134 spi.write(0x87); //data rate 400 Hz/ Enable
open4416 1:b42c3522a50a 135 SPI_CSXM = 1; //end spi talking
open4416 1:b42c3522a50a 136
open4416 1:b42c3522a50a 137 SPI_CSXM = 0; //start spi talking
open4416 1:b42c3522a50a 138 spi.write(CTRL_REG2_XM);
open4416 3:502b83f7761c 139 spi.write(0xC8); //cut off 50 Hz/ Scale +-4g
open4416 1:b42c3522a50a 140 SPI_CSXM = 1; //end spi talking
open4416 0:d68e088dfbcd 141 }
open4416 3:502b83f7761c 142 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of init_IMU funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
open4416 0:d68e088dfbcd 143
open4416 0:d68e088dfbcd 144
open4416 0:d68e088dfbcd 145
open4416 3:502b83f7761c 146 //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓read_IMU funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
open4416 3:502b83f7761c 147 void read_IMU(void) //read IMU data give raw data
open4416 0:d68e088dfbcd 148 {
open4416 3:502b83f7761c 149 //Wx
open4416 3:502b83f7761c 150 SPI_CSG = 0; //start spi talking Wx
open4416 3:502b83f7761c 151 spi.write(0xE8); //read B11101000 read/multi/address
open4416 3:502b83f7761c 152 low_byte = spi.write(0);
open4416 3:502b83f7761c 153 high_byte = spi.write(0);
open4416 3:502b83f7761c 154 Buff = high_byte << 8 |low_byte;
open4416 3:502b83f7761c 155 SPI_CSG = 1; //end spi talking
open4416 3:502b83f7761c 156 Wx = Buff * 2.663e-4; //1.526e-2 * d2r =
open4416 3:502b83f7761c 157 //Wy
open4416 3:502b83f7761c 158 SPI_CSG = 0; //start spi talking Wx
open4416 3:502b83f7761c 159 spi.write(0xEA); //read B11101010 read/multi/address
open4416 3:502b83f7761c 160 low_byte = spi.write(0);
open4416 3:502b83f7761c 161 high_byte = spi.write(0);
open4416 3:502b83f7761c 162 Buff = high_byte << 8 |low_byte;
open4416 3:502b83f7761c 163 SPI_CSG = 1; //end spi talking
open4416 3:502b83f7761c 164 Wy = Buff * 2.663e-4; //1.526e-2 * d2r =
open4416 3:502b83f7761c 165 //Wz
open4416 3:502b83f7761c 166 SPI_CSG = 0; //start spi talking Wx
open4416 3:502b83f7761c 167 spi.write(0xEC); //read B11101100 read/multi/address
open4416 3:502b83f7761c 168 low_byte = spi.write(0);
open4416 3:502b83f7761c 169 high_byte = spi.write(0);
open4416 3:502b83f7761c 170 Buff = high_byte << 8 |low_byte;
open4416 3:502b83f7761c 171 SPI_CSG = 1; //end spi talking
open4416 3:502b83f7761c 172 Wz = Buff * 2.663e-4; //1.526e-2 * d2r =
open4416 3:502b83f7761c 173 //Ax
open4416 3:502b83f7761c 174 SPI_CSXM = 0; //start spi talking Ax
open4416 2:0d90c0436797 175 spi.write(0xE8); //read B11101000 read/multi/address
open4416 2:0d90c0436797 176 low_byte = spi.write(0);
open4416 2:0d90c0436797 177 high_byte = spi.write(0);
open4416 3:502b83f7761c 178 Buff = high_byte << 8 |low_byte;
open4416 3:502b83f7761c 179 SPI_CSXM = 1; //end spi talking
open4416 3:502b83f7761c 180 Ax = Buff * 1.22e-4;
open4416 3:502b83f7761c 181 //Ay
open4416 3:502b83f7761c 182 SPI_CSXM = 0; //start spi talking Ax
open4416 3:502b83f7761c 183 spi.write(0xEA); //read B11101010 read/multi/address
open4416 3:502b83f7761c 184 low_byte = spi.write(0);
open4416 3:502b83f7761c 185 high_byte = spi.write(0);
open4416 3:502b83f7761c 186 Buff = high_byte << 8 |low_byte;
open4416 3:502b83f7761c 187 SPI_CSXM = 1; //end spi talking
open4416 3:502b83f7761c 188 Ay = Buff * 1.22e-4;
open4416 3:502b83f7761c 189 //Az
open4416 3:502b83f7761c 190 SPI_CSXM = 0; //start spi talking Ax
open4416 3:502b83f7761c 191 spi.write(0xEC); //read B11101100 read/multi/address
open4416 3:502b83f7761c 192 low_byte = spi.write(0);
open4416 3:502b83f7761c 193 high_byte = spi.write(0);
open4416 3:502b83f7761c 194 Buff = high_byte << 8 |low_byte;
open4416 3:502b83f7761c 195 SPI_CSXM = 1; //end spi talking
open4416 3:502b83f7761c 196 Az = Buff * 1.22e-4;
open4416 3:502b83f7761c 197 }
open4416 3:502b83f7761c 198 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of read_IMU funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
open4416 1:b42c3522a50a 199
open4416 1:b42c3522a50a 200
open4416 1:b42c3522a50a 201
open4416 3:502b83f7761c 202 //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓state_update funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
open4416 3:502b83f7761c 203 void state_update(void) //estimation of new attitude
open4416 3:502b83f7761c 204 {
open4416 3:502b83f7761c 205 //pridict
open4416 3:502b83f7761c 206 gDIR[0][0] = gDIR[0][0] - (Wy*gDIR[0][2] - Wz*gDIR[0][1])*dt;
open4416 3:502b83f7761c 207 gDIR[0][1] = gDIR[0][1] - (Wz*gDIR[0][0] - Wx*gDIR[0][2])*dt;
open4416 3:502b83f7761c 208 gDIR[0][2] = gDIR[0][2] - (Wx*gDIR[0][1] - Wy*gDIR[0][0])*dt;
open4416 3:502b83f7761c 209
open4416 3:502b83f7761c 210 //update
open4416 3:502b83f7761c 211
open4416 3:502b83f7761c 212
open4416 3:502b83f7761c 213 //nutralizing
open4416 3:502b83f7761c 214 gUnity = gDIR[0][0]*gDIR[0][0] + gDIR[0][1]*gDIR[0][1] + gDIR[0][2]*gDIR[0][2];
open4416 3:502b83f7761c 215 for(int i=0; i<3; i++) {
open4416 3:502b83f7761c 216 gDIR[0][i] = gDIR[0][i] / gUnity;
open4416 3:502b83f7761c 217 }
open4416 0:d68e088dfbcd 218 }
open4416 3:502b83f7761c 219 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of state_update funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//