main.cpp

00001 /**
00002 * HARP Version 2
00003 *
00004 * TODO: Test Servo Code
00005 *       Test Watchdog Timer
00006 */
00007 
00008 #include "mbed.h"
00009 #include "rtos.h"
00010 #include "buffered_serial.h"
00011 #include "ff.h"
00012 #include "TMP36GZ.h"
00013 #include "nmea_parser.h"
00014 #include "watchdog.h"
00015 #include "Servo.h"
00016 #include "config.h"
00017 
00018 Serial pc(USBTX, USBRX);
00019 BufferedSerial gps;
00020 AnalogIn gps_battery(p20);
00021 AnalogIn mbed_battery(p16);
00022 TMP36GZ temperature(p17);
00023 DigitalOut alarm(p18);
00024 
00025 NmeaParser nmea;
00026 
00027 Semaphore parachute_sem(0);
00028 Semaphore sd_sem(0);
00029 
00030 typedef struct {
00031     char    line[80];
00032 } gps_line;
00033 MemoryPool<gps_line, 16> mpool_gps_line;
00034 Queue<gps_line, 16> queue_gps_line;
00035 
00036 typedef struct {
00037     char line[80];
00038 } sensor_line;
00039 MemoryPool<sensor_line, 16> mpool_sensor_line;
00040 Queue<sensor_line, 16> queue_sensor_line;
00041 
00042 volatile float left_pos = 0.0; // servo position for logging
00043 volatile float right_pos = 0.0;
00044 
00045 volatile bool gps_wd = false;
00046 volatile bool sensor_wd = false;
00047 volatile bool log_wd = false;
00048 volatile bool parachute_wd = false;
00049 
00050 void gps_thread(void const *args)
00051 {
00052     char buffer[80];
00053     float alt, alt_prev;
00054     alt = alt_prev = 0;
00055     bool released = false;
00056 
00057     //DigitalOut gps_led(LED4);
00058 
00059     gps.baud(4800);
00060 
00061     while(true) {
00062         //gps_led = !gps_led;
00063         gps.get_line(buffer);
00064         int line_type = nmea.parse(buffer);
00065         //pc.puts(buffer);
00066         // send to log...
00067         gps_line *message = mpool_gps_line.alloc();
00068         strcpy(message->line, buffer);
00069         queue_gps_line.put(message);
00070 
00071         // test altitude direction - release parachute thread to run
00072         if(line_type == RMC && nmea.quality()) {
00073             if(RELEASE_AT_ALT) {
00074                 if(nmea.msl_altitude() >= RELEASE_ALT)
00075                 {
00076                     parachute_sem.release();
00077                     released = true;
00078                 }
00079             }
00080             if(UNLOCK_ON_FALL & released) {
00081                 if(alt == 0) { // first time
00082                     alt = nmea.msl_altitude();
00083                 } else {
00084                     alt_prev = alt;
00085                     alt = nmea.msl_altitude();
00086                     if(alt < alt_prev) { // going down
00087                         parachute_sem.release(); // let the parachute code execute
00088                         if(ALARM && alt < ALARM_ALT)
00089                             alarm = 1;
00090                     }
00091                 }
00092             } else if(released) {
00093                 parachute_sem.release();
00094             }
00095         }
00096 
00097         gps_wd = true; // kick the watchdog
00098     }
00099 }
00100 
00101 void log_thread(const void *args)
00102 {
00103     FATFS fs;
00104     FIL fp_gps, fp_sensor;
00105 
00106     DigitalOut log_led(LED3);
00107 
00108     log_wd = true; // kick the dog
00109     f_mount(0, &fs);
00110     f_open(&fp_gps, "0:gps.txt", FA_OPEN_EXISTING | FA_WRITE);
00111     f_lseek(&fp_gps, f_size(&fp_gps));
00112     f_open(&fp_sensor, "0:sensors.csv", FA_OPEN_EXISTING | FA_WRITE);
00113     f_lseek(&fp_sensor, f_size(&fp_sensor));
00114     log_wd = true; // kick the dog
00115 
00116     sd_sem.release(); // sd card initialized... start sensor thread
00117 
00118     while(1) {
00119         log_led = !log_led;
00120         osEvent evt1 = queue_gps_line.get(1);
00121         if (evt1.status == osEventMessage) {
00122             gps_line *message = (gps_line*)evt1.value.p;
00123 
00124             f_puts(message->line, &fp_gps);
00125             f_sync(&fp_gps);
00126 
00127             mpool_gps_line.free(message);
00128         }
00129 
00130         osEvent evt2 = queue_sensor_line.get(1);
00131         if(evt2.status == osEventMessage) {
00132             sensor_line *message = (sensor_line*)evt2.value.p;
00133 
00134             f_puts(message->line, &fp_sensor);
00135             f_sync(&fp_sensor);
00136 
00137             mpool_sensor_line.free(message);
00138         }
00139 
00140         log_wd = true; // kick the dog
00141     }
00142 }
00143 
00144 void sensor_thread(const void* args)
00145 {
00146     DigitalOut sensor_led(LED2);
00147     Timer t;
00148     float time;
00149     float gps_battery_voltage, mbed_battery_voltage;
00150     float temp;
00151     float distance, course_to, course;
00152 
00153     pc.baud(9600);
00154 
00155     while( sd_sem.wait(50) <= 0) // wait for the sd card to initialize and open files
00156         sensor_wd = true; // kick the dog
00157 
00158     if(WAIT_FOR_LOCK) {
00159         while(!nmea.date()) {
00160             Thread::wait(50); // wait for lock
00161             sensor_wd = true; // kick the dog
00162         }
00163     }
00164 
00165     t.start(); // start timer after lock
00166 
00167     sensor_line *message = mpool_sensor_line.alloc();
00168     sprintf(message->line, "Date: %d, Time: %f\r\nGPS Time (UTC),GPS Battery(V),mbed Battery(V),", nmea.date(), nmea.utc_time());
00169     queue_sensor_line.put(message);
00170 
00171     sensor_wd = true; // kick the dog
00172 
00173     message = mpool_sensor_line.alloc();
00174     sprintf(message->line, "Temperature,GPS Altitude,GPS Course,");
00175     queue_sensor_line.put(message);
00176 
00177     sensor_wd = true; // kick the dog
00178 
00179     message = mpool_sensor_line.alloc();
00180     sprintf(message->line, "Course to Target,Distance,Left Servo,Right Servo \r\n");
00181     queue_sensor_line.put(message);
00182 
00183     while(true) {
00184         //get sensor line memory
00185         sensor_led = !sensor_led;
00186 
00187         //timestamp
00188         time = nmea.utc_time();
00189 
00190         //gps battery
00191         gps_battery_voltage = gps_battery.read()*BAT_GPS_MUL;
00192 
00193         //mbed battery
00194         mbed_battery_voltage = mbed_battery.read()*BAT_MBED_MUL;
00195 
00196         //temperature
00197         temp = temperature.sample_f();
00198 
00199         //gps
00200         distance = nmea.calc_dist_to_km(target_lat, target_lon);
00201         course_to = nmea.calc_initial_bearing(target_lat, target_lon);
00202         course = nmea.track();
00203 
00204         pc.printf("course: %4.1f course_to: %4.1f distance: %f, alt: %f\r\n", course, course_to, distance, nmea.msl_altitude());
00205 
00206         sensor_line *message = mpool_sensor_line.alloc();
00207         sprintf(message->line, "%f,%f,%f,%f,%f,", time,gps_battery_voltage,mbed_battery_voltage,temp,nmea.calc_altitude_ft());
00208         queue_sensor_line.put(message);
00209 
00210         message = mpool_sensor_line.alloc();
00211         sprintf(message->line, "%f,%f,%f,%f,%f\r\n", course,course_to,distance,left_pos,right_pos);
00212         queue_sensor_line.put(message);
00213 
00214         sensor_wd = true; // kick the dog
00215         Thread::wait(200);
00216     }
00217 }
00218 
00219 void parachute_thread(const void *args)
00220 {
00221     DigitalOut left_turn_led(LED4);
00222     DigitalOut right_turn_led(LED1);
00223     bool is_released = false;
00224 
00225     // servos ////////////////////////// NOT TESTED!!! ///////////////////////////
00226     Servo left_s(p21);
00227     Servo right_s(p22);
00228     Servo release_s(p23);
00229 
00230     left_s.calibrate_max(SERVO_L_MAX);
00231     left_s.calibrate_min(SERVO_L_MIN);
00232     right_s.calibrate_max(SERVO_R_MAX);
00233     right_s.calibrate_min(SERVO_R_MIN);
00234     release_s.calibrate_max(SERVO_RELEASE_MAX);
00235     release_s.calibrate_min(SERVO_RELEASE_MIN);
00236 
00237     left_s = 1.0;
00238     right_s = 0.0;
00239     release_s = 0.0;
00240 
00241     ////////////////////////////////////////////////////////////////////////////////
00242 
00243     //right_turn_led = 1;  // remove with watchdog on - test to determine which led is which
00244     //Thread::wait(1000);
00245     //left_turn_led = 1;
00246     //Thread::wait(1000);
00247 
00248     int counter = 0;
00249 
00250     while(true) {
00251         parachute_wd = true; // kick the dog
00252         right_turn_led = left_turn_led = 0;
00253         while( parachute_sem.wait(50) <= 0) // didn't get it (timeout)
00254             parachute_wd = true; // kick the dog
00255 
00256         if(!is_released) {
00257             release_s = 1.0; // let go of the balloon
00258             is_released = true;
00259         }
00260 
00261         if(counter < test_length) { // test flight -- (NOT Tested)
00262             left_s = test_left[counter];
00263             right_s = test_right[counter];
00264             Thread::wait(1000);
00265             parachute_wd = true; // kick the watchdog
00266             Thread::wait(1000);
00267             counter++;
00268             continue;
00269         }
00270 
00271         float distance = nmea.calc_dist_to_km(target_lat, target_lon);
00272 
00273         if(distance < distance_fudge_km) {
00274             alarm = 1; // sound the alarm
00275             continue; // dont do anything
00276         }
00277 
00278         float course = nmea.track();
00279         float course_to = nmea.calc_initial_bearing(target_lat, target_lon);
00280         float course_diff = course_to - course;
00281 
00282         if(course == 0.0) // not moving fast enough
00283             continue; // do nothing
00284 
00285         if(course_diff < course_fudge && course_diff > neg_course_fudge) {
00286             right_turn_led = left_turn_led = 1;
00287             Thread::wait(400);
00288             continue; // don't do anything
00289         } else if(course_diff > 180.0 || course_diff < 0.0) { // turn left
00290             left_turn_led = 1;
00291             right_s = right_pos = 0.0;
00292             left_s = left_pos = 0.0;
00293             Thread::wait(400); // turn left
00294             left_s = 1.0;
00295         } else { // turn right
00296             right_turn_led = 1;
00297             left_s = left_pos = 1.0;
00298             right_s = right_pos = 1.0;
00299             Thread::wait(400); // turn righ
00300             right_s = right_pos = 0.0;
00301         }
00302     }
00303 }
00304 
00305 void watchdog_thread(const void *args)
00306 {
00307     Watchdog wdt; // NOT TESTED!!!
00308 
00309     wdt.kick(10.0);
00310 
00311     while(true) {
00312         if(gps_wd && sensor_wd && parachute_wd && log_wd) {
00313             wdt.kick();
00314             gps_wd = sensor_wd = parachute_wd = log_wd = false;
00315         } else {
00316             Thread::wait(50);
00317         }
00318     }
00319 }
00320 
00321 int main()
00322 {
00323     Thread thread(gps_thread, NULL, osPriorityHigh);
00324     Thread thread2(log_thread, NULL, osPriorityNormal);
00325     Thread thread3(sensor_thread, NULL, osPriorityNormal);
00326     Thread thread4(parachute_thread, NULL, osPriorityRealtime);
00327     if(WATCHDOG)
00328         Thread thread5(watchdog_thread, NULL, osPriorityHigh);
00329 
00330     while(true) {
00331         Thread::wait(osWaitForever);
00332     }
00333 }