MEMS sensor drivers and tilt-compensated compass using the STEVAL-MKI124V1 header board: LPS331 pressure sensor, LSM303DLHC magnetometer/accelerometer and L3GD20 gyroscope.
I used a header board for an STM MEMS evaluation kit in order to take a look at some common MEMS sensors:
- LPS301: Pressure and temperature sensor
- LG3D20: Gyroscope
- LSM303DLHC: Accelerometer and magnetometer
The header was an STEVAL-MKI124V1 which is designed to work with an STM motherboard evaluation system. I took a shortcut and used it with an LPC1768 MBED over I2C
Hook-up was trivial:
The schematic is here:
http://www.st.com/web/en/catalog/tools/PF253482
The orientation of the sensors on the board is like this:
The code sets up each of the sensors and then provides a continuous output of temperature, pressure and orientation. Rather than optimize for performance or efficiency, the code here is intended to show clearly how to access the sensors.
An interesting twist was to use the linear accelerometer to find the vector of the earth's gravitational field (i.e. down) and to use that to make a tilt-adjusted compass. Algorithm came from ST Apps note AN3192.
The sensors do need some calibration. Here is a scatter plot of the raw output of X and Y values from the magnetometer:
The chart should be a perfect circle around the origin (allowing for distortion on Excel charting).
- Blue points are the raw data
- Red is offset-corrected
- Green is offset and soft-iron corrected
As you can see, there is an offset error but also and X:Y ratio term. The latter is a soft iron error and is well described in a great article here:
main.cpp@1:3b2260aff305, 2014-03-18 (annotated)
- Committer:
- liamg
- Date:
- Tue Mar 18 16:48:10 2014 +0000
- Revision:
- 1:3b2260aff305
- Parent:
- 0:91b1274ec397
- Child:
- 2:2ef63ab235bf
1st commit including #defines
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
liamg | 0:91b1274ec397 | 1 | // MBED reference code for the ST Micro STEVAL-MKI124V1 header board |
liamg | 0:91b1274ec397 | 2 | // This board has: LPS331 pressure/temperature sensor, L3GD20 gyroscope and LSM303DLHC magnetometer/accelerometer |
liamg | 0:91b1274ec397 | 3 | // Code accesses each of the 3 MEMS sensors and calculates pressure, temp, heading, tilt, roll and angular velocity |
liamg | 0:91b1274ec397 | 4 | // Code is not optimized for efficienecy but instead for clarity of how you use the sensors |
liamg | 0:91b1274ec397 | 5 | // ST application note AN3192 was key in developing the tilt-corrected compass |
liamg | 0:91b1274ec397 | 6 | // Developed on an LPC1768 |
liamg | 0:91b1274ec397 | 7 | // By Liam Goudge. March 2014 |
liamg | 0:91b1274ec397 | 8 | |
liamg | 1:3b2260aff305 | 9 | #define LSM303_on |
liamg | 1:3b2260aff305 | 10 | #define L3GD20_on |
liamg | 1:3b2260aff305 | 11 | #define LPS301_on |
liamg | 1:3b2260aff305 | 12 | |
liamg | 0:91b1274ec397 | 13 | #include "mbed.h" |
liamg | 0:91b1274ec397 | 14 | #include "MKI124V1.h" |
liamg | 0:91b1274ec397 | 15 | #include "math.h" |
liamg | 0:91b1274ec397 | 16 | |
liamg | 0:91b1274ec397 | 17 | DigitalOut myled(LED1); |
liamg | 0:91b1274ec397 | 18 | Serial pc(USBTX, USBRX); // tx, rx for USB debug printf to terminal console |
liamg | 0:91b1274ec397 | 19 | I2C i2c(p28, p27); // LPC1768 I2C pin allocation |
liamg | 0:91b1274ec397 | 20 | |
liamg | 0:91b1274ec397 | 21 | // Globals |
liamg | 0:91b1274ec397 | 22 | int16_t const Offset_mX=-40.0; |
liamg | 0:91b1274ec397 | 23 | int16_t const Offset_mY=-115.0; |
liamg | 0:91b1274ec397 | 24 | float const RadtoDeg=(180.0/3.141592654); |
liamg | 0:91b1274ec397 | 25 | |
liamg | 0:91b1274ec397 | 26 | |
liamg | 0:91b1274ec397 | 27 | char readByte(char address, char reg) |
liamg | 0:91b1274ec397 | 28 | // Reads one byte from an I2C address |
liamg | 0:91b1274ec397 | 29 | // Didn't bother to make a multi-byte version to read in X,Y,Z low/high series of registers because... |
liamg | 0:91b1274ec397 | 30 | // 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... |
liamg | 0:91b1274ec397 | 31 | { |
liamg | 0:91b1274ec397 | 32 | char result; |
liamg | 0:91b1274ec397 | 33 | |
liamg | 0:91b1274ec397 | 34 | i2c.start(); |
liamg | 0:91b1274ec397 | 35 | i2c.write(address); // Slave address with direction=write |
liamg | 0:91b1274ec397 | 36 | i2c.write(reg); // Subaddress (register) |
liamg | 0:91b1274ec397 | 37 | |
liamg | 0:91b1274ec397 | 38 | i2c.start(); // Break transmission to change bus direction |
liamg | 0:91b1274ec397 | 39 | i2c.write(address + 1); // Slave address with direction=read [bit0=1] |
liamg | 0:91b1274ec397 | 40 | |
liamg | 0:91b1274ec397 | 41 | result = i2c.read(0); |
liamg | 0:91b1274ec397 | 42 | i2c.stop(); |
liamg | 0:91b1274ec397 | 43 | return (result); |
liamg | 0:91b1274ec397 | 44 | } |
liamg | 0:91b1274ec397 | 45 | |
liamg | 0:91b1274ec397 | 46 | void writeByte(char address, char reg,char value) |
liamg | 0:91b1274ec397 | 47 | // Sends 1 byte to an I2C address |
liamg | 0:91b1274ec397 | 48 | { |
liamg | 0:91b1274ec397 | 49 | i2c.start(); |
liamg | 0:91b1274ec397 | 50 | i2c.write(address); // Slave address |
liamg | 0:91b1274ec397 | 51 | i2c.write(reg); // Subaddress (register) |
liamg | 0:91b1274ec397 | 52 | i2c.write(value); |
liamg | 0:91b1274ec397 | 53 | i2c.stop(); |
liamg | 0:91b1274ec397 | 54 | |
liamg | 0:91b1274ec397 | 55 | } |
liamg | 0:91b1274ec397 | 56 | |
liamg | 0:91b1274ec397 | 57 | void initSensors (void) |
liamg | 0:91b1274ec397 | 58 | // Switch on and set up the 3 on-board sensors |
liamg | 0:91b1274ec397 | 59 | { |
liamg | 0:91b1274ec397 | 60 | pc.printf("--------------------------------------\n"); |
liamg | 0:91b1274ec397 | 61 | pc.printf("\nSTM MEMS eval board sensor init \n"); |
liamg | 0:91b1274ec397 | 62 | |
liamg | 1:3b2260aff305 | 63 | #ifdef LSM303_on |
liamg | 0:91b1274ec397 | 64 | // LSM303DLHC Magnetic sensor |
liamg | 0:91b1274ec397 | 65 | pc.printf("LSM303DLHC ping (should reply 0x48): %x \n",readByte(LSM303_m,mIRA_REG_M)); |
liamg | 0:91b1274ec397 | 66 | writeByte(LSM303_m,mCRA_REG_M,0x94); //switch on temperature sensor and set Output Data Rate to 30Hz |
liamg | 0:91b1274ec397 | 67 | writeByte(LSM303_m,mCRB_REG_M,0x20); // Set the gain for +/- 1.3 Gauss full scale range |
liamg | 0:91b1274ec397 | 68 | writeByte(LSM303_m,mMR_REG_M,0x0); // Continuous convertion mode |
liamg | 0:91b1274ec397 | 69 | |
liamg | 0:91b1274ec397 | 70 | // LSM303DLHC Accelerometer |
liamg | 0:91b1274ec397 | 71 | writeByte(LSM303_a,aCTRL_REG1_A ,0x37); // Set 25Hz ODR, everything else on |
liamg | 0:91b1274ec397 | 72 | writeByte(LSM303_a,aCTRL_REG4_A ,0x08); // Set full scale to +/- 2g sensitivity and high rez mode |
liamg | 1:3b2260aff305 | 73 | #endif |
liamg | 1:3b2260aff305 | 74 | |
liamg | 1:3b2260aff305 | 75 | #ifdef LPS331 |
liamg | 0:91b1274ec397 | 76 | // LPS331 Pressure sensor |
liamg | 0:91b1274ec397 | 77 | pc.printf("LPS331 ping (should reply 0xBB): %x \n",readByte(LPS331addr,pWHO_AM_I)); |
liamg | 0:91b1274ec397 | 78 | 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... |
liamg | 0:91b1274ec397 | 79 | 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) |
liamg | 1:3b2260aff305 | 80 | #endif |
liamg | 0:91b1274ec397 | 81 | |
liamg | 1:3b2260aff305 | 82 | #ifdef L3GD20 |
liamg | 0:91b1274ec397 | 83 | // L3GD20 gyro |
liamg | 0:91b1274ec397 | 84 | printf("Ping L3GD20 (should reply 0xD4): %x \n",readByte(L3GD20_ADDR,gWHO_AM_I)); |
liamg | 0:91b1274ec397 | 85 | writeByte(L3GD20_ADDR,gCTRL_REG1,0x0F); // Set ODR to 95Hz, BW to 12.5Hz, enable axes and turn on device |
liamg | 0:91b1274ec397 | 86 | writeByte(L3GD20_ADDR,gCTRL_REG4,0x10); // Full scale selected at 500dps (degrees per second) |
liamg | 0:91b1274ec397 | 87 | printf("L3GD20 gyro Temp: %x degrees C \n",readByte(L3GD20_ADDR,gOUT_TEMP)); |
liamg | 1:3b2260aff305 | 88 | #endif |
liamg | 0:91b1274ec397 | 89 | |
liamg | 0:91b1274ec397 | 90 | pc.printf("--------------------------------------\n \n"); |
liamg | 0:91b1274ec397 | 91 | wait(1); // Wait for settings to stabilize |
liamg | 0:91b1274ec397 | 92 | } |
liamg | 0:91b1274ec397 | 93 | |
liamg | 0:91b1274ec397 | 94 | void LPS331(SensorState_t state) |
liamg | 0:91b1274ec397 | 95 | // Read the pressure sensor |
liamg | 0:91b1274ec397 | 96 | { |
liamg | 0:91b1274ec397 | 97 | uint8_t tempL,tempH,pressXL,pressL,pressH; |
liamg | 0:91b1274ec397 | 98 | int16_t temp; |
liamg | 0:91b1274ec397 | 99 | int32_t press24; |
liamg | 0:91b1274ec397 | 100 | float pressure; |
liamg | 0:91b1274ec397 | 101 | |
liamg | 0:91b1274ec397 | 102 | // Measure temperature |
liamg | 0:91b1274ec397 | 103 | tempL=readByte(LPS331addr,pTEMP_OUT_L); |
liamg | 0:91b1274ec397 | 104 | tempH=readByte(LPS331addr,pTEMP_OUT_H); |
liamg | 0:91b1274ec397 | 105 | temp=(tempH << 8) | tempL; // 16-bit 2's complement data |
liamg | 0:91b1274ec397 | 106 | |
liamg | 0:91b1274ec397 | 107 | state.tempC=((float) temp / 480.0) + 42.5; // per equation on page 29 of the spec |
liamg | 0:91b1274ec397 | 108 | |
liamg | 0:91b1274ec397 | 109 | pc.printf("Pressure sensor temperature %.1f degreesC \n",state.tempC); |
liamg | 0:91b1274ec397 | 110 | |
liamg | 0:91b1274ec397 | 111 | // Pressure test |
liamg | 0:91b1274ec397 | 112 | pressXL=readByte(LPS331addr,pPRESS_OUT_XL); |
liamg | 0:91b1274ec397 | 113 | pressL=readByte(LPS331addr,pPRESS_OUT_L); |
liamg | 0:91b1274ec397 | 114 | pressH=readByte(LPS331addr,pPRESS_OUT_H); |
liamg | 0:91b1274ec397 | 115 | |
liamg | 0:91b1274ec397 | 116 | press24=(pressH << 16) | (pressL << 8) | pressXL ; // 24-bit 2's complement data |
liamg | 0:91b1274ec397 | 117 | pressure = (float)press24/4096.0; // Sensitivity is 4096 LSB per milibar |
liamg | 0:91b1274ec397 | 118 | |
liamg | 0:91b1274ec397 | 119 | pc.printf("Pressure %.1f mbars or %.1f inches Hg\n", pressure, (pressure*0.0295)+0.029); |
liamg | 0:91b1274ec397 | 120 | |
liamg | 0:91b1274ec397 | 121 | } |
liamg | 0:91b1274ec397 | 122 | |
liamg | 0:91b1274ec397 | 123 | void LSM303 (SensorState_t * state) |
liamg | 0:91b1274ec397 | 124 | // Magnetometer and accelerometer |
liamg | 0:91b1274ec397 | 125 | { |
liamg | 0:91b1274ec397 | 126 | char xL, xH, yL, yH, zL, zH; |
liamg | 0:91b1274ec397 | 127 | int16_t mX, mY, mZ,aX,aY,aZ; |
liamg | 0:91b1274ec397 | 128 | float pitch,roll,faX,faY; |
liamg | 0:91b1274ec397 | 129 | |
liamg | 0:91b1274ec397 | 130 | xL=readByte(LSM303_m,mOUT_X_L_M); |
liamg | 0:91b1274ec397 | 131 | xH=readByte(LSM303_m,mOUT_X_H_M); |
liamg | 0:91b1274ec397 | 132 | yL=readByte(LSM303_m,mOUT_Y_L_M); |
liamg | 0:91b1274ec397 | 133 | yH=readByte(LSM303_m,mOUT_Y_H_M); |
liamg | 0:91b1274ec397 | 134 | zL=readByte(LSM303_m,mOUT_Z_L_M); |
liamg | 0:91b1274ec397 | 135 | zH=readByte(LSM303_m,mOUT_Z_H_M); |
liamg | 0:91b1274ec397 | 136 | |
liamg | 0:91b1274ec397 | 137 | mX=(xH<<8) | (xL); // 16-bit 2's complement data |
liamg | 0:91b1274ec397 | 138 | mY=(yH<<8) | (yL); |
liamg | 0:91b1274ec397 | 139 | mZ=(zH<<8) | (zL); |
liamg | 0:91b1274ec397 | 140 | |
liamg | 0:91b1274ec397 | 141 | //pc.printf("mX=%hd %X mY=%hd %X mZ=%hd %X \n",mX,mX,mY,mY,mZ,mZ); |
liamg | 0:91b1274ec397 | 142 | |
liamg | 0:91b1274ec397 | 143 | mX=mX-Offset_mX; // These are callibration co-efficients to deal with non-zero soft iron magnetic offset |
liamg | 0:91b1274ec397 | 144 | mY=mY-Offset_mY; |
liamg | 0:91b1274ec397 | 145 | |
liamg | 0:91b1274ec397 | 146 | xL=readByte(LSM303_a,aOUT_X_L_A); |
liamg | 0:91b1274ec397 | 147 | xH=readByte(LSM303_a,aOUT_X_H_A); |
liamg | 0:91b1274ec397 | 148 | yL=readByte(LSM303_a,aOUT_Y_L_A); |
liamg | 0:91b1274ec397 | 149 | yH=readByte(LSM303_a,aOUT_Y_H_A); |
liamg | 0:91b1274ec397 | 150 | zL=readByte(LSM303_a,aOUT_Z_L_A); |
liamg | 0:91b1274ec397 | 151 | zH=readByte(LSM303_a,aOUT_Z_H_A); |
liamg | 0:91b1274ec397 | 152 | |
liamg | 0:91b1274ec397 | 153 | 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. |
liamg | 0:91b1274ec397 | 154 | aY=(signed short) ( (yH<<8) | (yL) ) >> 4; |
liamg | 0:91b1274ec397 | 155 | aZ=(signed short) ( (zH<<8) | (zL) ) >> 4; |
liamg | 0:91b1274ec397 | 156 | |
liamg | 0:91b1274ec397 | 157 | //pc.printf("aX=%hd %X aY=%hd %X aZ=%hd %X \n",aX,aX,aY,aY,aZ,aZ); |
liamg | 0:91b1274ec397 | 158 | |
liamg | 0:91b1274ec397 | 159 | 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. |
liamg | 0:91b1274ec397 | 160 | 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) |
liamg | 0:91b1274ec397 | 161 | //faZ=((float) aZ) /2000.0; // Not used in a calc so comment out to avoid the compiler warning |
liamg | 0:91b1274ec397 | 162 | |
liamg | 0:91b1274ec397 | 163 | // Trigonometry derived from STM app note AN3192 and from WikiRobots |
liamg | 0:91b1274ec397 | 164 | 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 |
liamg | 0:91b1274ec397 | 165 | roll = asin(faY*2/cos(pitch)); |
liamg | 0:91b1274ec397 | 166 | |
liamg | 0:91b1274ec397 | 167 | float xh = mX * cos(pitch) + mZ * sin(pitch); |
liamg | 0:91b1274ec397 | 168 | float yh = mX * sin(roll) * sin(pitch) + mY * cos(roll) - mZ * sin(roll) * cos(pitch); |
liamg | 0:91b1274ec397 | 169 | float zh = -mX * cos(roll) * sin(pitch) + mY * sin(roll) + mZ * cos(roll) * cos(pitch); |
liamg | 0:91b1274ec397 | 170 | |
liamg | 0:91b1274ec397 | 171 | float heading = atan2(yh, xh) * RadtoDeg; // Note use of atan2 rather than atan since better for working with quadrants |
liamg | 0:91b1274ec397 | 172 | if (yh < 0) |
liamg | 0:91b1274ec397 | 173 | heading=360+heading; |
liamg | 0:91b1274ec397 | 174 | |
liamg | 0:91b1274ec397 | 175 | state->heading=heading; |
liamg | 0:91b1274ec397 | 176 | state->pitch=pitch; |
liamg | 0:91b1274ec397 | 177 | state->roll=roll; |
liamg | 0:91b1274ec397 | 178 | |
liamg | 0:91b1274ec397 | 179 | pc.printf("Orientation (deg): Pitch: %5.1f Roll: %5.1f Heading: %5.1f \n",pitch*RadtoDeg,roll*RadtoDeg,heading); |
liamg | 0:91b1274ec397 | 180 | pc.printf("Acceleration (mg): Forward: %5hd Left: %5hd Up: %5hd \n",aX,aY,aZ); |
liamg | 0:91b1274ec397 | 181 | |
liamg | 0:91b1274ec397 | 182 | } |
liamg | 0:91b1274ec397 | 183 | |
liamg | 0:91b1274ec397 | 184 | void L3GD20(void) // Gyro |
liamg | 0:91b1274ec397 | 185 | { |
liamg | 0:91b1274ec397 | 186 | char xL, xH, yL, yH, zL, zH; |
liamg | 0:91b1274ec397 | 187 | int16_t gX, gY, gZ; |
liamg | 0:91b1274ec397 | 188 | float rorX,rorY,rorZ; |
liamg | 0:91b1274ec397 | 189 | |
liamg | 0:91b1274ec397 | 190 | xL=readByte(L3GD20_ADDR,gOUT_X_L); |
liamg | 0:91b1274ec397 | 191 | xH=readByte(L3GD20_ADDR,gOUT_X_H); |
liamg | 0:91b1274ec397 | 192 | yL=readByte(L3GD20_ADDR,gOUT_Y_L); |
liamg | 0:91b1274ec397 | 193 | yH=readByte(L3GD20_ADDR,gOUT_Y_H); |
liamg | 0:91b1274ec397 | 194 | zL=readByte(L3GD20_ADDR,gOUT_Z_L); |
liamg | 0:91b1274ec397 | 195 | zH=readByte(L3GD20_ADDR,gOUT_Z_H); |
liamg | 0:91b1274ec397 | 196 | |
liamg | 0:91b1274ec397 | 197 | gX=(xH<<8) | (xL); // 16-bit 2's complement data |
liamg | 0:91b1274ec397 | 198 | gY=(yH<<8) | (yL); |
liamg | 0:91b1274ec397 | 199 | gZ=(zH<<8) | (zL); |
liamg | 0:91b1274ec397 | 200 | |
liamg | 0:91b1274ec397 | 201 | rorX=(float) gX * (17.5/1000.0); // At 500dps sensitivity, L3GD20 returns 17.5/1000 dps per digit |
liamg | 0:91b1274ec397 | 202 | rorY=(float) gY * (17.5/1000.0); |
liamg | 0:91b1274ec397 | 203 | rorZ=(float) gZ * (17.5/1000.0); |
liamg | 0:91b1274ec397 | 204 | |
liamg | 0:91b1274ec397 | 205 | pc.printf("Rate of rotation (dps): X:%5.1f Y:%5.1f Z:%5.1f \n",rorX,rorY,rorZ); |
liamg | 0:91b1274ec397 | 206 | //pc.printf("gX: %x gY: %x gZ: %x \n",gX,gY,gZ); |
liamg | 0:91b1274ec397 | 207 | |
liamg | 0:91b1274ec397 | 208 | } |
liamg | 0:91b1274ec397 | 209 | |
liamg | 0:91b1274ec397 | 210 | int main() |
liamg | 0:91b1274ec397 | 211 | { |
liamg | 0:91b1274ec397 | 212 | SensorState_t state; |
liamg | 0:91b1274ec397 | 213 | initSensors(); |
liamg | 0:91b1274ec397 | 214 | |
liamg | 0:91b1274ec397 | 215 | while(1) |
liamg | 0:91b1274ec397 | 216 | { |
liamg | 1:3b2260aff305 | 217 | |
liamg | 1:3b2260aff305 | 218 | #ifdef LPS331_on |
liamg | 0:91b1274ec397 | 219 | LPS331(state); |
liamg | 1:3b2260aff305 | 220 | #endif |
liamg | 1:3b2260aff305 | 221 | |
liamg | 1:3b2260aff305 | 222 | #ifdef LSM303_on |
liamg | 0:91b1274ec397 | 223 | LSM303(&state); |
liamg | 1:3b2260aff305 | 224 | #endif |
liamg | 1:3b2260aff305 | 225 | |
liamg | 1:3b2260aff305 | 226 | #ifdef L3GD20 |
liamg | 0:91b1274ec397 | 227 | L3GD20(); |
liamg | 1:3b2260aff305 | 228 | #endif |
liamg | 0:91b1274ec397 | 229 | |
liamg | 0:91b1274ec397 | 230 | pc.printf("\n"); |
liamg | 0:91b1274ec397 | 231 | wait(.5); |
liamg | 0:91b1274ec397 | 232 | } |
liamg | 0:91b1274ec397 | 233 | } |
liamg | 0:91b1274ec397 | 234 | |
liamg | 0:91b1274ec397 | 235 | |
liamg | 0:91b1274ec397 | 236 |