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:
- 19:1cfe22ef30e2
- Parent:
- 18:29b19a25a963
- Child:
- 20:60759c5af3eb
- Child:
- 21:8799ee63c2cd
--- a/main.cpp Wed Dec 12 18:13:11 2012 +0000 +++ b/main.cpp Thu Dec 13 06:54:16 2012 +0000 @@ -5,6 +5,17 @@ #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.63186; // for setting the target location! +const float target_lon = -105.028206; + +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 + I2C i2c(p9, p10); // sda, scl BMP085 alt_sensor(i2c); @@ -16,7 +27,7 @@ AnalogIn gps_battery(p20); AnalogIn mbed_battery(p19); -GPS_Parser nmea_parser; +GPS_Parser nmea; Semaphore parachute_sem(0); @@ -45,23 +56,31 @@ while(true) { gps_led = !gps_led; gps.read_line(buffer); - nmea_parser.sample(buffer); + int line_type = nmea.parse(buffer); //pc.puts(buffer); // send to log... gps_line *message = mpool_gps_line.alloc(); strcpy(message->line, buffer); queue_gps_line.put(message); + + // debugging + //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 - if(nmea_parser.get_lock()) - { - if(alt != 0) { // first time - alt = nmea_parser.get_msl_altitude(); + // test altitude direction - release parachute thread to run + if(line_type == RMC && nmea.get_lock()) { + if(UNLOCK_ON_FALL) { + if(alt != 0) { // first time + alt = nmea.get_msl_altitude(); + } else { + alt_prev = alt; + alt = nmea.get_msl_altitude(); + if(alt < alt_prev) // going down + parachute_sem.release(); // let the parachute code execute + } } else { - alt_prev = alt; - alt = nmea_parser.get_msl_altitude(); - if(alt < alt_prev) // going down - parachute_sem.release(); // let the parachute code execute + parachute_sem.release(); } } } @@ -86,17 +105,17 @@ f_puts(message->line, &fp_gps); f_sync(&fp_gps); - + mpool_gps_line.free(message); } - + osEvent evt2 = queue_sensor_line.get(1); if(evt2.status == osEventMessage) { sensor_line *message = (sensor_line*)evt2.value.p; - + f_puts(message->line, &fp_sensor); f_sync(&fp_sensor); - + mpool_sensor_line.free(message); } } @@ -110,35 +129,36 @@ float gps_battery_voltage, mbed_battery_voltage; float bmp_temperature, bmp_altitude; int bmp_pressure; - - //while(!nmea_parser.get_lock()) Thread::wait(100); // wait for lock - + + if(WAIT_FOR_LOCK) { + while(!nmea.get_date()) Thread::wait(100); // wait for lock + } + 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_parser.get_date(), nmea_parser.get_time()); + 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()); queue_sensor_line.put(message); - - while(true) - { + + while(true) { //get sensor line memory sensor_led = !sensor_led; sensor_line *message = mpool_sensor_line.alloc(); - + //timestamp time = t.read(); - + //gps battery gps_battery_voltage = gps_battery.read()*BAT_GPS_MUL; - + //mbed battery mbed_battery_voltage = mbed_battery.read()*BAT_MBED_MUL; - + //BMP085 bmp_temperature = (float)alt_sensor.get_temperature() / 10.0; 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); queue_sensor_line.put(message); } @@ -147,11 +167,25 @@ void parachute_thread(const void *args) { DigitalOut para_led(LED1); - - while(true) - { + + para_led = !para_led; + while(true) { parachute_sem.wait(); para_led = !para_led; + + if(nmea.calc_dist_to_km(target_lat, target_lon) < distance_fudge_km) + 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) + 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 } }