Fabio Bobrow
Library containing Crazyflie 2.0 controller classes: - Attitude estimator - Horizontal estimator - Vertical estimator - Attitude controller - Horizontal controller - Vertical controller - Mixer
Diff: VerticalEstimator/VerticalEstimator.cpp
- Revision:
- 7:220ce3839be8
- Child:
- 9:15058b4fa090
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/VerticalEstimator/VerticalEstimator.cpp Thu Sep 27 17:50:16 2018 +0000
@@ -0,0 +1,24 @@
+#include "mbed.h"
+#include "VerticalEstimator.h"
+
+// Class constructor
+VerticalEstimator::VerticalEstimator() : range(PB_7,PB_6)
+{
+}
+
+// Initialize class
+void VerticalEstimator::init()
+{
+ // Initialize range sensor object
+ range.init();
+}
+
+//
+void VerticalEstimator::estimate(float phi, float theta)
+{
+ //
+ range.read();
+ float z_new = range.z*cos(phi)*cos(theta);
+ w = (z_new-z)/dt_pos;
+ z = z_new;
+}
\ No newline at end of file