Diff: main.cpp

Revision:

20:60759c5af3eb

Parent:

19:1cfe22ef30e2

--- a/main.cpp	Thu Dec 13 06:54:16 2012 +0000
+++ b/main.cpp	Thu Dec 13 18:20:56 2012 +0000
@@ -8,13 +8,13 @@
 #define WAIT_FOR_LOCK   1 // set to 1 to not open log file until gps lock
 #define UNLOCK_ON_FALL  0 // set to 1 to not signal parachute untill falling
 
-const float target_lat = 39.63186;    // for setting the target location!
-const float target_lon = -105.028206;
+const float target_lat = 39.920936;    // for setting the target location!
+const float target_lon = -105.009991;
 
 const float course_fudge = 5.0; // if -course_fudge < course > course_fudge then don't turn
 const float neg_course_fudge = -5.0;
 
-const float distance_fudge_km = 0.1; // stop turning if within distance_fudge km
+const float distance_fudge_km = 0.05; // stop turning if within distance_fudge km
 
 I2C i2c(p9, p10); // sda, scl
 BMP085 alt_sensor(i2c);
@@ -49,12 +49,12 @@
     float alt, alt_prev;
     alt = alt_prev = 0;
 
-    DigitalOut gps_led(LED4);
+    //DigitalOut gps_led(LED4);
 
     gps.baud(4800);
 
     while(true) {
-        gps_led = !gps_led;
+        //gps_led = !gps_led;
         gps.read_line(buffer);
         int line_type = nmea.parse(buffer);
         //pc.puts(buffer);
@@ -67,7 +67,7 @@
         //pc.printf("%d, %f, %f, %f\r\n", nmea.get_date(), nmea.get_time(), nmea.get_msl_altitude(), nmea.get_altitude_ft());
         //pc.printf("%f, %f\r\n", nmea.get_dec_longitude(), nmea.get_dec_latitude());
         //pc.printf("%f, %f, %f\r\n", nmea.calc_dist_to_mi(lat,lon), nmea.calc_dist_to_km(lat,lon), nmea.calc_course_to(lat,lon));
-        
+
         // test altitude direction - release parachute thread to run
         if(line_type == RMC && nmea.get_lock()) {
             if(UNLOCK_ON_FALL) {
@@ -89,13 +89,14 @@
 void log_thread(const void *args)
 {
     FATFS fs;
-    FIL fp_gps, fp_sensor;
+    FIL fp_gps, fp_sensor, fp_para;
 
     DigitalOut log_led(LED3);
 
     f_mount(0, &fs);
     f_open(&fp_gps, "0:gps.txt", FA_CREATE_ALWAYS | FA_WRITE);
     f_open(&fp_sensor, "0:sensors.csv", FA_CREATE_ALWAYS | FA_WRITE);
+    f_open(&fp_para, "0:para.csv", FA_CREATE_ALWAYS | FA_WRITE);
 
     while(1) {
         log_led = !log_led;
@@ -118,6 +119,8 @@
 
             mpool_sensor_line.free(message);
         }
+
+        
     }
 }
 
@@ -131,7 +134,7 @@
     int bmp_pressure;
 
     if(WAIT_FOR_LOCK) {
-        while(!nmea.get_date())  Thread::wait(100); // wait for lock
+        while(!nmea.get_date() || !nmea.get_time() || !nmea.get_lock())  Thread::wait(100); // wait for lock
     }
 
     t.start(); // start timer after lock
@@ -166,26 +169,47 @@
 
 void parachute_thread(const void *args)
 {
-    DigitalOut para_led(LED1);
+    DigitalOut left_turn(LED4);
+    DigitalOut right_turn(LED1);
+
+
 
-    para_led = !para_led;
+    right_turn = 1;
+    Thread::wait(400);
+    left_turn = 1;
+    Thread::wait(400);
     while(true) {
+        right_turn = left_turn = 0;
         parachute_sem.wait();
-        para_led = !para_led;
-
-        if(nmea.calc_dist_to_km(target_lat, target_lon) < distance_fudge_km)
+        float distance;
+        if((distance = nmea.calc_dist_to_km(target_lat, target_lon)) < distance_fudge_km) {
+            Thread::wait(100);
+            right_turn = left_turn = 1;
+            Thread::wait(100);
+            right_turn = left_turn = 0;
+            Thread::wait(100);
+            right_turn = left_turn = 1;
+            Thread::wait(100);
             continue; // dont do anything
+        }
 
         float course = nmea.get_course_d();
         float course_to = nmea.calc_course_to(target_lat, target_lon);
         float course_diff = course_to - course;
 
-        if(course_diff < course_fudge && course_diff > neg_course_fudge)
+        if(course_diff < course_fudge && course_diff > neg_course_fudge) {
+            right_turn = left_turn = 1;
+            Thread::wait(400);
             continue; // don't do anything
-        else if(course_diff > 180.0 || course_diff < 0.0)
-            Thread::wait(200); // turn right
-        else
-            Thread::wait(200); // turn left
+        } else if(course_diff > 180.0 || course_diff < 0.0) {
+            right_turn = 1;
+            Thread::wait(400); // turn right
+        } else {
+            left_turn = 1;
+            Thread::wait(400); // turn left
+        }
+
+
     }
 }