Diff: AttitudeEstimator/AttitudeEstimator.cpp

Branch:

yuri

Revision:

8:c96125e9ac70

Parent:

1:579511e9f0b8

diff -r 7a447d4ae677 -r c96125e9ac70 AttitudeEstimator/AttitudeEstimator.cpp
--- a/AttitudeEstimator/AttitudeEstimator.cpp	Wed Oct 17 12:18:29 2018 +0000
+++ b/AttitudeEstimator/AttitudeEstimator.cpp	Wed Nov 21 10:07:00 2018 +0000
@@ -26,14 +26,17 @@
     q_bias = 0.0f;
     r_bias = 0.0f;
 
-    for(count = 1; count <= taxa; ++count) 
+    for(count = 1; count <= taxa; count++) 
     {
         imu.read();
-        p_bias += imu.gx/200.0f;
-        q_bias += imu.gy/200.0f;
-        r_bias += imu.gz/200.0f;
+        p_bias += imu.gx;
+        q_bias += imu.gy;
+        r_bias += imu.gz;
         wait(0.005);
     }
+    p_bias = p_bias/200.0f;
+    q_bias = q_bias/200.0f;
+    r_bias = r_bias/200.0f;
 }
 
 // Estimate Euler angles (rad ) from accelerometer data
@@ -92,10 +95,7 @@
     imu.read();
     estimate_accel();
     estimate_gyro();
-    phi = rho*phi_accel + (1-rho)*phi_gyro;
-    theta = rho*theta_accel + (1-rho)*theta_gyro;
+    phi = rho*phi_accel + (1.0f-rho)*phi_gyro;
+    theta = rho*theta_accel + (1.0f-rho)*theta_gyro;
     psi = psi_gyro;
-}
-
-
-
+}
\ No newline at end of file