Diff: HorizontaEstimator/HorizontalEstimator.cpp

Revision:

19:83b357d6806e

Parent:

18:60516a4cba27

Child:

22:5f2323e30cdc

--- a/HorizontaEstimator/HorizontalEstimator.cpp	Wed Oct 10 10:48:52 2018 +0000
+++ b/HorizontaEstimator/HorizontalEstimator.cpp	Thu Oct 18 12:53:22 2018 +0000
@@ -27,7 +27,7 @@
     v = v;
 }
 
-// Correct horizontal velocity with measurement
+// Correct horizontal velocity with measurements
 void HorizontalEstimator::correct(float phi, float theta, float p, float q, float z)
 {
     float div = (cos(phi)*cos(theta));
@@ -35,8 +35,8 @@
     {
         flow.read();
         float d = z/div;
-        float u_m = (flow.x+q)*d;
-        float v_m = (flow.y-p)*d;
+        float u_m = (flow.px+q)*d;
+        float v_m = (flow.py-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;