Fabio Bobrow
Library containing Crazyflie 2.0 controller classes: - Attitude estimator - Horizontal estimator - Vertical estimator - Attitude controller - Horizontal controller - Vertical controller - Mixer
Diff: HorizontaEstimator/HorizontalEstimator.cpp
- Revision:
- 8:d05fe9f8bfb6
- Child:
- 10:7074bc7038d6
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/HorizontaEstimator/HorizontalEstimator.cpp Thu Sep 27 18:55:22 2018 +0000
@@ -0,0 +1,23 @@
+#include "mbed.h"
+#include "HorizontalEstimator.h"
+
+// Class constructor
+HorizontalEstimator::HorizontalEstimator() : flow(PA_7,PA_6,PA_5,PB_4)
+{
+}
+
+// Initialize class
+void HorizontalEstimator::init()
+{
+ // Initialize flow sensor object
+ flow.init();
+}
+
+//
+void HorizontalEstimator::estimate(float p, float q, float z)
+{
+ //
+ flow.read();
+ u = 0.9f*u+0.1f*(flow.x-p)*z;
+ v = 0.9f*v+0.1f*(flow.y-q)*z;
+}
\ No newline at end of file