testing gyro get orientation,acceleration states get difference in two states of the device
Dependencies: mbed
Fork of testing_gyro by
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?
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 | 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 |