Fabio Bobrow / CrazyflieController

Library containing Crazyflie 2.0 controller classes: - Attitude estimator - Horizontal estimator - Vertical estimator - Attitude controller - Horizontal controller - Vertical controller - Mixer

VerticalEstimator/VerticalEstimator.cpp@19:83b357d6806e, 2018-10-18 (annotated)

Committer:
fbob
Date:
Thu Oct 18 12:53:22 2018 +0000
Revision:
19:83b357d6806e
Parent:
18:60516a4cba27 
Included zeta and omega_n

Who changed what in which revision?

UserRevisionLine numberNew contents of line
fbob 7:220ce3839be8 1 #include "mbed.h"
fbob 7:220ce3839be8 2 #include "VerticalEstimator.h"
fbob 7:220ce3839be8 3
fbob 7:220ce3839be8 4 // Class constructor
fbob 7:220ce3839be8 5 VerticalEstimator::VerticalEstimator() : range(PB_7,PB_6)
fbob 7:220ce3839be8 6 {
fbob 15:155ca63b7884 7 z = 0.0f;
fbob 15:155ca63b7884 8 w = 0.0f;
fbob 15:155ca63b7884 9 z_m_last = 0.0f;
fbob 7:220ce3839be8 10 }
fbob 7:220ce3839be8 11
fbob 7:220ce3839be8 12 // Initialize class
fbob 7:220ce3839be8 13 void VerticalEstimator::init()
fbob 7:220ce3839be8 14 {
fbob 7:220ce3839be8 15 range.init();
fbob 7:220ce3839be8 16 }
fbob 7:220ce3839be8 17
fbob 17:f682b4a5686d 18 // Predict vertical position and velocity from model
fbob 9:15058b4fa090 19 void VerticalEstimator::predict()
fbob 9:15058b4fa090 20 {
fbob 13:1a871ebd35bb 21 z = z+w*dt;
fbob 11:fad579538b4c 22 w = w;
fbob 11:fad579538b4c 23 }
fbob 11:fad579538b4c 24
fbob 18:60516a4cba27 25 // Correct vertical position and velocity with measurement
fbob 18:60516a4cba27 26 void VerticalEstimator::correct(float phi, float theta)
fbob 11:fad579538b4c 27 {
fbob 11:fad579538b4c 28 range.read();
fbob 19:83b357d6806e 29 if (range.d < 2.0f)
fbob 19:83b357d6806e 30 {
fbob 19:83b357d6806e 31 float z_m = range.d*cos(phi)*cos(theta);
fbob 19:83b357d6806e 32 float w_m = (z_m-z_m_last)/dt_range;
fbob 19:83b357d6806e 33 z = (1-rho_ver)*z+rho_ver*z_m;
fbob 19:83b357d6806e 34 w = (1-rho_ver)*w+rho_ver*w_m;
fbob 19:83b357d6806e 35 z_m_last = z_m;
fbob 19:83b357d6806e 36 }
fbob 11:fad579538b4c 37 }
fbob 11:fad579538b4c 38
fbob 11:fad579538b4c 39