Diff: AttitudeEstimator/AttitudeEstimator.cpp

Revision:

5:b9947e3d20cf

Parent:

1:24effec9e9aa

Child:

13:1a871ebd35bb

--- a/AttitudeEstimator/AttitudeEstimator.cpp	Wed Sep 26 21:47:37 2018 +0000
+++ b/AttitudeEstimator/AttitudeEstimator.cpp	Thu Sep 27 13:51:54 2018 +0000
@@ -63,33 +63,33 @@
     q = imu.gy - q_bias;
     r = imu.gz - r_bias;
     // Estimate Euler angles (rad)
-    phi_gyro = phi + (p+q*sin(phi)*tan(theta)+r*cos(phi)*tan(theta))*dt;
-    theta_gyro = theta + (q*cos(phi)-r*sin(phi))*dt;
-    psi_gyro = psi + (q*sin(phi)/cos(theta)+r*cos(phi)/cos(theta))*dt;
+    phi_gyro = phi + (p+q*sin(phi)*tan(theta)+r*cos(phi)*tan(theta))*dt_att;
+    theta_gyro = theta + (q*cos(phi)-r*sin(phi))*dt_att;
+    psi_gyro = psi + (q*sin(phi)/cos(theta)+r*cos(phi)/cos(theta))*dt_att;
     // Adjust Euler angles (rad) to be in the interval of +/- pi
-    if(phi_gyro > pi)
+    if(phi_gyro > PI)
     {
-        phi_gyro = phi_gyro - 2*pi;
+        phi_gyro = phi_gyro - 2*PI;
     }
-    else if(phi_gyro < -pi)
+    else if(phi_gyro < -PI)
     {
-        phi_gyro = phi_gyro + 2*pi;
+        phi_gyro = phi_gyro + 2*PI;
     }
     
-    if(theta_gyro > pi)
+    if(theta_gyro > PI)
     {
-        theta_gyro = theta_gyro - 2*pi;
+        theta_gyro = theta_gyro - 2*PI;
     }
-    else if(theta_gyro < -pi)
+    else if(theta_gyro < -PI)
     {
-        theta_gyro = theta_gyro + 2*pi;
+        theta_gyro = theta_gyro + 2*PI;
     }
-    if(psi_gyro > pi)
+    if(psi_gyro > PI)
     {
-        psi_gyro = psi_gyro - 2*pi;
+        psi_gyro = psi_gyro - 2*PI;
     }
-    else if(psi_gyro < -pi)
+    else if(psi_gyro < -PI)
     {
-        psi_gyro = psi_gyro + 2*pi;
+        psi_gyro = psi_gyro + 2*PI;
     }
 }
\ No newline at end of file