Fabio Bobrow / CrazyflieController

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Diff: HorizontaEstimator/HorizontalEstimator.cpp

Revision:
12:9fed6f656f88
Parent:
10:7074bc7038d6
Child:
15:155ca63b7884
--- a/HorizontaEstimator/HorizontalEstimator.cpp	Fri Sep 28 18:52:33 2018 +0000
+++ b/HorizontaEstimator/HorizontalEstimator.cpp	Fri Sep 28 21:45:50 2018 +0000
@@ -13,11 +13,21 @@
     flow.init();
 }
 
+// Predict vertical position and velocity
+void HorizontalEstimator::predict()
+{
+    u = u;
+    v = v;
+}
+
 //
-void HorizontalEstimator::estimate(float p, float q, float z)
+void HorizontalEstimator::update(float phi, float theta, float p, float q, float z)
 {
     // 
     flow.read();
-    u = 0.5f*u+0.5f*(flow.x-p)*z;
-    v = 0.5f*v+0.5f*(flow.y-q)*z;
+    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;
 }
\ No newline at end of file