Diff: main.cpp

Revision:

2:2dfe6101b283

Parent:

0:cd56e3a52a36

Child:

4:55f97d30c14f

diff -r 87db26a201d3 -r 2dfe6101b283 main.cpp
--- a/main.cpp	Sat Sep 29 19:17:49 2018 +0000
+++ b/main.cpp	Sat Sep 29 20:14:12 2018 +0000
@@ -18,7 +18,7 @@
 uint16_t adc_value;
 
 double gyroXangle, gyroYangle; // Angle calculate using the gyro only
-double compAngleX, compAngleY; // Calculated angle using a complementary filter
+//double compAngleX, compAngleY; // Calculated angle using a complementary filter
 double kalAngleX, kalAngleY; // Calculated angle using a Kalman filter
 
 void gyro_getdata();
@@ -44,8 +44,8 @@
             kalmanY.setAngle(pitch);
             gyroXangle = roll;
             gyroYangle = pitch;
-            compAngleX = roll;
-            compAngleY = pitch;
+  //          compAngleX = roll;
+    //        compAngleY = pitch;
             t1 = t.read_us();
     while(1)
     {
@@ -92,7 +92,7 @@
   // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
   if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
     kalmanX.setAngle(roll);
-    compAngleX = roll;
+    //compAngleX = roll;
     kalAngleX = roll;
     gyroXangle = roll;
   } else
@@ -105,7 +105,7 @@
   // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
   if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) {
     kalmanY.setAngle(pitch);
-    compAngleY = pitch;
+    //compAngleY = pitch;
     kalAngleY = pitch;
     gyroYangle = pitch;
   } else
@@ -121,8 +121,8 @@
   gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate
   gyroYangle += kalmanY.getRate() * dt;
 
-  compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter
-  compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch;
+  //compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter
+  //compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch;
 
   // Reset the gyro angle when it has drifted too much
   if (gyroXangle < -180 || gyroXangle > 180)