Revision 32:4f811b397720, committed 2013-01-21

Comitter:

tylerjw

Date:

Mon Jan 21 06:14:41 2013 +0000

Parent:

31:b9ac7d61b15b

Child:

33:61f0be6af3c0

Commit message:

low altitude flight

Changed in this revision

--- a/config.h	Thu Jan 17 18:09:16 2013 +0000
+++ b/config.h	Mon Jan 21 06:14:41 2013 +0000
@@ -3,13 +3,14 @@
 #ifndef HARP_CONFIG_H
 #define HARP_CONFIG_H
 
-#define WAIT_FOR_LOCK   0 // 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
-#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 
+#define WAIT_FOR_LOCK   1 // set to 1 to not open log file until gps lock
+#define UNLOCK_ON_FALL  1 // set to 1 to not signal parachute untill falling
+#define RELEASE_AT_ALT  1 // set to 1 for release at target altitude (must also set UNLOCK_ON_FALL to 1)
+#define ALARM           1 // set to 1 to key alarm on descent, 0 = no alarm 
+#define WATCHDOG        1 // set to 1 to enable watchdog
 
-const float target_lat = 39.910325;    // for setting the target location!
-const float target_lon = -105.004685;
+const float target_lat = 39.684303;    // for setting the target location!
+const float target_lon = -104.470882;
 
 const float course_fudge = 15.0; // if -course_fudge < course > course_fudge then don't turn
 const float neg_course_fudge = -15.0;
@@ -19,8 +20,8 @@
 const float BAT_GPS_MUL = 16.14; // values for calculating battery voltage
 const float BAT_MBED_MUL = 10.35;
 
-const float SERVO_L_MAX = 0.0007; // servo calibration
-const float SERVO_L_MIN = -0.0013; // servo calibration
+const float SERVO_L_MAX = 0.0008; // servo calibration
+const float SERVO_L_MIN = -0.0012; // servo calibration
 const float SERVO_R_MAX = 0.0006; // servo calibration
 const float SERVO_R_MIN = -0.0015; // servo calibration
 
@@ -28,10 +29,10 @@
 const float SERVO_RELEASE_MAX = 0.0004;
 
 const int RELEASE_ALT = 4572; // 15k ft in meeters
-const int ALARM_ALT = 2133; // altitude to key alarm on descent
+const int ALARM_ALT = 1981; // altitude to key alarm on descent (6500ft)
 
 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};
+const float test_left[test_length] = {1.0, 1.0, 0.8, 1.0, 1.0, 1.0, 0.6, 1.0, 1.0, 1.0, 0.4, 1.0, 1.0, 1.0, 0.2, 1.0, 1.0, 1.0, 0.0, 1.0};
 
 #endif
\ No newline at end of file

--- a/main.cpp	Thu Jan 17 18:09:16 2013 +0000
+++ b/main.cpp	Mon Jan 21 06:14:41 2013 +0000
@@ -52,6 +52,7 @@
     char buffer[80];
     float alt, alt_prev;
     alt = alt_prev = 0;
+    bool released = false;
 
     //DigitalOut gps_led(LED4);
 
@@ -69,28 +70,30 @@
 
         // test altitude direction - release parachute thread to run
         if(line_type == RMC && nmea.quality()) {
-            if(UNLOCK_ON_FALL) {
-                if(RELEASE_AT_ALT) {
-                    if(nmea.msl_altitude() >= RELEASE_ALT)
-                        parachute_sem.release();
+            if(RELEASE_AT_ALT) {
+                if(nmea.msl_altitude() >= RELEASE_ALT)
+                {
+                    parachute_sem.release();
+                    released = true;
                 }
+            }
+            if(UNLOCK_ON_FALL & released) {
                 if(alt == 0) { // first time
                     alt = nmea.msl_altitude();
                 } else {
                     alt_prev = alt;
                     alt = nmea.msl_altitude();
-                    if(alt < alt_prev) // going down
-                    {
+                    if(alt < alt_prev) { // going down
                         parachute_sem.release(); // let the parachute code execute
                         if(ALARM && alt < ALARM_ALT)
                             alarm = 1;
                     }
                 }
-            } else {
+            } else if(released) {
                 parachute_sem.release();
             }
         }
-        
+
         gps_wd = true; // kick the watchdog
     }
 }
@@ -146,7 +149,7 @@
     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
@@ -197,8 +200,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);
+
+        pc.printf("course: %4.1f course_to: %4.1f distance: %f, alt: %f\r\n", course, course_to, distance, nmea.msl_altitude());
 
         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());
@@ -231,7 +234,7 @@
     release_s.calibrate_max(SERVO_RELEASE_MAX);
     release_s.calibrate_min(SERVO_RELEASE_MIN);
 
-    left_s = 0.0;
+    left_s = 1.0;
     right_s = 0.0;
     release_s = 0.0;
 
@@ -241,23 +244,21 @@
     //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)
-        {
+        if(!is_released) {
             release_s = 1.0; // let go of the balloon
             is_released = true;
         }
-        
-        if(counter < test_length) // test flight -- (NOT Tested)
-        {
+
+        if(counter < test_length) { // test flight -- (NOT Tested)
             left_s = test_left[counter];
             right_s = test_right[counter];
             Thread::wait(1000);
@@ -266,11 +267,10 @@
             counter++;
             continue;
         }
-        
+
         float distance = nmea.calc_dist_to_km(target_lat, target_lon);
 
-        if(distance < distance_fudge_km)
-        {
+        if(distance < distance_fudge_km) {
             alarm = 1; // sound the alarm
             continue; // dont do anything
         }
@@ -286,15 +286,15 @@
             right_turn_led = left_turn_led = 1;
             Thread::wait(400);
             continue; // don't do anything
-        } else if(course_diff > 180.0 || course_diff < 0.0) {
+        } else if(course_diff > 180.0 || course_diff < 0.0) { // turn left
             left_turn_led = 1;
-            right_s = right_pos = 0.0;                                      
-            left_s = left_pos = 1.0;
+            right_s = right_pos = 0.0;
+            left_s = left_pos = 0.0;
             Thread::wait(400); // turn left
-            left_s = 0.0;
-        } else {
+            left_s = 1.0;
+        } else { // turn right
             right_turn_led = 1;
-            left_s = left_pos = 0.0;
+            left_s = left_pos = 1.0;
             right_s = right_pos = 1.0;
             Thread::wait(400); // turn righ
             right_s = right_pos = 0.0;
@@ -306,7 +306,7 @@
 {
     Watchdog wdt; // NOT TESTED!!!
 
-    wdt.kick(2.0);
+    wdt.kick(10.0);
 
     while(true) {
         if(gps_wd && sensor_wd && parachute_wd && log_wd) {
@@ -324,7 +324,8 @@
     Thread thread2(log_thread, NULL, osPriorityNormal);
     Thread thread3(sensor_thread, NULL, osPriorityNormal);
     Thread thread4(parachute_thread, NULL, osPriorityRealtime);
-    Thread thread5(watchdog_thread, NULL, osPriorityHigh);
+    if(WATCHDOG)
+        Thread thread5(watchdog_thread, NULL, osPriorityHigh);
 
     while(true) {
         Thread::wait(osWaitForever);