Crazyflie Attittude Controller
Dependencies: mbed CrazyflieController CrazyflieSensors USBDevice
main.cpp@0:c5109053e100, 2018-10-08 (annotated)
- Committer:
- IgneousGuikas
- Date:
- Mon Oct 08 11:08:56 2018 +0000
- Revision:
- 0:c5109053e100
- Child:
- 1:65d0334fc83a
Drone Controle de Atitude
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
IgneousGuikas | 0:c5109053e100 | 1 | #include "mbed.h" |
IgneousGuikas | 0:c5109053e100 | 2 | #include "AttitudeEstimator.h" |
IgneousGuikas | 0:c5109053e100 | 3 | #include "AttitudeController.h" |
IgneousGuikas | 0:c5109053e100 | 4 | #include "Mixer.h" |
IgneousGuikas | 0:c5109053e100 | 5 | |
IgneousGuikas | 0:c5109053e100 | 6 | //Declare attitude estimator object |
IgneousGuikas | 0:c5109053e100 | 7 | AttitudeEstimator att_est; |
IgneousGuikas | 0:c5109053e100 | 8 | //Declare attitude controller object |
IgneousGuikas | 0:c5109053e100 | 9 | AttitudeController att_cont; |
IgneousGuikas | 0:c5109053e100 | 10 | // Declare mixer |
IgneousGuikas | 0:c5109053e100 | 11 | Mixer mixer; |
IgneousGuikas | 0:c5109053e100 | 12 | // Declare timer object |
IgneousGuikas | 0:c5109053e100 | 13 | Timer timer; |
IgneousGuikas | 0:c5109053e100 | 14 | // Last interrupt time |
IgneousGuikas | 0:c5109053e100 | 15 | float last_time = 0.0f; |
IgneousGuikas | 0:c5109053e100 | 16 | |
IgneousGuikas | 0:c5109053e100 | 17 | int main() |
IgneousGuikas | 0:c5109053e100 | 18 | { |
IgneousGuikas | 0:c5109053e100 | 19 | // Wait 5s for safety |
IgneousGuikas | 0:c5109053e100 | 20 | wait(5); |
IgneousGuikas | 0:c5109053e100 | 21 | //Initialize attitude estimator |
IgneousGuikas | 0:c5109053e100 | 22 | att_est.init(); |
IgneousGuikas | 0:c5109053e100 | 23 | //Start timer |
IgneousGuikas | 0:c5109053e100 | 24 | timer.start(); |
IgneousGuikas | 0:c5109053e100 | 25 | //Run controller for only 5s |
IgneousGuikas | 0:c5109053e100 | 26 | while( timer.read() <=5.0f) |
IgneousGuikas | 0:c5109053e100 | 27 | { |
IgneousGuikas | 0:c5109053e100 | 28 | // Wait 5ms (200 Hz) to perfom next estimate and control update |
IgneousGuikas | 0:c5109053e100 | 29 | while(( timer.read()-last_time ) <=0.005f) |
IgneousGuikas | 0:c5109053e100 | 30 | { |
IgneousGuikas | 0:c5109053e100 | 31 | } |
IgneousGuikas | 0:c5109053e100 | 32 | // Reset timmer |
IgneousGuikas | 0:c5109053e100 | 33 | last_time = timer.read(); |
IgneousGuikas | 0:c5109053e100 | 34 | // Estimate attitude |
IgneousGuikas | 0:c5109053e100 | 35 | att_est.estimate(); |
IgneousGuikas | 0:c5109053e100 | 36 | // Calculate torques (N.m) given estimated attitude ( rad and rad /s) |
IgneousGuikas | 0:c5109053e100 | 37 | 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); |
IgneousGuikas | 0:c5109053e100 | 38 | // Actuate motors with given force and calculated torques |
IgneousGuikas | 0:c5109053e100 | 39 | mixer.actuate(0.25f, att_cont.tau_phi, att_cont.tau_theta, att_cont.tau_psi); |
IgneousGuikas | 0:c5109053e100 | 40 | } |
IgneousGuikas | 0:c5109053e100 | 41 | //Turn off all motors |
IgneousGuikas | 0:c5109053e100 | 42 | mixer.actuate(0.0f, 0.0f, 0.0f, 0.0f); |
IgneousGuikas | 0:c5109053e100 | 43 | while (1) |
IgneousGuikas | 0:c5109053e100 | 44 | { |
IgneousGuikas | 0:c5109053e100 | 45 | |
IgneousGuikas | 0:c5109053e100 | 46 | } |
IgneousGuikas | 0:c5109053e100 | 47 | } |