HARP: High Altitude Recovery Payload

File content as of revision 8:513b31554e5f:

/* Copyright (c) 2012 Tyler Weaver, MIT License
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
 * and associated documentation files (the "Software"), to deal in the Software without restriction,
 * including without limitation the rights to use, copy, modify, merge, publish, distribute,
 * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all copies or
 * substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
 * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */

#include "mbed.h"
#include "rtos.h"
#include "watchdog.h"
#include "Servo.h"
#include "ITG3200.h"

#define CENTER  'C'
#define LEFT    'L'
#define RIGHT   'R'

// Setup the watchdog timer
Watchdog wdt;

// status leds
BusOut status_led(LED4, LED3, LED2, LED1);

typedef struct {
    char msg;   // the direction to turn in
} messageXbeeT;

MemoryPool<messageXbeeT, 16> mpool_xbee;
Queue<messageXbeeT, 16> queue_xbee;

Mutex direction_mutex;
char direction = CENTER;

/**
* xbee_thread
* this thread reads characters from the xbee serial connection and posts messages containing them
*/
void xbee_thread(void const *argument)
{
    // xbee serial connection
    Serial xbee(p9,p10);
    xbee.baud(9600);
    while (true) {
        if (xbee.readable()) {
            messageXbeeT *message = mpool_xbee.alloc();
            message->msg = xbee.getc();

            queue_xbee.put(message);
        }
        Thread::wait(100);
    }
}

/**
parachute_thread
this thread recieves messages from the main thread and turns the servos for control of the parachute
*/
void parachute_thread(void const *argument)
{
    // servos
    Servo left_s(p21);
    Servo right_s(p22);

    float left = 0;
    float right = 0;

    left_s.calibrate_max(0.0007);
    left_s.calibrate_min(-0.0014);
    right_s.calibrate(0.0009);

    for(float i = 0.0; i <= 1.0; i+=0.1) {
        left_s = i;
        right_s = i;
        Thread::wait(100);
    }
    right_s = left;
    left_s = right;
    
    bool new_message = false;

    char state = CENTER;
    
    while (true) {
        osEvent evt_xbee = queue_xbee.get(20); // 20 millisecond wait
        if (evt_xbee.status == osEventMessage) {
            messageXbeeT *message = (messageXbeeT*)evt_xbee.value.p;
            state = message->msg;
            new_message = true;
        }
        switch (state) {
            case CENTER: // center
                status_led = 0x6;
                direction_mutex.lock();
                if(new_message)
                    left = right = 0;
                if(direction != CENTER) {
                    if(direction == LEFT) {
                        right += 0.02;
                        left -= 0.02;
                    } else { // right
                        right -= 0.02;
                        left += 0.02;
                    }
                }
                direction_mutex.unlock();
                break;
            case LEFT: // left
                status_led = 0x3;
                direction_mutex.lock();
                if(direction != LEFT) {
                    left += 0.02;
                    right = 0;
                }
                direction_mutex.unlock();
                break;
            case RIGHT: // right
                status_led = 0xC;
                direction_mutex.lock();
                if(direction != RIGHT) {
                    right += 0.02;
                    left = 0;
                }
                direction_mutex.unlock();
                break;
        }
        if(left > 1.0) left = 1.0;
        if(left < 0.0) left = 0.0;
        if(right > 1.0) right = 1.0;
        if(right < 0.0) right = 0.0;
        right_s = right;
        left_s = left;
        new_message = false;
    }

}

/**
sensor thread
for sampling from sensors
*/

void sensor_thread(void const* arg)
{
    Serial pc(USBTX, USBRX);
    pc.baud(9600);
    
    AnalogIn battery(p19);
    DigitalOut battery_warning(p24);
    battery_warning = 1;

    const float BAT_MUL = 10.26;
    float battery_voltage;

    pc.puts("\n\rInitalizing Gyro, hold still.. ");
    ITG3200 gyro(p28, p27); // sda, scl - gyro
    gyro.setLpBandwidth(LPFBW_5HZ);
    gyro.calibrate(0.5);
    double gyro_readings[3];
    pc.puts("OK\n\r");

    while(1) {
        gyro.getGyroXYZDegrees(gyro_readings);
        pc.printf("\r\n%f,%f,%f \r\n", gyro_readings[0], gyro_readings[1], gyro_readings[2]);

        direction_mutex.lock();
        if(gyro_readings[2] > 10.0)
            direction = LEFT;
        else if (gyro_readings[2] < -10.0)
            direction = RIGHT;
        else
            direction = CENTER;
        pc.putc(direction);
        direction_mutex.unlock();
        
        battery_voltage = battery.read() * BAT_MUL;
        if(battery_voltage < 6.4)
            battery_warning = 0;
            
        Thread::wait(100);
    }
}

/**
main thread
this thread initializes everything then recieves messages from the xbee and sends messages to the parachute
*/
int main (void)
{
    status_led = 0x9;
    // setup watchdog
    wdt.kick(2.0); // 2 second watchdog
    // setup xbee serial

    Thread thread1(xbee_thread);
    Thread thread2(parachute_thread);
    Thread thread3(sensor_thread);

    while (true) {
        Thread::wait(500);
        wdt.kick();
    }
}