Rodrigo Gikas / Mbed 2 deprecated Drone_Controlador_Atitude

Crazyflie Attittude Controller

Dependencies:   mbed CrazyflieController CrazyflieSensors USBDevice

main.cpp@2:6ca1b4dcb9f7, 2018-11-21 (annotated)

Committer:
IgneousGuikas
Date:
Wed Nov 21 10:47:14 2018 +0000
Revision:
2:6ca1b4dcb9f7
Parent:
1:65d0334fc83a 
Teste

Who changed what in which revision?

UserRevisionLine numberNew contents of line
IgneousGuikas 0:c5109053e100 1 #include "mbed.h"
IgneousGuikas 1:65d0334fc83a 2 #include "CrazyflieController.h"
IgneousGuikas 0:c5109053e100 3
IgneousGuikas 0:c5109053e100 4 AttitudeEstimator att_est;
IgneousGuikas 0:c5109053e100 5 AttitudeController att_cont;
IgneousGuikas 2:6ca1b4dcb9f7 6 VerticalEstimator ver_est;
IgneousGuikas 2:6ca1b4dcb9f7 7 VerticalController ver_cont;
IgneousGuikas 2:6ca1b4dcb9f7 8 HorizontalEstimator hor_est;
IgneousGuikas 2:6ca1b4dcb9f7 9 HorizontalController hor_cont;
IgneousGuikas 0:c5109053e100 10 Mixer mixer;
IgneousGuikas 1:65d0334fc83a 11
IgneousGuikas 2:6ca1b4dcb9f7 12 // Tickers
IgneousGuikas 2:6ca1b4dcb9f7 13 Timer tim;
IgneousGuikas 1:65d0334fc83a 14 Ticker tic;
IgneousGuikas 2:6ca1b4dcb9f7 15 Ticker tic_range;
IgneousGuikas 2:6ca1b4dcb9f7 16 Ticker tic_flow;
IgneousGuikas 1:65d0334fc83a 17
IgneousGuikas 2:6ca1b4dcb9f7 18 // Interrupt flags
IgneousGuikas 1:65d0334fc83a 19 bool flag = false;
IgneousGuikas 2:6ca1b4dcb9f7 20 bool flag_range = false;
IgneousGuikas 2:6ca1b4dcb9f7 21 bool flag_flow = false;
IgneousGuikas 1:65d0334fc83a 22
IgneousGuikas 2:6ca1b4dcb9f7 23 // Callback function
IgneousGuikas 1:65d0334fc83a 24 void callback()
IgneousGuikas 1:65d0334fc83a 25 {
IgneousGuikas 1:65d0334fc83a 26 flag = true;
IgneousGuikas 1:65d0334fc83a 27 }
IgneousGuikas 2:6ca1b4dcb9f7 28
IgneousGuikas 2:6ca1b4dcb9f7 29 // Callback function ( range correct )
IgneousGuikas 2:6ca1b4dcb9f7 30 void callback_range()
IgneousGuikas 2:6ca1b4dcb9f7 31 {
IgneousGuikas 2:6ca1b4dcb9f7 32 flag_range = true;
IgneousGuikas 2:6ca1b4dcb9f7 33 }
IgneousGuikas 2:6ca1b4dcb9f7 34
IgneousGuikas 2:6ca1b4dcb9f7 35 // Callback function ( flow correct )
IgneousGuikas 2:6ca1b4dcb9f7 36 void callback_flow()
IgneousGuikas 2:6ca1b4dcb9f7 37 {
IgneousGuikas 2:6ca1b4dcb9f7 38 flag_flow = true;
IgneousGuikas 2:6ca1b4dcb9f7 39 }
IgneousGuikas 0:c5109053e100 40
IgneousGuikas 0:c5109053e100 41 int main()
IgneousGuikas 0:c5109053e100 42 {
IgneousGuikas 2:6ca1b4dcb9f7 43 float x_r = 0.0f;
IgneousGuikas 2:6ca1b4dcb9f7 44 float y_r = 0.0f;
IgneousGuikas 2:6ca1b4dcb9f7 45 float z_r = 1.0f;
IgneousGuikas 2:6ca1b4dcb9f7 46 float psi_r = 0.0f;
IgneousGuikas 2:6ca1b4dcb9f7 47
IgneousGuikas 0:c5109053e100 48 // Wait 5s for safety
IgneousGuikas 0:c5109053e100 49 wait(5);
IgneousGuikas 2:6ca1b4dcb9f7 50
IgneousGuikas 2:6ca1b4dcb9f7 51 // Initialize estimators objects
IgneousGuikas 0:c5109053e100 52 att_est.init();
IgneousGuikas 2:6ca1b4dcb9f7 53 ver_est.init();
IgneousGuikas 2:6ca1b4dcb9f7 54 hor_est.init();
IgneousGuikas 2:6ca1b4dcb9f7 55
IgneousGuikas 2:6ca1b4dcb9f7 56 // Initialize interrupts
IgneousGuikas 1:65d0334fc83a 57 tic.attach(&callback,dt);
IgneousGuikas 2:6ca1b4dcb9f7 58 tic_range.attach(&callback_range,dt_range);
IgneousGuikas 2:6ca1b4dcb9f7 59 tic_flow.attach(&callback_flow,dt_flow);
IgneousGuikas 1:65d0334fc83a 60
IgneousGuikas 2:6ca1b4dcb9f7 61 tim.start();
IgneousGuikas 2:6ca1b4dcb9f7 62
IgneousGuikas 2:6ca1b4dcb9f7 63 float qpi = pi/4.0f;
IgneousGuikas 2:6ca1b4dcb9f7 64 float piq = 5.0f*pi;
IgneousGuikas 2:6ca1b4dcb9f7 65
IgneousGuikas 2:6ca1b4dcb9f7 66 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)
IgneousGuikas 0:c5109053e100 67 {
IgneousGuikas 2:6ca1b4dcb9f7 68 /*if(tim.read() <= 5.0f)
IgneousGuikas 2:6ca1b4dcb9f7 69 {
IgneousGuikas 2:6ca1b4dcb9f7 70 float tr = (float)tim.read();
IgneousGuikas 2:6ca1b4dcb9f7 71 z_r = tr/5.0f;
IgneousGuikas 2:6ca1b4dcb9f7 72 }*/
IgneousGuikas 1:65d0334fc83a 73
IgneousGuikas 1:65d0334fc83a 74 if(flag)
IgneousGuikas 0:c5109053e100 75 {
IgneousGuikas 1:65d0334fc83a 76 flag = false;
IgneousGuikas 1:65d0334fc83a 77 att_est.estimate();
IgneousGuikas 2:6ca1b4dcb9f7 78 ver_est.predict();
IgneousGuikas 2:6ca1b4dcb9f7 79 hor_est.predict();
IgneousGuikas 2:6ca1b4dcb9f7 80
IgneousGuikas 2:6ca1b4dcb9f7 81 if(ver_est.z >= 0.1f)
IgneousGuikas 2:6ca1b4dcb9f7 82 {
IgneousGuikas 2:6ca1b4dcb9f7 83 hor_cont.control(x_r, y_r, hor_est.x, hor_est.y, hor_est.u, hor_est.v);
IgneousGuikas 2:6ca1b4dcb9f7 84 }
IgneousGuikas 2:6ca1b4dcb9f7 85 ver_cont.control(z_r, ver_est.z, ver_est.w);
IgneousGuikas 2:6ca1b4dcb9f7 86 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);
IgneousGuikas 2:6ca1b4dcb9f7 87 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);
IgneousGuikas 0:c5109053e100 88 }
IgneousGuikas 2:6ca1b4dcb9f7 89 // Correct vertical estimation and print values
IgneousGuikas 2:6ca1b4dcb9f7 90 if(flag_range)
IgneousGuikas 1:65d0334fc83a 91 {
IgneousGuikas 2:6ca1b4dcb9f7 92 flag_range = false;
IgneousGuikas 2:6ca1b4dcb9f7 93 ver_est.correct(att_est.phi,att_est.theta);
IgneousGuikas 2:6ca1b4dcb9f7 94 }
IgneousGuikas 2:6ca1b4dcb9f7 95 // Correct horizontal estimation
IgneousGuikas 2:6ca1b4dcb9f7 96 if ( flag_flow )
IgneousGuikas 2:6ca1b4dcb9f7 97 {
IgneousGuikas 2:6ca1b4dcb9f7 98 flag_flow = false;
IgneousGuikas 2:6ca1b4dcb9f7 99 if(ver_est.z >= 0.1f)
IgneousGuikas 2:6ca1b4dcb9f7 100 {
IgneousGuikas 2:6ca1b4dcb9f7 101 hor_est.correct(att_est.phi , att_est.theta , att_est.p, att_est.q, ver_est.z);
IgneousGuikas 2:6ca1b4dcb9f7 102 }
IgneousGuikas 1:65d0334fc83a 103 }
IgneousGuikas 0:c5109053e100 104 }
IgneousGuikas 2:6ca1b4dcb9f7 105 mixer.actuate(0.0f,0.0f,0.0f,0.0f);
IgneousGuikas 2:6ca1b4dcb9f7 106 while(1);
IgneousGuikas 0:c5109053e100 107 }