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:
- 32:4f811b397720
- Parent:
- 31:b9ac7d61b15b
--- a/main.cpp Thu Jan 17 18:09:16 2013 +0000 +++ b/main.cpp Mon Jan 21 06:14:41 2013 +0000 @@ -52,6 +52,7 @@ char buffer[80]; float alt, alt_prev; alt = alt_prev = 0; + bool released = false; //DigitalOut gps_led(LED4); @@ -69,28 +70,30 @@ // 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(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 - { + if(alt < alt_prev) { // going down parachute_sem.release(); // let the parachute code execute if(ALARM && alt < ALARM_ALT) alarm = 1; } } - } else { + } else if(released) { parachute_sem.release(); } } - + gps_wd = true; // kick the watchdog } } @@ -146,7 +149,7 @@ 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 @@ -197,8 +200,8 @@ 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); + + 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()); @@ -231,7 +234,7 @@ release_s.calibrate_max(SERVO_RELEASE_MAX); release_s.calibrate_min(SERVO_RELEASE_MIN); - left_s = 0.0; + left_s = 1.0; right_s = 0.0; release_s = 0.0; @@ -241,23 +244,21 @@ //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) - { + if(!is_released) { release_s = 1.0; // let go of the balloon is_released = true; } - - if(counter < test_length) // test flight -- (NOT Tested) - { + + if(counter < test_length) { // test flight -- (NOT Tested) left_s = test_left[counter]; right_s = test_right[counter]; Thread::wait(1000); @@ -266,11 +267,10 @@ counter++; continue; } - + float distance = nmea.calc_dist_to_km(target_lat, target_lon); - if(distance < distance_fudge_km) - { + if(distance < distance_fudge_km) { alarm = 1; // sound the alarm continue; // dont do anything } @@ -286,15 +286,15 @@ 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) { + } 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 = 1.0; + right_s = right_pos = 0.0; + left_s = left_pos = 0.0; Thread::wait(400); // turn left - left_s = 0.0; - } else { + left_s = 1.0; + } else { // turn right right_turn_led = 1; - left_s = left_pos = 0.0; + left_s = left_pos = 1.0; right_s = right_pos = 1.0; Thread::wait(400); // turn righ right_s = right_pos = 0.0; @@ -306,7 +306,7 @@ { Watchdog wdt; // NOT TESTED!!! - wdt.kick(2.0); + wdt.kick(10.0); while(true) { if(gps_wd && sensor_wd && parachute_wd && log_wd) { @@ -324,7 +324,8 @@ Thread thread2(log_thread, NULL, osPriorityNormal); Thread thread3(sensor_thread, NULL, osPriorityNormal); Thread thread4(parachute_thread, NULL, osPriorityRealtime); - Thread thread5(watchdog_thread, NULL, osPriorityHigh); + if(WATCHDOG) + Thread thread5(watchdog_thread, NULL, osPriorityHigh); while(true) { Thread::wait(osWaitForever);