Dimitar Borisov
/
testing_gyro
testing gyro
Fork of MEMSdemoBoardMKI124V1 by
Revision 4:e89d74a1d9f5, committed 2015-03-25
- Comitter:
- dborisov
- Date:
- Wed Mar 25 13:31:01 2015 +0000
- Parent:
- 3:95fecaa76b4a
- Commit message:
- //testing; added functions for average value for orientation and acceleration; added function for calculating difference in two states of orientation and acceleration
Changed in this revision
MKI124V1.h | Show annotated file Show diff for this revision Revisions of this file |
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 95fecaa76b4a -r e89d74a1d9f5 MKI124V1.h --- a/MKI124V1.h Sun Mar 22 16:17:19 2015 +0000 +++ b/MKI124V1.h Wed Mar 25 13:31:01 2015 +0000 @@ -64,9 +64,9 @@ typedef struct{ float pitch; float roll; - float Xrotation; - float Yrotation; - float Zrotation; + int16_t aX; + int16_t aY; + int16_t aZ; float heading; float tempC; float pressuremB; @@ -74,4 +74,11 @@ - +typedef struct{ + float x; + float y; + float z; + int16_t aX; + int16_t aY; + int16_t aZ; +}Result_avrg;
diff -r 95fecaa76b4a -r e89d74a1d9f5 main.cpp --- a/main.cpp Sun Mar 22 16:17:19 2015 +0000 +++ b/main.cpp Wed Mar 25 13:31:01 2015 +0000 @@ -7,8 +7,6 @@ // By Liam Goudge. March 2014 #define LSM303_on -#define L3GD20_on -#define LPS301_on #include "mbed.h" #include "MKI124V1.h" @@ -17,7 +15,7 @@ DigitalOut myled(LED1); Serial pc(USBTX, USBRX); // tx, rx for USB debug printf to terminal console I2C i2c(p28, p27); // LPC1768 I2C pin allocation - +DigitalIn din(p23); // used as a test button // Globals int16_t const Offset_mX=-40.0; @@ -72,41 +70,11 @@ writeByte(LSM303_a,aCTRL_REG1_A ,0x37); // Set 25Hz ODR, everything else on writeByte(LSM303_a,aCTRL_REG4_A ,0x08); // Set full scale to +/- 2g sensitivity and high rez mode #endif - -#ifdef LPS331_on - // LPS331 Pressure sensor - pc.printf("LPS331 ping (should reply 0xBB): %x \n",readByte(LPS331addr,pWHO_AM_I)); - 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... - 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) -#endif - -#ifdef L3GD20_on - // L3GD20 gyro - printf("Ping L3GD20 (should reply 0xD4): %x \n",readByte(L3GD20_ADDR,gWHO_AM_I)); - writeByte(L3GD20_ADDR,gCTRL_REG1,0x0F); // Set ODR to 95Hz, BW to 12.5Hz, enable axes and turn on device - writeByte(L3GD20_ADDR,gCTRL_REG4,0x10); // Full scale selected at 500dps (degrees per second) - printf("L3GD20 gyro Temp: %x degrees C \n",readByte(L3GD20_ADDR,gOUT_TEMP)); -#endif pc.printf("--------------------------------------\n \n"); wait(2); // Wait for settings to stabilize } - -void LPS331(SensorState_t state) -// Read the pressure sensor -{ - uint8_t tempL,tempH; - int16_t temp; - - // Measure temperature - tempL=readByte(LPS331addr,pTEMP_OUT_L); - tempH=readByte(LPS331addr,pTEMP_OUT_H); - temp=(tempH << 8) | tempL; // 16-bit 2's complement data - - state.tempC=((float) temp / 480.0) + 42.5; // per equation on page 29 of the spec - pc.printf("Pressure sensor temperature %.1f degreesC \n",state.tempC); -} void LSM303 (SensorState_t * state) // Magnetometer and accelerometer { @@ -162,59 +130,99 @@ state->heading=heading; state->pitch=pitch; state->roll=roll; + state->aX = (float)aX; + state->aY = (float)aY; + state->aZ = (float)aZ; //5.1f - pc.printf("Orientation (deg): Rot_X: %0.0f Rot_Y: %0.0f Rot_Z: %0.0f \n",roll*RadtoDeg,pitch*RadtoDeg,heading); - pc.printf("Acceleration (mg): X: %5hd Y: %5hd Z: %5hd \n",aX,aY,aZ); + //pc.printf("Orientation (deg): Rot_X: %0.0f Rot_Y: %0.0f Rot_Z: %0.0f \n",roll*RadtoDeg,pitch*RadtoDeg,heading); + //pc.printf("Acceleration (mg): X: %5hd Y: %5hd Z: %5hd \n",aX,aY,aZ); } -void L3GD20(void) // Gyro -{ - char xL, xH, yL, yH, zL, zH; - int16_t gX, gY, gZ; - float rorX,rorY,rorZ; +//calculates the difference for acceleration in int16_t value +void calc_avrg_ac(Result_avrg* result,int samples){ + int i = 0; + result -> aX = 0; + result -> aY = 0; + result -> aZ = 0; + SensorState_t state; + for(i = 0;i<samples;i++){ + #ifdef LSM303_on + LSM303(&state); + #endif + result -> aX += state.aX; + result -> aY += state.aY; + result -> aZ += state.aZ; + //pc.printf("Acceleration (mg): X: %5hd Y: %5hd Z: %5hd \n",state.aX,state.aY,state.aZ); + + wait(0.01); + } + //pc.printf("Acceleration (mg): X: %5hd Y: %5hd Z: %5hd \n",result->aX,result->aY,result->aZ); + result -> aX = result -> aX / samples; + result -> aY = result -> aY / samples; + result -> aZ = result -> aZ / samples; + //pc.printf("rAcceleration (mg): X: %5hd Y: %5hd Z: %5hd \n",result->aX,result->aY,result->aZ); +} + +//calculates the difference for orientation in float value +void calc_avrg_or(Result_avrg* result,int samples){ + int i = 0; + result -> x = 0; + result -> y = 0; + result -> z = 0; + SensorState_t state; + for(i = 0;i<samples;i++){ + #ifdef LSM303_on + LSM303(&state); + #endif + result -> x += state.roll*RadtoDeg; + result -> y += state.pitch*RadtoDeg; + result -> z += state.heading; + //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); + wait(0.01); + } + //pc.printf("Orientation (deg): Rot_X: %0.0f Rot_Y: %0.0f Rot_Z: %0.0f \n", result -> x ,result -> y,result -> z); + result -> x = result -> x / samples; + result -> y = result -> y / samples; + result -> z = result -> z / samples; + //pc.printf("Orientation (deg): rRot_X: %0.0f rRot_Y: %0.0f rRot_Z: %0.0f \n", result -> x ,result -> y,result -> z); +} + +//gets the two results and saves the answer in r1 structure +void calc_diff(Result_avrg* r1, Result_avrg* r2){ + r1 -> x = abs(r1->x - r2->x); + r1 -> y = abs(r1->y - r2->y); + r1 -> z = abs(r1->z - r2->z); - xL=readByte(L3GD20_ADDR,gOUT_X_L); - xH=readByte(L3GD20_ADDR,gOUT_X_H); - yL=readByte(L3GD20_ADDR,gOUT_Y_L); - yH=readByte(L3GD20_ADDR,gOUT_Y_H); - zL=readByte(L3GD20_ADDR,gOUT_Z_L); - zH=readByte(L3GD20_ADDR,gOUT_Z_H); - - gX=(xH<<8) | (xL); // 16-bit 2's complement data - gY=(yH<<8) | (yL); - gZ=(zH<<8) | (zL); - - rorX=(float) gX * (17.5/1000.0); // At 500dps sensitivity, L3GD20 returns 17.5/1000 dps per digit - rorY=(float) gY * (17.5/1000.0); - rorZ=(float) gZ * (17.5/1000.0); - - pc.printf("Rate of rotation (dps): X:%5.1f Y:%5.1f Z:%5.1f \n",rorX,rorY,rorZ); - //pc.printf("gX: %x gY: %x gZ: %x \n",gX,gY,gZ); - + r1 -> aX = abs(r1->aX - r2->aX); + r1 -> aY = abs(r1->aY - r2->aY); + r1 -> aZ = abs(r1->aZ - r2->aZ); } int main() { SensorState_t state; + Result_avrg result1; + Result_avrg result2; initSensors(); pc.baud(115200); - while(1) - { - -#ifdef LPS331_on - //LPS331(state); -#endif - + calc_avrg_or(&result1,3); + calc_avrg_ac(&result1,3); + pc.printf("Orientation (deg): Rot_X: %0.0f Rot_Y: %0.0f Rot_Z: %0.0f \n", result1.x ,result1.y,result1.z); + pc.printf("Acceleration (mg): X: %5hd Y: %5hd Z: %5hd \n",result1.aX,result1.aY,result1.aZ); + calc_avrg_or(&result2,3); + calc_avrg_ac(&result2,3); + pc.printf("Orientation (deg): Rot_X: %0.0f Rot_Y: %0.0f Rot_Z: %0.0f \n", result2.x ,result2.y,result2.z); + pc.printf("Acceleration (mg): X: %5hd Y: %5hd Z: %5hd \n",result2.aX,result2.aY,result2.aZ); + calc_diff(&result1,&result2); + pc.printf("Orientation (deg): Rot_X: %0.0f Rot_Y: %0.0f Rot_Z: %0.0f \n", result1.x ,result1.y,result1.z); + pc.printf("Acceleration (mg): X: %5hd Y: %5hd Z: %5hd \n",result1.aX,result1.aY,result1.aZ); #ifdef LSM303_on LSM303(&state); #endif -#ifdef L3GD20_on - //L3GD20(); -#endif + //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); + //pc.printf("Acceleration (mg): X: %5hd Y: %5hd Z: %5hd \n",state.aX,state.aY,state.aZ); + //pc.printf("\n"); - pc.printf("\n"); - wait(.1); - } } \ No newline at end of file