File content as of revision 0:c5109053e100:

#include "mbed.h"
#include "AttitudeEstimator.h"
#include "AttitudeController.h"
#include "Mixer.h"

//Declare attitude estimator object
AttitudeEstimator att_est;
//Declare attitude controller object
AttitudeController att_cont;
// Declare mixer
Mixer mixer;
// Declare timer object
Timer timer;
// Last interrupt time
float last_time = 0.0f;

int main()
{
    // Wait 5s for safety
    wait(5);
    //Initialize attitude estimator
    att_est.init();
    //Start timer
    timer.start();
    //Run controller for only 5s
    while( timer.read() <=5.0f)
    {
        // Wait 5ms (200 Hz) to perfom next estimate and control update
        while(( timer.read()-last_time ) <=0.005f)
        {
        }
        // Reset timmer
        last_time = timer.read();
        // 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, att_cont.tau_psi);
    }
    //Turn off all motors
    mixer.actuate(0.0f, 0.0f, 0.0f, 0.0f);
    while (1)
    {
    
    }
}