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:
- 22:becb67846755
- Parent:
- 21:8799ee63c2cd
- Child:
- 23:5a7b5db55be5
--- a/main.cpp Thu Dec 13 18:27:41 2012 +0000 +++ b/main.cpp Thu Dec 13 23:08:10 2012 +0000 @@ -4,24 +4,11 @@ #include "ff.h" #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.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.05; // stop turning if within distance_fudge km +#include "config.h" I2C i2c(p9, p10); // sda, scl BMP085 alt_sensor(i2c); -const float BAT_GPS_MUL = 15.51; -const float BAT_MBED_MUL = 10.26; - Serial pc(USBTX, USBRX); BufferedSerial gps(NC, p14); AnalogIn gps_battery(p20); @@ -67,7 +54,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) { @@ -137,7 +124,7 @@ 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.get_date(), nmea.get_time()); + sprintf(message->line, "Date: %d, Time: %f\r\nGPS Time (UTC),GPS Battery(V),mbed Battery(V),BMP085 Temperature(C),Pressure,Altitude(ft),GPS Altitude, GPS Course\r\n", nmea.get_date(), nmea.get_time()); queue_sensor_line.put(message); while(true) { @@ -146,7 +133,7 @@ sensor_line *message = mpool_sensor_line.alloc(); //timestamp - time = t.read(); + time = nmea.get_time(); //gps battery gps_battery_voltage = gps_battery.read()*BAT_GPS_MUL; @@ -159,7 +146,7 @@ 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); + sprintf(message->line, "%f,%f,%f,%f,%d,%f,%f,%f\r\n", time,gps_battery_voltage,mbed_battery_voltage,bmp_temperature,bmp_pressure,bmp_altitude,nmea.get_altitude_ft(),nmea.get_course_d()); queue_sensor_line.put(message); } } @@ -176,7 +163,7 @@ while(true) { right_turn = left_turn = 0; parachute_sem.wait(); - + float distance = nmea.calc_dist_to_km(target_lat, target_lon); if(distance < distance_fudge_km) @@ -185,17 +172,20 @@ 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 == 0.0) // not moving fast enough + continue; // do nothing 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) { - right_turn = 1; - Thread::wait(400); // turn right - } else { left_turn = 1; Thread::wait(400); // turn left + } else { + right_turn = 1; + Thread::wait(400); // turn righ } } }