Liam G / Mbed 2 deprecated MEMSdemoBoardMKI124V1

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

Dependencies:   mbed

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:

/media/uploads/liamg/setup2.png

The schematic is here:

http://www.st.com/web/en/catalog/tools/PF253482

The orientation of the sensors on the board is like this:

/media/uploads/liamg/board_shot.png

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:

/media/uploads/liamg/calibration.png

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:

http://www.sensorsmag.com/sensors/motion-velocity-displacement/compensating-tilt-hard-iron-and-soft-iron-effects-6475

main.cpp@2:2ef63ab235bf, 2014-03-18 (annotated)

Committer:
liamg
Date:
Tue Mar 18 17:29:44 2014 +0000
Revision:
2:2ef63ab235bf
Parent:
1:3b2260aff305 
#ifdef fixed

Who changed what in which revision?

UserRevisionLine numberNew 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 2:2ef63ab235bf 75 #ifdef LPS331_on
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 2:2ef63ab235bf 82 #ifdef L3GD20_on
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 2:2ef63ab235bf 226 #ifdef L3GD20_on
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