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:
13:1a871ebd35bb
Parent:
11:fad579538b4c
Child:
15:155ca63b7884
--- a/VerticalEstimator/VerticalEstimator.cpp	Fri Sep 28 21:45:50 2018 +0000
+++ b/VerticalEstimator/VerticalEstimator.cpp	Wed Oct 03 17:50:16 2018 +0000
@@ -15,7 +15,7 @@
 // Predict vertical position and velocity
 void VerticalEstimator::predict()
 {
-    z = z+w*dt_att;
+    z = z+w*dt;
     w = w;
 }
 
@@ -24,7 +24,7 @@
 {
     range.read();
     float z_m = range.z*cos(phi)*cos(theta);
-    float w_m = (z_m-z_m_last)/dt_pos;
+    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;