File content as of revision 5:d8e1cf0be63d:

#include "mbed.h"
#include "CrazyflieController.h"
// Crazyflie controller objects
//dfu-util -d 0483:df11 -a 0 -s 0x08000000 -D controladoatitude.NUCLEO_F446ZE.bin
AttitudeEstimator att_est ;
AttitudeController att_cont ;
Mixer mixer ;
// Timer and tickers
Timer tim;
Ticker tic ;
// Interrupt flag
bool flag = false ;
// Callback function
void callback ()
{
    flag = true ;
}
// Current time
float current_time ;
// Main program
int main ()
{
    // Set references
    float f_t = 1.0f * m * g;
    float phi_r = 0.0f;
    float theta_r = 0.0f;
    float psi_r = 0.0f;
    // Wait 5s for safety
    wait (5);
    // Initialize estimator object
    att_est.init () ;
    // Initialize interrupts
    tic.attach (&callback , dt);
    // Start timer
    tim.start () ;
    while ( current_time <=10.0f)
    {
        // Update current time
        current_time = tim.read () ;
        // Estimate and control
        if ( flag )
        {
            flag = false ;
            att_est.estimate () ;
            att_cont.control (phi_r , theta_r ,psi_r , att_est.phi , att_est.theta , att_est.psi);
            mixer.actuate (f_t ,0.0f, att_cont.tau_theta ,0.0f);
        }
    }
    // Turn off all motors
    mixer.actuate (0.0f ,0.0f ,0.0f ,0.0f);
    while (1) ;
}