File content as of revision 28:61f7356325c3:

#include "mbed.h"
#include "rtos.h"
#define _MBED_
//#include "controller.h"
#include "sensor.h"
#include "quadcopter.h"

Serial pc(USBTX, USBRX);
MRF24J40 mrf(p11, p12, p13, p14, p21);
Quadcopter myQuadcopter(&pc, &mrf); // instantiate Quadcopter object

// pwm outputs for the 4 motors (motor 1 points into x direction, 2 into y direction, 3 -x direction, -y direction
PwmOut motor_1(p23);
PwmOut motor_2(p24);
PwmOut motor_3(p25);
PwmOut motor_4(p26);

// to read the battery voltage
AnalogIn battery(p20);
DigitalOut batteryLed(LED1);

int emergencyOff = 0;

void controller_thread(void const *args)
{
    while(emergencyOff != 1) {
        myQuadcopter.readSensorValues();

        myQuadcopter.controller();
        motors motorsPwm = myQuadcopter.getPwm();

        motor_1 = motorsPwm.m1;
        motor_2 = motorsPwm.m2;
        motor_3 = motorsPwm.m3;
        motor_4 = motorsPwm.m4;

        // pc.printf("m1: %f m2: %f m3: %f m4: %f \n\r",  motorsPwm.m1, motorsPwm.m2, motorsPwm.m3, motorsPwm.m4);

        Thread::wait(10);
    }
}

void rc_thread(void const *args)
{
    while(true) {
        myQuadcopter.readRc();
        Thread::wait(50);  // wait for some number of miliseconds
    }
}

void battery_thread(void const *args)
{
    float threshold_voltage = 13.0; // desired lowest battery voltage
    float emergencyVoltage = 12.5; // switch off motors below it
    float max_voltage = 14.8; // max voltage level of battery
    float saturating_voltage = 18.38; // voltage at which ADC == 1
    float max_adc = 0.80522; // when battery is at 14.8V
    float threshold_adc = max_adc * threshold_voltage / max_voltage;
    float emergency_adc = max_adc * emergencyVoltage / max_voltage;

    while(true) {
        if (battery.read() < threshold_adc) {
            printf("low battery! %f\r\n", battery.read() * saturating_voltage);
            batteryLed=1;
            if (battery.read() < emergency_adc) {
                emergencyOff=1;
                motor_1=0.1;
                motor_2=0.1;
                motor_3=0.1;
                motor_4=0.1;
            }
        }
        Thread::wait(1000);  // wait for some number of miliseconds
    }
}



int main()
{
    // get desired values from joystick (to be implemented)
    // myQuadcopter.setDes(...)

    motor_1.period(0.01);          // motor requires a 2ms period
    motor_2.period(0.01);          // motor requires a 2ms period
    motor_3.period(0.01);          // motor requires a 2ms period
    motor_4.period(0.01);          // motor requires a 2ms period

    Thread threadR(rc_thread);
    double forceThreshold = -0.3;
    double forceRc = myQuadcopter.getForce();

   // pc.printf("forceRc %f\n\r", forceRc);
    while (forceRc > forceThreshold) { // wait until Joystick is in starting position
        forceRc = myQuadcopter.getForce();
        wait(1);
        //        pc.printf("forceRc %f\n\r", forceRc);

    }

    motor_1 = 0.1;
    motor_2 = 0.1;
    motor_3 = 0.1;
    motor_4 = 0.1;

    wait(2); // hold startup duty cycle for 2 seconds


// TODO assign priorities to threads, test if it really works as we expect
  //     Thread battery(battery_thread);
    Thread thread(controller_thread);

    //pc.printf("Entering control loop\n\r");

    while (1) {

        //pc.printf("F: %f M_x: %f M_y: %f M_z: %f\n\r",  F, M_x, M_y, M_z);
        // pc.printf("m1: %f m2: %f \n\r",  motorsPwm.m2, motorsPwm.m4);
        //pc.printf("M_x: %f\tM_y: %f\tM_z: %f\tF: %f\n\r", result.M_x, result.M_y, result.M_z, result.F);
    }
}