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@7:220ce3839be8, 2018-09-27 (annotated)

Committer:
fbob
Date:
Thu Sep 27 17:50:16 2018 +0000
Revision:
7:220ce3839be8
Child:
9:15058b4fa090
Vertical estimator

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 // Initialize range sensor object
fbob 7:220ce3839be8 13 range.init();
fbob 7:220ce3839be8 14 }
fbob 7:220ce3839be8 15
fbob 7:220ce3839be8 16 //
fbob 7:220ce3839be8 17 void VerticalEstimator::estimate(float phi, float theta)
fbob 7:220ce3839be8 18 {
fbob 7:220ce3839be8 19 //
fbob 7:220ce3839be8 20 range.read();
fbob 7:220ce3839be8 21 float z_new = range.z*cos(phi)*cos(theta);
fbob 7:220ce3839be8 22 w = (z_new-z)/dt_pos;
fbob 7:220ce3839be8 23 z = z_new;
fbob 7:220ce3839be8 24 }