MEMS sensor drivers and tilt-compensated compass using the STEVAL-MKI124V1 header board: LPS331 pressure sensor, LSM303DLHC magnetometer/accelerometer and L3GD20 gyroscope.

Dependencies:   mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

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