Chun Feng Huang / ATTITUDE_ESTIMATION_WIPV

Attitude estimation using IMU (3-DoF ver.)

Fork of ATTITUDE_ESTIMATION by LDSC_Robotics_TAs

Diff: ATTITUDE_ESTIMATION.cpp

Revision:
3:7deaf89fbe33
Parent:
2:46d355d7abaa
Child:
4:040a642214c1
--- a/ATTITUDE_ESTIMATION.cpp	Tue May 03 22:15:07 2016 +0000
+++ b/ATTITUDE_ESTIMATION.cpp	Tue May 03 23:05:49 2016 +0000
@@ -50,7 +50,7 @@
 {
     Set_vector3(ys,y_in);
     Set_vector3(_x_est,ys); // _x_est be set as y_in
-    _init_flag = 1;    
+    ++_init_flag;    
 }
 void ATTITUDE::Run(float y_in[], float omega_in[]) // Main alogorithm
 {
@@ -60,7 +60,7 @@
     Set_vector3(ys,y_in);
     Set_vector3(omega,omega_in);
     
-    if(_init_flag == 0)
+    if(_init_flag < 3)
         ATTITUDE::Init(ys);
     else
     {
@@ -71,6 +71,7 @@
         Set_vector3(_x_est,x_est_plus);
     }
     //
+    Get_Estimation(x_est);
     gVector_to_EulerAngle(EulerAngle,_x_est);
 }
 void ATTITUDE::Get_Estimation(float v[])