Diff: main.cpp

Revision:

1:44a18ed16bae

Parent:

0:3af3f05e8f7d

Child:

2:1e20c0376eb6

--- a/main.cpp	Fri Nov 02 05:58:27 2018 +0000
+++ b/main.cpp	Fri Nov 02 13:11:21 2018 +0000
@@ -52,10 +52,31 @@
 //        VCC,   SCE,  RST,  D/C,  MOSI,S CLK, LED
 
 DigitalIn boardbtn(USER_BUTTON);
-
-
+static float acc_xyz[3]={0,0,0};
+static float v_xyz[3]={0,0,0};
+float displacement_xyz[3]={0,0,0};
+void AccelToVelocity(float ax,float ay,float az,float dt){
+    static float old_acc[3]={0,0,0};
+    static float old_v[3]={0,0,0};
+    float delta_ax=ax-old_acc[0];
+    float delta_ay=ay-old_acc[1];
+    float delta_az=az-old_acc[2];
+    old_acc[0]=ax;
+    old_acc[1]=ay;
+    old_acc[2]=az;
+    old_v[0]=v_xyz[0]-old_v[0];
+    old_v[1]=v_xyz[1]-old_v[1];
+    old_v[2]=v_xyz[2]-old_v[2];
+    v_xyz[0]=old_v[0]+delta_ax*dt;
+    v_xyz[1]=old_v[1]+delta_ay*dt;
+    v_xyz[2]=old_v[2]+delta_az*dt;
+    
+    //pc.printf("%f %f %f \n",old_acc[]);
+    
+    
+    }
 int main()
-{
+{   
     pc.baud(9600);
 
     //Set up I2C
@@ -89,7 +110,7 @@
         pc.printf("x-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[3]);
         pc.printf("y-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[4]);
         pc.printf("z-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[5]);*/
-     if(boardbtn==0){
+//     if(boardbtn==0){
         // wait(2);
         mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
         pc.printf("x gyro bias = %f\n\r", gyroBias[0]);
@@ -100,7 +121,7 @@
         pc.printf("z accel bias = %f\n\r", accelBias[2]);
         wait(2);
         
-///*
+/*
 // -----------------------------------------------------------------
         }else{
         gyroBias[0]=-1.244275;
@@ -111,7 +132,7 @@
         accelBias[2]=-0.020508;
         }
 //------------------------------------------------------------------
-//*/
+*/
         mpu9250.initMPU9250();
 //        pc.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
         mpu9250.initAK8963(magCalibration);
@@ -239,7 +260,7 @@
             yaw   -= 85.0f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
             roll  *= 180.0f / PI;
 
-            pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
+  //          pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
  //   pc.printf("average rate = %f\n\r", (float) sumCount/sum);
 //    sprintf(buffer, "YPR: %f %f %f", yaw, pitch, roll);
 //    lcd.printString(buffer, 0, 4);
@@ -248,7 +269,13 @@
 
             myled= !myled;
             count = t.read_ms();
-
+    acc_xyz[0]=ax;
+        acc_xyz[1]=ay;
+        acc_xyz[2]=az;
+        
+        AccelToVelocity(acc_xyz[0],acc_xyz[1],acc_xyz[2],float(delt_t));//deltat*1000000
+        pc.printf("%f %f %f \n",v_xyz[0],v_xyz[1],v_xyz[2]);
+        //pc.printf("%d\n",count);
             if(count > 1<<21) {
                 t.start(); // start the timer over again if ~30 minutes has passed
                 count = 0;
@@ -257,7 +284,10 @@
             }
             sum = 0;
             sumCount = 0;
+//-----------------------------------------------------------------------------------------        
+        
+        
         }
     }
-
+    
 }
\ No newline at end of file