File content as of revision 43:0e98d5488bf2:

#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, &desired_mutex); // 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 lowThrust= {0,0,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.

    motor_1 = OFF_PWM; // starts at 11.72% duty cycle
    motor_2 = OFF_PWM; // starts at 11.61% duty cycle
    motor_3 = OFF_PWM; // starts at 11.61% duty cycle
    motor_4 = OFF_PWM; // starts at 11.71% duty cycle

    wait(1); // hold startup duty cycle for 2 seconds
    
    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 startup duty cycle for 2 seconds

    // 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;
    while (1) {
        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();

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

        // can be used to test responsiveness of the IMU
        /*
        state mystate = myQuadcopter.getState();
        if (mystate.phi > 0.0785) {
            quicknessLed = 1;
        } else {
            quicknessLed = 0;
        }
        */

        myQuadcopter.controller();
        motors motorsPwm = myQuadcopter.getPwm();
        motor_1 = motorsPwm.m1;
        motor_2 = motorsPwm.m2;
        motor_3 = motorsPwm.m3;
        motor_4 = motorsPwm.m4;
    }
}