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@29:8cdb56a0fe57, 2013-01-10 (annotated)

Committer:
tylerjw
Date:
Thu Jan 10 20:37:07 2013 +0000
Revision:
29:8cdb56a0fe57
Parent:
28:032d55fa57b8
Child:
30:b81274979e73
added alarm

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tylerjw 24:7477105103e5 1 /**
tylerjw 24:7477105103e5 2 * HARP Version 2
tylerjw 26:85cdb1031eb1 3 *
tylerjw 26:85cdb1031eb1 4 * TODO: Test Servo Code
tylerjw 26:85cdb1031eb1 5 * Test Watchdog Timer
tylerjw 26:85cdb1031eb1 6 * Test Append file and f_size() macro
tylerjw 24:7477105103e5 7 */
tylerjw 24:7477105103e5 8
tylerjw 0:ce5f06c3895f 9 #include "mbed.h"
tylerjw 7:d8ecabe16c9e 10 #include "rtos.h"
tylerjw 10:b13416bbb4cd 11 #include "buffered_serial.h"
tylerjw 12:0d943d69d3ec 12 #include "ff.h"
tylerjw 26:85cdb1031eb1 13 #include "TMP36GZ.h"
tylerjw 24:7477105103e5 14 #include "nmea_parser.h"
tylerjw 25:81c3696ba2c9 15 #include "watchdog.h"
tylerjw 25:81c3696ba2c9 16 #include "Servo.h"
tylerjw 22:becb67846755 17 #include "config.h"
tylerjw 19:1cfe22ef30e2 18
tylerjw 0:ce5f06c3895f 19 Serial pc(USBTX, USBRX);
tylerjw 24:7477105103e5 20 BufferedSerial gps;
tylerjw 10:b13416bbb4cd 21 AnalogIn gps_battery(p20);
tylerjw 26:85cdb1031eb1 22 AnalogIn mbed_battery(p16);
tylerjw 26:85cdb1031eb1 23 TMP36GZ temperature(p17);
tylerjw 29:8cdb56a0fe57 24 DigitalOut alarm(p18);
tylerjw 7:d8ecabe16c9e 25
tylerjw 24:7477105103e5 26 NmeaParser nmea;
tylerjw 17:4927053e120f 27
tylerjw 18:29b19a25a963 28 Semaphore parachute_sem(0);
tylerjw 26:85cdb1031eb1 29 Semaphore sd_sem(0);
tylerjw 18:29b19a25a963 30
tylerjw 13:db6af0620264 31 typedef struct {
tylerjw 13:db6af0620264 32 char line[80];
tylerjw 13:db6af0620264 33 } gps_line;
tylerjw 13:db6af0620264 34 MemoryPool<gps_line, 16> mpool_gps_line;
tylerjw 13:db6af0620264 35 Queue<gps_line, 16> queue_gps_line;
tylerjw 13:db6af0620264 36
tylerjw 13:db6af0620264 37 typedef struct {
tylerjw 14:dce4d8c29b17 38 char line[80];
tylerjw 13:db6af0620264 39 } sensor_line;
tylerjw 13:db6af0620264 40 MemoryPool<sensor_line, 16> mpool_sensor_line;
tylerjw 13:db6af0620264 41 Queue<sensor_line, 16> queue_sensor_line;
tylerjw 12:0d943d69d3ec 42
tylerjw 28:032d55fa57b8 43 volatile float left_pos = 0.0; // servo position for logging
tylerjw 28:032d55fa57b8 44 volatile float right_pos = 0.0;
tylerjw 28:032d55fa57b8 45
tylerjw 29:8cdb56a0fe57 46 volatile bool gps_wd = false;
tylerjw 29:8cdb56a0fe57 47 volatile bool sensor_wd = false;
tylerjw 29:8cdb56a0fe57 48 volatile bool log_wd = false;
tylerjw 29:8cdb56a0fe57 49 volatile bool parachute_wd = false;
tylerjw 29:8cdb56a0fe57 50
tylerjw 7:d8ecabe16c9e 51 void gps_thread(void const *args)
tylerjw 7:d8ecabe16c9e 52 {
tylerjw 10:b13416bbb4cd 53 char buffer[80];
tylerjw 18:29b19a25a963 54 float alt, alt_prev;
tylerjw 18:29b19a25a963 55 alt = alt_prev = 0;
tylerjw 9:4debfbc1fb3e 56
tylerjw 21:8799ee63c2cd 57 //DigitalOut gps_led(LED4);
tylerjw 7:d8ecabe16c9e 58
tylerjw 7:d8ecabe16c9e 59 gps.baud(4800);
tylerjw 7:d8ecabe16c9e 60
tylerjw 7:d8ecabe16c9e 61 while(true) {
tylerjw 21:8799ee63c2cd 62 //gps_led = !gps_led;
tylerjw 24:7477105103e5 63 gps.get_line(buffer);
tylerjw 19:1cfe22ef30e2 64 int line_type = nmea.parse(buffer);
tylerjw 14:dce4d8c29b17 65 //pc.puts(buffer);
tylerjw 13:db6af0620264 66 // send to log...
tylerjw 13:db6af0620264 67 gps_line *message = mpool_gps_line.alloc();
tylerjw 13:db6af0620264 68 strcpy(message->line, buffer);
tylerjw 13:db6af0620264 69 queue_gps_line.put(message);
tylerjw 19:1cfe22ef30e2 70
tylerjw 19:1cfe22ef30e2 71 // test altitude direction - release parachute thread to run
tylerjw 24:7477105103e5 72 if(line_type == RMC && nmea.quality()) {
tylerjw 19:1cfe22ef30e2 73 if(UNLOCK_ON_FALL) {
tylerjw 29:8cdb56a0fe57 74 if(RELEASE_AT_ALT) {
tylerjw 29:8cdb56a0fe57 75 if(nmea.msl_altitude() >= RELEASE_ALT)
tylerjw 29:8cdb56a0fe57 76 parachute_sem.release();
tylerjw 29:8cdb56a0fe57 77 }
tylerjw 29:8cdb56a0fe57 78 if(alt == 0) { // first time
tylerjw 24:7477105103e5 79 alt = nmea.msl_altitude();
tylerjw 19:1cfe22ef30e2 80 } else {
tylerjw 19:1cfe22ef30e2 81 alt_prev = alt;
tylerjw 24:7477105103e5 82 alt = nmea.msl_altitude();
tylerjw 19:1cfe22ef30e2 83 if(alt < alt_prev) // going down
tylerjw 29:8cdb56a0fe57 84 {
tylerjw 19:1cfe22ef30e2 85 parachute_sem.release(); // let the parachute code execute
tylerjw 29:8cdb56a0fe57 86 if(ALARM && alt < ALARM_ALT)
tylerjw 29:8cdb56a0fe57 87 alarm = 1;
tylerjw 29:8cdb56a0fe57 88 }
tylerjw 19:1cfe22ef30e2 89 }
tylerjw 18:29b19a25a963 90 } else {
tylerjw 19:1cfe22ef30e2 91 parachute_sem.release();
tylerjw 18:29b19a25a963 92 }
tylerjw 18:29b19a25a963 93 }
tylerjw 29:8cdb56a0fe57 94
tylerjw 29:8cdb56a0fe57 95 gps_wd = true; // kick the watchdog
tylerjw 13:db6af0620264 96 }
tylerjw 13:db6af0620264 97 }
tylerjw 13:db6af0620264 98
tylerjw 13:db6af0620264 99 void log_thread(const void *args)
tylerjw 13:db6af0620264 100 {
tylerjw 13:db6af0620264 101 FATFS fs;
tylerjw 13:db6af0620264 102 FIL fp_gps, fp_sensor;
tylerjw 13:db6af0620264 103
tylerjw 13:db6af0620264 104 DigitalOut log_led(LED3);
tylerjw 13:db6af0620264 105
tylerjw 29:8cdb56a0fe57 106 log_wd = true; // kick the dog
tylerjw 13:db6af0620264 107 f_mount(0, &fs);
tylerjw 25:81c3696ba2c9 108 f_open(&fp_gps, "0:gps.txt", FA_OPEN_EXISTING | FA_WRITE);
tylerjw 26:85cdb1031eb1 109 f_lseek(&fp_gps, f_size(&fp_gps));
tylerjw 25:81c3696ba2c9 110 f_open(&fp_sensor, "0:sensors.csv", FA_OPEN_EXISTING | FA_WRITE);
tylerjw 26:85cdb1031eb1 111 f_lseek(&fp_sensor, f_size(&fp_sensor));
tylerjw 29:8cdb56a0fe57 112 log_wd = true; // kick the dog
tylerjw 13:db6af0620264 113
tylerjw 26:85cdb1031eb1 114 sd_sem.release(); // sd card initialized... start sensor thread
tylerjw 29:8cdb56a0fe57 115
tylerjw 13:db6af0620264 116 while(1) {
tylerjw 18:29b19a25a963 117 log_led = !log_led;
tylerjw 14:dce4d8c29b17 118 osEvent evt1 = queue_gps_line.get(1);
tylerjw 14:dce4d8c29b17 119 if (evt1.status == osEventMessage) {
tylerjw 14:dce4d8c29b17 120 gps_line *message = (gps_line*)evt1.value.p;
tylerjw 13:db6af0620264 121
tylerjw 13:db6af0620264 122 f_puts(message->line, &fp_gps);
tylerjw 13:db6af0620264 123 f_sync(&fp_gps);
tylerjw 19:1cfe22ef30e2 124
tylerjw 13:db6af0620264 125 mpool_gps_line.free(message);
tylerjw 13:db6af0620264 126 }
tylerjw 19:1cfe22ef30e2 127
tylerjw 14:dce4d8c29b17 128 osEvent evt2 = queue_sensor_line.get(1);
tylerjw 14:dce4d8c29b17 129 if(evt2.status == osEventMessage) {
tylerjw 14:dce4d8c29b17 130 sensor_line *message = (sensor_line*)evt2.value.p;
tylerjw 19:1cfe22ef30e2 131
tylerjw 14:dce4d8c29b17 132 f_puts(message->line, &fp_sensor);
tylerjw 14:dce4d8c29b17 133 f_sync(&fp_sensor);
tylerjw 19:1cfe22ef30e2 134
tylerjw 13:db6af0620264 135 mpool_sensor_line.free(message);
tylerjw 13:db6af0620264 136 }
tylerjw 29:8cdb56a0fe57 137
tylerjw 29:8cdb56a0fe57 138 log_wd = true; // kick the dog
tylerjw 13:db6af0620264 139 }
tylerjw 13:db6af0620264 140 }
tylerjw 13:db6af0620264 141
tylerjw 13:db6af0620264 142 void sensor_thread(const void* args)
tylerjw 13:db6af0620264 143 {
tylerjw 14:dce4d8c29b17 144 DigitalOut sensor_led(LED2);
tylerjw 16:653df0cfe6ee 145 Timer t;
tylerjw 18:29b19a25a963 146 float time;
tylerjw 18:29b19a25a963 147 float gps_battery_voltage, mbed_battery_voltage;
tylerjw 26:85cdb1031eb1 148 float temp;
tylerjw 27:24fd8e32511c 149 float distance, course_to, course;
tylerjw 29:8cdb56a0fe57 150
tylerjw 29:8cdb56a0fe57 151 while( sd_sem.wait(50) <= 0) // wait for the sd card to initialize and open files
tylerjw 29:8cdb56a0fe57 152 sensor_wd = true; // kick the dog
tylerjw 19:1cfe22ef30e2 153
tylerjw 19:1cfe22ef30e2 154 if(WAIT_FOR_LOCK) {
tylerjw 29:8cdb56a0fe57 155 while(!nmea.date()) {
tylerjw 29:8cdb56a0fe57 156 Thread::wait(50); // wait for lock
tylerjw 29:8cdb56a0fe57 157 sensor_wd = true; // kick the dog
tylerjw 29:8cdb56a0fe57 158 }
tylerjw 19:1cfe22ef30e2 159 }
tylerjw 19:1cfe22ef30e2 160
tylerjw 17:4927053e120f 161 t.start(); // start timer after lock
tylerjw 19:1cfe22ef30e2 162
tylerjw 14:dce4d8c29b17 163 sensor_line *message = mpool_sensor_line.alloc();
tylerjw 29:8cdb56a0fe57 164 sprintf(message->line, "Date: %d, Time: %f\r\nGPS Time (UTC),GPS Battery(V),mbed Battery(V),", nmea.date(), nmea.utc_time());
tylerjw 26:85cdb1031eb1 165 queue_sensor_line.put(message);
tylerjw 29:8cdb56a0fe57 166
tylerjw 29:8cdb56a0fe57 167 sensor_wd = true; // kick the dog
tylerjw 29:8cdb56a0fe57 168
tylerjw 26:85cdb1031eb1 169 message = mpool_sensor_line.alloc();
tylerjw 29:8cdb56a0fe57 170 sprintf(message->line, "Temperature,GPS Altitude,GPS Course,");
tylerjw 29:8cdb56a0fe57 171 queue_sensor_line.put(message);
tylerjw 29:8cdb56a0fe57 172
tylerjw 29:8cdb56a0fe57 173 sensor_wd = true; // kick the dog
tylerjw 29:8cdb56a0fe57 174
tylerjw 29:8cdb56a0fe57 175 message = mpool_sensor_line.alloc();
tylerjw 29:8cdb56a0fe57 176 sprintf(message->line, "Course to Target,Distance,Left Servo,Right Servo \r\n");
tylerjw 14:dce4d8c29b17 177 queue_sensor_line.put(message);
tylerjw 19:1cfe22ef30e2 178
tylerjw 19:1cfe22ef30e2 179 while(true) {
tylerjw 13:db6af0620264 180 //get sensor line memory
tylerjw 18:29b19a25a963 181 sensor_led = !sensor_led;
tylerjw 19:1cfe22ef30e2 182
tylerjw 16:653df0cfe6ee 183 //timestamp
tylerjw 24:7477105103e5 184 time = nmea.utc_time();
tylerjw 19:1cfe22ef30e2 185
tylerjw 13:db6af0620264 186 //gps battery
tylerjw 18:29b19a25a963 187 gps_battery_voltage = gps_battery.read()*BAT_GPS_MUL;
tylerjw 19:1cfe22ef30e2 188
tylerjw 16:653df0cfe6ee 189 //mbed battery
tylerjw 18:29b19a25a963 190 mbed_battery_voltage = mbed_battery.read()*BAT_MBED_MUL;
tylerjw 19:1cfe22ef30e2 191
tylerjw 26:85cdb1031eb1 192 //temperature
tylerjw 26:85cdb1031eb1 193 temp = temperature.sample_f();
tylerjw 29:8cdb56a0fe57 194
tylerjw 27:24fd8e32511c 195 //gps
tylerjw 27:24fd8e32511c 196 distance = nmea.calc_dist_to_km(target_lat, target_lon);
tylerjw 27:24fd8e32511c 197 course_to = nmea.calc_initial_bearing(target_lat, target_lon);
tylerjw 27:24fd8e32511c 198 course = nmea.track();
tylerjw 19:1cfe22ef30e2 199
tylerjw 29:8cdb56a0fe57 200 sensor_line *message = mpool_sensor_line.alloc();
tylerjw 29:8cdb56a0fe57 201 sprintf(message->line, "%f,%f,%f,%f,%f,", time,gps_battery_voltage,mbed_battery_voltage,temp,nmea.calc_altitude_ft());
tylerjw 14:dce4d8c29b17 202 queue_sensor_line.put(message);
tylerjw 29:8cdb56a0fe57 203
tylerjw 29:8cdb56a0fe57 204 message = mpool_sensor_line.alloc();
tylerjw 29:8cdb56a0fe57 205 sprintf(message->line, "%f,%f,%f,%f,%f\r\n", course,course_to,distance,left_pos,right_pos);
tylerjw 29:8cdb56a0fe57 206 queue_sensor_line.put(message);
tylerjw 29:8cdb56a0fe57 207
tylerjw 29:8cdb56a0fe57 208 sensor_wd = true; // kick the dog
tylerjw 26:85cdb1031eb1 209 Thread::wait(100);
tylerjw 0:ce5f06c3895f 210 }
tylerjw 7:d8ecabe16c9e 211 }
tylerjw 7:d8ecabe16c9e 212
tylerjw 17:4927053e120f 213 void parachute_thread(const void *args)
tylerjw 17:4927053e120f 214 {
tylerjw 28:032d55fa57b8 215 DigitalOut left_turn_led(LED4);
tylerjw 28:032d55fa57b8 216 DigitalOut right_turn_led(LED1);
tylerjw 29:8cdb56a0fe57 217 bool is_released = false;
tylerjw 29:8cdb56a0fe57 218
tylerjw 25:81c3696ba2c9 219 // servos ////////////////////////// NOT TESTED!!! ///////////////////////////
tylerjw 25:81c3696ba2c9 220 Servo left_s(p21);
tylerjw 25:81c3696ba2c9 221 Servo right_s(p22);
tylerjw 29:8cdb56a0fe57 222 Servo release_s(p23);
tylerjw 25:81c3696ba2c9 223
tylerjw 29:8cdb56a0fe57 224 left_s.calibrate_max(SERVO_L_MAX);
tylerjw 29:8cdb56a0fe57 225 left_s.calibrate_min(SERVO_L_MIN);
tylerjw 29:8cdb56a0fe57 226 right_s.calibrate_max(SERVO_R_MAX);
tylerjw 29:8cdb56a0fe57 227 right_s.calibrate_min(SERVO_R_MIN);
tylerjw 29:8cdb56a0fe57 228
tylerjw 29:8cdb56a0fe57 229 // TODO: Calibrate release servo
tylerjw 29:8cdb56a0fe57 230
tylerjw 25:81c3696ba2c9 231 left_s = 0.0;
tylerjw 25:81c3696ba2c9 232 right_s = 0.0;
tylerjw 29:8cdb56a0fe57 233
tylerjw 29:8cdb56a0fe57 234 // TODO: Move release servo in and out to allow attachment on startup
tylerjw 29:8cdb56a0fe57 235 // release_s = 0.0;
tylerjw 29:8cdb56a0fe57 236 // Thread::wait(500);
tylerjw 29:8cdb56a0fe57 237 // release_s = 1.0
tylerjw 29:8cdb56a0fe57 238
tylerjw 25:81c3696ba2c9 239 ////////////////////////////////////////////////////////////////////////////////
tylerjw 19:1cfe22ef30e2 240
tylerjw 28:032d55fa57b8 241 right_turn_led = 1;
tylerjw 21:8799ee63c2cd 242 Thread::wait(400);
tylerjw 28:032d55fa57b8 243 left_turn_led = 1;
tylerjw 21:8799ee63c2cd 244 Thread::wait(400);
tylerjw 19:1cfe22ef30e2 245 while(true) {
tylerjw 28:032d55fa57b8 246 right_turn_led = left_turn_led = 0;
tylerjw 29:8cdb56a0fe57 247 while( parachute_sem.wait(50) <= 0) // didn't get it (timeout)
tylerjw 29:8cdb56a0fe57 248 parachute_wd = true; // kick the dog
tylerjw 29:8cdb56a0fe57 249
tylerjw 29:8cdb56a0fe57 250 //if(!is_released)
tylerjw 29:8cdb56a0fe57 251 //release_s = 0.0; // let go of the balloon
tylerjw 22:becb67846755 252
tylerjw 21:8799ee63c2cd 253 float distance = nmea.calc_dist_to_km(target_lat, target_lon);
tylerjw 19:1cfe22ef30e2 254
tylerjw 21:8799ee63c2cd 255 if(distance < distance_fudge_km)
tylerjw 19:1cfe22ef30e2 256 continue; // dont do anything
tylerjw 19:1cfe22ef30e2 257
tylerjw 24:7477105103e5 258 float course = nmea.track();
tylerjw 24:7477105103e5 259 float course_to = nmea.calc_initial_bearing(target_lat, target_lon);
tylerjw 19:1cfe22ef30e2 260 float course_diff = course_to - course;
tylerjw 29:8cdb56a0fe57 261
tylerjw 22:becb67846755 262 if(course == 0.0) // not moving fast enough
tylerjw 22:becb67846755 263 continue; // do nothing
tylerjw 19:1cfe22ef30e2 264
tylerjw 21:8799ee63c2cd 265 if(course_diff < course_fudge && course_diff > neg_course_fudge) {
tylerjw 28:032d55fa57b8 266 right_turn_led = left_turn_led = 1;
tylerjw 21:8799ee63c2cd 267 Thread::wait(400);
tylerjw 19:1cfe22ef30e2 268 continue; // don't do anything
tylerjw 21:8799ee63c2cd 269 } else if(course_diff > 180.0 || course_diff < 0.0) {
tylerjw 28:032d55fa57b8 270 left_turn_led = 1;
tylerjw 28:032d55fa57b8 271 right_s = right_pos = 0.0; // NOT TESTED!!!
tylerjw 28:032d55fa57b8 272 left_s = left_pos = 1.0;
tylerjw 21:8799ee63c2cd 273 Thread::wait(400); // turn left
tylerjw 25:81c3696ba2c9 274 left_s = 0.0;
tylerjw 22:becb67846755 275 } else {
tylerjw 28:032d55fa57b8 276 right_turn_led = 1;
tylerjw 28:032d55fa57b8 277 left_s = left_pos = 0.0;
tylerjw 28:032d55fa57b8 278 right_s = right_pos = 1.0;
tylerjw 22:becb67846755 279 Thread::wait(400); // turn righ
tylerjw 28:032d55fa57b8 280 right_s = right_pos = 0.0;
tylerjw 21:8799ee63c2cd 281 }
tylerjw 29:8cdb56a0fe57 282 parachute_wd = true; // kick the dog
tylerjw 29:8cdb56a0fe57 283 }
tylerjw 29:8cdb56a0fe57 284 }
tylerjw 29:8cdb56a0fe57 285
tylerjw 29:8cdb56a0fe57 286 void watchdog_thread(const void *args)
tylerjw 29:8cdb56a0fe57 287 {
tylerjw 29:8cdb56a0fe57 288 Watchdog wdt; // NOT TESTED!!!
tylerjw 29:8cdb56a0fe57 289
tylerjw 29:8cdb56a0fe57 290 wdt.kick(2.0);
tylerjw 29:8cdb56a0fe57 291
tylerjw 29:8cdb56a0fe57 292 while(true) {
tylerjw 29:8cdb56a0fe57 293 if(gps_wd && sensor_wd && parachute_wd && log_wd) {
tylerjw 29:8cdb56a0fe57 294 wdt.kick();
tylerjw 29:8cdb56a0fe57 295 gps_wd = sensor_wd = parachute_wd = log_wd = false;
tylerjw 29:8cdb56a0fe57 296 } else {
tylerjw 29:8cdb56a0fe57 297 Thread::wait(50);
tylerjw 29:8cdb56a0fe57 298 }
tylerjw 18:29b19a25a963 299 }
tylerjw 17:4927053e120f 300 }
tylerjw 17:4927053e120f 301
tylerjw 7:d8ecabe16c9e 302 int main()
tylerjw 7:d8ecabe16c9e 303 {
tylerjw 9:4debfbc1fb3e 304 Thread thread(gps_thread, NULL, osPriorityHigh);
tylerjw 13:db6af0620264 305 Thread thread2(log_thread, NULL, osPriorityNormal);
tylerjw 14:dce4d8c29b17 306 Thread thread3(sensor_thread, NULL, osPriorityNormal);
tylerjw 17:4927053e120f 307 Thread thread4(parachute_thread, NULL, osPriorityRealtime);
tylerjw 29:8cdb56a0fe57 308 Thread thread5(watchdog_thread, NULL, osPriorityHigh);
tylerjw 13:db6af0620264 309
tylerjw 25:81c3696ba2c9 310 while(true) {
tylerjw 29:8cdb56a0fe57 311 Thread::wait(osWaitForever);
tylerjw 25:81c3696ba2c9 312 }
tylerjw 0:ce5f06c3895f 313 }