Generation 3 of the Harp project
Dependencies: Servo TMP36 GZ buffered-serial1 chan_fatfs_sd nmea_parser watchdog mbed-rtos mbed
Fork of HARP2 by
Diff: main.cpp
- Revision:
- 20:60759c5af3eb
- Parent:
- 19:1cfe22ef30e2
--- a/main.cpp Thu Dec 13 06:54:16 2012 +0000 +++ b/main.cpp Thu Dec 13 18:20:56 2012 +0000 @@ -8,13 +8,13 @@ #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 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.1; // stop turning if within distance_fudge km +const float distance_fudge_km = 0.05; // stop turning if within distance_fudge km I2C i2c(p9, p10); // sda, scl BMP085 alt_sensor(i2c); @@ -49,12 +49,12 @@ float alt, alt_prev; alt = alt_prev = 0; - DigitalOut gps_led(LED4); + //DigitalOut gps_led(LED4); gps.baud(4800); while(true) { - gps_led = !gps_led; + //gps_led = !gps_led; gps.read_line(buffer); int line_type = nmea.parse(buffer); //pc.puts(buffer); @@ -67,7 +67,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) { @@ -89,13 +89,14 @@ void log_thread(const void *args) { FATFS fs; - FIL fp_gps, fp_sensor; + FIL fp_gps, fp_sensor, fp_para; DigitalOut log_led(LED3); f_mount(0, &fs); f_open(&fp_gps, "0:gps.txt", FA_CREATE_ALWAYS | FA_WRITE); f_open(&fp_sensor, "0:sensors.csv", FA_CREATE_ALWAYS | FA_WRITE); + f_open(&fp_para, "0:para.csv", FA_CREATE_ALWAYS | FA_WRITE); while(1) { log_led = !log_led; @@ -118,6 +119,8 @@ mpool_sensor_line.free(message); } + + } } @@ -131,7 +134,7 @@ int bmp_pressure; if(WAIT_FOR_LOCK) { - while(!nmea.get_date()) Thread::wait(100); // wait for lock + while(!nmea.get_date() || !nmea.get_time() || !nmea.get_lock()) Thread::wait(100); // wait for lock } t.start(); // start timer after lock @@ -166,26 +169,47 @@ void parachute_thread(const void *args) { - DigitalOut para_led(LED1); + DigitalOut left_turn(LED4); + DigitalOut right_turn(LED1); + + - para_led = !para_led; + right_turn = 1; + Thread::wait(400); + left_turn = 1; + Thread::wait(400); while(true) { + right_turn = left_turn = 0; parachute_sem.wait(); - para_led = !para_led; - - if(nmea.calc_dist_to_km(target_lat, target_lon) < distance_fudge_km) + float distance; + if((distance = nmea.calc_dist_to_km(target_lat, target_lon)) < distance_fudge_km) { + Thread::wait(100); + right_turn = left_turn = 1; + Thread::wait(100); + right_turn = left_turn = 0; + Thread::wait(100); + right_turn = left_turn = 1; + Thread::wait(100); 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) + 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) - Thread::wait(200); // turn right - else - Thread::wait(200); // turn left + } 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 + } + + } }