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@7:f53b52e23818, 2017-02-13 (annotated)

Committer:
open4416
Date:
Mon Feb 13 07:38:06 2017 +0000
Revision:
7:f53b52e23818
Parent:
6:c2efb0a3a543 
works fine on STM

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