File content as of revision 0:4c04e4fd1310:

#include "mbed.h"

Serial pc(USBTX, USBRX);

double controller(double phi)
{
    double g=9.81;  // gravity [m/s^2]
    double m=1; // mass [kg]

    // proportional attitude control gains
    double kp_phi = 1;
    double kp_theta = 1;
    double kp_psi = 1;

    // derivative attitude control gains
    double kd_phi = 0;
    double kd_theta = 0;
    double kd_psi = 0;

    // measured values (will come from IMU/parameter class/Input to function later)
    // double phi = 0; // roll // commented out, because input to function
    double theta = 0; // pitch
    double psi = 0; // yaw

    // angular velocities in body coordinate system
    double p = 0; // omega_x
    double q = 0;  // omega_y
    double r = 0;  // omega_z

    // desired values
    double F_des = 0; // desired thrust force (excluding weight compensation)
    double phi_des = 0; // roll
    double theta_des = 0; // pitch
    double psi_des = 0; // yaw
    // these will probably not be changed -> we could use const keyword in case of storage shortage
    double p_des = 0; // omega_x
    double q_des = 0;  // omega_y
    double r_des = 0;  // omega_z

    // total thrust force
    double F=0;
    // control moments
    double M_x=0;
    double M_y=0;
    double M_z=0;

    // get IMU data
    // phi = 1;
    theta =0;
    psi =0;
    p = 0;
    q = 0;
    r = 0;

    // compute desired angles (in the case we decide not to set
    // the angles, but for instance the velocity with the Joystick

    // desired values
    // dirctly from Joystick, or compute from desired velocities.
    F_des=1;
    phi_des = 0;
    theta_des =1;
    psi_des =0;

    // PD controller
    F = m*g + F_des;
    M_x = kp_phi*(phi_des-phi)+kd_phi*(p_des-p);
    M_y = kp_theta*(theta_des-theta)+kd_theta*(q_des-q);
    M_z = kp_psi*(psi_des-psi)+kd_psi*(r_des-r);

    //pc.printf("F: %f M_x: %f M_y: %f M_z: %f\n\r",  F, M_x, M_y, M_z);
    //        pc.printf("F: %f\n\r",  F);

    //  pc.printf("Mx: %f\n\r",  F);
    //pc.printf("F: %f\n\r",  F);

    return M_x;
}

int main()
{
    double F=0;
    double M_x=0;
    double M_y=0;
    double M_z=0;
    
    double *orientation = (double *) malloc(6 * sizeof(double));

    while (1) {
        // get sensor values
        double phi=1;

        // call controller
        M_x= controller(phi);

        // plot

        //pc.printf("F: %f M_x: %f M_y: %f M_z: %f\n\r",  F, M_x, M_y, M_z);
        pc.printf("F: %f M_x: %f M_y: %f M_z: %f\n\r",  F, M_x, M_y, M_z);
    }
    free(orientation);
}