File content as of revision 1:65d0334fc83a:

#include "mbed.h"
#include "CrazyflieController.h"

//Declare attitude estimator object
AttitudeEstimator att_est;
//Declare attitude controller object
AttitudeController att_cont;
// Declare mixer
Mixer mixer;
// Declare timer object
Timer timer;

Ticker tic;

bool flag = false;

void callback()
{
    flag = true;
}
// Last interrupt time
float current_time = 0.0f;

int main()
{
    // Wait 5s for safety
    wait(5);
    //Initialize attitude estimator
    att_est.init();
    //Start timer
    tic.attach(&callback,dt);
    
    timer.start();
    //Run controller for only 5s
    while( current_time <=10.0f)
    {
        // Wait 5ms (200 Hz) to perfom next estimate and control update
        current_time = timer.read();
        
        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);
        }
        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)
        {
            break;
        }
    }
    //Turn off all motors
    mixer.actuate(0.0f, 0.0f, 0.0f, 0.0f);
    while (1);
}