File content as of revision 31:d473eacfc271:

#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);
DigitalOut shutDownLed(LED2);

Timer timer; // timer ;) 

int emergencyOff = 0;
//int lowThrust= {0,0,0};
int nLowThrust = 0;

void emergencyShutdown()
{
    emergencyOff = 1;
    motor_1 = 0.1;
    motor_2 = 0.1;
    motor_3 = 0.1;
    motor_4 = 0.1;
}

int getLowThrust(double threshold)
{
    double force = myQuadcopter.getForce();
    if (force < threshold) { // if low thrust signal is detected
        nLowThrust++;
        printf("Negative thrust! %f, nLowThrust %d\r\n",myQuadcopter.getForce(),nLowThrust);
        if (nLowThrust > 5) {
            return 1;
        }
    } else {
        nLowThrust = 0;
    }
    return 0;
}

void controller_thread(void const *args)
{
    while(emergencyOff != 1) {
       // printf(" thrust: %f\r\n",myQuadcopter.getForce());

        int shutdown=  getLowThrust(-0.3);
        if (shutdown==1) {
            emergencyShutdown();
            printf("too long negative thrust! %f\r\n",myQuadcopter.getForce());
            shutDownLed = 1;
            break;
        }

        myQuadcopter.readSensorValues();

        myQuadcopter.controller(timer.read());
        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);
    }
}

void rc_thread(void const *args)
{
    while(true) {
        myQuadcopter.readRc();
    }
}

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) {
                emergencyShutdown();
                break;
            }
        }
        Thread::wait(1000);  // wait for some number of miliseconds
    }
}



int main()
{
    // ESCs requires a 100Hz frequency
    motor_1.period(0.01);
    motor_2.period(0.01);
    motor_3.period(0.01);
    motor_4.period(0.01);

    Thread threadR(rc_thread);


    int startLoop= 0;

    while (!startLoop) { // wait until Joystick is in starting position
        startLoop=  getLowThrust(-0.3);
    }
    nLowThrust = 0; // reset for later use in controller.

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

    wait(3); // hold startup duty cycle for 2 seconds
    
    timer.start();

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

    // TODO is this needed?
    while (1);
}