Fabio Bobrow / CrazyflieController

Library containing Crazyflie 2.0 controller classes: - Attitude estimator - Horizontal estimator - Vertical estimator - Attitude controller - Horizontal controller - Vertical controller - Mixer

Diff: AttitudeEstimator/AttitudeEstimator.cpp

Revision:
17:f682b4a5686d
Parent:
15:155ca63b7884
Child:
21:169cc2b1d2ff
--- a/AttitudeEstimator/AttitudeEstimator.cpp	Fri Oct 05 13:19:03 2018 +0000
+++ b/AttitudeEstimator/AttitudeEstimator.cpp	Fri Oct 05 19:32:35 2018 +0000
@@ -2,7 +2,7 @@
 #include "AttitudeEstimator.h"
 
 // Class constructor
-AttitudeEstimator::AttitudeEstimator() : imu(PC_9,PA_8)
+AttitudeEstimator::AttitudeEstimator() : imu(PC_9,PA_8), led_3_blue(PD_2)
 {
     phi = 0.0f;
     theta = 0.0f;
@@ -18,6 +18,7 @@
     p_bias = 0.0f;
     q_bias = 0.0f;
     r_bias = 0.0f;  
+    led_3_blue = 0;
 }
 
 // Initialize class 
@@ -39,6 +40,10 @@
         p_bias += imu.gx/600.0f;
         q_bias += imu.gy/600.0f;
         r_bias += imu.gz/600.0f;
+        if ((i%25)==0)
+        {
+           led_3_blue = !led_3_blue;
+        }
         wait(dt);
     }
 }