File content as of revision 47:ae478c380136:

#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);
Timer timer; // timer ;)
//Mutex desired_mutex;

int started = 0;

Quadcopter myQuadcopter(&pc, &mrf, &timer); // 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);
//DigitalOut quicknessLed(LED3); // used for check delay in IMU measurement

int emergencyOff = 0;
int nLowThrust = 0;

//void print_count(int id)
//{
//    static int count = 0;
//    if (count % 1000 == 0) {
//        printf("%d\t%d\r\n", id, count);
//    }
//    count++;
//}

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

int getLowThrust(double threshold)
{
    double force = myQuadcopter.getForce();
    //printf("%f\r\n", force);
    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());
        myQuadcopter.readSensorValues();

        myQuadcopter.controller();
        static int count = 0;
        if (count % 100 == 0) {
            printf("%s\t%d\r\n", "controller", count);
        }
        count++;
        motors motorsPwm = myQuadcopter.getPwm();

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

        Thread::wait(10);
        Thread::yield();
        // 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)
{
    static int count = 0;
    while(true) {
        if (count % 1000 == 0) {
            printf("%s\t%d\r\n", "rc", count);
        }
        count++;
        myQuadcopter.readRc();

        if (!started) {
            continue;
        }
        int shutdown = getLowThrust(-0.3);
        if (shutdown==1) {
            emergencyShutdown();
            printf("too long negative thrust! %f\r\n",myQuadcopter.getForce());
            shutDownLed = 1;
            break;
        }
        Thread::wait(10);
        Thread::yield();
    }
}
*/
bool low_battery()
{
    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;

    if (battery.read() < threshold_adc) {
        printf("low battery! %f\r\n", battery.read() * saturating_voltage);
        batteryLed = 1;
        if (battery.read() < emergency_adc) {
            emergencyShutdown();
            return true;
        }
    }
    return false;
}

//void counting_thread1(void const *args)
//{
//    while (true) {
//        print_count(1);
//        Thread::wait(1);
//    }
//}
//
//void counting_thread2(void const *args)
//{
//    while (true) {
//        print_count(2);
//    }
//}

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 counting1(counting_thread1);
    //Thread counting2(counting_thread2);
    //counting1.set_priority(osPriorityHigh);
    //counting2.set_priority(osPriorityHigh);
    //Thread thread_rc(rc_thread);

    printf("Waiting for start signal!\r\n");

    while (!started) { // wait until Joystick is in starting position
        myQuadcopter.readRc();
        started = getLowThrust(-0.3);
    }
    printf("Started!\r\n");
    nLowThrust = 0; // reset for later use in controller.
    // start ESCs up with 10% duty cycle
    motor_1 = OFF_PWM; 
    motor_2 = OFF_PWM; 
    motor_3 = OFF_PWM; 
    motor_4 = OFF_PWM; 

    wait(1); // hold startup duty cycle for 1 second
    
    // Idle thrust
    motor_1 = MIN_PWM_1; // starts at 11.72% duty cycle
    motor_2 = MIN_PWM_2; // starts at 11.61% duty cycle
    motor_3 = MIN_PWM_3; // starts at 11.61% duty cycle
    motor_4 = MIN_PWM_4; // starts at 11.71% duty cycle

    wait(1); // hold idle thrust for 1 second

    // TODO assign priorities to threads, test if it really works as we expect
    //Thread battery(battery_thread);
    //Thread controller(controller_thread);
    // TODO is this needed?
    //controller.set_priority(osPriorityRealtime);
    //thread_rc.set_priority(osPriorityRealtime);

    int i = 0;
    float previousTime = 0;
    float currentTime = 0;
    float checkBattery = 0.0;
    int shutdown = 0;
    
    // Enter while loop with control
    while (1) {
        // Check battery level
        i++;
        currentTime = timer.read();
        checkBattery += currentTime - previousTime;
        if (checkBattery > 1.0) {
            checkBattery = 0.0;
            if (low_battery()) {
                break;
            }
        }
        previousTime = currentTime;

        // can be used to measure frequency
        // TODO why did it get to 430Hz
//        if (i % 1000 == 0) {
//            currentTime = timer.read();
//            printf("%d, %f, %f\r\n", i, 1000.0 / (currentTime - previousTime), currentTime);
//            previousTime = currentTime;
//
//        }
        
        myQuadcopter.readRc();

        // shutdown
        shutdown = getLowThrust(-0.3);
        if (shutdown == 1) {
            emergencyShutdown();
            printf("too long negative thrust! %f\r\n",myQuadcopter.getForce());
            shutDownLed = 1;
            break;
        }
        // read sensor values
        myQuadcopter.readSensorValues();
        // compute controller and get desired pwm
        myQuadcopter.controller();
        motors motorsPwm = myQuadcopter.getPwm();
        // set pwm
        motor_1 = motorsPwm.m1;
        motor_2 = motorsPwm.m2;
        motor_3 = motorsPwm.m3;
        motor_4 = motorsPwm.m4;
    }
}