Fabio Bobrow / CrazyflieController

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

VerticalEstimator/VerticalEstimator.cpp@9:15058b4fa090, 2018-09-28 (annotated)

Committer:
fbob
Date:
Fri Sep 28 15:08:41 2018 +0000
Revision:
9:15058b4fa090
Parent:
7:220ce3839be8
Child:
11:fad579538b4c
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 // Estimate vertical position and velocity
fbob 7:220ce3839be8 16 void VerticalEstimator::estimate(float phi, float theta)
fbob 7:220ce3839be8 17 {
fbob 7:220ce3839be8 18 range.read();
fbob 9:15058b4fa090 19 measure(phi,theta);
fbob 9:15058b4fa090 20 predict();
fbob 9:15058b4fa090 21 z = rho_ver*z_m+(1-rho_ver)*z_p;
fbob 9:15058b4fa090 22 w = rho_ver*w_m+(1-rho_ver)*w_p;
fbob 9:15058b4fa090 23 }
fbob 9:15058b4fa090 24
fbob 9:15058b4fa090 25 // Measure vertical position and velocity
fbob 9:15058b4fa090 26 void VerticalEstimator::measure(float phi, float theta)
fbob 9:15058b4fa090 27 {
fbob 9:15058b4fa090 28 float z_m_new = range.z*cos(phi)*cos(theta);
fbob 9:15058b4fa090 29 w_m = (z_m_new-z_m)/dt_pos;
fbob 9:15058b4fa090 30 z_m = z_m_new;
fbob 9:15058b4fa090 31 }
fbob 9:15058b4fa090 32
fbob 9:15058b4fa090 33 // Predict vertical position and velocity
fbob 9:15058b4fa090 34 void VerticalEstimator::predict()
fbob 9:15058b4fa090 35 {
fbob 9:15058b4fa090 36 z_p = z+w*dt_pos;
fbob 9:15058b4fa090 37 w_p = w;
fbob 7:220ce3839be8 38 }