Crazyflie Attittude Controller
Dependencies: mbed CrazyflieController CrazyflieSensors USBDevice
Diff: main.cpp
- Revision:
- 2:6ca1b4dcb9f7
- Parent:
- 1:65d0334fc83a
diff -r 65d0334fc83a -r 6ca1b4dcb9f7 main.cpp --- 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