SHENG-HEN HSIEH
/
LSM9DS0_STM32compatible
works fine on STM
Fork of Sample_manerine_SPI_LSM9DS0 by
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?
User | Revision | Line number | New 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↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// |