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