Yuri De Stefani / Mbed 2 deprecated controladoatitude_cteste

yuri

Dependencies:   mbed CrazyflieController CrazyflieSensors USBDevice

main.cpp@5:d8e1cf0be63d, 2018-11-21 (annotated)

Committer:
yurindes
Date:
Wed Nov 21 10:10:08 2018 +0000
Revision:
5:d8e1cf0be63d
Parent:
1:0f858d1630a2 
yu

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yvesyuzo 0:cc252ccf8183 1 #include "mbed.h"
yvesyuzo 0:cc252ccf8183 2 #include "CrazyflieController.h"
yurindes 5:d8e1cf0be63d 3 // Crazyflie controller objects
yurindes 5:d8e1cf0be63d 4 //dfu-util -d 0483:df11 -a 0 -s 0x08000000 -D controladoatitude.NUCLEO_F446ZE.bin
yurindes 5:d8e1cf0be63d 5 AttitudeEstimator att_est ;
yurindes 5:d8e1cf0be63d 6 AttitudeController att_cont ;
yurindes 5:d8e1cf0be63d 7 Mixer mixer ;
yurindes 5:d8e1cf0be63d 8 // Timer and tickers
yurindes 5:d8e1cf0be63d 9 Timer tim;
yurindes 5:d8e1cf0be63d 10 Ticker tic ;
yurindes 5:d8e1cf0be63d 11 // Interrupt flag
yurindes 5:d8e1cf0be63d 12 bool flag = false ;
yurindes 5:d8e1cf0be63d 13 // Callback function
yurindes 5:d8e1cf0be63d 14 void callback ()
yurindes 5:d8e1cf0be63d 15 {
yurindes 5:d8e1cf0be63d 16 flag = true ;
yurindes 5:d8e1cf0be63d 17 }
yurindes 5:d8e1cf0be63d 18 // Current time
yurindes 5:d8e1cf0be63d 19 float current_time ;
yurindes 5:d8e1cf0be63d 20 // Main program
yurindes 5:d8e1cf0be63d 21 int main ()
yurindes 5:d8e1cf0be63d 22 {
yurindes 5:d8e1cf0be63d 23 // Set references
yurindes 5:d8e1cf0be63d 24 float f_t = 1.0f * m * g;
yurindes 5:d8e1cf0be63d 25 float phi_r = 0.0f;
yurindes 5:d8e1cf0be63d 26 float theta_r = 0.0f;
yurindes 5:d8e1cf0be63d 27 float psi_r = 0.0f;
yvesyuzo 0:cc252ccf8183 28 // Wait 5s for safety
yvesyuzo 0:cc252ccf8183 29 wait (5);
yurindes 5:d8e1cf0be63d 30 // Initialize estimator object
yurindes 5:d8e1cf0be63d 31 att_est.init () ;
yurindes 5:d8e1cf0be63d 32 // Initialize interrupts
yurindes 5:d8e1cf0be63d 33 tic.attach (&callback , dt);
yvesyuzo 0:cc252ccf8183 34 // Start timer
yurindes 5:d8e1cf0be63d 35 tim.start () ;
yurindes 5:d8e1cf0be63d 36 while ( current_time <=10.0f)
yvesyuzo 0:cc252ccf8183 37 {
yurindes 5:d8e1cf0be63d 38 // Update current time
yurindes 5:d8e1cf0be63d 39 current_time = tim.read () ;
yurindes 5:d8e1cf0be63d 40 // Estimate and control
yurindes 5:d8e1cf0be63d 41 if ( flag )
yvesyuzo 0:cc252ccf8183 42 {
yurindes 5:d8e1cf0be63d 43 flag = false ;
yurindes 5:d8e1cf0be63d 44 att_est.estimate () ;
yurindes 5:d8e1cf0be63d 45 att_cont.control (phi_r , theta_r ,psi_r , att_est.phi , att_est.theta , att_est.psi);
yurindes 5:d8e1cf0be63d 46 mixer.actuate (f_t ,0.0f, att_cont.tau_theta ,0.0f);
yvesyuzo 0:cc252ccf8183 47 }
yvesyuzo 0:cc252ccf8183 48 }
yvesyuzo 0:cc252ccf8183 49 // Turn off all motors
yvesyuzo 0:cc252ccf8183 50 mixer.actuate (0.0f ,0.0f ,0.0f ,0.0f);
yurindes 5:d8e1cf0be63d 51 while (1) ;
yurindes 5:d8e1cf0be63d 52 }