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@17:f682b4a5686d, 2018-10-05 (annotated)

Committer:
fbob
Date:
Fri Oct 05 19:32:35 2018 +0000
Revision:
17:f682b4a5686d
Parent:
15:155ca63b7884
Child:
18:60516a4cba27
Comments;

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 15:155ca63b7884 7 u = 0.0f;
fbob 15:155ca63b7884 8 v = 0.0f;
fbob 8:d05fe9f8bfb6 9 }
fbob 8:d05fe9f8bfb6 10
fbob 8:d05fe9f8bfb6 11 // Initialize class
fbob 8:d05fe9f8bfb6 12 void HorizontalEstimator::init()
fbob 8:d05fe9f8bfb6 13 {
fbob 8:d05fe9f8bfb6 14 flow.init();
fbob 8:d05fe9f8bfb6 15 }
fbob 8:d05fe9f8bfb6 16
fbob 17:f682b4a5686d 17 // Predict horizontal velocity from model
fbob 12:9fed6f656f88 18 void HorizontalEstimator::predict()
fbob 12:9fed6f656f88 19 {
fbob 12:9fed6f656f88 20 u = u;
fbob 12:9fed6f656f88 21 v = v;
fbob 12:9fed6f656f88 22 }
fbob 12:9fed6f656f88 23
fbob 17:f682b4a5686d 24 // Update horizontal velocity with measurement
fbob 12:9fed6f656f88 25 void HorizontalEstimator::update(float phi, float theta, float p, float q, float z)
fbob 8:d05fe9f8bfb6 26 {
fbob 8:d05fe9f8bfb6 27 flow.read();
fbob 15:155ca63b7884 28 float div = (cos(phi)*cos(theta));
fbob 15:155ca63b7884 29 if (div>0.5f)
fbob 15:155ca63b7884 30 {
fbob 15:155ca63b7884 31 float d = z/div;
fbob 15:155ca63b7884 32 float u_m = (flow.x+q)*d;
fbob 15:155ca63b7884 33 float v_m = (flow.y-p)*d;
fbob 15:155ca63b7884 34 u = (1-rho_hor)*u+rho_hor*u_m;
fbob 15:155ca63b7884 35 v = (1-rho_hor)*v+rho_hor*v_m;
fbob 15:155ca63b7884 36 }
fbob 8:d05fe9f8bfb6 37 }