File content as of revision 0:115db3c33311:

# include "mbed.h"
# include "CrazyflieController.h"
// Crazyflie controller objects
//dfu-util -d 0483:df11 -a 0 -s 0x08000000 -D love.bin
AttitudeEstimator att_est ;
AttitudeController att_cont ;
VerticalEstimator ver_est ;
VerticalController ver_cont;
Mixer mixer;
// Timer and tickers
Timer tim;
Ticker tic;
Ticker tic_range;
// Interrupt flags
bool flag = false;
bool flag_range = false;
// Callback function
void callback()
{
    flag = true;
}
// Callback function ( range correct )
void callback_range()
{
    flag_range = true;
}
// Current time
float current_time;
// Main program
int main ()
    {
    // Set references
    float z_r = 0.1f;
    float phi_r = 0.0f;
    float theta_r = 0.0f;
    float psi_r = 0.0f;
    // Wait 5s for safety
    wait (5);
    // Initialize estimators objects
    att_est.init();
    ver_est.init();
    // Initialize interrupts
    tic.attach(&callback,dt);
    tic_range.attach(&callback_range ,dt_range);
    // Start timer
    tim.start();
    while (tim.read()<=5.0f)
    {
        // Estimate and control
        if (flag)
        {
            flag = false;
            att_est.estimate();
            ver_est.predict();
            ver_cont.control(z_r,ver_est.z,ver_est.w);
            att_cont.control(phi_r,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,att_cont.tau_phi,att_cont.tau_theta,att_cont.tau_ps);
        }
        // Correct vertical estimation
        if (flag_range)
        {
            flag_range = false;
            ver_est.correct(att_est.phi,att_est.theta);
    }
    // Turn off all motors
    mixer.actuate(0.0f ,0.0f ,0.0f ,0.0f);
    while(1);
}