Fabio Bobrow / CrazyflieController

Library containing Crazyflie 2.0 controller classes: - Attitude estimator - Horizontal estimator - Vertical estimator - Attitude controller - Horizontal controller - Vertical controller - Mixer

Diff: HorizontaEstimator/HorizontalEstimator.cpp

Revision:
15:155ca63b7884
Parent:
12:9fed6f656f88
Child:
17:f682b4a5686d
--- a/HorizontaEstimator/HorizontalEstimator.cpp	Wed Oct 03 19:39:48 2018 +0000
+++ b/HorizontaEstimator/HorizontalEstimator.cpp	Wed Oct 03 22:12:38 2018 +0000
@@ -4,6 +4,8 @@
 // Class constructor
 HorizontalEstimator::HorizontalEstimator() : flow(PA_7,PA_6,PA_5,PB_4)
 {
+    u = 0.0f;
+    v = 0.0f;
 }
 
 // Initialize class 
@@ -25,9 +27,13 @@
 {
     // 
     flow.read();
-    float d = z/(cos(phi)*cos(theta));
-    float u_m = (flow.x+q)*d;
-    float v_m = (flow.y-p)*d;
-    u = (1-rho_hor)*u+rho_hor*u_m;
-    v = (1-rho_hor)*v+rho_hor*v_m;
+    float div = (cos(phi)*cos(theta));
+    if (div>0.5f)
+    {
+        float d = z/div;
+        float u_m = (flow.x+q)*d;
+        float v_m = (flow.y-p)*d;
+        u = (1-rho_hor)*u+rho_hor*u_m;
+        v = (1-rho_hor)*v+rho_hor*v_m;
+    }
 }
\ No newline at end of file