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@10:7074bc7038d6, 2018-09-28 (annotated)

Committer:
fbob
Date:
Fri Sep 28 15:40:17 2018 +0000
Revision:
10:7074bc7038d6
Parent:
8:d05fe9f8bfb6
Child:
12:9fed6f656f88
Horizontal estimator 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 8:d05fe9f8bfb6 16 //
fbob 8:d05fe9f8bfb6 17 void HorizontalEstimator::estimate(float p, float q, float z)
fbob 8:d05fe9f8bfb6 18 {
fbob 8:d05fe9f8bfb6 19 //
fbob 8:d05fe9f8bfb6 20 flow.read();
fbob 10:7074bc7038d6 21 u = 0.5f*u+0.5f*(flow.x-p)*z;
fbob 10:7074bc7038d6 22 v = 0.5f*v+0.5f*(flow.y-q)*z;
fbob 8:d05fe9f8bfb6 23 }