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@0:91b1274ec397, 2014-03-18 (annotated)

Committer:
liamg
Date:
Tue Mar 18 05:22:01 2014 +0000
Revision:
0:91b1274ec397
Child:
1:3b2260aff305
Completed without multibyte read function

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 0:91b1274ec397 9 #include "mbed.h"
liamg 0:91b1274ec397 10 #include "MKI124V1.h"
liamg 0:91b1274ec397 11 #include "math.h"
liamg 0:91b1274ec397 12
liamg 0:91b1274ec397 13 DigitalOut myled(LED1);
liamg 0:91b1274ec397 14 Serial pc(USBTX, USBRX); // tx, rx for USB debug printf to terminal console
liamg 0:91b1274ec397 15 I2C i2c(p28, p27); // LPC1768 I2C pin allocation
liamg 0:91b1274ec397 16
liamg 0:91b1274ec397 17 // Globals
liamg 0:91b1274ec397 18 int16_t const Offset_mX=-40.0;
liamg 0:91b1274ec397 19 int16_t const Offset_mY=-115.0;
liamg 0:91b1274ec397 20 float const RadtoDeg=(180.0/3.141592654);
liamg 0:91b1274ec397 21
liamg 0:91b1274ec397 22
liamg 0:91b1274ec397 23 char readByte(char address, char reg)
liamg 0:91b1274ec397 24 // Reads one byte from an I2C address
liamg 0:91b1274ec397 25 // Didn't bother to make a multi-byte version to read in X,Y,Z low/high series of registers because...
liamg 0:91b1274ec397 26 // 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 27 {
liamg 0:91b1274ec397 28 char result;
liamg 0:91b1274ec397 29
liamg 0:91b1274ec397 30 i2c.start();
liamg 0:91b1274ec397 31 i2c.write(address); // Slave address with direction=write
liamg 0:91b1274ec397 32 i2c.write(reg); // Subaddress (register)
liamg 0:91b1274ec397 33
liamg 0:91b1274ec397 34 i2c.start(); // Break transmission to change bus direction
liamg 0:91b1274ec397 35 i2c.write(address + 1); // Slave address with direction=read [bit0=1]
liamg 0:91b1274ec397 36
liamg 0:91b1274ec397 37 result = i2c.read(0);
liamg 0:91b1274ec397 38 i2c.stop();
liamg 0:91b1274ec397 39 return (result);
liamg 0:91b1274ec397 40 }
liamg 0:91b1274ec397 41
liamg 0:91b1274ec397 42 void writeByte(char address, char reg,char value)
liamg 0:91b1274ec397 43 // Sends 1 byte to an I2C address
liamg 0:91b1274ec397 44 {
liamg 0:91b1274ec397 45 i2c.start();
liamg 0:91b1274ec397 46 i2c.write(address); // Slave address
liamg 0:91b1274ec397 47 i2c.write(reg); // Subaddress (register)
liamg 0:91b1274ec397 48 i2c.write(value);
liamg 0:91b1274ec397 49 i2c.stop();
liamg 0:91b1274ec397 50
liamg 0:91b1274ec397 51 }
liamg 0:91b1274ec397 52
liamg 0:91b1274ec397 53 void initSensors (void)
liamg 0:91b1274ec397 54 // Switch on and set up the 3 on-board sensors
liamg 0:91b1274ec397 55 {
liamg 0:91b1274ec397 56 pc.printf("--------------------------------------\n");
liamg 0:91b1274ec397 57 pc.printf("\nSTM MEMS eval board sensor init \n");
liamg 0:91b1274ec397 58
liamg 0:91b1274ec397 59 // LSM303DLHC Magnetic sensor
liamg 0:91b1274ec397 60 pc.printf("LSM303DLHC ping (should reply 0x48): %x \n",readByte(LSM303_m,mIRA_REG_M));
liamg 0:91b1274ec397 61 writeByte(LSM303_m,mCRA_REG_M,0x94); //switch on temperature sensor and set Output Data Rate to 30Hz
liamg 0:91b1274ec397 62 writeByte(LSM303_m,mCRB_REG_M,0x20); // Set the gain for +/- 1.3 Gauss full scale range
liamg 0:91b1274ec397 63 writeByte(LSM303_m,mMR_REG_M,0x0); // Continuous convertion mode
liamg 0:91b1274ec397 64
liamg 0:91b1274ec397 65 // LSM303DLHC Accelerometer
liamg 0:91b1274ec397 66 writeByte(LSM303_a,aCTRL_REG1_A ,0x37); // Set 25Hz ODR, everything else on
liamg 0:91b1274ec397 67 writeByte(LSM303_a,aCTRL_REG4_A ,0x08); // Set full scale to +/- 2g sensitivity and high rez mode
liamg 0:91b1274ec397 68
liamg 0:91b1274ec397 69 // LPS331 Pressure sensor
liamg 0:91b1274ec397 70 pc.printf("LPS331 ping (should reply 0xBB): %x \n",readByte(LPS331addr,pWHO_AM_I));
liamg 0:91b1274ec397 71 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 72 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 0:91b1274ec397 73
liamg 0:91b1274ec397 74 // L3GD20 gyro
liamg 0:91b1274ec397 75 printf("Ping L3GD20 (should reply 0xD4): %x \n",readByte(L3GD20_ADDR,gWHO_AM_I));
liamg 0:91b1274ec397 76 writeByte(L3GD20_ADDR,gCTRL_REG1,0x0F); // Set ODR to 95Hz, BW to 12.5Hz, enable axes and turn on device
liamg 0:91b1274ec397 77 writeByte(L3GD20_ADDR,gCTRL_REG4,0x10); // Full scale selected at 500dps (degrees per second)
liamg 0:91b1274ec397 78 printf("L3GD20 gyro Temp: %x degrees C \n",readByte(L3GD20_ADDR,gOUT_TEMP));
liamg 0:91b1274ec397 79
liamg 0:91b1274ec397 80 pc.printf("--------------------------------------\n \n");
liamg 0:91b1274ec397 81 wait(1); // Wait for settings to stabilize
liamg 0:91b1274ec397 82 }
liamg 0:91b1274ec397 83
liamg 0:91b1274ec397 84 void LPS331(SensorState_t state)
liamg 0:91b1274ec397 85 // Read the pressure sensor
liamg 0:91b1274ec397 86 {
liamg 0:91b1274ec397 87 uint8_t tempL,tempH,pressXL,pressL,pressH;
liamg 0:91b1274ec397 88 int16_t temp;
liamg 0:91b1274ec397 89 int32_t press24;
liamg 0:91b1274ec397 90 float pressure;
liamg 0:91b1274ec397 91
liamg 0:91b1274ec397 92 // Measure temperature
liamg 0:91b1274ec397 93 tempL=readByte(LPS331addr,pTEMP_OUT_L);
liamg 0:91b1274ec397 94 tempH=readByte(LPS331addr,pTEMP_OUT_H);
liamg 0:91b1274ec397 95 temp=(tempH << 8) | tempL; // 16-bit 2's complement data
liamg 0:91b1274ec397 96
liamg 0:91b1274ec397 97 state.tempC=((float) temp / 480.0) + 42.5; // per equation on page 29 of the spec
liamg 0:91b1274ec397 98
liamg 0:91b1274ec397 99 pc.printf("Pressure sensor temperature %.1f degreesC \n",state.tempC);
liamg 0:91b1274ec397 100
liamg 0:91b1274ec397 101 // Pressure test
liamg 0:91b1274ec397 102 pressXL=readByte(LPS331addr,pPRESS_OUT_XL);
liamg 0:91b1274ec397 103 pressL=readByte(LPS331addr,pPRESS_OUT_L);
liamg 0:91b1274ec397 104 pressH=readByte(LPS331addr,pPRESS_OUT_H);
liamg 0:91b1274ec397 105
liamg 0:91b1274ec397 106 press24=(pressH << 16) | (pressL << 8) | pressXL ; // 24-bit 2's complement data
liamg 0:91b1274ec397 107 pressure = (float)press24/4096.0; // Sensitivity is 4096 LSB per milibar
liamg 0:91b1274ec397 108
liamg 0:91b1274ec397 109 pc.printf("Pressure %.1f mbars or %.1f inches Hg\n", pressure, (pressure*0.0295)+0.029);
liamg 0:91b1274ec397 110
liamg 0:91b1274ec397 111 }
liamg 0:91b1274ec397 112
liamg 0:91b1274ec397 113 void LSM303 (SensorState_t * state)
liamg 0:91b1274ec397 114 // Magnetometer and accelerometer
liamg 0:91b1274ec397 115 {
liamg 0:91b1274ec397 116 char xL, xH, yL, yH, zL, zH;
liamg 0:91b1274ec397 117 int16_t mX, mY, mZ,aX,aY,aZ;
liamg 0:91b1274ec397 118 float pitch,roll,faX,faY;
liamg 0:91b1274ec397 119
liamg 0:91b1274ec397 120 xL=readByte(LSM303_m,mOUT_X_L_M);
liamg 0:91b1274ec397 121 xH=readByte(LSM303_m,mOUT_X_H_M);
liamg 0:91b1274ec397 122 yL=readByte(LSM303_m,mOUT_Y_L_M);
liamg 0:91b1274ec397 123 yH=readByte(LSM303_m,mOUT_Y_H_M);
liamg 0:91b1274ec397 124 zL=readByte(LSM303_m,mOUT_Z_L_M);
liamg 0:91b1274ec397 125 zH=readByte(LSM303_m,mOUT_Z_H_M);
liamg 0:91b1274ec397 126
liamg 0:91b1274ec397 127 mX=(xH<<8) | (xL); // 16-bit 2's complement data
liamg 0:91b1274ec397 128 mY=(yH<<8) | (yL);
liamg 0:91b1274ec397 129 mZ=(zH<<8) | (zL);
liamg 0:91b1274ec397 130
liamg 0:91b1274ec397 131 //pc.printf("mX=%hd %X mY=%hd %X mZ=%hd %X \n",mX,mX,mY,mY,mZ,mZ);
liamg 0:91b1274ec397 132
liamg 0:91b1274ec397 133 mX=mX-Offset_mX; // These are callibration co-efficients to deal with non-zero soft iron magnetic offset
liamg 0:91b1274ec397 134 mY=mY-Offset_mY;
liamg 0:91b1274ec397 135
liamg 0:91b1274ec397 136 xL=readByte(LSM303_a,aOUT_X_L_A);
liamg 0:91b1274ec397 137 xH=readByte(LSM303_a,aOUT_X_H_A);
liamg 0:91b1274ec397 138 yL=readByte(LSM303_a,aOUT_Y_L_A);
liamg 0:91b1274ec397 139 yH=readByte(LSM303_a,aOUT_Y_H_A);
liamg 0:91b1274ec397 140 zL=readByte(LSM303_a,aOUT_Z_L_A);
liamg 0:91b1274ec397 141 zH=readByte(LSM303_a,aOUT_Z_H_A);
liamg 0:91b1274ec397 142
liamg 0:91b1274ec397 143 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 144 aY=(signed short) ( (yH<<8) | (yL) ) >> 4;
liamg 0:91b1274ec397 145 aZ=(signed short) ( (zH<<8) | (zL) ) >> 4;
liamg 0:91b1274ec397 146
liamg 0:91b1274ec397 147 //pc.printf("aX=%hd %X aY=%hd %X aZ=%hd %X \n",aX,aX,aY,aY,aZ,aZ);
liamg 0:91b1274ec397 148
liamg 0:91b1274ec397 149 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 150 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 151 //faZ=((float) aZ) /2000.0; // Not used in a calc so comment out to avoid the compiler warning
liamg 0:91b1274ec397 152
liamg 0:91b1274ec397 153 // Trigonometry derived from STM app note AN3192 and from WikiRobots
liamg 0:91b1274ec397 154 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 155 roll = asin(faY*2/cos(pitch));
liamg 0:91b1274ec397 156
liamg 0:91b1274ec397 157 float xh = mX * cos(pitch) + mZ * sin(pitch);
liamg 0:91b1274ec397 158 float yh = mX * sin(roll) * sin(pitch) + mY * cos(roll) - mZ * sin(roll) * cos(pitch);
liamg 0:91b1274ec397 159 float zh = -mX * cos(roll) * sin(pitch) + mY * sin(roll) + mZ * cos(roll) * cos(pitch);
liamg 0:91b1274ec397 160
liamg 0:91b1274ec397 161 float heading = atan2(yh, xh) * RadtoDeg; // Note use of atan2 rather than atan since better for working with quadrants
liamg 0:91b1274ec397 162 if (yh < 0)
liamg 0:91b1274ec397 163 heading=360+heading;
liamg 0:91b1274ec397 164
liamg 0:91b1274ec397 165 state->heading=heading;
liamg 0:91b1274ec397 166 state->pitch=pitch;
liamg 0:91b1274ec397 167 state->roll=roll;
liamg 0:91b1274ec397 168
liamg 0:91b1274ec397 169 pc.printf("Orientation (deg): Pitch: %5.1f Roll: %5.1f Heading: %5.1f \n",pitch*RadtoDeg,roll*RadtoDeg,heading);
liamg 0:91b1274ec397 170 pc.printf("Acceleration (mg): Forward: %5hd Left: %5hd Up: %5hd \n",aX,aY,aZ);
liamg 0:91b1274ec397 171
liamg 0:91b1274ec397 172 }
liamg 0:91b1274ec397 173
liamg 0:91b1274ec397 174 void L3GD20(void) // Gyro
liamg 0:91b1274ec397 175 {
liamg 0:91b1274ec397 176 char xL, xH, yL, yH, zL, zH;
liamg 0:91b1274ec397 177 int16_t gX, gY, gZ;
liamg 0:91b1274ec397 178 float rorX,rorY,rorZ;
liamg 0:91b1274ec397 179
liamg 0:91b1274ec397 180 xL=readByte(L3GD20_ADDR,gOUT_X_L);
liamg 0:91b1274ec397 181 xH=readByte(L3GD20_ADDR,gOUT_X_H);
liamg 0:91b1274ec397 182 yL=readByte(L3GD20_ADDR,gOUT_Y_L);
liamg 0:91b1274ec397 183 yH=readByte(L3GD20_ADDR,gOUT_Y_H);
liamg 0:91b1274ec397 184 zL=readByte(L3GD20_ADDR,gOUT_Z_L);
liamg 0:91b1274ec397 185 zH=readByte(L3GD20_ADDR,gOUT_Z_H);
liamg 0:91b1274ec397 186
liamg 0:91b1274ec397 187 gX=(xH<<8) | (xL); // 16-bit 2's complement data
liamg 0:91b1274ec397 188 gY=(yH<<8) | (yL);
liamg 0:91b1274ec397 189 gZ=(zH<<8) | (zL);
liamg 0:91b1274ec397 190
liamg 0:91b1274ec397 191 rorX=(float) gX * (17.5/1000.0); // At 500dps sensitivity, L3GD20 returns 17.5/1000 dps per digit
liamg 0:91b1274ec397 192 rorY=(float) gY * (17.5/1000.0);
liamg 0:91b1274ec397 193 rorZ=(float) gZ * (17.5/1000.0);
liamg 0:91b1274ec397 194
liamg 0:91b1274ec397 195 pc.printf("Rate of rotation (dps): X:%5.1f Y:%5.1f Z:%5.1f \n",rorX,rorY,rorZ);
liamg 0:91b1274ec397 196 //pc.printf("gX: %x gY: %x gZ: %x \n",gX,gY,gZ);
liamg 0:91b1274ec397 197
liamg 0:91b1274ec397 198 }
liamg 0:91b1274ec397 199
liamg 0:91b1274ec397 200 int main()
liamg 0:91b1274ec397 201 {
liamg 0:91b1274ec397 202 SensorState_t state;
liamg 0:91b1274ec397 203
liamg 0:91b1274ec397 204 initSensors();
liamg 0:91b1274ec397 205
liamg 0:91b1274ec397 206 while(1)
liamg 0:91b1274ec397 207 {
liamg 0:91b1274ec397 208 LPS331(state);
liamg 0:91b1274ec397 209 LSM303(&state);
liamg 0:91b1274ec397 210 L3GD20();
liamg 0:91b1274ec397 211
liamg 0:91b1274ec397 212 pc.printf("\n");
liamg 0:91b1274ec397 213 wait(.5);
liamg 0:91b1274ec397 214 }
liamg 0:91b1274ec397 215 }
liamg 0:91b1274ec397 216
liamg 0:91b1274ec397 217
liamg 0:91b1274ec397 218