Diff: main.cpp

Revision:

29:8cdb56a0fe57

Parent:

28:032d55fa57b8

Child:

30:b81274979e73

--- a/main.cpp	Thu Jan 10 19:25:13 2013 +0000
+++ b/main.cpp	Thu Jan 10 20:37:07 2013 +0000
@@ -21,6 +21,7 @@
 AnalogIn gps_battery(p20);
 AnalogIn mbed_battery(p16);
 TMP36GZ temperature(p17);
+DigitalOut alarm(p18);
 
 NmeaParser nmea;
 
@@ -42,6 +43,11 @@
 volatile float left_pos = 0.0; // servo position for logging
 volatile float right_pos = 0.0;
 
+volatile bool gps_wd = false;
+volatile bool sensor_wd = false;
+volatile bool log_wd = false;
+volatile bool parachute_wd = false;
+
 void gps_thread(void const *args)
 {
     char buffer[80];
@@ -65,18 +71,28 @@
         // test altitude direction - release parachute thread to run
         if(line_type == RMC && nmea.quality()) {
             if(UNLOCK_ON_FALL) {
-                if(alt != 0) { // first time
+                if(RELEASE_AT_ALT) {
+                    if(nmea.msl_altitude() >= RELEASE_ALT)
+                        parachute_sem.release();
+                }
+                if(alt == 0) { // first time
                     alt = nmea.msl_altitude();
                 } else {
                     alt_prev = alt;
                     alt = nmea.msl_altitude();
                     if(alt < alt_prev) // going down
+                    {
                         parachute_sem.release(); // let the parachute code execute
+                        if(ALARM && alt < ALARM_ALT)
+                            alarm = 1;
+                    }
                 }
             } else {
                 parachute_sem.release();
             }
         }
+        
+        gps_wd = true; // kick the watchdog
     }
 }
 
@@ -87,14 +103,16 @@
 
     DigitalOut log_led(LED3);
 
+    log_wd = true; // kick the dog
     f_mount(0, &fs);
     f_open(&fp_gps, "0:gps.txt", FA_OPEN_EXISTING | FA_WRITE);
     f_lseek(&fp_gps, f_size(&fp_gps));
     f_open(&fp_sensor, "0:sensors.csv", FA_OPEN_EXISTING | FA_WRITE);
     f_lseek(&fp_sensor, f_size(&fp_sensor));
+    log_wd = true; // kick the dog
 
     sd_sem.release(); // sd card initialized... start sensor thread
-    
+
     while(1) {
         log_led = !log_led;
         osEvent evt1 = queue_gps_line.get(1);
@@ -116,6 +134,8 @@
 
             mpool_sensor_line.free(message);
         }
+
+        log_wd = true; // kick the dog
     }
 }
 
@@ -127,27 +147,38 @@
     float gps_battery_voltage, mbed_battery_voltage;
     float temp;
     float distance, course_to, course;
-    
-    sd_sem.wait(); // wait for the sd card to initialize and open files
+
+    while( sd_sem.wait(50) <= 0) // wait for the sd card to initialize and open files
+        sensor_wd = true; // kick the dog
 
     if(WAIT_FOR_LOCK) {
-        while(!nmea.date())  Thread::wait(100); // wait for lock
+        while(!nmea.date()) {
+            Thread::wait(50); // wait for lock
+            sensor_wd = true; // kick the dog
+        }
     }
 
     t.start(); // start timer after lock
 
     sensor_line *message = mpool_sensor_line.alloc();
-    sprintf(message->line, "Date: %d, Time: %f\r\nGPS Time (UTC),GPS Battery(V),mbed Battery(V)", nmea.date(), nmea.utc_time());
+    sprintf(message->line, "Date: %d, Time: %f\r\nGPS Time (UTC),GPS Battery(V),mbed Battery(V),", nmea.date(), nmea.utc_time());
     queue_sensor_line.put(message);
-    
+
+    sensor_wd = true; // kick the dog
+
     message = mpool_sensor_line.alloc();
-    sprintf(message->line, ",Temperature,GPS Altitude,GPS Course,Course to Target,Distance,Left Servo,Right Servo \r\n");
+    sprintf(message->line, "Temperature,GPS Altitude,GPS Course,");
+    queue_sensor_line.put(message);
+
+    sensor_wd = true; // kick the dog
+
+    message = mpool_sensor_line.alloc();
+    sprintf(message->line, "Course to Target,Distance,Left Servo,Right Servo \r\n");
     queue_sensor_line.put(message);
 
     while(true) {
         //get sensor line memory
         sensor_led = !sensor_led;
-        sensor_line *message = mpool_sensor_line.alloc();
 
         //timestamp
         time = nmea.utc_time();
@@ -160,15 +191,21 @@
 
         //temperature
         temp = temperature.sample_f();
-        
+
         //gps
         distance = nmea.calc_dist_to_km(target_lat, target_lon);
         course_to = nmea.calc_initial_bearing(target_lat, target_lon);
         course = nmea.track();
 
-        sprintf(message->line, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\r\n", time,gps_battery_voltage,mbed_battery_voltage,temp,nmea.calc_altitude_ft(),course,course_to,distance,left_pos,right_pos);
+        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());
         queue_sensor_line.put(message);
-        
+
+        message = mpool_sensor_line.alloc();
+        sprintf(message->line, "%f,%f,%f,%f,%f\r\n", course,course_to,distance,left_pos,right_pos);
+        queue_sensor_line.put(message);
+
+        sensor_wd = true; // kick the dog
         Thread::wait(100);
     }
 }
@@ -177,17 +214,28 @@
 {
     DigitalOut left_turn_led(LED4);
     DigitalOut right_turn_led(LED1);
-    
+    bool is_released = false;
+
     // servos ////////////////////////// NOT TESTED!!! ///////////////////////////
     Servo left_s(p21);
     Servo right_s(p22);
+    Servo release_s(p23);
 
-    left_s.calibrate_max(0.0007);
-    left_s.calibrate_min(-0.0014);
-    right_s.calibrate(0.0009);
-    
+    left_s.calibrate_max(SERVO_L_MAX);
+    left_s.calibrate_min(SERVO_L_MIN);
+    right_s.calibrate_max(SERVO_R_MAX);
+    right_s.calibrate_min(SERVO_R_MIN);
+
+    // TODO: Calibrate release servo
+
     left_s = 0.0;
     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
+
     ////////////////////////////////////////////////////////////////////////////////
 
     right_turn_led = 1;
@@ -196,7 +244,11 @@
     Thread::wait(400);
     while(true) {
         right_turn_led = left_turn_led = 0;
-        parachute_sem.wait();
+        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
 
         float distance = nmea.calc_dist_to_km(target_lat, target_lon);
 
@@ -206,7 +258,7 @@
         float course = nmea.track();
         float course_to = nmea.calc_initial_bearing(target_lat, target_lon);
         float course_diff = course_to - course;
-        
+
         if(course == 0.0) // not moving fast enough
             continue; // do nothing
 
@@ -227,23 +279,35 @@
             Thread::wait(400); // turn righ
             right_s = right_pos = 0.0;
         }
+        parachute_wd = true; // kick the dog
+    }
+}
+
+void watchdog_thread(const void *args)
+{
+    Watchdog wdt; // NOT TESTED!!!
+
+    wdt.kick(2.0);
+
+    while(true) {
+        if(gps_wd && sensor_wd && parachute_wd && log_wd) {
+            wdt.kick();
+            gps_wd = sensor_wd = parachute_wd = log_wd = false;
+        } else {
+            Thread::wait(50);
+        }
     }
 }
 
 int main()
 {
-    pc.baud(9600);
     Thread thread(gps_thread, NULL, osPriorityHigh);
     Thread thread2(log_thread, NULL, osPriorityNormal);
     Thread thread3(sensor_thread, NULL, osPriorityNormal);
     Thread thread4(parachute_thread, NULL, osPriorityRealtime);
-    
-    Watchdog wdt; // NOT TESTED!!!
-    
-    wdt.kick(2.0);
+    Thread thread5(watchdog_thread, NULL, osPriorityHigh);
 
     while(true) {
-        wdt.kick();
-        Thread::wait(500);
+        Thread::wait(osWaitForever);
     }
 }
\ No newline at end of file