Fabio Bobrow / CrazyflieController

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

HorizontaEstimator/HorizontalEstimator.cpp@24:7b9e3beb61d5, 2018-12-06 (annotated)

Committer:
fbob
Date:
Thu Dec 06 16:44:40 2018 +0000
Revision:
24:7b9e3beb61d5
Parent:
22:5f2323e30cdc 
Adjust

Who changed what in which revision?

UserRevisionLine numberNew contents of line
fbob 8:d05fe9f8bfb6 1 #include "mbed.h"
fbob 8:d05fe9f8bfb6 2 #include "HorizontalEstimator.h"
fbob 8:d05fe9f8bfb6 3
fbob 8:d05fe9f8bfb6 4 // Class constructor
fbob 8:d05fe9f8bfb6 5 HorizontalEstimator::HorizontalEstimator() : flow(PA_7,PA_6,PA_5,PB_4)
fbob 8:d05fe9f8bfb6 6 {
fbob 18:60516a4cba27 7 x = 0.0f;
fbob 18:60516a4cba27 8 y = 0.0f;
fbob 15:155ca63b7884 9 u = 0.0f;
fbob 15:155ca63b7884 10 v = 0.0f;
fbob 18:60516a4cba27 11 x_m_last = 0.0f;
fbob 18:60516a4cba27 12 y_m_last = 0.0f;
fbob 8:d05fe9f8bfb6 13 }
fbob 8:d05fe9f8bfb6 14
fbob 8:d05fe9f8bfb6 15 // Initialize class
fbob 8:d05fe9f8bfb6 16 void HorizontalEstimator::init()
fbob 8:d05fe9f8bfb6 17 {
fbob 8:d05fe9f8bfb6 18 flow.init();
fbob 8:d05fe9f8bfb6 19 }
fbob 8:d05fe9f8bfb6 20
fbob 17:f682b4a5686d 21 // Predict horizontal velocity from model
fbob 12:9fed6f656f88 22 void HorizontalEstimator::predict()
fbob 12:9fed6f656f88 23 {
fbob 18:60516a4cba27 24 x = x+u*dt;
fbob 18:60516a4cba27 25 y = y+v*dt;
fbob 12:9fed6f656f88 26 u = u;
fbob 12:9fed6f656f88 27 v = v;
fbob 12:9fed6f656f88 28 }
fbob 12:9fed6f656f88 29
fbob 19:83b357d6806e 30 // Correct horizontal velocity with measurements
fbob 18:60516a4cba27 31 void HorizontalEstimator::correct(float phi, float theta, float p, float q, float z)
fbob 8:d05fe9f8bfb6 32 {
fbob 15:155ca63b7884 33 float div = (cos(phi)*cos(theta));
fbob 15:155ca63b7884 34 if (div>0.5f)
fbob 15:155ca63b7884 35 {
fbob 18:60516a4cba27 36 flow.read();
fbob 15:155ca63b7884 37 float d = z/div;
fbob 22:5f2323e30cdc 38 float u_m = (flow.px*sigma+q)*d;
fbob 22:5f2323e30cdc 39 float v_m = (flow.py*sigma-p)*d;
fbob 18:60516a4cba27 40 float x_m = x_m_last + u_m*dt_flow;
fbob 18:60516a4cba27 41 float y_m = y_m_last + v_m*dt_flow;
fbob 18:60516a4cba27 42 x = (1-rho_hor)*x+rho_hor*x_m;
fbob 18:60516a4cba27 43 y = (1-rho_hor)*y+rho_hor*y_m;
fbob 15:155ca63b7884 44 u = (1-rho_hor)*u+rho_hor*u_m;
fbob 15:155ca63b7884 45 v = (1-rho_hor)*v+rho_hor*v_m;
fbob 18:60516a4cba27 46 x_m_last = x_m;
fbob 18:60516a4cba27 47 y_m_last = y_m;
fbob 15:155ca63b7884 48 }
fbob 8:d05fe9f8bfb6 49 }