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@12:9fed6f656f88, 2018-09-28 (annotated)

Committer:
fbob
Date:
Fri Sep 28 21:45:50 2018 +0000
Revision:
12:9fed6f656f88
Parent:
10:7074bc7038d6
Child:
15:155ca63b7884
Parameters improvements

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 8:d05fe9f8bfb6 7 }
fbob 8:d05fe9f8bfb6 8
fbob 8:d05fe9f8bfb6 9 // Initialize class
fbob 8:d05fe9f8bfb6 10 void HorizontalEstimator::init()
fbob 8:d05fe9f8bfb6 11 {
fbob 8:d05fe9f8bfb6 12 // Initialize flow sensor object
fbob 8:d05fe9f8bfb6 13 flow.init();
fbob 8:d05fe9f8bfb6 14 }
fbob 8:d05fe9f8bfb6 15
fbob 12:9fed6f656f88 16 // Predict vertical position and velocity
fbob 12:9fed6f656f88 17 void HorizontalEstimator::predict()
fbob 12:9fed6f656f88 18 {
fbob 12:9fed6f656f88 19 u = u;
fbob 12:9fed6f656f88 20 v = v;
fbob 12:9fed6f656f88 21 }
fbob 12:9fed6f656f88 22
fbob 8:d05fe9f8bfb6 23 //
fbob 12:9fed6f656f88 24 void HorizontalEstimator::update(float phi, float theta, float p, float q, float z)
fbob 8:d05fe9f8bfb6 25 {
fbob 8:d05fe9f8bfb6 26 //
fbob 8:d05fe9f8bfb6 27 flow.read();
fbob 12:9fed6f656f88 28 float d = z/(cos(phi)*cos(theta));
fbob 12:9fed6f656f88 29 float u_m = (flow.x+q)*d;
fbob 12:9fed6f656f88 30 float v_m = (flow.y-p)*d;
fbob 12:9fed6f656f88 31 u = (1-rho_hor)*u+rho_hor*u_m;
fbob 12:9fed6f656f88 32 v = (1-rho_hor)*v+rho_hor*v_m;
fbob 8:d05fe9f8bfb6 33 }