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@18:60516a4cba27, 2018-10-10 (annotated)

Committer:
fbob
Date:
Wed Oct 10 10:48:52 2018 +0000
Revision:
18:60516a4cba27
Parent:
17:f682b4a5686d
Child:
19:83b357d6806e
Update -> Correct

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 11:fad579538b4c 29 float z_m = range.z*cos(phi)*cos(theta);
fbob 13:1a871ebd35bb 30 float w_m = (z_m-z_m_last)/dt_range;
fbob 11:fad579538b4c 31 z = (1-rho_ver)*z+rho_ver*z_m;
fbob 11:fad579538b4c 32 w = (1-rho_ver)*w+rho_ver*w_m;
fbob 11:fad579538b4c 33 z_m_last = z_m;
fbob 11:fad579538b4c 34 }
fbob 11:fad579538b4c 35
fbob 11:fad579538b4c 36