Diff: main.cpp

Revision:

2:6ca1b4dcb9f7

Parent:

1:65d0334fc83a

--- a/main.cpp	Tue Oct 09 18:02:04 2018 +0000
+++ b/main.cpp	Wed Nov 21 10:47:14 2018 +0000
@@ -1,58 +1,107 @@
 #include "mbed.h"
 #include "CrazyflieController.h"
 
-//Declare attitude estimator object
 AttitudeEstimator att_est;
-//Declare attitude controller object
 AttitudeController att_cont;
-// Declare mixer
+VerticalEstimator ver_est;
+VerticalController ver_cont;
+HorizontalEstimator hor_est;
+HorizontalController hor_cont;
 Mixer mixer;
-// Declare timer object
-Timer timer;
 
+// Tickers
+Timer tim;
 Ticker tic;
+Ticker tic_range;
+Ticker tic_flow;
 
+// Interrupt flags
 bool flag = false;
+bool flag_range = false;
+bool flag_flow = false;
 
+// Callback function
 void callback()
 {
     flag = true;
 }
-// Last interrupt time
-float current_time = 0.0f;
+
+// Callback function ( range correct )
+void callback_range()
+{
+    flag_range = true;
+}
+
+// Callback function ( flow correct )
+void callback_flow()
+{
+    flag_flow = true;
+}
 
 int main()
 {
+    float x_r = 0.0f;
+    float y_r = 0.0f;
+    float z_r = 1.0f;
+    float psi_r = 0.0f;
+    
     // Wait 5s for safety
     wait(5);
-    //Initialize attitude estimator
+    
+    // Initialize estimators objects
     att_est.init();
-    //Start timer
+    ver_est.init();
+    hor_est.init();
+    
+    // Initialize interrupts
     tic.attach(&callback,dt);
+    tic_range.attach(&callback_range,dt_range);
+    tic_flow.attach(&callback_flow,dt_flow);
     
-    timer.start();
-    //Run controller for only 5s
-    while( current_time <=10.0f)
+    tim.start();
+    
+    float qpi = pi/4.0f;
+    float piq = 5.0f*pi;
+    
+    while(abs(att_est.phi) <= qpi && abs(att_est.theta) <= qpi && abs(att_est.p) <= piq && abs(att_est.q) <= piq && abs(att_est.r) <= piq)
     {
-        // Wait 5ms (200 Hz) to perfom next estimate and control update
-        current_time = timer.read();
+        /*if(tim.read() <= 5.0f)
+        {
+            float tr = (float)tim.read();
+            z_r = tr/5.0f;
+        }*/
         
         if(flag)
         {
             flag = false;
-            // Estimate attitude
             att_est.estimate();
-            // Calculate torques (N.m) given estimated attitude ( rad and rad /s)
-            att_cont.control(0.0f, 0.0f, 0.0f, att_est.phi, att_est.theta, att_est.psi, att_est.p, att_est.q, att_est.r);
-            // Actuate motors with given force and calculated torques
-            mixer.actuate(0.25f, att_cont.tau_phi, att_cont.tau_theta, 0.0f);
+            ver_est.predict();
+            hor_est.predict();
+            
+            if(ver_est.z >= 0.1f)
+            {
+                hor_cont.control(x_r, y_r, hor_est.x, hor_est.y, hor_est.u, hor_est.v);
+            }
+            ver_cont.control(z_r, ver_est.z, ver_est.w);
+            att_cont.control(hor_cont.phi_r, hor_cont.theta_r, psi_r, att_est.phi, att_est.theta, att_est.psi, att_est.p, att_est.q, att_est.r);
+            mixer.actuate(ver_cont.f_t/(cos(att_est.phi)*cos(att_est.theta)), att_cont.tau_phi, att_cont.tau_theta, att_cont.tau_psi);
         }
-        if(att_est.phi >= pi/4.0f || att_est.theta >= pi/4.0f || att_est.p >= 4.0f*pi || att_est.q >= 4.0f*pi)
+        // Correct vertical estimation and print values
+        if(flag_range)
         {
-            break;
+            flag_range = false;
+            ver_est.correct(att_est.phi,att_est.theta);
+        }
+        // Correct horizontal estimation
+        if ( flag_flow )
+        {
+            flag_flow = false;
+            if(ver_est.z >= 0.1f)
+            {
+                hor_est.correct(att_est.phi , att_est.theta , att_est.p, att_est.q, ver_est.z);
+            }
         }
     }
-    //Turn off all motors
-    mixer.actuate(0.0f, 0.0f, 0.0f, 0.0f);
-    while (1);
+    mixer.actuate(0.0f,0.0f,0.0f,0.0f);
+    while(1);
 }
\ No newline at end of file