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@11:fad579538b4c, 2018-09-28 (annotated)

Committer:
fbob
Date:
Fri Sep 28 18:52:33 2018 +0000
Revision:
11:fad579538b4c
Parent:
9:15058b4fa090
Child:
13:1a871ebd35bb
Vertical estimator improvements

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 7:220ce3839be8 7 }
fbob 7:220ce3839be8 8
fbob 7:220ce3839be8 9 // Initialize class
fbob 7:220ce3839be8 10 void VerticalEstimator::init()
fbob 7:220ce3839be8 11 {
fbob 7:220ce3839be8 12 range.init();
fbob 7:220ce3839be8 13 }
fbob 7:220ce3839be8 14
fbob 9:15058b4fa090 15 // Predict vertical position and velocity
fbob 9:15058b4fa090 16 void VerticalEstimator::predict()
fbob 9:15058b4fa090 17 {
fbob 11:fad579538b4c 18 z = z+w*dt_att;
fbob 11:fad579538b4c 19 w = w;
fbob 11:fad579538b4c 20 }
fbob 11:fad579538b4c 21
fbob 11:fad579538b4c 22 // Update vertical position and velocity by merging range sensor readings
fbob 11:fad579538b4c 23 void VerticalEstimator::update(float phi, float theta)
fbob 11:fad579538b4c 24 {
fbob 11:fad579538b4c 25 range.read();
fbob 11:fad579538b4c 26 float z_m = range.z*cos(phi)*cos(theta);
fbob 11:fad579538b4c 27 float w_m = (z_m-z_m_last)/dt_pos;
fbob 11:fad579538b4c 28 z = (1-rho_ver)*z+rho_ver*z_m;
fbob 11:fad579538b4c 29 w = (1-rho_ver)*w+rho_ver*w_m;
fbob 11:fad579538b4c 30 z_m_last = z_m;
fbob 11:fad579538b4c 31 }
fbob 11:fad579538b4c 32
fbob 11:fad579538b4c 33