Diff: main.cpp

Revision:

30:b81274979e73

Parent:

29:8cdb56a0fe57

Child:

31:b9ac7d61b15b

--- a/main.cpp	Thu Jan 10 20:37:07 2013 +0000
+++ b/main.cpp	Thu Jan 17 01:44:32 2013 +0000
@@ -147,6 +147,8 @@
     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
@@ -196,6 +198,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);
 
         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());
@@ -206,7 +210,7 @@
         queue_sensor_line.put(message);
 
         sensor_wd = true; // kick the dog
-        Thread::wait(100);
+        Thread::wait(500);
     }
 }
 
@@ -232,28 +236,48 @@
     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
+    release_s = 0.0; // NOT TESTED!
+    Thread::wait(1000);
+    parachute_wd = true; // kick the dog
+    Thread::wait(1000);
+    release_s = 1.0;
 
     ////////////////////////////////////////////////////////////////////////////////
 
-    right_turn_led = 1;
-    Thread::wait(400);
-    left_turn_led = 1;
-    Thread::wait(400);
+    //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 = 0.0; // let go of the balloon
-
+        if(!is_released)
+            release_s = 0.0; // let go of the balloon (NOT Tested)
+        
+        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);
@@ -268,7 +292,7 @@
             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;                                      // NOT TESTED!!!
+            right_s = right_pos = 0.0;                                      
             left_s = left_pos = 1.0;
             Thread::wait(400); // turn left
             left_s = 0.0;
@@ -279,7 +303,6 @@
             Thread::wait(400); // turn righ
             right_s = right_pos = 0.0;
         }
-        parachute_wd = true; // kick the dog
     }
 }