testing gyro get orientation,acceleration states get difference in two states of the device
Dependencies: mbed
Fork of testing_gyro by
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
- Child:
- 3:95fecaa76b4a
#ifdef fixed
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 | 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 |