Diff: main.cpp

Revision:

22:becb67846755

Parent:

21:8799ee63c2cd

Child:

23:5a7b5db55be5

--- a/main.cpp	Thu Dec 13 18:27:41 2012 +0000
+++ b/main.cpp	Thu Dec 13 23:08:10 2012 +0000
@@ -4,24 +4,11 @@
 #include "ff.h"
 #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.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.05; // stop turning if within distance_fudge km
+#include "config.h"
 
 I2C i2c(p9, p10); // sda, scl
 BMP085 alt_sensor(i2c);
 
-const float BAT_GPS_MUL = 15.51;
-const float BAT_MBED_MUL = 10.26;
-
 Serial pc(USBTX, USBRX);
 BufferedSerial gps(NC, p14);
 AnalogIn gps_battery(p20);
@@ -67,7 +54,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) {
@@ -137,7 +124,7 @@
     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.get_date(), nmea.get_time());
+    sprintf(message->line, "Date: %d, Time: %f\r\nGPS Time (UTC),GPS Battery(V),mbed Battery(V),BMP085 Temperature(C),Pressure,Altitude(ft),GPS Altitude, GPS Course\r\n", nmea.get_date(), nmea.get_time());
     queue_sensor_line.put(message);
 
     while(true) {
@@ -146,7 +133,7 @@
         sensor_line *message = mpool_sensor_line.alloc();
 
         //timestamp
-        time = t.read();
+        time = nmea.get_time();
 
         //gps battery
         gps_battery_voltage = gps_battery.read()*BAT_GPS_MUL;
@@ -159,7 +146,7 @@
         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);
+        sprintf(message->line, "%f,%f,%f,%f,%d,%f,%f,%f\r\n", time,gps_battery_voltage,mbed_battery_voltage,bmp_temperature,bmp_pressure,bmp_altitude,nmea.get_altitude_ft(),nmea.get_course_d());
         queue_sensor_line.put(message);
     }
 }
@@ -176,7 +163,7 @@
     while(true) {
         right_turn = left_turn = 0;
         parachute_sem.wait();
-        
+
         float distance = nmea.calc_dist_to_km(target_lat, target_lon);
 
         if(distance < distance_fudge_km)
@@ -185,17 +172,20 @@
         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 == 0.0) // not moving fast enough
+            continue; // do nothing
 
         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) {
-            right_turn = 1;
-            Thread::wait(400); // turn right
-        } else {
             left_turn = 1;
             Thread::wait(400); // turn left
+        } else {
+            right_turn = 1;
+            Thread::wait(400); // turn righ
         }
     }
 }