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@25:81c3696ba2c9, 2012-12-28 (annotated)

Committer:
tylerjw
Date:
Fri Dec 28 17:12:27 2012 +0000
Revision:
25:81c3696ba2c9
Parent:
24:7477105103e5
Child:
26:85cdb1031eb1
Added watchdog, append file, and servo... NOT TESTED!!!

Who changed what in which revision?

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