Diff: ATTITUDE_ESTIMATION.cpp

Revision:

9:84fad91d3587

Parent:

8:3882cb4be9d3

Child:

10:166006e89252

--- a/ATTITUDE_ESTIMATION.cpp	Tue Dec 27 11:28:49 2016 +0000
+++ b/ATTITUDE_ESTIMATION.cpp	Wed Dec 28 16:53:58 2016 +0000
@@ -41,9 +41,8 @@
 }
 
 //-------------------------------------------------------//
-ATTITUDE::ATTITUDE(float alpha_in, float one_over_gamma_in, float Ts_in):
+ATTITUDE::ATTITUDE(float alpha_in, float Ts_in):
                         alpha(alpha_in),
-                        one_over_gamma(one_over_gamma_in),
                         Ts(Ts_in),
                         lpfv_y_acce(3, Ts_in, 10.0), // Input filter for accelerometers
                         lpfv_y_mag(3, Ts_in, 200.0), // Input filter for magenetometers
@@ -56,6 +55,8 @@
 
     // Default: close the gyro-bias estimation
     enable_biasEst = false;
+    one_over_gamma = 0.0;
+    // Default: close the estimation for magenetic field
     enable_magEst = false;
 
     // Unit transformation
@@ -125,7 +126,7 @@
     pitch = atan2(cos(roll)*vg_in[2],vg_in[0]); // psi, pitch
     */
 
-    // Eular angle: 1-3-2, zs is pointing forwasd
+    // Eular angle: 1-3-2, zs is pointing forward
     // yaw = 0.0; // phi, yaw
     pitch = atan2(-vg_in[2],-vg_in[0]); // psi, pitch
     if (abs(pitch) <  0.7854){ // pi/4
@@ -154,6 +155,10 @@
     alpha = alpha_in;
     L1_diag.assign(n,alpha_in);
 }
+void ATTITUDE::enable_gyroBiasEst(float gamma_in){ // Enable the gyro-bias estimation
+    enable_biasEst = true;
+    one_over_gamma = 1/gamma_in;
+}
 //
 void ATTITUDE::Init(void) // Let x_est = ys
 {