Yuri De Stefani / CrazyflieController_final

programa final

VerticalEstimator/VerticalEstimator.cpp@8:1ad52489f6f3, 2018-11-30 (annotated)

Committer:
yurindes
Date:
Fri Nov 30 19:23:29 2018 +0000
Branch:
yuri
Revision:
8:1ad52489f6f3
Parent:
6:7a447d4ae677 
final;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yurindes 5:b1f5ea192d12 1 #include "mbed.h"
yurindes 5:b1f5ea192d12 2 #include "VerticalEstimator.h"
yurindes 8:1ad52489f6f3 3 //#include <math.h>
yurindes 8:1ad52489f6f3 4 #include "Library.h"
yurindes 8:1ad52489f6f3 5
yurindes 8:1ad52489f6f3 6 // Class constructor
yurindes 8:1ad52489f6f3 7 VerticalEstimator::VerticalEstimator():range (PB_7 , PB_6 )
yurindes 8:1ad52489f6f3 8 {
yurindes 8:1ad52489f6f3 9 }
yurindes 8:1ad52489f6f3 10
yurindes 8:1ad52489f6f3 11 // Initialize class
yurindes 8:1ad52489f6f3 12 void VerticalEstimator::init ()
yurindes 5:b1f5ea192d12 13 {
yurindes 8:1ad52489f6f3 14 VL53L0X range (PB_7 , PB_6 );
yurindes 8:1ad52489f6f3 15 range.init() ;
yurindes 8:1ad52489f6f3 16
yurindes 8:1ad52489f6f3 17 z = 0.0f;//leitura da distancia vertical para função predict
yurindes 8:1ad52489f6f3 18 zm = 0.0f;//definido uma leitura para distancia vertical
yurindes 8:1ad52489f6f3 19 zml = 0.0f;//definindo uma segunda leitura para calcular a velocidade vertical
yurindes 8:1ad52489f6f3 20 z_est = 0.0f;
yurindes 8:1ad52489f6f3 21
yurindes 8:1ad52489f6f3 22 w = 0.0f;//definido a velocidade vertical para função predict
yurindes 8:1ad52489f6f3 23 wm = 0.0f;
yurindes 8:1ad52489f6f3 24 w_est = 0.0f;
yurindes 8:1ad52489f6f3 25
yurindes 5:b1f5ea192d12 26 }
yurindes 5:b1f5ea192d12 27 // Predict vertical position and velocity from model
yurindes 8:1ad52489f6f3 28 void VerticalEstimator::predict ()
yurindes 5:b1f5ea192d12 29 {
yurindes 8:1ad52489f6f3 30 range.read();
yurindes 8:1ad52489f6f3 31 z = range.d;
yurindes 8:1ad52489f6f3 32 z = z + w*dt_ver_pre;//velocidade prevista, como a leitura é feita com 500Hz 0.002f sao os 2ms equivalentes
yurindes 5:b1f5ea192d12 33 }
yurindes 8:1ad52489f6f3 34
yurindes 5:b1f5ea192d12 35 // Correct vertical position and velocity with measurement
yurindes 8:1ad52489f6f3 36 void VerticalEstimator::correct ( float phi , float theta )//phi e theta são os angulos de euler estimados
yurindes 5:b1f5ea192d12 37 {
yurindes 8:1ad52489f6f3 38
yurindes 8:1ad52489f6f3 39 zm = range.d;
yurindes 8:1ad52489f6f3 40 zm = zm*cos(phi)*cos(theta);//zm é a distancia medida e phi e theta sao os angulos de Euler para correcao da medida
yurindes 8:1ad52489f6f3 41 wait(dt_ver_cor);
yurindes 8:1ad52489f6f3 42 zml = range.d;
yurindes 8:1ad52489f6f3 43 zm = zml*cos(phi)*cos(theta);//d é a distancia medida e phi e theta sao os angulos de Euler para correcao da medida
yurindes 8:1ad52489f6f3 44 wm = (zm-zml)/dt_ver_cor;
yurindes 8:1ad52489f6f3 45
yurindes 8:1ad52489f6f3 46 z_est = (1-p_verti)*z + p_verti*zm;
yurindes 8:1ad52489f6f3 47 w_est = (1-p_verti)*w + p_verti*wm;
yurindes 8:1ad52489f6f3 48
yurindes 8:1ad52489f6f3 49 }