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