Diff: VerticalEstimator/VerticalEstimator.cpp

Branch:

yuri

Revision:

8:1ad52489f6f3

Parent:

6:7a447d4ae677

--- a/VerticalEstimator/VerticalEstimator.cpp	Wed Oct 17 12:18:29 2018 +0000
+++ b/VerticalEstimator/VerticalEstimator.cpp	Fri Nov 30 19:23:29 2018 +0000
@@ -1,31 +1,49 @@
 #include "mbed.h"
 #include "VerticalEstimator.h"
-// Class constructor hhh
-VerticalEstimator :: VerticalEstimator() : range (PB_7 , PB_6 )
+//#include <math.h>
+#include "Library.h"
+
+// Class constructor
+VerticalEstimator::VerticalEstimator():range (PB_7 , PB_6 )
+{
+}
+
+// Initialize class
+void VerticalEstimator::init ()
 {
-    z=0;
-    w=0;
-    z_m_last=0;
-}
-// Initialize class
-void VerticalEstimator :: init()
-{
-    range.init();
+    VL53L0X range (PB_7 , PB_6 );
+    range.init() ;
+    
+    z = 0.0f;//leitura da distancia vertical para função predict
+    zm = 0.0f;//definido uma leitura para distancia vertical
+    zml = 0.0f;//definindo uma segunda leitura para calcular a velocidade vertical
+    z_est = 0.0f;
+    
+    w = 0.0f;//definido a velocidade vertical para função predict
+    wm = 0.0f;
+    w_est = 0.0f;  
+    
 }
 // Predict vertical position and velocity from model
-void VerticalEstimator :: predict()
+void VerticalEstimator::predict ()
 {
-    z=z+0.05f*w;
-    w=w;
+    range.read();
+    z = range.d;
+    z = z + w*dt_ver_pre;//velocidade prevista, como a leitura é feita com 500Hz 0.002f sao os 2ms equivalentes
 }
+
 // Correct vertical position and velocity with measurement
-void VerticalEstimator :: correct ( float phi , float theta )
+void VerticalEstimator::correct ( float phi , float theta )//phi e theta são os angulos de euler estimados
 {
-    p=0.5f;
-    range.read;
-    float z_m = range.z;
-    float w_m = (z-z_m)/0.05f;
-    z=(1-p)*z+p*z_m;
-    w=(1-p)*w+p*w_m;
-    z_m_last = z_m;
-}
+
+    zm = range.d;
+    zm = zm*cos(phi)*cos(theta);//zm é a distancia medida e phi e theta sao os angulos de Euler para correcao da medida
+    wait(dt_ver_cor);
+    zml = range.d;
+    zm = zml*cos(phi)*cos(theta);//d é a distancia medida e phi e theta sao os angulos de Euler para correcao da medida
+    wm = (zm-zml)/dt_ver_cor;
+
+    z_est = (1-p_verti)*z + p_verti*zm;
+    w_est = (1-p_verti)*w + p_verti*wm;
+
+}
\ No newline at end of file