Tyler Weaver / HARP3

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 Tyler Weaver

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?

UserRevisionLine numberNew 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 }