Diff: main.cpp

Revision:

19:1cfe22ef30e2

Parent:

18:29b19a25a963

Child:

20:60759c5af3eb

Child:

21:8799ee63c2cd

--- a/main.cpp	Wed Dec 12 18:13:11 2012 +0000
+++ b/main.cpp	Thu Dec 13 06:54:16 2012 +0000
@@ -5,6 +5,17 @@
 #include "BMP085.h"
 #include "GPS_parser.h"
 
+#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 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
+
 I2C i2c(p9, p10); // sda, scl
 BMP085 alt_sensor(i2c);
 
@@ -16,7 +27,7 @@
 AnalogIn gps_battery(p20);
 AnalogIn mbed_battery(p19);
 
-GPS_Parser nmea_parser;
+GPS_Parser nmea;
 
 Semaphore parachute_sem(0);
 
@@ -45,23 +56,31 @@
     while(true) {
         gps_led = !gps_led;
         gps.read_line(buffer);
-        nmea_parser.sample(buffer);
+        int line_type = nmea.parse(buffer);
         //pc.puts(buffer);
         // send to log...
         gps_line *message = mpool_gps_line.alloc();
         strcpy(message->line, buffer);
         queue_gps_line.put(message);
+
+        // debugging
+        //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
-        if(nmea_parser.get_lock())
-        {
-            if(alt != 0) { // first time
-                alt = nmea_parser.get_msl_altitude();
+        // test altitude direction - release parachute thread to run
+        if(line_type == RMC && nmea.get_lock()) {
+            if(UNLOCK_ON_FALL) {
+                if(alt != 0) { // first time
+                    alt = nmea.get_msl_altitude();
+                } else {
+                    alt_prev = alt;
+                    alt = nmea.get_msl_altitude();
+                    if(alt < alt_prev) // going down
+                        parachute_sem.release(); // let the parachute code execute
+                }
             } else {
-                alt_prev = alt;
-                alt = nmea_parser.get_msl_altitude();
-                if(alt < alt_prev) // going down
-                    parachute_sem.release(); // let the parachute code execute
+                parachute_sem.release();
             }
         }
     }
@@ -86,17 +105,17 @@
 
             f_puts(message->line, &fp_gps);
             f_sync(&fp_gps);
-            
+
             mpool_gps_line.free(message);
         }
-        
+
         osEvent evt2 = queue_sensor_line.get(1);
         if(evt2.status == osEventMessage) {
             sensor_line *message = (sensor_line*)evt2.value.p;
-            
+
             f_puts(message->line, &fp_sensor);
             f_sync(&fp_sensor);
-            
+
             mpool_sensor_line.free(message);
         }
     }
@@ -110,35 +129,36 @@
     float gps_battery_voltage, mbed_battery_voltage;
     float bmp_temperature, bmp_altitude;
     int bmp_pressure;
-    
-    //while(!nmea_parser.get_lock())  Thread::wait(100); // wait for lock
-    
+
+    if(WAIT_FOR_LOCK) {
+        while(!nmea.get_date())  Thread::wait(100); // wait for lock
+    }
+
     t.start(); // start timer after lock
-    
+
     sensor_line *message = mpool_sensor_line.alloc();
-    sprintf(message->line, "Date: %d, Time: %f\r\nTime(s),GPS Battery(V),mbed Battery(V),BMP085 Temperature(C),Pressure,Altitude(ft)\r\n", nmea_parser.get_date(), nmea_parser.get_time());
+    sprintf(message->line, "Date: %d, Time: %f\r\nTime(s),GPS Battery(V),mbed Battery(V),BMP085 Temperature(C),Pressure,Altitude(ft)\r\n", nmea.get_date(), nmea.get_time());
     queue_sensor_line.put(message);
-    
-    while(true)
-    {
+
+    while(true) {
         //get sensor line memory
         sensor_led = !sensor_led;
         sensor_line *message = mpool_sensor_line.alloc();
-        
+
         //timestamp
         time = t.read();
-        
+
         //gps battery
         gps_battery_voltage = gps_battery.read()*BAT_GPS_MUL;
-        
+
         //mbed battery
         mbed_battery_voltage = mbed_battery.read()*BAT_MBED_MUL;
-        
+
         //BMP085
         bmp_temperature = (float)alt_sensor.get_temperature() / 10.0;
         bmp_pressure = alt_sensor.get_pressure();
         bmp_altitude = alt_sensor.get_altitude_ft();
-                        
+
         sprintf(message->line, "%f,%f,%f,%f,%d,%f\r\n", time,gps_battery_voltage,mbed_battery_voltage,bmp_temperature,bmp_pressure,bmp_altitude);
         queue_sensor_line.put(message);
     }
@@ -147,11 +167,25 @@
 void parachute_thread(const void *args)
 {
     DigitalOut para_led(LED1);
-    
-    while(true) 
-    {
+
+    para_led = !para_led;
+    while(true) {
         parachute_sem.wait();
         para_led = !para_led;
+
+        if(nmea.calc_dist_to_km(target_lat, target_lon) < distance_fudge_km)
+            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)
+            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
     }
 }