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@4:e89d74a1d9f5, 2015-03-25 (annotated)

Committer:
dborisov
Date:
Wed Mar 25 13:31:01 2015 +0000
Revision:
4:e89d74a1d9f5
Parent:
3:95fecaa76b4a 
//testing; added functions for average value for orientation and acceleration; added function for calculating difference in two states of orientation and acceleration

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
liamg 0:91b1274ec397 11 #include "mbed.h"
liamg 0:91b1274ec397 12 #include "MKI124V1.h"
liamg 0:91b1274ec397 13 #include "math.h"
liamg 0:91b1274ec397 14
liamg 0:91b1274ec397 15 DigitalOut myled(LED1);
liamg 0:91b1274ec397 16 Serial pc(USBTX, USBRX); // tx, rx for USB debug printf to terminal console
liamg 0:91b1274ec397 17 I2C i2c(p28, p27); // LPC1768 I2C pin allocation
dborisov 4:e89d74a1d9f5 18 DigitalIn din(p23); // used as a test button
dborisov 3:95fecaa76b4a 19
liamg 0:91b1274ec397 20 // Globals
liamg 0:91b1274ec397 21 int16_t const Offset_mX=-40.0;
liamg 0:91b1274ec397 22 int16_t const Offset_mY=-115.0;
liamg 0:91b1274ec397 23 float const RadtoDeg=(180.0/3.141592654);
liamg 0:91b1274ec397 24
liamg 0:91b1274ec397 25
liamg 0:91b1274ec397 26 char readByte(char address, char reg)
liamg 0:91b1274ec397 27 // Reads one byte from an I2C address
liamg 0:91b1274ec397 28 // Didn't bother to make a multi-byte version to read in X,Y,Z low/high series of registers because...
liamg 0:91b1274ec397 29 // 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 30 {
liamg 0:91b1274ec397 31 char result;
liamg 0:91b1274ec397 32
liamg 0:91b1274ec397 33 i2c.start();
liamg 0:91b1274ec397 34 i2c.write(address); // Slave address with direction=write
liamg 0:91b1274ec397 35 i2c.write(reg); // Subaddress (register)
liamg 0:91b1274ec397 36
liamg 0:91b1274ec397 37 i2c.start(); // Break transmission to change bus direction
liamg 0:91b1274ec397 38 i2c.write(address + 1); // Slave address with direction=read [bit0=1]
liamg 0:91b1274ec397 39
liamg 0:91b1274ec397 40 result = i2c.read(0);
liamg 0:91b1274ec397 41 i2c.stop();
liamg 0:91b1274ec397 42 return (result);
liamg 0:91b1274ec397 43 }
liamg 0:91b1274ec397 44
liamg 0:91b1274ec397 45 void writeByte(char address, char reg,char value)
liamg 0:91b1274ec397 46 // Sends 1 byte to an I2C address
liamg 0:91b1274ec397 47 {
liamg 0:91b1274ec397 48 i2c.start();
liamg 0:91b1274ec397 49 i2c.write(address); // Slave address
liamg 0:91b1274ec397 50 i2c.write(reg); // Subaddress (register)
liamg 0:91b1274ec397 51 i2c.write(value);
liamg 0:91b1274ec397 52 i2c.stop();
liamg 0:91b1274ec397 53
liamg 0:91b1274ec397 54 }
liamg 0:91b1274ec397 55
liamg 0:91b1274ec397 56 void initSensors (void)
liamg 0:91b1274ec397 57 // Switch on and set up the 3 on-board sensors
liamg 0:91b1274ec397 58 {
liamg 0:91b1274ec397 59 pc.printf("--------------------------------------\n");
liamg 0:91b1274ec397 60 pc.printf("\nSTM MEMS eval board sensor init \n");
liamg 0:91b1274ec397 61
liamg 1:3b2260aff305 62 #ifdef LSM303_on
liamg 0:91b1274ec397 63 // LSM303DLHC Magnetic sensor
liamg 0:91b1274ec397 64 pc.printf("LSM303DLHC ping (should reply 0x48): %x \n",readByte(LSM303_m,mIRA_REG_M));
liamg 0:91b1274ec397 65 writeByte(LSM303_m,mCRA_REG_M,0x94); //switch on temperature sensor and set Output Data Rate to 30Hz
liamg 0:91b1274ec397 66 writeByte(LSM303_m,mCRB_REG_M,0x20); // Set the gain for +/- 1.3 Gauss full scale range
liamg 0:91b1274ec397 67 writeByte(LSM303_m,mMR_REG_M,0x0); // Continuous convertion mode
liamg 0:91b1274ec397 68
liamg 0:91b1274ec397 69 // LSM303DLHC Accelerometer
liamg 0:91b1274ec397 70 writeByte(LSM303_a,aCTRL_REG1_A ,0x37); // Set 25Hz ODR, everything else on
liamg 0:91b1274ec397 71 writeByte(LSM303_a,aCTRL_REG4_A ,0x08); // Set full scale to +/- 2g sensitivity and high rez mode
liamg 1:3b2260aff305 72 #endif
liamg 0:91b1274ec397 73
liamg 0:91b1274ec397 74 pc.printf("--------------------------------------\n \n");
dborisov 3:95fecaa76b4a 75 wait(2); // Wait for settings to stabilize
liamg 0:91b1274ec397 76 }
liamg 0:91b1274ec397 77
liamg 0:91b1274ec397 78 void LSM303 (SensorState_t * state)
liamg 0:91b1274ec397 79 // Magnetometer and accelerometer
liamg 0:91b1274ec397 80 {
liamg 0:91b1274ec397 81 char xL, xH, yL, yH, zL, zH;
liamg 0:91b1274ec397 82 int16_t mX, mY, mZ,aX,aY,aZ;
liamg 0:91b1274ec397 83 float pitch,roll,faX,faY;
liamg 0:91b1274ec397 84
liamg 0:91b1274ec397 85 xL=readByte(LSM303_m,mOUT_X_L_M);
liamg 0:91b1274ec397 86 xH=readByte(LSM303_m,mOUT_X_H_M);
liamg 0:91b1274ec397 87 yL=readByte(LSM303_m,mOUT_Y_L_M);
liamg 0:91b1274ec397 88 yH=readByte(LSM303_m,mOUT_Y_H_M);
liamg 0:91b1274ec397 89 zL=readByte(LSM303_m,mOUT_Z_L_M);
liamg 0:91b1274ec397 90 zH=readByte(LSM303_m,mOUT_Z_H_M);
liamg 0:91b1274ec397 91
liamg 0:91b1274ec397 92 mX=(xH<<8) | (xL); // 16-bit 2's complement data
liamg 0:91b1274ec397 93 mY=(yH<<8) | (yL);
liamg 0:91b1274ec397 94 mZ=(zH<<8) | (zL);
liamg 0:91b1274ec397 95
liamg 0:91b1274ec397 96 //pc.printf("mX=%hd %X mY=%hd %X mZ=%hd %X \n",mX,mX,mY,mY,mZ,mZ);
liamg 0:91b1274ec397 97
liamg 0:91b1274ec397 98 mX=mX-Offset_mX; // These are callibration co-efficients to deal with non-zero soft iron magnetic offset
liamg 0:91b1274ec397 99 mY=mY-Offset_mY;
liamg 0:91b1274ec397 100
liamg 0:91b1274ec397 101 xL=readByte(LSM303_a,aOUT_X_L_A);
liamg 0:91b1274ec397 102 xH=readByte(LSM303_a,aOUT_X_H_A);
liamg 0:91b1274ec397 103 yL=readByte(LSM303_a,aOUT_Y_L_A);
liamg 0:91b1274ec397 104 yH=readByte(LSM303_a,aOUT_Y_H_A);
liamg 0:91b1274ec397 105 zL=readByte(LSM303_a,aOUT_Z_L_A);
liamg 0:91b1274ec397 106 zH=readByte(LSM303_a,aOUT_Z_H_A);
liamg 0:91b1274ec397 107
liamg 0:91b1274ec397 108 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 109 aY=(signed short) ( (yH<<8) | (yL) ) >> 4;
liamg 0:91b1274ec397 110 aZ=(signed short) ( (zH<<8) | (zL) ) >> 4;
liamg 0:91b1274ec397 111
liamg 0:91b1274ec397 112 //pc.printf("aX=%hd %X aY=%hd %X aZ=%hd %X \n",aX,aX,aY,aY,aZ,aZ);
liamg 0:91b1274ec397 113
liamg 0:91b1274ec397 114 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 115 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 116 //faZ=((float) aZ) /2000.0; // Not used in a calc so comment out to avoid the compiler warning
liamg 0:91b1274ec397 117
liamg 0:91b1274ec397 118 // Trigonometry derived from STM app note AN3192 and from WikiRobots
liamg 0:91b1274ec397 119 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 120 roll = asin(faY*2/cos(pitch));
liamg 0:91b1274ec397 121
liamg 0:91b1274ec397 122 float xh = mX * cos(pitch) + mZ * sin(pitch);
liamg 0:91b1274ec397 123 float yh = mX * sin(roll) * sin(pitch) + mY * cos(roll) - mZ * sin(roll) * cos(pitch);
liamg 0:91b1274ec397 124 float zh = -mX * cos(roll) * sin(pitch) + mY * sin(roll) + mZ * cos(roll) * cos(pitch);
liamg 0:91b1274ec397 125
liamg 0:91b1274ec397 126 float heading = atan2(yh, xh) * RadtoDeg; // Note use of atan2 rather than atan since better for working with quadrants
liamg 0:91b1274ec397 127 if (yh < 0)
liamg 0:91b1274ec397 128 heading=360+heading;
liamg 0:91b1274ec397 129
liamg 0:91b1274ec397 130 state->heading=heading;
liamg 0:91b1274ec397 131 state->pitch=pitch;
liamg 0:91b1274ec397 132 state->roll=roll;
dborisov 4:e89d74a1d9f5 133 state->aX = (float)aX;
dborisov 4:e89d74a1d9f5 134 state->aY = (float)aY;
dborisov 4:e89d74a1d9f5 135 state->aZ = (float)aZ;
dborisov 3:95fecaa76b4a 136 //5.1f
dborisov 4:e89d74a1d9f5 137 //pc.printf("Orientation (deg): Rot_X: %0.0f Rot_Y: %0.0f Rot_Z: %0.0f \n",roll*RadtoDeg,pitch*RadtoDeg,heading);
dborisov 4:e89d74a1d9f5 138 //pc.printf("Acceleration (mg): X: %5hd Y: %5hd Z: %5hd \n",aX,aY,aZ);
liamg 0:91b1274ec397 139
liamg 0:91b1274ec397 140 }
liamg 0:91b1274ec397 141
dborisov 4:e89d74a1d9f5 142 //calculates the difference for acceleration in int16_t value
dborisov 4:e89d74a1d9f5 143 void calc_avrg_ac(Result_avrg* result,int samples){
dborisov 4:e89d74a1d9f5 144 int i = 0;
dborisov 4:e89d74a1d9f5 145 result -> aX = 0;
dborisov 4:e89d74a1d9f5 146 result -> aY = 0;
dborisov 4:e89d74a1d9f5 147 result -> aZ = 0;
dborisov 4:e89d74a1d9f5 148 SensorState_t state;
dborisov 4:e89d74a1d9f5 149 for(i = 0;i<samples;i++){
dborisov 4:e89d74a1d9f5 150 #ifdef LSM303_on
dborisov 4:e89d74a1d9f5 151 LSM303(&state);
dborisov 4:e89d74a1d9f5 152 #endif
dborisov 4:e89d74a1d9f5 153 result -> aX += state.aX;
dborisov 4:e89d74a1d9f5 154 result -> aY += state.aY;
dborisov 4:e89d74a1d9f5 155 result -> aZ += state.aZ;
dborisov 4:e89d74a1d9f5 156 //pc.printf("Acceleration (mg): X: %5hd Y: %5hd Z: %5hd \n",state.aX,state.aY,state.aZ);
dborisov 4:e89d74a1d9f5 157
dborisov 4:e89d74a1d9f5 158 wait(0.01);
dborisov 4:e89d74a1d9f5 159 }
dborisov 4:e89d74a1d9f5 160 //pc.printf("Acceleration (mg): X: %5hd Y: %5hd Z: %5hd \n",result->aX,result->aY,result->aZ);
dborisov 4:e89d74a1d9f5 161 result -> aX = result -> aX / samples;
dborisov 4:e89d74a1d9f5 162 result -> aY = result -> aY / samples;
dborisov 4:e89d74a1d9f5 163 result -> aZ = result -> aZ / samples;
dborisov 4:e89d74a1d9f5 164 //pc.printf("rAcceleration (mg): X: %5hd Y: %5hd Z: %5hd \n",result->aX,result->aY,result->aZ);
dborisov 4:e89d74a1d9f5 165 }
dborisov 4:e89d74a1d9f5 166
dborisov 4:e89d74a1d9f5 167 //calculates the difference for orientation in float value
dborisov 4:e89d74a1d9f5 168 void calc_avrg_or(Result_avrg* result,int samples){
dborisov 4:e89d74a1d9f5 169 int i = 0;
dborisov 4:e89d74a1d9f5 170 result -> x = 0;
dborisov 4:e89d74a1d9f5 171 result -> y = 0;
dborisov 4:e89d74a1d9f5 172 result -> z = 0;
dborisov 4:e89d74a1d9f5 173 SensorState_t state;
dborisov 4:e89d74a1d9f5 174 for(i = 0;i<samples;i++){
dborisov 4:e89d74a1d9f5 175 #ifdef LSM303_on
dborisov 4:e89d74a1d9f5 176 LSM303(&state);
dborisov 4:e89d74a1d9f5 177 #endif
dborisov 4:e89d74a1d9f5 178 result -> x += state.roll*RadtoDeg;
dborisov 4:e89d74a1d9f5 179 result -> y += state.pitch*RadtoDeg;
dborisov 4:e89d74a1d9f5 180 result -> z += state.heading;
dborisov 4:e89d74a1d9f5 181 //pc.printf("Orientation (deg): Rot_X: %0.0f Rot_Y: %0.0f Rot_Z: %0.0f \n",state.roll*RadtoDeg,state.pitch*RadtoDeg,state.heading);
dborisov 4:e89d74a1d9f5 182 wait(0.01);
dborisov 4:e89d74a1d9f5 183 }
dborisov 4:e89d74a1d9f5 184 //pc.printf("Orientation (deg): Rot_X: %0.0f Rot_Y: %0.0f Rot_Z: %0.0f \n", result -> x ,result -> y,result -> z);
dborisov 4:e89d74a1d9f5 185 result -> x = result -> x / samples;
dborisov 4:e89d74a1d9f5 186 result -> y = result -> y / samples;
dborisov 4:e89d74a1d9f5 187 result -> z = result -> z / samples;
dborisov 4:e89d74a1d9f5 188 //pc.printf("Orientation (deg): rRot_X: %0.0f rRot_Y: %0.0f rRot_Z: %0.0f \n", result -> x ,result -> y,result -> z);
dborisov 4:e89d74a1d9f5 189 }
dborisov 4:e89d74a1d9f5 190
dborisov 4:e89d74a1d9f5 191 //gets the two results and saves the answer in r1 structure
dborisov 4:e89d74a1d9f5 192 void calc_diff(Result_avrg* r1, Result_avrg* r2){
dborisov 4:e89d74a1d9f5 193 r1 -> x = abs(r1->x - r2->x);
dborisov 4:e89d74a1d9f5 194 r1 -> y = abs(r1->y - r2->y);
dborisov 4:e89d74a1d9f5 195 r1 -> z = abs(r1->z - r2->z);
liamg 0:91b1274ec397 196
dborisov 4:e89d74a1d9f5 197 r1 -> aX = abs(r1->aX - r2->aX);
dborisov 4:e89d74a1d9f5 198 r1 -> aY = abs(r1->aY - r2->aY);
dborisov 4:e89d74a1d9f5 199 r1 -> aZ = abs(r1->aZ - r2->aZ);
liamg 0:91b1274ec397 200 }
liamg 0:91b1274ec397 201
liamg 0:91b1274ec397 202 int main()
liamg 0:91b1274ec397 203 {
liamg 0:91b1274ec397 204 SensorState_t state;
dborisov 4:e89d74a1d9f5 205 Result_avrg result1;
dborisov 4:e89d74a1d9f5 206 Result_avrg result2;
liamg 0:91b1274ec397 207 initSensors();
dborisov 3:95fecaa76b4a 208 pc.baud(115200);
dborisov 4:e89d74a1d9f5 209 calc_avrg_or(&result1,3);
dborisov 4:e89d74a1d9f5 210 calc_avrg_ac(&result1,3);
dborisov 4:e89d74a1d9f5 211 pc.printf("Orientation (deg): Rot_X: %0.0f Rot_Y: %0.0f Rot_Z: %0.0f \n", result1.x ,result1.y,result1.z);
dborisov 4:e89d74a1d9f5 212 pc.printf("Acceleration (mg): X: %5hd Y: %5hd Z: %5hd \n",result1.aX,result1.aY,result1.aZ);
dborisov 4:e89d74a1d9f5 213 calc_avrg_or(&result2,3);
dborisov 4:e89d74a1d9f5 214 calc_avrg_ac(&result2,3);
dborisov 4:e89d74a1d9f5 215 pc.printf("Orientation (deg): Rot_X: %0.0f Rot_Y: %0.0f Rot_Z: %0.0f \n", result2.x ,result2.y,result2.z);
dborisov 4:e89d74a1d9f5 216 pc.printf("Acceleration (mg): X: %5hd Y: %5hd Z: %5hd \n",result2.aX,result2.aY,result2.aZ);
dborisov 4:e89d74a1d9f5 217 calc_diff(&result1,&result2);
dborisov 4:e89d74a1d9f5 218 pc.printf("Orientation (deg): Rot_X: %0.0f Rot_Y: %0.0f Rot_Z: %0.0f \n", result1.x ,result1.y,result1.z);
dborisov 4:e89d74a1d9f5 219 pc.printf("Acceleration (mg): X: %5hd Y: %5hd Z: %5hd \n",result1.aX,result1.aY,result1.aZ);
liamg 1:3b2260aff305 220 #ifdef LSM303_on
liamg 0:91b1274ec397 221 LSM303(&state);
liamg 1:3b2260aff305 222 #endif
liamg 1:3b2260aff305 223
dborisov 4:e89d74a1d9f5 224 //pc.printf("Orientation (deg): Rot_X: %0.0f Rot_Y: %0.0f Rot_Z: %0.0f \n",state.roll*RadtoDeg,state.pitch*RadtoDeg,state.heading);
dborisov 4:e89d74a1d9f5 225 //pc.printf("Acceleration (mg): X: %5hd Y: %5hd Z: %5hd \n",state.aX,state.aY,state.aZ);
dborisov 4:e89d74a1d9f5 226 //pc.printf("\n");
liamg 0:91b1274ec397 227
dborisov 3:95fecaa76b4a 228 }