Fabio Bobrow / CrazyflieController

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

Diff: VerticalEstimator/VerticalEstimator.cpp

Revision:
19:83b357d6806e
Parent:
18:60516a4cba27
--- a/VerticalEstimator/VerticalEstimator.cpp	Wed Oct 10 10:48:52 2018 +0000
+++ b/VerticalEstimator/VerticalEstimator.cpp	Thu Oct 18 12:53:22 2018 +0000
@@ -26,11 +26,14 @@
 void VerticalEstimator::correct(float phi, float theta)
 {
     range.read();
-    float z_m = range.z*cos(phi)*cos(theta);
-    float w_m = (z_m-z_m_last)/dt_range;
-    z = (1-rho_ver)*z+rho_ver*z_m;
-    w = (1-rho_ver)*w+rho_ver*w_m;
-    z_m_last = z_m;
+    if (range.d < 2.0f)
+    {
+        float z_m = range.d*cos(phi)*cos(theta);
+        float w_m = (z_m-z_m_last)/dt_range;
+        z = (1-rho_ver)*z+rho_ver*z_m;
+        w = (1-rho_ver)*w+rho_ver*w_m;
+        z_m_last = z_m;
+    }
 }