Diff: main.cpp

Revision:

5:f66e4666de56

Parent:

4:4144ef007743

Child:

6:9712c04e13bf

diff -r 4144ef007743 -r f66e4666de56 main.cpp
--- a/main.cpp	Thu Jan 12 22:11:45 2017 +0000
+++ b/main.cpp	Fri Jan 13 07:29:19 2017 +0000
@@ -151,7 +151,7 @@
     uint32_t failures = 0;
     
     DigitalIn uSDdetect(P2_2, PullUp);
-    static const uint32_t N = 8000;
+    static const uint32_t N = 14400;
     uint32_t samples = 0;
     float accYaxisBuff[N];
     float accZaxisBuff[N];
@@ -273,8 +273,7 @@
         BMI160::SensorTime sensorTime;
         
         //Complementary Filter vars, filter weight K
-        static const float K = 0.9F;
-        float thetaAcc = 0.0F;
+        static const float K = 0.99995F;
         float pitch = 0.0F;
         
         //PID coefficients
@@ -542,23 +541,22 @@
         fprintf(fp, "Time, Y-Acc, Gyro dps, Gyro Estimate, Acc Estimate, Filter Estimate, PW\n");
         
         float accPitch = 0.0F;
-        float gyroPitch = 0.0F;
+        float gyroDegrees = 0.0F;
         float pitch = 0.0F;
         float k = loopCoeffs[1];
         float dt = loopCoeffs[5];
         
         for(uint32_t idx = 0; idx < numSamples; idx++) 
         {
-            //Time
-            fprintf(fp, "%f,", idx*loopCoeffs[5]);
+            fprintf(fp, "%f,", idx*dt);
             fprintf(fp, "%5.4f,", accY[idx]);
             fprintf(fp, "%6.2f,", gyroX[idx]);
-            gyroPitch += (gyroX[idx]*loopCoeffs[5]);
-            fprintf(fp, "%5.2f,", gyroPitch);
+            gyroDegrees += (gyroX[idx] * dt);
+            fprintf(fp, "%5.2f,", gyroDegrees);
             accPitch = ((180.0F/3.14159F) * atan(accY[idx]/accZ[idx]));
             fprintf(fp, "%5.2f,", accPitch);
             pitch = ((k * (pitch + (gyroX[idx] * dt))) + ((1.0F - k) * accY[idx]));
-            fprintf(fp, "%5.2f", pitch);
+            fprintf(fp, "%5.2f,", pitch);
             fprintf(fp, "%f", pw[idx]);
             fprintf(fp, "\n");
         }