MEMS sensor drivers and tilt-compensated compass using the STEVAL-MKI124V1 header board: LPS331 pressure sensor, LSM303DLHC magnetometer/accelerometer and L3GD20 gyroscope.
main.cpp
00001 // MBED reference code for the ST Micro STEVAL-MKI124V1 header board 00002 // This board has: LPS331 pressure/temperature sensor, L3GD20 gyroscope and LSM303DLHC magnetometer/accelerometer 00003 // Code accesses each of the 3 MEMS sensors and calculates pressure, temp, heading, tilt, roll and angular velocity 00004 // Code is not optimized for efficienecy but instead for clarity of how you use the sensors 00005 // ST application note AN3192 was key in developing the tilt-corrected compass 00006 // Developed on an LPC1768 00007 // By Liam Goudge. March 2014 00008 00009 #define LSM303_on 00010 #define L3GD20_on 00011 #define LPS301_on 00012 00013 #include "mbed.h" 00014 #include "MKI124V1.h" 00015 #include "math.h" 00016 00017 DigitalOut myled(LED1); 00018 Serial pc(USBTX, USBRX); // tx, rx for USB debug printf to terminal console 00019 I2C i2c(p28, p27); // LPC1768 I2C pin allocation 00020 00021 // Globals 00022 int16_t const Offset_mX=-40.0; 00023 int16_t const Offset_mY=-115.0; 00024 float const RadtoDeg=(180.0/3.141592654); 00025 00026 00027 char readByte(char address, char reg) 00028 // Reads one byte from an I2C address 00029 // Didn't bother to make a multi-byte version to read in X,Y,Z low/high series of registers because... 00030 // the data registers of all sensors they are in the same XL,XH,YL,YH,ZL,ZH order apart from the magnetometer which is XH,XL,ZH,ZL,YH,YL... 00031 { 00032 char result; 00033 00034 i2c.start(); 00035 i2c.write(address); // Slave address with direction=write 00036 i2c.write(reg); // Subaddress (register) 00037 00038 i2c.start(); // Break transmission to change bus direction 00039 i2c.write(address + 1); // Slave address with direction=read [bit0=1] 00040 00041 result = i2c.read(0); 00042 i2c.stop(); 00043 return (result); 00044 } 00045 00046 void writeByte(char address, char reg,char value) 00047 // Sends 1 byte to an I2C address 00048 { 00049 i2c.start(); 00050 i2c.write(address); // Slave address 00051 i2c.write(reg); // Subaddress (register) 00052 i2c.write(value); 00053 i2c.stop(); 00054 00055 } 00056 00057 void initSensors (void) 00058 // Switch on and set up the 3 on-board sensors 00059 { 00060 pc.printf("--------------------------------------\n"); 00061 pc.printf("\nSTM MEMS eval board sensor init \n"); 00062 00063 #ifdef LSM303_on 00064 // LSM303DLHC Magnetic sensor 00065 pc.printf("LSM303DLHC ping (should reply 0x48): %x \n",readByte(LSM303_m,mIRA_REG_M)); 00066 writeByte(LSM303_m,mCRA_REG_M,0x94); //switch on temperature sensor and set Output Data Rate to 30Hz 00067 writeByte(LSM303_m,mCRB_REG_M,0x20); // Set the gain for +/- 1.3 Gauss full scale range 00068 writeByte(LSM303_m,mMR_REG_M,0x0); // Continuous convertion mode 00069 00070 // LSM303DLHC Accelerometer 00071 writeByte(LSM303_a,aCTRL_REG1_A ,0x37); // Set 25Hz ODR, everything else on 00072 writeByte(LSM303_a,aCTRL_REG4_A ,0x08); // Set full scale to +/- 2g sensitivity and high rez mode 00073 #endif 00074 00075 #ifdef LPS331_on 00076 // LPS331 Pressure sensor 00077 pc.printf("LPS331 ping (should reply 0xBB): %x \n",readByte(LPS331addr,pWHO_AM_I)); 00078 writeByte(LPS331addr,pCTRL_REG1,0x90); // Switch on pressure sensor and select 1Hz ODR. If you select one-shot then sensor powers itself down after every reading... 00079 writeByte(LPS331addr,pRES_CONF,0x70); // Temp and pressure noise reduction. Sets # of internal measurements that are averaged to 1 reading. Default is 0x7A (temp rez=128, press=512) 00080 #endif 00081 00082 #ifdef L3GD20_on 00083 // L3GD20 gyro 00084 printf("Ping L3GD20 (should reply 0xD4): %x \n",readByte(L3GD20_ADDR,gWHO_AM_I)); 00085 writeByte(L3GD20_ADDR,gCTRL_REG1,0x0F); // Set ODR to 95Hz, BW to 12.5Hz, enable axes and turn on device 00086 writeByte(L3GD20_ADDR,gCTRL_REG4,0x10); // Full scale selected at 500dps (degrees per second) 00087 printf("L3GD20 gyro Temp: %x degrees C \n",readByte(L3GD20_ADDR,gOUT_TEMP)); 00088 #endif 00089 00090 pc.printf("--------------------------------------\n \n"); 00091 wait(1); // Wait for settings to stabilize 00092 } 00093 00094 void LPS331(SensorState_t state) 00095 // Read the pressure sensor 00096 { 00097 uint8_t tempL,tempH,pressXL,pressL,pressH; 00098 int16_t temp; 00099 int32_t press24; 00100 float pressure; 00101 00102 // Measure temperature 00103 tempL=readByte(LPS331addr,pTEMP_OUT_L); 00104 tempH=readByte(LPS331addr,pTEMP_OUT_H); 00105 temp=(tempH << 8) | tempL; // 16-bit 2's complement data 00106 00107 state.tempC=((float) temp / 480.0) + 42.5; // per equation on page 29 of the spec 00108 00109 pc.printf("Pressure sensor temperature %.1f degreesC \n",state.tempC); 00110 00111 // Pressure test 00112 pressXL=readByte(LPS331addr,pPRESS_OUT_XL); 00113 pressL=readByte(LPS331addr,pPRESS_OUT_L); 00114 pressH=readByte(LPS331addr,pPRESS_OUT_H); 00115 00116 press24=(pressH << 16) | (pressL << 8) | pressXL ; // 24-bit 2's complement data 00117 pressure = (float)press24/4096.0; // Sensitivity is 4096 LSB per milibar 00118 00119 pc.printf("Pressure %.1f mbars or %.1f inches Hg\n", pressure, (pressure*0.0295)+0.029); 00120 00121 } 00122 00123 void LSM303 (SensorState_t * state) 00124 // Magnetometer and accelerometer 00125 { 00126 char xL, xH, yL, yH, zL, zH; 00127 int16_t mX, mY, mZ,aX,aY,aZ; 00128 float pitch,roll,faX,faY; 00129 00130 xL=readByte(LSM303_m,mOUT_X_L_M); 00131 xH=readByte(LSM303_m,mOUT_X_H_M); 00132 yL=readByte(LSM303_m,mOUT_Y_L_M); 00133 yH=readByte(LSM303_m,mOUT_Y_H_M); 00134 zL=readByte(LSM303_m,mOUT_Z_L_M); 00135 zH=readByte(LSM303_m,mOUT_Z_H_M); 00136 00137 mX=(xH<<8) | (xL); // 16-bit 2's complement data 00138 mY=(yH<<8) | (yL); 00139 mZ=(zH<<8) | (zL); 00140 00141 //pc.printf("mX=%hd %X mY=%hd %X mZ=%hd %X \n",mX,mX,mY,mY,mZ,mZ); 00142 00143 mX=mX-Offset_mX; // These are callibration co-efficients to deal with non-zero soft iron magnetic offset 00144 mY=mY-Offset_mY; 00145 00146 xL=readByte(LSM303_a,aOUT_X_L_A); 00147 xH=readByte(LSM303_a,aOUT_X_H_A); 00148 yL=readByte(LSM303_a,aOUT_Y_L_A); 00149 yH=readByte(LSM303_a,aOUT_Y_H_A); 00150 zL=readByte(LSM303_a,aOUT_Z_L_A); 00151 zH=readByte(LSM303_a,aOUT_Z_H_A); 00152 00153 aX=(signed short) ( (xH<<8) | (xL) ) >> 4; // 12-bit data from ADC. Cast ensures that the 2's complement sign is not lost in the right shift. 00154 aY=(signed short) ( (yH<<8) | (yL) ) >> 4; 00155 aZ=(signed short) ( (zH<<8) | (zL) ) >> 4; 00156 00157 //pc.printf("aX=%hd %X aY=%hd %X aZ=%hd %X \n",aX,aX,aY,aY,aZ,aZ); 00158 00159 faX=((float) aX) /2000.0; // Accelerometer scale I chose is 1mg per LSB with range +/-2g. So to normalize for full scale need to divide by 2000. 00160 faY=((float) aY) /2000.0; // If you don't do this the pitch and roll calcs will not work (inverse cosine of a value greater than 1) 00161 //faZ=((float) aZ) /2000.0; // Not used in a calc so comment out to avoid the compiler warning 00162 00163 // Trigonometry derived from STM app note AN3192 and from WikiRobots 00164 pitch = asin((float) -faX*2); // Dividing faX and faY by 1000 rather than 2000 seems to give better tilt immunity. Do it here rather than above to preserve true mg units of faX etc 00165 roll = asin(faY*2/cos(pitch)); 00166 00167 float xh = mX * cos(pitch) + mZ * sin(pitch); 00168 float yh = mX * sin(roll) * sin(pitch) + mY * cos(roll) - mZ * sin(roll) * cos(pitch); 00169 float zh = -mX * cos(roll) * sin(pitch) + mY * sin(roll) + mZ * cos(roll) * cos(pitch); 00170 00171 float heading = atan2(yh, xh) * RadtoDeg; // Note use of atan2 rather than atan since better for working with quadrants 00172 if (yh < 0) 00173 heading=360+heading; 00174 00175 state->heading=heading; 00176 state->pitch=pitch; 00177 state->roll=roll; 00178 00179 pc.printf("Orientation (deg): Pitch: %5.1f Roll: %5.1f Heading: %5.1f \n",pitch*RadtoDeg,roll*RadtoDeg,heading); 00180 pc.printf("Acceleration (mg): Forward: %5hd Left: %5hd Up: %5hd \n",aX,aY,aZ); 00181 00182 } 00183 00184 void L3GD20(void) // Gyro 00185 { 00186 char xL, xH, yL, yH, zL, zH; 00187 int16_t gX, gY, gZ; 00188 float rorX,rorY,rorZ; 00189 00190 xL=readByte(L3GD20_ADDR,gOUT_X_L); 00191 xH=readByte(L3GD20_ADDR,gOUT_X_H); 00192 yL=readByte(L3GD20_ADDR,gOUT_Y_L); 00193 yH=readByte(L3GD20_ADDR,gOUT_Y_H); 00194 zL=readByte(L3GD20_ADDR,gOUT_Z_L); 00195 zH=readByte(L3GD20_ADDR,gOUT_Z_H); 00196 00197 gX=(xH<<8) | (xL); // 16-bit 2's complement data 00198 gY=(yH<<8) | (yL); 00199 gZ=(zH<<8) | (zL); 00200 00201 rorX=(float) gX * (17.5/1000.0); // At 500dps sensitivity, L3GD20 returns 17.5/1000 dps per digit 00202 rorY=(float) gY * (17.5/1000.0); 00203 rorZ=(float) gZ * (17.5/1000.0); 00204 00205 pc.printf("Rate of rotation (dps): X:%5.1f Y:%5.1f Z:%5.1f \n",rorX,rorY,rorZ); 00206 //pc.printf("gX: %x gY: %x gZ: %x \n",gX,gY,gZ); 00207 00208 } 00209 00210 int main() 00211 { 00212 SensorState_t state; 00213 initSensors(); 00214 00215 while(1) 00216 { 00217 00218 #ifdef LPS331_on 00219 LPS331(state); 00220 #endif 00221 00222 #ifdef LSM303_on 00223 LSM303(&state); 00224 #endif 00225 00226 #ifdef L3GD20_on 00227 L3GD20(); 00228 #endif 00229 00230 pc.printf("\n"); 00231 wait(.5); 00232 } 00233 } 00234 00235 00236
Generated on Thu Jul 28 2022 13:29:59 by 1.7.2