Rodrigo Gikas / CrazyflieController

Crazyflie 2.0 Controller

Dependents:   Drone_Controlador_Atitude

VerticalEstimator/VerticalEstimator.cpp@2:9e07bed8e8ed, 2018-11-21 (annotated)

Committer:
IgneousGuikas
Date:
Wed Nov 21 10:46:29 2018 +0000
Revision:
2:9e07bed8e8ed
Teste;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
IgneousGuikas 2:9e07bed8e8ed 1 #include "mbed.h"
IgneousGuikas 2:9e07bed8e8ed 2 #include "VerticalEstimator.h"
IgneousGuikas 2:9e07bed8e8ed 3
IgneousGuikas 2:9e07bed8e8ed 4 // Class constructor
IgneousGuikas 2:9e07bed8e8ed 5 VerticalEstimator::VerticalEstimator() : range(PB_7,PB_6)
IgneousGuikas 2:9e07bed8e8ed 6 {
IgneousGuikas 2:9e07bed8e8ed 7 z = 0.0f;
IgneousGuikas 2:9e07bed8e8ed 8 w = 0.0f;
IgneousGuikas 2:9e07bed8e8ed 9 z_m_last = 0.0f;
IgneousGuikas 2:9e07bed8e8ed 10 }
IgneousGuikas 2:9e07bed8e8ed 11 // Initialize class
IgneousGuikas 2:9e07bed8e8ed 12 void VerticalEstimator::init()
IgneousGuikas 2:9e07bed8e8ed 13 {
IgneousGuikas 2:9e07bed8e8ed 14 range.init();
IgneousGuikas 2:9e07bed8e8ed 15 }
IgneousGuikas 2:9e07bed8e8ed 16 // Predict vertical position and velocity from model
IgneousGuikas 2:9e07bed8e8ed 17 void VerticalEstimator::predict()
IgneousGuikas 2:9e07bed8e8ed 18 {
IgneousGuikas 2:9e07bed8e8ed 19 z = z + w*dt;
IgneousGuikas 2:9e07bed8e8ed 20 }
IgneousGuikas 2:9e07bed8e8ed 21 // Correct vertical position and velocity with measurement
IgneousGuikas 2:9e07bed8e8ed 22 void VerticalEstimator::correct(float phi,float theta)
IgneousGuikas 2:9e07bed8e8ed 23 {
IgneousGuikas 2:9e07bed8e8ed 24 range.read();
IgneousGuikas 2:9e07bed8e8ed 25 float z_m = range.z*cos(phi)*cos(theta);
IgneousGuikas 2:9e07bed8e8ed 26 float w_m = (z_m - z_m_last)/dt_range;
IgneousGuikas 2:9e07bed8e8ed 27
IgneousGuikas 2:9e07bed8e8ed 28 z = (1.0f - rho_vert_corr)*z + rho_vert_corr*z_m;
IgneousGuikas 2:9e07bed8e8ed 29 w = (1.0f - rho_vert_corr)*w + rho_vert_corr*w_m;
IgneousGuikas 2:9e07bed8e8ed 30
IgneousGuikas 2:9e07bed8e8ed 31 z_m_last = z_m;
IgneousGuikas 2:9e07bed8e8ed 32 }