Team Alpha / Mbed 2 deprecated Accelerometer_gyro

testing gyro get orientation,acceleration states get difference in two states of the device

Dependencies:   mbed

Fork of testing_gyro by Dimitar Borisov

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?

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