File content as of revision 4:8734045fab59:

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

//cmd command to upload code to drone
//dfu-util -d 0483:df11 -a 0 -s 0x08000000 -D love.bin

// Declare attitude estimator object
AttitudeEstimator AttitudeEstimator;
// Declare attitude controller object
AttitudeController AttitudeController;
// 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
    AttitudeEstimator.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
        AttitudeEstimator.estimate ();
        // Calculate torques (N.m) given estimated attitude ( rad and rad /s)
        AttitudeController.control (0.0f , 0.0f , 0.0f, AttitudeEstimator.phi , AttitudeEstimator.theta , AttitudeEstimator.psi) ;
        // Actuate motors with given force and calculated torques
        mixer.actuate (0.0f ,0.0f, AttitudeController.tau_theta ,0.0f);
    }
    // Turn off all motors
    mixer.actuate (0.0f ,0.0f ,0.0f ,0.0f);
    while (1) {
    }
}