Generation 2 of the Harp project
Dependencies: Servo TMP36 GZ chan_fatfs_sd buffered-serial1 nmea_parser watchdog mbed-rtos mbed
main.cpp@20:60759c5af3eb, 2012-12-13 (annotated)
- Committer:
- tylerjw
- Date:
- Thu Dec 13 18:20:56 2012 +0000
- Revision:
- 20:60759c5af3eb
- Parent:
- 19:1cfe22ef30e2
not working, blue lights of death
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
tylerjw | 0:ce5f06c3895f | 1 | #include "mbed.h" |
tylerjw | 7:d8ecabe16c9e | 2 | #include "rtos.h" |
tylerjw | 10:b13416bbb4cd | 3 | #include "buffered_serial.h" |
tylerjw | 12:0d943d69d3ec | 4 | #include "ff.h" |
tylerjw | 15:ceac642f6b75 | 5 | #include "BMP085.h" |
tylerjw | 17:4927053e120f | 6 | #include "GPS_parser.h" |
tylerjw | 15:ceac642f6b75 | 7 | |
tylerjw | 19:1cfe22ef30e2 | 8 | #define WAIT_FOR_LOCK 1 // set to 1 to not open log file until gps lock |
tylerjw | 19:1cfe22ef30e2 | 9 | #define UNLOCK_ON_FALL 0 // set to 1 to not signal parachute untill falling |
tylerjw | 19:1cfe22ef30e2 | 10 | |
tylerjw | 20:60759c5af3eb | 11 | const float target_lat = 39.920936; // for setting the target location! |
tylerjw | 20:60759c5af3eb | 12 | const float target_lon = -105.009991; |
tylerjw | 19:1cfe22ef30e2 | 13 | |
tylerjw | 19:1cfe22ef30e2 | 14 | const float course_fudge = 5.0; // if -course_fudge < course > course_fudge then don't turn |
tylerjw | 19:1cfe22ef30e2 | 15 | const float neg_course_fudge = -5.0; |
tylerjw | 19:1cfe22ef30e2 | 16 | |
tylerjw | 20:60759c5af3eb | 17 | const float distance_fudge_km = 0.05; // stop turning if within distance_fudge km |
tylerjw | 19:1cfe22ef30e2 | 18 | |
tylerjw | 15:ceac642f6b75 | 19 | I2C i2c(p9, p10); // sda, scl |
tylerjw | 15:ceac642f6b75 | 20 | BMP085 alt_sensor(i2c); |
tylerjw | 15:ceac642f6b75 | 21 | |
tylerjw | 16:653df0cfe6ee | 22 | const float BAT_GPS_MUL = 15.51; |
tylerjw | 16:653df0cfe6ee | 23 | const float BAT_MBED_MUL = 10.26; |
tylerjw | 0:ce5f06c3895f | 24 | |
tylerjw | 0:ce5f06c3895f | 25 | Serial pc(USBTX, USBRX); |
tylerjw | 10:b13416bbb4cd | 26 | BufferedSerial gps(NC, p14); |
tylerjw | 10:b13416bbb4cd | 27 | AnalogIn gps_battery(p20); |
tylerjw | 16:653df0cfe6ee | 28 | AnalogIn mbed_battery(p19); |
tylerjw | 7:d8ecabe16c9e | 29 | |
tylerjw | 19:1cfe22ef30e2 | 30 | GPS_Parser nmea; |
tylerjw | 17:4927053e120f | 31 | |
tylerjw | 18:29b19a25a963 | 32 | Semaphore parachute_sem(0); |
tylerjw | 18:29b19a25a963 | 33 | |
tylerjw | 13:db6af0620264 | 34 | typedef struct { |
tylerjw | 13:db6af0620264 | 35 | char line[80]; |
tylerjw | 13:db6af0620264 | 36 | } gps_line; |
tylerjw | 13:db6af0620264 | 37 | MemoryPool<gps_line, 16> mpool_gps_line; |
tylerjw | 13:db6af0620264 | 38 | Queue<gps_line, 16> queue_gps_line; |
tylerjw | 13:db6af0620264 | 39 | |
tylerjw | 13:db6af0620264 | 40 | typedef struct { |
tylerjw | 14:dce4d8c29b17 | 41 | char line[80]; |
tylerjw | 13:db6af0620264 | 42 | } sensor_line; |
tylerjw | 13:db6af0620264 | 43 | MemoryPool<sensor_line, 16> mpool_sensor_line; |
tylerjw | 13:db6af0620264 | 44 | Queue<sensor_line, 16> queue_sensor_line; |
tylerjw | 12:0d943d69d3ec | 45 | |
tylerjw | 7:d8ecabe16c9e | 46 | void gps_thread(void const *args) |
tylerjw | 7:d8ecabe16c9e | 47 | { |
tylerjw | 10:b13416bbb4cd | 48 | char buffer[80]; |
tylerjw | 18:29b19a25a963 | 49 | float alt, alt_prev; |
tylerjw | 18:29b19a25a963 | 50 | alt = alt_prev = 0; |
tylerjw | 9:4debfbc1fb3e | 51 | |
tylerjw | 20:60759c5af3eb | 52 | //DigitalOut gps_led(LED4); |
tylerjw | 7:d8ecabe16c9e | 53 | |
tylerjw | 7:d8ecabe16c9e | 54 | gps.baud(4800); |
tylerjw | 7:d8ecabe16c9e | 55 | |
tylerjw | 7:d8ecabe16c9e | 56 | while(true) { |
tylerjw | 20:60759c5af3eb | 57 | //gps_led = !gps_led; |
tylerjw | 10:b13416bbb4cd | 58 | gps.read_line(buffer); |
tylerjw | 19:1cfe22ef30e2 | 59 | int line_type = nmea.parse(buffer); |
tylerjw | 14:dce4d8c29b17 | 60 | //pc.puts(buffer); |
tylerjw | 13:db6af0620264 | 61 | // send to log... |
tylerjw | 13:db6af0620264 | 62 | gps_line *message = mpool_gps_line.alloc(); |
tylerjw | 13:db6af0620264 | 63 | strcpy(message->line, buffer); |
tylerjw | 13:db6af0620264 | 64 | queue_gps_line.put(message); |
tylerjw | 19:1cfe22ef30e2 | 65 | |
tylerjw | 19:1cfe22ef30e2 | 66 | // debugging |
tylerjw | 19:1cfe22ef30e2 | 67 | //pc.printf("%d, %f, %f, %f\r\n", nmea.get_date(), nmea.get_time(), nmea.get_msl_altitude(), nmea.get_altitude_ft()); |
tylerjw | 19:1cfe22ef30e2 | 68 | //pc.printf("%f, %f\r\n", nmea.get_dec_longitude(), nmea.get_dec_latitude()); |
tylerjw | 19:1cfe22ef30e2 | 69 | //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)); |
tylerjw | 20:60759c5af3eb | 70 | |
tylerjw | 19:1cfe22ef30e2 | 71 | // test altitude direction - release parachute thread to run |
tylerjw | 19:1cfe22ef30e2 | 72 | if(line_type == RMC && nmea.get_lock()) { |
tylerjw | 19:1cfe22ef30e2 | 73 | if(UNLOCK_ON_FALL) { |
tylerjw | 19:1cfe22ef30e2 | 74 | if(alt != 0) { // first time |
tylerjw | 19:1cfe22ef30e2 | 75 | alt = nmea.get_msl_altitude(); |
tylerjw | 19:1cfe22ef30e2 | 76 | } else { |
tylerjw | 19:1cfe22ef30e2 | 77 | alt_prev = alt; |
tylerjw | 19:1cfe22ef30e2 | 78 | alt = nmea.get_msl_altitude(); |
tylerjw | 19:1cfe22ef30e2 | 79 | if(alt < alt_prev) // going down |
tylerjw | 19:1cfe22ef30e2 | 80 | parachute_sem.release(); // let the parachute code execute |
tylerjw | 19:1cfe22ef30e2 | 81 | } |
tylerjw | 18:29b19a25a963 | 82 | } else { |
tylerjw | 19:1cfe22ef30e2 | 83 | parachute_sem.release(); |
tylerjw | 18:29b19a25a963 | 84 | } |
tylerjw | 18:29b19a25a963 | 85 | } |
tylerjw | 13:db6af0620264 | 86 | } |
tylerjw | 13:db6af0620264 | 87 | } |
tylerjw | 13:db6af0620264 | 88 | |
tylerjw | 13:db6af0620264 | 89 | void log_thread(const void *args) |
tylerjw | 13:db6af0620264 | 90 | { |
tylerjw | 13:db6af0620264 | 91 | FATFS fs; |
tylerjw | 20:60759c5af3eb | 92 | FIL fp_gps, fp_sensor, fp_para; |
tylerjw | 13:db6af0620264 | 93 | |
tylerjw | 13:db6af0620264 | 94 | DigitalOut log_led(LED3); |
tylerjw | 13:db6af0620264 | 95 | |
tylerjw | 13:db6af0620264 | 96 | f_mount(0, &fs); |
tylerjw | 13:db6af0620264 | 97 | f_open(&fp_gps, "0:gps.txt", FA_CREATE_ALWAYS | FA_WRITE); |
tylerjw | 17:4927053e120f | 98 | f_open(&fp_sensor, "0:sensors.csv", FA_CREATE_ALWAYS | FA_WRITE); |
tylerjw | 20:60759c5af3eb | 99 | f_open(&fp_para, "0:para.csv", FA_CREATE_ALWAYS | FA_WRITE); |
tylerjw | 13:db6af0620264 | 100 | |
tylerjw | 13:db6af0620264 | 101 | while(1) { |
tylerjw | 18:29b19a25a963 | 102 | log_led = !log_led; |
tylerjw | 14:dce4d8c29b17 | 103 | osEvent evt1 = queue_gps_line.get(1); |
tylerjw | 14:dce4d8c29b17 | 104 | if (evt1.status == osEventMessage) { |
tylerjw | 14:dce4d8c29b17 | 105 | gps_line *message = (gps_line*)evt1.value.p; |
tylerjw | 13:db6af0620264 | 106 | |
tylerjw | 13:db6af0620264 | 107 | f_puts(message->line, &fp_gps); |
tylerjw | 13:db6af0620264 | 108 | f_sync(&fp_gps); |
tylerjw | 19:1cfe22ef30e2 | 109 | |
tylerjw | 13:db6af0620264 | 110 | mpool_gps_line.free(message); |
tylerjw | 13:db6af0620264 | 111 | } |
tylerjw | 19:1cfe22ef30e2 | 112 | |
tylerjw | 14:dce4d8c29b17 | 113 | osEvent evt2 = queue_sensor_line.get(1); |
tylerjw | 14:dce4d8c29b17 | 114 | if(evt2.status == osEventMessage) { |
tylerjw | 14:dce4d8c29b17 | 115 | sensor_line *message = (sensor_line*)evt2.value.p; |
tylerjw | 19:1cfe22ef30e2 | 116 | |
tylerjw | 14:dce4d8c29b17 | 117 | f_puts(message->line, &fp_sensor); |
tylerjw | 14:dce4d8c29b17 | 118 | f_sync(&fp_sensor); |
tylerjw | 19:1cfe22ef30e2 | 119 | |
tylerjw | 13:db6af0620264 | 120 | mpool_sensor_line.free(message); |
tylerjw | 13:db6af0620264 | 121 | } |
tylerjw | 20:60759c5af3eb | 122 | |
tylerjw | 20:60759c5af3eb | 123 | |
tylerjw | 13:db6af0620264 | 124 | } |
tylerjw | 13:db6af0620264 | 125 | } |
tylerjw | 13:db6af0620264 | 126 | |
tylerjw | 13:db6af0620264 | 127 | void sensor_thread(const void* args) |
tylerjw | 13:db6af0620264 | 128 | { |
tylerjw | 14:dce4d8c29b17 | 129 | DigitalOut sensor_led(LED2); |
tylerjw | 16:653df0cfe6ee | 130 | Timer t; |
tylerjw | 18:29b19a25a963 | 131 | float time; |
tylerjw | 18:29b19a25a963 | 132 | float gps_battery_voltage, mbed_battery_voltage; |
tylerjw | 18:29b19a25a963 | 133 | float bmp_temperature, bmp_altitude; |
tylerjw | 18:29b19a25a963 | 134 | int bmp_pressure; |
tylerjw | 19:1cfe22ef30e2 | 135 | |
tylerjw | 19:1cfe22ef30e2 | 136 | if(WAIT_FOR_LOCK) { |
tylerjw | 20:60759c5af3eb | 137 | while(!nmea.get_date() || !nmea.get_time() || !nmea.get_lock()) Thread::wait(100); // wait for lock |
tylerjw | 19:1cfe22ef30e2 | 138 | } |
tylerjw | 19:1cfe22ef30e2 | 139 | |
tylerjw | 17:4927053e120f | 140 | t.start(); // start timer after lock |
tylerjw | 19:1cfe22ef30e2 | 141 | |
tylerjw | 14:dce4d8c29b17 | 142 | sensor_line *message = mpool_sensor_line.alloc(); |
tylerjw | 19:1cfe22ef30e2 | 143 | 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()); |
tylerjw | 14:dce4d8c29b17 | 144 | queue_sensor_line.put(message); |
tylerjw | 19:1cfe22ef30e2 | 145 | |
tylerjw | 19:1cfe22ef30e2 | 146 | while(true) { |
tylerjw | 13:db6af0620264 | 147 | //get sensor line memory |
tylerjw | 18:29b19a25a963 | 148 | sensor_led = !sensor_led; |
tylerjw | 13:db6af0620264 | 149 | sensor_line *message = mpool_sensor_line.alloc(); |
tylerjw | 19:1cfe22ef30e2 | 150 | |
tylerjw | 16:653df0cfe6ee | 151 | //timestamp |
tylerjw | 18:29b19a25a963 | 152 | time = t.read(); |
tylerjw | 19:1cfe22ef30e2 | 153 | |
tylerjw | 13:db6af0620264 | 154 | //gps battery |
tylerjw | 18:29b19a25a963 | 155 | gps_battery_voltage = gps_battery.read()*BAT_GPS_MUL; |
tylerjw | 19:1cfe22ef30e2 | 156 | |
tylerjw | 16:653df0cfe6ee | 157 | //mbed battery |
tylerjw | 18:29b19a25a963 | 158 | mbed_battery_voltage = mbed_battery.read()*BAT_MBED_MUL; |
tylerjw | 19:1cfe22ef30e2 | 159 | |
tylerjw | 15:ceac642f6b75 | 160 | //BMP085 |
tylerjw | 18:29b19a25a963 | 161 | bmp_temperature = (float)alt_sensor.get_temperature() / 10.0; |
tylerjw | 18:29b19a25a963 | 162 | bmp_pressure = alt_sensor.get_pressure(); |
tylerjw | 18:29b19a25a963 | 163 | bmp_altitude = alt_sensor.get_altitude_ft(); |
tylerjw | 19:1cfe22ef30e2 | 164 | |
tylerjw | 16:653df0cfe6ee | 165 | sprintf(message->line, "%f,%f,%f,%f,%d,%f\r\n", time,gps_battery_voltage,mbed_battery_voltage,bmp_temperature,bmp_pressure,bmp_altitude); |
tylerjw | 14:dce4d8c29b17 | 166 | queue_sensor_line.put(message); |
tylerjw | 0:ce5f06c3895f | 167 | } |
tylerjw | 7:d8ecabe16c9e | 168 | } |
tylerjw | 7:d8ecabe16c9e | 169 | |
tylerjw | 17:4927053e120f | 170 | void parachute_thread(const void *args) |
tylerjw | 17:4927053e120f | 171 | { |
tylerjw | 20:60759c5af3eb | 172 | DigitalOut left_turn(LED4); |
tylerjw | 20:60759c5af3eb | 173 | DigitalOut right_turn(LED1); |
tylerjw | 20:60759c5af3eb | 174 | |
tylerjw | 20:60759c5af3eb | 175 | |
tylerjw | 19:1cfe22ef30e2 | 176 | |
tylerjw | 20:60759c5af3eb | 177 | right_turn = 1; |
tylerjw | 20:60759c5af3eb | 178 | Thread::wait(400); |
tylerjw | 20:60759c5af3eb | 179 | left_turn = 1; |
tylerjw | 20:60759c5af3eb | 180 | Thread::wait(400); |
tylerjw | 19:1cfe22ef30e2 | 181 | while(true) { |
tylerjw | 20:60759c5af3eb | 182 | right_turn = left_turn = 0; |
tylerjw | 18:29b19a25a963 | 183 | parachute_sem.wait(); |
tylerjw | 20:60759c5af3eb | 184 | float distance; |
tylerjw | 20:60759c5af3eb | 185 | if((distance = nmea.calc_dist_to_km(target_lat, target_lon)) < distance_fudge_km) { |
tylerjw | 20:60759c5af3eb | 186 | Thread::wait(100); |
tylerjw | 20:60759c5af3eb | 187 | right_turn = left_turn = 1; |
tylerjw | 20:60759c5af3eb | 188 | Thread::wait(100); |
tylerjw | 20:60759c5af3eb | 189 | right_turn = left_turn = 0; |
tylerjw | 20:60759c5af3eb | 190 | Thread::wait(100); |
tylerjw | 20:60759c5af3eb | 191 | right_turn = left_turn = 1; |
tylerjw | 20:60759c5af3eb | 192 | Thread::wait(100); |
tylerjw | 19:1cfe22ef30e2 | 193 | continue; // dont do anything |
tylerjw | 20:60759c5af3eb | 194 | } |
tylerjw | 19:1cfe22ef30e2 | 195 | |
tylerjw | 19:1cfe22ef30e2 | 196 | float course = nmea.get_course_d(); |
tylerjw | 19:1cfe22ef30e2 | 197 | float course_to = nmea.calc_course_to(target_lat, target_lon); |
tylerjw | 19:1cfe22ef30e2 | 198 | float course_diff = course_to - course; |
tylerjw | 19:1cfe22ef30e2 | 199 | |
tylerjw | 20:60759c5af3eb | 200 | if(course_diff < course_fudge && course_diff > neg_course_fudge) { |
tylerjw | 20:60759c5af3eb | 201 | right_turn = left_turn = 1; |
tylerjw | 20:60759c5af3eb | 202 | Thread::wait(400); |
tylerjw | 19:1cfe22ef30e2 | 203 | continue; // don't do anything |
tylerjw | 20:60759c5af3eb | 204 | } else if(course_diff > 180.0 || course_diff < 0.0) { |
tylerjw | 20:60759c5af3eb | 205 | right_turn = 1; |
tylerjw | 20:60759c5af3eb | 206 | Thread::wait(400); // turn right |
tylerjw | 20:60759c5af3eb | 207 | } else { |
tylerjw | 20:60759c5af3eb | 208 | left_turn = 1; |
tylerjw | 20:60759c5af3eb | 209 | Thread::wait(400); // turn left |
tylerjw | 20:60759c5af3eb | 210 | } |
tylerjw | 20:60759c5af3eb | 211 | |
tylerjw | 20:60759c5af3eb | 212 | |
tylerjw | 18:29b19a25a963 | 213 | } |
tylerjw | 17:4927053e120f | 214 | } |
tylerjw | 17:4927053e120f | 215 | |
tylerjw | 7:d8ecabe16c9e | 216 | int main() |
tylerjw | 7:d8ecabe16c9e | 217 | { |
tylerjw | 10:b13416bbb4cd | 218 | pc.baud(9600); |
tylerjw | 9:4debfbc1fb3e | 219 | Thread thread(gps_thread, NULL, osPriorityHigh); |
tylerjw | 13:db6af0620264 | 220 | Thread thread2(log_thread, NULL, osPriorityNormal); |
tylerjw | 14:dce4d8c29b17 | 221 | Thread thread3(sensor_thread, NULL, osPriorityNormal); |
tylerjw | 17:4927053e120f | 222 | Thread thread4(parachute_thread, NULL, osPriorityRealtime); |
tylerjw | 13:db6af0620264 | 223 | |
tylerjw | 18:29b19a25a963 | 224 | while(true) ; |
tylerjw | 0:ce5f06c3895f | 225 | } |