Diff: main.cpp

Revision:

32:4f811b397720

Parent:

31:b9ac7d61b15b

diff -r b9ac7d61b15b -r 4f811b397720 main.cpp
--- 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);