Diff: main.cpp

Revision:

4:e89d74a1d9f5

Parent:

3:95fecaa76b4a

--- 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