Revision 30:b81274979e73, committed 2013-01-17

Comitter:

tylerjw

Date:

Thu Jan 17 01:44:32 2013 +0000

Parent:

29:8cdb56a0fe57

Child:

31:b9ac7d61b15b

Commit message:

Implemented release, watchdog improvements, and test routine - NOT TESTED; ; TODO: Calibrate release servo.

Changed in this revision

--- a/config.h	Thu Jan 10 20:37:07 2013 +0000
+++ b/config.h	Thu Jan 17 01:44:32 2013 +0000
@@ -8,8 +8,8 @@
 #define RELEASE_AT_ALT  0 // set to 1 for release at target altitude (must also set UNLOCK_ON_FALL to 1)
 #define ALARM           0 // set to 1 to key alarm on descent, 0 = no alarm 
 
-const float target_lat = 39.921664;    // for setting the target location!
-const float target_lon = -105.008167;
+const float target_lat = 39.910325;    // for setting the target location!
+const float target_lon = -105.004685;
 
 const float course_fudge = 15.0; // if -course_fudge < course > course_fudge then don't turn
 const float neg_course_fudge = -15.0;
@@ -27,4 +27,8 @@
 const int RELEASE_ALT = 4572; // 15k ft in meeters
 const int ALARM_ALT = 2133; // altitude to key alarm on descent
 
+const int test_length = 20;
+const float test_right[test_length] = {0.2, 0.0, 0.0, 0.0, 0.4, 0.0, 0.0, 0.0, 0.6, 0.0, 0.0, 0.0, 0.8, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0};
+const float test_left[test_length] = {0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.4, 0.0, 0.0, 0.0, 0.6, 0.0, 0.0, 0.0, 0.8, 0.0, 0.0, 0.0, 1.0, 0.0};
+
 #endif
\ No newline at end of file

--- 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
     }
 }