Generation 2 of the Harp project
Dependencies: Servo TMP36 GZ chan_fatfs_sd buffered-serial1 nmea_parser watchdog mbed-rtos mbed
Diff: main.cpp
- Revision:
- 29:8cdb56a0fe57
- Parent:
- 28:032d55fa57b8
- Child:
- 30:b81274979e73
diff -r 032d55fa57b8 -r 8cdb56a0fe57 main.cpp --- a/main.cpp Thu Jan 10 19:25:13 2013 +0000 +++ b/main.cpp Thu Jan 10 20:37:07 2013 +0000 @@ -21,6 +21,7 @@ AnalogIn gps_battery(p20); AnalogIn mbed_battery(p16); TMP36GZ temperature(p17); +DigitalOut alarm(p18); NmeaParser nmea; @@ -42,6 +43,11 @@ 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]; @@ -65,18 +71,28 @@ // test altitude direction - release parachute thread to run if(line_type == RMC && nmea.quality()) { if(UNLOCK_ON_FALL) { - if(alt != 0) { // first time + 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 } } @@ -87,14 +103,16 @@ 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); @@ -116,6 +134,8 @@ mpool_sensor_line.free(message); } + + log_wd = true; // kick the dog } } @@ -127,27 +147,38 @@ float gps_battery_voltage, mbed_battery_voltage; float temp; float distance, course_to, course; - - sd_sem.wait(); // wait for the sd card to initialize and open files + + 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(100); // 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()); + 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,Course to Target,Distance,Left Servo,Right Servo \r\n"); + 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; - sensor_line *message = mpool_sensor_line.alloc(); //timestamp time = nmea.utc_time(); @@ -160,15 +191,21 @@ //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(); - sprintf(message->line, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\r\n", time,gps_battery_voltage,mbed_battery_voltage,temp,nmea.calc_altitude_ft(),course,course_to,distance,left_pos,right_pos); + 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(100); } } @@ -177,17 +214,28 @@ { 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(0.0007); - left_s.calibrate_min(-0.0014); - right_s.calibrate(0.0009); - + 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); + + // TODO: Calibrate release servo + left_s = 0.0; right_s = 0.0; + + // TODO: Move release servo in and out to allow attachment on startup + // release_s = 0.0; + // Thread::wait(500); + // release_s = 1.0 + //////////////////////////////////////////////////////////////////////////////// right_turn_led = 1; @@ -196,7 +244,11 @@ Thread::wait(400); while(true) { right_turn_led = left_turn_led = 0; - parachute_sem.wait(); + while( parachute_sem.wait(50) <= 0) // didn't get it (timeout) + parachute_wd = true; // kick the dog + + //if(!is_released) + //release_s = 0.0; // let go of the balloon float distance = nmea.calc_dist_to_km(target_lat, target_lon); @@ -206,7 +258,7 @@ 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 @@ -227,23 +279,35 @@ Thread::wait(400); // turn righ right_s = right_pos = 0.0; } + parachute_wd = true; // kick the dog + } +} + +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() { - pc.baud(9600); Thread thread(gps_thread, NULL, osPriorityHigh); Thread thread2(log_thread, NULL, osPriorityNormal); Thread thread3(sensor_thread, NULL, osPriorityNormal); Thread thread4(parachute_thread, NULL, osPriorityRealtime); - - Watchdog wdt; // NOT TESTED!!! - - wdt.kick(2.0); + Thread thread5(watchdog_thread, NULL, osPriorityHigh); while(true) { - wdt.kick(); - Thread::wait(500); + Thread::wait(osWaitForever); } } \ No newline at end of file