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@22:becb67846755, 2012-12-13 (annotated)

Committer:
tylerjw
Date:
Thu Dec 13 23:08:10 2012 +0000
Revision:
22:becb67846755
Parent:
21:8799ee63c2cd
Child:
23:5a7b5db55be5
swapped right and left turn;

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 22:becb67846755 7 #include "config.h"
tylerjw 19:1cfe22ef30e2 8
tylerjw 15:ceac642f6b75 9 I2C i2c(p9, p10); // sda, scl
tylerjw 15:ceac642f6b75 10 BMP085 alt_sensor(i2c);
tylerjw 15:ceac642f6b75 11
tylerjw 0:ce5f06c3895f 12 Serial pc(USBTX, USBRX);
tylerjw 10:b13416bbb4cd 13 BufferedSerial gps(NC, p14);
tylerjw 10:b13416bbb4cd 14 AnalogIn gps_battery(p20);
tylerjw 16:653df0cfe6ee 15 AnalogIn mbed_battery(p19);
tylerjw 7:d8ecabe16c9e 16
tylerjw 19:1cfe22ef30e2 17 GPS_Parser nmea;
tylerjw 17:4927053e120f 18
tylerjw 18:29b19a25a963 19 Semaphore parachute_sem(0);
tylerjw 18:29b19a25a963 20
tylerjw 13:db6af0620264 21 typedef struct {
tylerjw 13:db6af0620264 22 char line[80];
tylerjw 13:db6af0620264 23 } gps_line;
tylerjw 13:db6af0620264 24 MemoryPool<gps_line, 16> mpool_gps_line;
tylerjw 13:db6af0620264 25 Queue<gps_line, 16> queue_gps_line;
tylerjw 13:db6af0620264 26
tylerjw 13:db6af0620264 27 typedef struct {
tylerjw 14:dce4d8c29b17 28 char line[80];
tylerjw 13:db6af0620264 29 } sensor_line;
tylerjw 13:db6af0620264 30 MemoryPool<sensor_line, 16> mpool_sensor_line;
tylerjw 13:db6af0620264 31 Queue<sensor_line, 16> queue_sensor_line;
tylerjw 12:0d943d69d3ec 32
tylerjw 7:d8ecabe16c9e 33 void gps_thread(void const *args)
tylerjw 7:d8ecabe16c9e 34 {
tylerjw 10:b13416bbb4cd 35 char buffer[80];
tylerjw 18:29b19a25a963 36 float alt, alt_prev;
tylerjw 18:29b19a25a963 37 alt = alt_prev = 0;
tylerjw 9:4debfbc1fb3e 38
tylerjw 21:8799ee63c2cd 39 //DigitalOut gps_led(LED4);
tylerjw 7:d8ecabe16c9e 40
tylerjw 7:d8ecabe16c9e 41 gps.baud(4800);
tylerjw 7:d8ecabe16c9e 42
tylerjw 7:d8ecabe16c9e 43 while(true) {
tylerjw 21:8799ee63c2cd 44 //gps_led = !gps_led;
tylerjw 10:b13416bbb4cd 45 gps.read_line(buffer);
tylerjw 19:1cfe22ef30e2 46 int line_type = nmea.parse(buffer);
tylerjw 14:dce4d8c29b17 47 //pc.puts(buffer);
tylerjw 13:db6af0620264 48 // send to log...
tylerjw 13:db6af0620264 49 gps_line *message = mpool_gps_line.alloc();
tylerjw 13:db6af0620264 50 strcpy(message->line, buffer);
tylerjw 13:db6af0620264 51 queue_gps_line.put(message);
tylerjw 19:1cfe22ef30e2 52
tylerjw 19:1cfe22ef30e2 53 // debugging
tylerjw 19:1cfe22ef30e2 54 //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 55 //pc.printf("%f, %f\r\n", nmea.get_dec_longitude(), nmea.get_dec_latitude());
tylerjw 19:1cfe22ef30e2 56 //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 22:becb67846755 57
tylerjw 19:1cfe22ef30e2 58 // test altitude direction - release parachute thread to run
tylerjw 19:1cfe22ef30e2 59 if(line_type == RMC && nmea.get_lock()) {
tylerjw 19:1cfe22ef30e2 60 if(UNLOCK_ON_FALL) {
tylerjw 19:1cfe22ef30e2 61 if(alt != 0) { // first time
tylerjw 19:1cfe22ef30e2 62 alt = nmea.get_msl_altitude();
tylerjw 19:1cfe22ef30e2 63 } else {
tylerjw 19:1cfe22ef30e2 64 alt_prev = alt;
tylerjw 19:1cfe22ef30e2 65 alt = nmea.get_msl_altitude();
tylerjw 19:1cfe22ef30e2 66 if(alt < alt_prev) // going down
tylerjw 19:1cfe22ef30e2 67 parachute_sem.release(); // let the parachute code execute
tylerjw 19:1cfe22ef30e2 68 }
tylerjw 18:29b19a25a963 69 } else {
tylerjw 19:1cfe22ef30e2 70 parachute_sem.release();
tylerjw 18:29b19a25a963 71 }
tylerjw 18:29b19a25a963 72 }
tylerjw 13:db6af0620264 73 }
tylerjw 13:db6af0620264 74 }
tylerjw 13:db6af0620264 75
tylerjw 13:db6af0620264 76 void log_thread(const void *args)
tylerjw 13:db6af0620264 77 {
tylerjw 13:db6af0620264 78 FATFS fs;
tylerjw 13:db6af0620264 79 FIL fp_gps, fp_sensor;
tylerjw 13:db6af0620264 80
tylerjw 13:db6af0620264 81 DigitalOut log_led(LED3);
tylerjw 13:db6af0620264 82
tylerjw 13:db6af0620264 83 f_mount(0, &fs);
tylerjw 13:db6af0620264 84 f_open(&fp_gps, "0:gps.txt", FA_CREATE_ALWAYS | FA_WRITE);
tylerjw 17:4927053e120f 85 f_open(&fp_sensor, "0:sensors.csv", FA_CREATE_ALWAYS | FA_WRITE);
tylerjw 13:db6af0620264 86
tylerjw 13:db6af0620264 87 while(1) {
tylerjw 18:29b19a25a963 88 log_led = !log_led;
tylerjw 14:dce4d8c29b17 89 osEvent evt1 = queue_gps_line.get(1);
tylerjw 14:dce4d8c29b17 90 if (evt1.status == osEventMessage) {
tylerjw 14:dce4d8c29b17 91 gps_line *message = (gps_line*)evt1.value.p;
tylerjw 13:db6af0620264 92
tylerjw 13:db6af0620264 93 f_puts(message->line, &fp_gps);
tylerjw 13:db6af0620264 94 f_sync(&fp_gps);
tylerjw 19:1cfe22ef30e2 95
tylerjw 13:db6af0620264 96 mpool_gps_line.free(message);
tylerjw 13:db6af0620264 97 }
tylerjw 19:1cfe22ef30e2 98
tylerjw 14:dce4d8c29b17 99 osEvent evt2 = queue_sensor_line.get(1);
tylerjw 14:dce4d8c29b17 100 if(evt2.status == osEventMessage) {
tylerjw 14:dce4d8c29b17 101 sensor_line *message = (sensor_line*)evt2.value.p;
tylerjw 19:1cfe22ef30e2 102
tylerjw 14:dce4d8c29b17 103 f_puts(message->line, &fp_sensor);
tylerjw 14:dce4d8c29b17 104 f_sync(&fp_sensor);
tylerjw 19:1cfe22ef30e2 105
tylerjw 13:db6af0620264 106 mpool_sensor_line.free(message);
tylerjw 13:db6af0620264 107 }
tylerjw 13:db6af0620264 108 }
tylerjw 13:db6af0620264 109 }
tylerjw 13:db6af0620264 110
tylerjw 13:db6af0620264 111 void sensor_thread(const void* args)
tylerjw 13:db6af0620264 112 {
tylerjw 14:dce4d8c29b17 113 DigitalOut sensor_led(LED2);
tylerjw 16:653df0cfe6ee 114 Timer t;
tylerjw 18:29b19a25a963 115 float time;
tylerjw 18:29b19a25a963 116 float gps_battery_voltage, mbed_battery_voltage;
tylerjw 18:29b19a25a963 117 float bmp_temperature, bmp_altitude;
tylerjw 18:29b19a25a963 118 int bmp_pressure;
tylerjw 19:1cfe22ef30e2 119
tylerjw 19:1cfe22ef30e2 120 if(WAIT_FOR_LOCK) {
tylerjw 19:1cfe22ef30e2 121 while(!nmea.get_date()) Thread::wait(100); // wait for lock
tylerjw 19:1cfe22ef30e2 122 }
tylerjw 19:1cfe22ef30e2 123
tylerjw 17:4927053e120f 124 t.start(); // start timer after lock
tylerjw 19:1cfe22ef30e2 125
tylerjw 14:dce4d8c29b17 126 sensor_line *message = mpool_sensor_line.alloc();
tylerjw 22:becb67846755 127 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());
tylerjw 14:dce4d8c29b17 128 queue_sensor_line.put(message);
tylerjw 19:1cfe22ef30e2 129
tylerjw 19:1cfe22ef30e2 130 while(true) {
tylerjw 13:db6af0620264 131 //get sensor line memory
tylerjw 18:29b19a25a963 132 sensor_led = !sensor_led;
tylerjw 13:db6af0620264 133 sensor_line *message = mpool_sensor_line.alloc();
tylerjw 19:1cfe22ef30e2 134
tylerjw 16:653df0cfe6ee 135 //timestamp
tylerjw 22:becb67846755 136 time = nmea.get_time();
tylerjw 19:1cfe22ef30e2 137
tylerjw 13:db6af0620264 138 //gps battery
tylerjw 18:29b19a25a963 139 gps_battery_voltage = gps_battery.read()*BAT_GPS_MUL;
tylerjw 19:1cfe22ef30e2 140
tylerjw 16:653df0cfe6ee 141 //mbed battery
tylerjw 18:29b19a25a963 142 mbed_battery_voltage = mbed_battery.read()*BAT_MBED_MUL;
tylerjw 19:1cfe22ef30e2 143
tylerjw 15:ceac642f6b75 144 //BMP085
tylerjw 18:29b19a25a963 145 bmp_temperature = (float)alt_sensor.get_temperature() / 10.0;
tylerjw 18:29b19a25a963 146 bmp_pressure = alt_sensor.get_pressure();
tylerjw 18:29b19a25a963 147 bmp_altitude = alt_sensor.get_altitude_ft();
tylerjw 19:1cfe22ef30e2 148
tylerjw 22:becb67846755 149 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());
tylerjw 14:dce4d8c29b17 150 queue_sensor_line.put(message);
tylerjw 0:ce5f06c3895f 151 }
tylerjw 7:d8ecabe16c9e 152 }
tylerjw 7:d8ecabe16c9e 153
tylerjw 17:4927053e120f 154 void parachute_thread(const void *args)
tylerjw 17:4927053e120f 155 {
tylerjw 21:8799ee63c2cd 156 DigitalOut left_turn(LED4);
tylerjw 21:8799ee63c2cd 157 DigitalOut right_turn(LED1);
tylerjw 19:1cfe22ef30e2 158
tylerjw 21:8799ee63c2cd 159 right_turn = 1;
tylerjw 21:8799ee63c2cd 160 Thread::wait(400);
tylerjw 21:8799ee63c2cd 161 left_turn = 1;
tylerjw 21:8799ee63c2cd 162 Thread::wait(400);
tylerjw 19:1cfe22ef30e2 163 while(true) {
tylerjw 21:8799ee63c2cd 164 right_turn = left_turn = 0;
tylerjw 18:29b19a25a963 165 parachute_sem.wait();
tylerjw 22:becb67846755 166
tylerjw 21:8799ee63c2cd 167 float distance = nmea.calc_dist_to_km(target_lat, target_lon);
tylerjw 19:1cfe22ef30e2 168
tylerjw 21:8799ee63c2cd 169 if(distance < distance_fudge_km)
tylerjw 19:1cfe22ef30e2 170 continue; // dont do anything
tylerjw 19:1cfe22ef30e2 171
tylerjw 19:1cfe22ef30e2 172 float course = nmea.get_course_d();
tylerjw 19:1cfe22ef30e2 173 float course_to = nmea.calc_course_to(target_lat, target_lon);
tylerjw 19:1cfe22ef30e2 174 float course_diff = course_to - course;
tylerjw 22:becb67846755 175
tylerjw 22:becb67846755 176 if(course == 0.0) // not moving fast enough
tylerjw 22:becb67846755 177 continue; // do nothing
tylerjw 19:1cfe22ef30e2 178
tylerjw 21:8799ee63c2cd 179 if(course_diff < course_fudge && course_diff > neg_course_fudge) {
tylerjw 21:8799ee63c2cd 180 right_turn = left_turn = 1;
tylerjw 21:8799ee63c2cd 181 Thread::wait(400);
tylerjw 19:1cfe22ef30e2 182 continue; // don't do anything
tylerjw 21:8799ee63c2cd 183 } else if(course_diff > 180.0 || course_diff < 0.0) {
tylerjw 21:8799ee63c2cd 184 left_turn = 1;
tylerjw 21:8799ee63c2cd 185 Thread::wait(400); // turn left
tylerjw 22:becb67846755 186 } else {
tylerjw 22:becb67846755 187 right_turn = 1;
tylerjw 22:becb67846755 188 Thread::wait(400); // turn righ
tylerjw 21:8799ee63c2cd 189 }
tylerjw 18:29b19a25a963 190 }
tylerjw 17:4927053e120f 191 }
tylerjw 17:4927053e120f 192
tylerjw 7:d8ecabe16c9e 193 int main()
tylerjw 7:d8ecabe16c9e 194 {
tylerjw 10:b13416bbb4cd 195 pc.baud(9600);
tylerjw 9:4debfbc1fb3e 196 Thread thread(gps_thread, NULL, osPriorityHigh);
tylerjw 13:db6af0620264 197 Thread thread2(log_thread, NULL, osPriorityNormal);
tylerjw 14:dce4d8c29b17 198 Thread thread3(sensor_thread, NULL, osPriorityNormal);
tylerjw 17:4927053e120f 199 Thread thread4(parachute_thread, NULL, osPriorityRealtime);
tylerjw 13:db6af0620264 200
tylerjw 18:29b19a25a963 201 while(true) ;
tylerjw 0:ce5f06c3895f 202 }