SHENG-HEN HSIEH
/
LSM9DS0_STM32compatible
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of
Sample_manerine_SPI_LSM9DS0
by 
SHENG-HEN HSIEH
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #include "LSM9DS0_SH.h" 00003 00004 #define pi 3.141592f 00005 #define d2r 0.01745329f 00006 00007 #define Rms 5000 //TT rate 00008 #define dt 0.005f 00009 #define NN 200 00010 00011 #define Kp 3.6f 00012 #define Ki 5.0f 00013 #define Kd 0.12f 00014 #define Kcon 0.00f 00015 00016 #define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) 00017 00018 //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓GPIO registor↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// 00019 //~~~structure~~~// 00020 DigitalOut led(D13); //detection 00021 DigitalOut TT_ext(D12); 00022 00023 //~~~IMU_SPI~~~// 00024 DigitalOut SPI_CSG(D7,1); //low for GYRO enable 00025 DigitalOut SPI_CSXM(D6,1); //low for ACC/MAG enable 00026 SPI spi(D4, D5, D3); //MOSI MISO SCLK 00027 00028 //~~~Serial~~~// 00029 Serial pc(D1, D0); //Serial reg(TX RX) 00030 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of GPIO registor↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// 00031 00032 00033 00034 //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓Varible registor↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// 00035 //~~~globle~~~// 00036 Ticker TT; //call a timer 00037 int count = 0; //one second counter for extrenal led blink 00038 00039 //~~~IMU_SPI~~~// 00040 short low_byte = 0x00; //buffer 00041 short high_byte = 0x00; 00042 short Buff = 0x00; 00043 float Wx = 0.0; 00044 float Wy = 0.0; 00045 float Wz = 0.0; 00046 float Ax = 0.0; 00047 float Ay = 0.0; 00048 float Az = 0.0; 00049 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of Varible registor↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// 00050 00051 00052 00053 //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓Function registor↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// 00054 void init_TIMER(); //set TT_main() rate 00055 void TT_main(); //timebase function rated by TT 00056 void init_IO(); //initialize IO state 00057 00058 void init_IMU(); //initialize IMU 00059 void read_IMU(); //read IMU data give raw data 00060 00061 float lpf(float input, float output_old, float frequency); //lpf discrete 00062 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of Function registor↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// 00063 00064 00065 00066 //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓main funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// 00067 int main() 00068 { 00069 pc.baud(115200); //set baud rate 00070 00071 init_IO(); //initialized value 00072 init_IMU(); //initialize IMU 00073 00074 init_TIMER(); //start TT_main 00075 00076 while(1) { //main() loop 00077 if(count >= NN) { //check if main working 00078 count=0; 00079 led = !led; 00080 } 00081 } 00082 00083 } 00084 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of main funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// 00085 00086 00087 00088 //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓Timebase funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// 00089 void init_TIMER() //set TT_main{} rate 00090 { 00091 TT.attach_us(&TT_main, Rms); 00092 } 00093 void TT_main() //interrupt function by TT 00094 { 00095 TT_ext = !TT_ext; //indicate TT_main() function working 00096 count = count+1; //one second counter 00097 00098 read_IMU(); //read IMU data give raw data 00099 00100 //for Serial-Oscilloscope 00101 pc.printf("%.2f,%.2f,%.2f\r", Ax, Ay, Az); 00102 00103 } 00104 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of Timebase funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// 00105 00106 00107 00108 //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓init_IO funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// 00109 void init_IO(void) //initialize 00110 { 00111 TT_ext = 0; 00112 led = 1; 00113 } 00114 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of init_IO funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// 00115 00116 00117 00118 //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓init_IMU funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// 00119 void init_IMU(void) //initialize 00120 { 00121 //gloable config 00122 SPI_CSXM = 1; //high as init for disable SPI 00123 SPI_CSG = 1; 00124 spi.format(8, 3); //byte width, spi mode 00125 spi.frequency(4000000); //8MHz 00126 00127 //for GYRO config 00128 SPI_CSG = 0; //start spi talking 00129 spi.write(CTRL_REG1_G); 00130 spi.write(0x9F); //data rate 380 Hz/ cut off 25 Hz 00131 SPI_CSG = 1; //end spi talking 00132 00133 SPI_CSG = 0; //start spi talking 00134 spi.write(CTRL_REG4_G); 00135 spi.write(0x10); //Scle 500dps 00136 SPI_CSG = 1; //end spi talking 00137 00138 //for ACC config 00139 SPI_CSXM = 0; //start spi talking 00140 spi.write(CTRL_REG1_XM); 00141 spi.write(0x87); //data rate 400 Hz/ Enable 00142 SPI_CSXM = 1; //end spi talking 00143 00144 SPI_CSXM = 0; //start spi talking 00145 spi.write(CTRL_REG2_XM); 00146 spi.write(0xC8); //cut off 50 Hz/ Scale +-4g 00147 SPI_CSXM = 1; //end spi talking 00148 } 00149 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of init_IMU funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// 00150 00151 00152 00153 //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓read_IMU funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// 00154 void read_IMU(void) //read IMU data give raw data 00155 { 00156 //Wx 00157 SPI_CSG = 0; //start spi talking Wx 00158 spi.write(0xE8); //read B11101000 read/multi/address 00159 low_byte = spi.write(0); 00160 high_byte = spi.write(0); 00161 Buff = high_byte << 8 |low_byte; 00162 SPI_CSG = 1; //end spi talking 00163 // Wx = Buff * Gpx + Gdx; 00164 Wx = lpf(Buff * Gpx, Wx, 48.0f); 00165 //Wy 00166 SPI_CSG = 0; //start spi talking Wx 00167 spi.write(0xEA); //read B11101010 read/multi/address 00168 low_byte = spi.write(0); 00169 high_byte = spi.write(0); 00170 Buff = high_byte << 8 |low_byte; 00171 SPI_CSG = 1; //end spi talking 00172 // Wy = Buff * Gpy + Gdy; 00173 Wy = lpf(Buff * Gpy, Wy, 48.0f); 00174 //Wz 00175 SPI_CSG = 0; //start spi talking Wx 00176 spi.write(0xEC); //read B11101100 read/multi/address 00177 low_byte = spi.write(0); 00178 high_byte = spi.write(0); 00179 Buff = high_byte << 8 |low_byte; 00180 SPI_CSG = 1; //end spi talking 00181 // Wz = Buff * Gpz + Gdz; 00182 Wz = lpf(Buff * Gpz, Wz, 48.0f); 00183 //Ax 00184 SPI_CSXM = 0; //start spi talking Ax 00185 spi.write(0xE8); //read B11101000 read/multi/address 00186 low_byte = spi.write(0); 00187 high_byte = spi.write(0); 00188 Buff = high_byte << 8 |low_byte; 00189 SPI_CSXM = 1; //end spi talking 00190 Ax = lpf(Buff * Apx, Ax, 13.0f); 00191 //Ay 00192 SPI_CSXM = 0; //start spi talking Ax 00193 spi.write(0xEA); //read B11101010 read/multi/address 00194 low_byte = spi.write(0); 00195 high_byte = spi.write(0); 00196 Buff = high_byte << 8 |low_byte; 00197 SPI_CSXM = 1; //end spi talking 00198 Ay = lpf(Buff * Apy, Ay, 13.0f); 00199 //Az 00200 SPI_CSXM = 0; //start spi talking Ax 00201 spi.write(0xEC); //read B11101100 read/multi/address 00202 low_byte = spi.write(0); 00203 high_byte = spi.write(0); 00204 Buff = high_byte << 8 |low_byte; 00205 SPI_CSXM = 1; //end spi talking 00206 Az = lpf(Buff * Apz, Az, 13.0f); 00207 } 00208 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of read_IMU funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// 00209 00210 00211 00212 //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓lpf funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// 00213 float lpf(float input, float output_old, float frequency) 00214 { 00215 float output = 0; 00216 output = (output_old + frequency*dt*input) / (1 + frequency*dt); 00217 return output; 00218 } 00219 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of lpf funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
Generated on Thu Jul 14 2022 03:32:17 by
1.7.2