File content as of revision 33:61f0be6af3c0:

/**
* HARP Version 2
*
* TODO: Test Servo Code
*       Test Watchdog Timer
*/

#include "mbed.h"
#include "rtos.h"
#include "buffered_serial.h"
#include "ff.h"
#include "TMP36GZ.h"
#include "nmea_parser.h"
#include "watchdog.h"
#include "Servo.h"
#include "config.h"

Serial pc(USBTX, USBRX);
BufferedSerial gps;
AnalogIn gps_battery(p20);
AnalogIn mbed_battery(p16);
TMP36GZ temperature(p17);
DigitalOut alarm(p18);

NmeaParser nmea;

Semaphore parachute_sem(0);
Semaphore sd_sem(0);

typedef struct {
    char    line[80];
} gps_line;
MemoryPool<gps_line, 16> mpool_gps_line;
Queue<gps_line, 16> queue_gps_line;

typedef struct {
    char line[80];
} sensor_line;
MemoryPool<sensor_line, 16> mpool_sensor_line;
Queue<sensor_line, 16> queue_sensor_line;

volatile float left_pos = 0.0; // servo position for logging
volatile float right_pos = 0.0;

volatile bool gps_wd = false;
volatile bool sensor_wd = false;
volatile bool log_wd = false;
volatile bool parachute_wd = false;

void gps_thread(void const *args)
{
    char buffer[80];
    float alt, alt_prev;
    alt = alt_prev = 0;
    bool released = false;

    //DigitalOut gps_led(LED4);

    gps.baud(4800);

    while(true) {
        //gps_led = !gps_led;
        gps.get_line(buffer);
        int line_type = nmea.parse(buffer);
        //pc.puts(buffer);
        // send to log...
        gps_line *message = mpool_gps_line.alloc();
        strcpy(message->line, buffer);
        queue_gps_line.put(message);

        // test altitude direction - release parachute thread to run
        if(line_type == RMC && nmea.quality()) {
            if(RELEASE_AT_ALT) {
                if(nmea.msl_altitude() >= RELEASE_ALT)
                {
                    parachute_sem.release();
                    released = true;
                }
            }
            if(UNLOCK_ON_FALL & released) {
                if(alt == 0) { // first time
                    alt = nmea.msl_altitude();
                } else {
                    alt_prev = alt;
                    alt = nmea.msl_altitude();
                    if(alt < alt_prev) { // going down
                        parachute_sem.release(); // let the parachute code execute
                        if(ALARM && alt < ALARM_ALT)
                            alarm = 1;
                    }
                }
            } else if(released) {
                parachute_sem.release();
            }
        }

        gps_wd = true; // kick the watchdog
    }
}

void log_thread(const void *args)
{
    FATFS fs;
    FIL fp_gps, fp_sensor;

    DigitalOut log_led(LED3);

    log_wd = true; // kick the dog
    f_mount(0, &fs);
    f_open(&fp_gps, "0:gps.txt", FA_OPEN_EXISTING | FA_WRITE);
    f_lseek(&fp_gps, f_size(&fp_gps));
    f_open(&fp_sensor, "0:sensors.csv", FA_OPEN_EXISTING | FA_WRITE);
    f_lseek(&fp_sensor, f_size(&fp_sensor));
    log_wd = true; // kick the dog

    sd_sem.release(); // sd card initialized... start sensor thread

    while(1) {
        log_led = !log_led;
        osEvent evt1 = queue_gps_line.get(1);
        if (evt1.status == osEventMessage) {
            gps_line *message = (gps_line*)evt1.value.p;

            f_puts(message->line, &fp_gps);
            f_sync(&fp_gps);

            mpool_gps_line.free(message);
        }

        osEvent evt2 = queue_sensor_line.get(1);
        if(evt2.status == osEventMessage) {
            sensor_line *message = (sensor_line*)evt2.value.p;

            f_puts(message->line, &fp_sensor);
            f_sync(&fp_sensor);

            mpool_sensor_line.free(message);
        }

        log_wd = true; // kick the dog
    }
}

void sensor_thread(const void* args)
{
    DigitalOut sensor_led(LED2);
    Timer t;
    float time;
    float gps_battery_voltage, mbed_battery_voltage;
    float temp;
    float distance, course_to, course;

    pc.baud(9600);

    while( sd_sem.wait(50) <= 0) // wait for the sd card to initialize and open files
        sensor_wd = true; // kick the dog

    if(WAIT_FOR_LOCK) {
        while(!nmea.date()) {
            Thread::wait(50); // wait for lock
            sensor_wd = true; // kick the dog
        }
    }

    t.start(); // start timer after lock

    sensor_line *message = mpool_sensor_line.alloc();
    sprintf(message->line, "Date: %d, Time: %f\r\nGPS Time (UTC),GPS Battery(V),mbed Battery(V),", nmea.date(), nmea.utc_time());
    queue_sensor_line.put(message);

    sensor_wd = true; // kick the dog

    message = mpool_sensor_line.alloc();
    sprintf(message->line, "Temperature,GPS Altitude,GPS Course,");
    queue_sensor_line.put(message);

    sensor_wd = true; // kick the dog

    message = mpool_sensor_line.alloc();
    sprintf(message->line, "Course to Target,Distance,Left Servo,Right Servo \r\n");
    queue_sensor_line.put(message);

    while(true) {
        //get sensor line memory
        sensor_led = !sensor_led;

        //timestamp
        time = nmea.utc_time();

        //gps battery
        gps_battery_voltage = gps_battery.read()*BAT_GPS_MUL;

        //mbed battery
        mbed_battery_voltage = mbed_battery.read()*BAT_MBED_MUL;

        //temperature
        temp = temperature.sample_f();

        //gps
        distance = nmea.calc_dist_to_km(target_lat, target_lon);
        course_to = nmea.calc_initial_bearing(target_lat, target_lon);
        course = nmea.track();

        pc.printf("course: %4.1f course_to: %4.1f distance: %f, alt: %f\r\n", course, course_to, distance, nmea.msl_altitude());

        sensor_line *message = mpool_sensor_line.alloc();
        sprintf(message->line, "%f,%f,%f,%f,%f,", time,gps_battery_voltage,mbed_battery_voltage,temp,nmea.calc_altitude_ft());
        queue_sensor_line.put(message);

        message = mpool_sensor_line.alloc();
        sprintf(message->line, "%f,%f,%f,%f,%f\r\n", course,course_to,distance,left_pos,right_pos);
        queue_sensor_line.put(message);

        sensor_wd = true; // kick the dog
        Thread::wait(200);
    }
}

void parachute_thread(const void *args)
{
    DigitalOut left_turn_led(LED4);
    DigitalOut right_turn_led(LED1);
    bool is_released = false;

    // servos ////////////////////////// NOT TESTED!!! ///////////////////////////
    Servo left_s(p21);
    Servo right_s(p22);
    Servo release_s(p23);

    left_s.calibrate_max(SERVO_L_MAX);
    left_s.calibrate_min(SERVO_L_MIN);
    right_s.calibrate_max(SERVO_R_MAX);
    right_s.calibrate_min(SERVO_R_MIN);
    release_s.calibrate_max(SERVO_RELEASE_MAX);
    release_s.calibrate_min(SERVO_RELEASE_MIN);

    left_s = 1.0;
    right_s = 0.0;
    release_s = 0.0;

    ////////////////////////////////////////////////////////////////////////////////

    //right_turn_led = 1;  // remove with watchdog on - test to determine which led is which
    //Thread::wait(1000);
    //left_turn_led = 1;
    //Thread::wait(1000);

    int counter = 0;

    while(true) {
        parachute_wd = true; // kick the dog
        right_turn_led = left_turn_led = 0;
        while( parachute_sem.wait(50) <= 0) // didn't get it (timeout)
            parachute_wd = true; // kick the dog

        if(!is_released) {
            release_s = 1.0; // let go of the balloon
            is_released = true;
        }

        if(counter < test_length) { // test flight -- (NOT Tested)
            left_s = test_left[counter];
            right_s = test_right[counter];
            Thread::wait(1000);
            parachute_wd = true; // kick the watchdog
            Thread::wait(1000);
            counter++;
            continue;
        }

        float distance = nmea.calc_dist_to_km(target_lat, target_lon);

        if(distance < distance_fudge_km) {
            alarm = 1; // sound the alarm
            continue; // dont do anything
        }

        float course = nmea.track();
        float course_to = nmea.calc_initial_bearing(target_lat, target_lon);
        float course_diff = course_to - course;

        if(course == 0.0) // not moving fast enough
            continue; // do nothing

        if(course_diff < course_fudge && course_diff > neg_course_fudge) {
            right_turn_led = left_turn_led = 1;
            Thread::wait(400);
            continue; // don't do anything
        } else if(course_diff > 180.0 || course_diff < 0.0) { // turn left
            left_turn_led = 1;
            right_s = right_pos = 0.0;
            left_s = left_pos = 0.0;
            Thread::wait(400); // turn left
            left_s = 1.0;
        } else { // turn right
            right_turn_led = 1;
            left_s = left_pos = 1.0;
            right_s = right_pos = 1.0;
            Thread::wait(400); // turn righ
            right_s = right_pos = 0.0;
        }
    }
}

void watchdog_thread(const void *args)
{
    Watchdog wdt; // NOT TESTED!!!

    wdt.kick(10.0);

    while(true) {
        if(gps_wd && sensor_wd && parachute_wd && log_wd) {
            wdt.kick();
            gps_wd = sensor_wd = parachute_wd = log_wd = false;
        } else {
            Thread::wait(50);
        }
    }
}

int main()
{
    Thread thread(gps_thread, NULL, osPriorityHigh);
    Thread thread2(log_thread, NULL, osPriorityNormal);
    Thread thread3(sensor_thread, NULL, osPriorityNormal);
    Thread thread4(parachute_thread, NULL, osPriorityRealtime);
    if(WATCHDOG)
        Thread thread5(watchdog_thread, NULL, osPriorityHigh);

    while(true) {
        Thread::wait(osWaitForever);
    }
}