Generation 2 of the Harp project
Dependencies: Servo TMP36 GZ chan_fatfs_sd buffered-serial1 nmea_parser watchdog mbed-rtos mbed
main.cpp
- Committer:
- tylerjw
- Date:
- 2013-01-17
- Revision:
- 31:b9ac7d61b15b
- Parent:
- 30:b81274979e73
- Child:
- 32:4f811b397720
File content as of revision 31:b9ac7d61b15b:
/** * 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; //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(UNLOCK_ON_FALL) { if(RELEASE_AT_ALT) { if(nmea.msl_altitude() >= RELEASE_ALT) parachute_sem.release(); } 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 { 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\r\n", course, course_to, distance); 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 = 0.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) { left_turn_led = 1; right_s = right_pos = 0.0; left_s = left_pos = 1.0; Thread::wait(400); // turn left left_s = 0.0; } else { right_turn_led = 1; left_s = left_pos = 0.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(2.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); Thread thread5(watchdog_thread, NULL, osPriorityHigh); while(true) { Thread::wait(osWaitForever); } }