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:
22:5f2323e30cdc
Parent:
19:83b357d6806e
diff -r 169cc2b1d2ff -r 5f2323e30cdc HorizontaEstimator/HorizontalEstimator.cpp
--- a/HorizontaEstimator/HorizontalEstimator.cpp	Wed Oct 24 14:24:56 2018 +0000
+++ b/HorizontaEstimator/HorizontalEstimator.cpp	Thu Oct 25 12:41:50 2018 +0000
@@ -35,8 +35,8 @@
     {
         flow.read();
         float d = z/div;
-        float u_m = (flow.px+q)*d;
-        float v_m = (flow.py-p)*d;
+        float u_m = (flow.px*sigma+q)*d;
+        float v_m = (flow.py*sigma-p)*d;
         float x_m = x_m_last + u_m*dt_flow;
         float y_m = y_m_last + v_m*dt_flow;
         x = (1-rho_hor)*x+rho_hor*x_m;