Diff: AttitudeEstimator/AttitudeEstimator.cpp

Revision:

13:1a871ebd35bb

Parent:

5:b9947e3d20cf

Child:

14:7439203a9e44

--- a/AttitudeEstimator/AttitudeEstimator.cpp	Fri Sep 28 21:45:50 2018 +0000
+++ b/AttitudeEstimator/AttitudeEstimator.cpp	Wed Oct 03 17:50:16 2018 +0000
@@ -63,9 +63,9 @@
     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_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;
+    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;
     // Adjust Euler angles (rad) to be in the interval of +/- pi
     if(phi_gyro > PI)
     {