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@28:032d55fa57b8, 2013-01-10 (annotated)

Committer:
tylerjw
Date:
Thu Jan 10 19:25:13 2013 +0000
Revision:
28:032d55fa57b8
Parent:
27:24fd8e32511c
Child:
29:8cdb56a0fe57
added logging of servo positions - 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 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 7:d8ecabe16c9e 24
tylerjw 24:7477105103e5 25 NmeaParser nmea;
tylerjw 17:4927053e120f 26
tylerjw 18:29b19a25a963 27 Semaphore parachute_sem(0);
tylerjw 26:85cdb1031eb1 28 Semaphore sd_sem(0);
tylerjw 18:29b19a25a963 29
tylerjw 13:db6af0620264 30 typedef struct {
tylerjw 13:db6af0620264 31 char line[80];
tylerjw 13:db6af0620264 32 } gps_line;
tylerjw 13:db6af0620264 33 MemoryPool<gps_line, 16> mpool_gps_line;
tylerjw 13:db6af0620264 34 Queue<gps_line, 16> queue_gps_line;
tylerjw 13:db6af0620264 35
tylerjw 13:db6af0620264 36 typedef struct {
tylerjw 14:dce4d8c29b17 37 char line[80];
tylerjw 13:db6af0620264 38 } sensor_line;
tylerjw 13:db6af0620264 39 MemoryPool<sensor_line, 16> mpool_sensor_line;
tylerjw 13:db6af0620264 40 Queue<sensor_line, 16> queue_sensor_line;
tylerjw 12:0d943d69d3ec 41
tylerjw 28:032d55fa57b8 42 volatile float left_pos = 0.0; // servo position for logging
tylerjw 28:032d55fa57b8 43 volatile float right_pos = 0.0;
tylerjw 28:032d55fa57b8 44
tylerjw 7:d8ecabe16c9e 45 void gps_thread(void const *args)
tylerjw 7:d8ecabe16c9e 46 {
tylerjw 10:b13416bbb4cd 47 char buffer[80];
tylerjw 18:29b19a25a963 48 float alt, alt_prev;
tylerjw 18:29b19a25a963 49 alt = alt_prev = 0;
tylerjw 9:4debfbc1fb3e 50
tylerjw 21:8799ee63c2cd 51 //DigitalOut gps_led(LED4);
tylerjw 7:d8ecabe16c9e 52
tylerjw 7:d8ecabe16c9e 53 gps.baud(4800);
tylerjw 7:d8ecabe16c9e 54
tylerjw 7:d8ecabe16c9e 55 while(true) {
tylerjw 21:8799ee63c2cd 56 //gps_led = !gps_led;
tylerjw 24:7477105103e5 57 gps.get_line(buffer);
tylerjw 19:1cfe22ef30e2 58 int line_type = nmea.parse(buffer);
tylerjw 14:dce4d8c29b17 59 //pc.puts(buffer);
tylerjw 13:db6af0620264 60 // send to log...
tylerjw 13:db6af0620264 61 gps_line *message = mpool_gps_line.alloc();
tylerjw 13:db6af0620264 62 strcpy(message->line, buffer);
tylerjw 13:db6af0620264 63 queue_gps_line.put(message);
tylerjw 19:1cfe22ef30e2 64
tylerjw 19:1cfe22ef30e2 65 // test altitude direction - release parachute thread to run
tylerjw 24:7477105103e5 66 if(line_type == RMC && nmea.quality()) {
tylerjw 19:1cfe22ef30e2 67 if(UNLOCK_ON_FALL) {
tylerjw 19:1cfe22ef30e2 68 if(alt != 0) { // first time
tylerjw 24:7477105103e5 69 alt = nmea.msl_altitude();
tylerjw 19:1cfe22ef30e2 70 } else {
tylerjw 19:1cfe22ef30e2 71 alt_prev = alt;
tylerjw 24:7477105103e5 72 alt = nmea.msl_altitude();
tylerjw 19:1cfe22ef30e2 73 if(alt < alt_prev) // going down
tylerjw 19:1cfe22ef30e2 74 parachute_sem.release(); // let the parachute code execute
tylerjw 19:1cfe22ef30e2 75 }
tylerjw 18:29b19a25a963 76 } else {
tylerjw 19:1cfe22ef30e2 77 parachute_sem.release();
tylerjw 18:29b19a25a963 78 }
tylerjw 18:29b19a25a963 79 }
tylerjw 13:db6af0620264 80 }
tylerjw 13:db6af0620264 81 }
tylerjw 13:db6af0620264 82
tylerjw 13:db6af0620264 83 void log_thread(const void *args)
tylerjw 13:db6af0620264 84 {
tylerjw 13:db6af0620264 85 FATFS fs;
tylerjw 13:db6af0620264 86 FIL fp_gps, fp_sensor;
tylerjw 13:db6af0620264 87
tylerjw 13:db6af0620264 88 DigitalOut log_led(LED3);
tylerjw 13:db6af0620264 89
tylerjw 13:db6af0620264 90 f_mount(0, &fs);
tylerjw 25:81c3696ba2c9 91 f_open(&fp_gps, "0:gps.txt", FA_OPEN_EXISTING | FA_WRITE);
tylerjw 26:85cdb1031eb1 92 f_lseek(&fp_gps, f_size(&fp_gps));
tylerjw 25:81c3696ba2c9 93 f_open(&fp_sensor, "0:sensors.csv", FA_OPEN_EXISTING | FA_WRITE);
tylerjw 26:85cdb1031eb1 94 f_lseek(&fp_sensor, f_size(&fp_sensor));
tylerjw 13:db6af0620264 95
tylerjw 26:85cdb1031eb1 96 sd_sem.release(); // sd card initialized... start sensor thread
tylerjw 26:85cdb1031eb1 97
tylerjw 13:db6af0620264 98 while(1) {
tylerjw 18:29b19a25a963 99 log_led = !log_led;
tylerjw 14:dce4d8c29b17 100 osEvent evt1 = queue_gps_line.get(1);
tylerjw 14:dce4d8c29b17 101 if (evt1.status == osEventMessage) {
tylerjw 14:dce4d8c29b17 102 gps_line *message = (gps_line*)evt1.value.p;
tylerjw 13:db6af0620264 103
tylerjw 13:db6af0620264 104 f_puts(message->line, &fp_gps);
tylerjw 13:db6af0620264 105 f_sync(&fp_gps);
tylerjw 19:1cfe22ef30e2 106
tylerjw 13:db6af0620264 107 mpool_gps_line.free(message);
tylerjw 13:db6af0620264 108 }
tylerjw 19:1cfe22ef30e2 109
tylerjw 14:dce4d8c29b17 110 osEvent evt2 = queue_sensor_line.get(1);
tylerjw 14:dce4d8c29b17 111 if(evt2.status == osEventMessage) {
tylerjw 14:dce4d8c29b17 112 sensor_line *message = (sensor_line*)evt2.value.p;
tylerjw 19:1cfe22ef30e2 113
tylerjw 14:dce4d8c29b17 114 f_puts(message->line, &fp_sensor);
tylerjw 14:dce4d8c29b17 115 f_sync(&fp_sensor);
tylerjw 19:1cfe22ef30e2 116
tylerjw 13:db6af0620264 117 mpool_sensor_line.free(message);
tylerjw 13:db6af0620264 118 }
tylerjw 13:db6af0620264 119 }
tylerjw 13:db6af0620264 120 }
tylerjw 13:db6af0620264 121
tylerjw 13:db6af0620264 122 void sensor_thread(const void* args)
tylerjw 13:db6af0620264 123 {
tylerjw 14:dce4d8c29b17 124 DigitalOut sensor_led(LED2);
tylerjw 16:653df0cfe6ee 125 Timer t;
tylerjw 18:29b19a25a963 126 float time;
tylerjw 18:29b19a25a963 127 float gps_battery_voltage, mbed_battery_voltage;
tylerjw 26:85cdb1031eb1 128 float temp;
tylerjw 27:24fd8e32511c 129 float distance, course_to, course;
tylerjw 26:85cdb1031eb1 130
tylerjw 26:85cdb1031eb1 131 sd_sem.wait(); // wait for the sd card to initialize and open files
tylerjw 19:1cfe22ef30e2 132
tylerjw 19:1cfe22ef30e2 133 if(WAIT_FOR_LOCK) {
tylerjw 24:7477105103e5 134 while(!nmea.date()) Thread::wait(100); // wait for lock
tylerjw 19:1cfe22ef30e2 135 }
tylerjw 19:1cfe22ef30e2 136
tylerjw 17:4927053e120f 137 t.start(); // start timer after lock
tylerjw 19:1cfe22ef30e2 138
tylerjw 14:dce4d8c29b17 139 sensor_line *message = mpool_sensor_line.alloc();
tylerjw 26:85cdb1031eb1 140 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 141 queue_sensor_line.put(message);
tylerjw 26:85cdb1031eb1 142
tylerjw 26:85cdb1031eb1 143 message = mpool_sensor_line.alloc();
tylerjw 28:032d55fa57b8 144 sprintf(message->line, ",Temperature,GPS Altitude,GPS Course,Course to Target,Distance,Left Servo,Right Servo \r\n");
tylerjw 14:dce4d8c29b17 145 queue_sensor_line.put(message);
tylerjw 19:1cfe22ef30e2 146
tylerjw 19:1cfe22ef30e2 147 while(true) {
tylerjw 13:db6af0620264 148 //get sensor line memory
tylerjw 18:29b19a25a963 149 sensor_led = !sensor_led;
tylerjw 13:db6af0620264 150 sensor_line *message = mpool_sensor_line.alloc();
tylerjw 19:1cfe22ef30e2 151
tylerjw 16:653df0cfe6ee 152 //timestamp
tylerjw 24:7477105103e5 153 time = nmea.utc_time();
tylerjw 19:1cfe22ef30e2 154
tylerjw 13:db6af0620264 155 //gps battery
tylerjw 18:29b19a25a963 156 gps_battery_voltage = gps_battery.read()*BAT_GPS_MUL;
tylerjw 19:1cfe22ef30e2 157
tylerjw 16:653df0cfe6ee 158 //mbed battery
tylerjw 18:29b19a25a963 159 mbed_battery_voltage = mbed_battery.read()*BAT_MBED_MUL;
tylerjw 19:1cfe22ef30e2 160
tylerjw 26:85cdb1031eb1 161 //temperature
tylerjw 26:85cdb1031eb1 162 temp = temperature.sample_f();
tylerjw 27:24fd8e32511c 163
tylerjw 27:24fd8e32511c 164 //gps
tylerjw 27:24fd8e32511c 165 distance = nmea.calc_dist_to_km(target_lat, target_lon);
tylerjw 27:24fd8e32511c 166 course_to = nmea.calc_initial_bearing(target_lat, target_lon);
tylerjw 27:24fd8e32511c 167 course = nmea.track();
tylerjw 19:1cfe22ef30e2 168
tylerjw 28:032d55fa57b8 169 sprintf(message->line, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\r\n", time,gps_battery_voltage,mbed_battery_voltage,temp,nmea.calc_altitude_ft(),course,course_to,distance,left_pos,right_pos);
tylerjw 14:dce4d8c29b17 170 queue_sensor_line.put(message);
tylerjw 26:85cdb1031eb1 171
tylerjw 26:85cdb1031eb1 172 Thread::wait(100);
tylerjw 0:ce5f06c3895f 173 }
tylerjw 7:d8ecabe16c9e 174 }
tylerjw 7:d8ecabe16c9e 175
tylerjw 17:4927053e120f 176 void parachute_thread(const void *args)
tylerjw 17:4927053e120f 177 {
tylerjw 28:032d55fa57b8 178 DigitalOut left_turn_led(LED4);
tylerjw 28:032d55fa57b8 179 DigitalOut right_turn_led(LED1);
tylerjw 25:81c3696ba2c9 180
tylerjw 25:81c3696ba2c9 181 // servos ////////////////////////// NOT TESTED!!! ///////////////////////////
tylerjw 25:81c3696ba2c9 182 Servo left_s(p21);
tylerjw 25:81c3696ba2c9 183 Servo right_s(p22);
tylerjw 25:81c3696ba2c9 184
tylerjw 25:81c3696ba2c9 185 left_s.calibrate_max(0.0007);
tylerjw 25:81c3696ba2c9 186 left_s.calibrate_min(-0.0014);
tylerjw 25:81c3696ba2c9 187 right_s.calibrate(0.0009);
tylerjw 25:81c3696ba2c9 188
tylerjw 25:81c3696ba2c9 189 left_s = 0.0;
tylerjw 25:81c3696ba2c9 190 right_s = 0.0;
tylerjw 25:81c3696ba2c9 191 ////////////////////////////////////////////////////////////////////////////////
tylerjw 19:1cfe22ef30e2 192
tylerjw 28:032d55fa57b8 193 right_turn_led = 1;
tylerjw 21:8799ee63c2cd 194 Thread::wait(400);
tylerjw 28:032d55fa57b8 195 left_turn_led = 1;
tylerjw 21:8799ee63c2cd 196 Thread::wait(400);
tylerjw 19:1cfe22ef30e2 197 while(true) {
tylerjw 28:032d55fa57b8 198 right_turn_led = left_turn_led = 0;
tylerjw 18:29b19a25a963 199 parachute_sem.wait();
tylerjw 22:becb67846755 200
tylerjw 21:8799ee63c2cd 201 float distance = nmea.calc_dist_to_km(target_lat, target_lon);
tylerjw 19:1cfe22ef30e2 202
tylerjw 21:8799ee63c2cd 203 if(distance < distance_fudge_km)
tylerjw 19:1cfe22ef30e2 204 continue; // dont do anything
tylerjw 19:1cfe22ef30e2 205
tylerjw 24:7477105103e5 206 float course = nmea.track();
tylerjw 24:7477105103e5 207 float course_to = nmea.calc_initial_bearing(target_lat, target_lon);
tylerjw 19:1cfe22ef30e2 208 float course_diff = course_to - course;
tylerjw 22:becb67846755 209
tylerjw 22:becb67846755 210 if(course == 0.0) // not moving fast enough
tylerjw 22:becb67846755 211 continue; // do nothing
tylerjw 19:1cfe22ef30e2 212
tylerjw 21:8799ee63c2cd 213 if(course_diff < course_fudge && course_diff > neg_course_fudge) {
tylerjw 28:032d55fa57b8 214 right_turn_led = left_turn_led = 1;
tylerjw 21:8799ee63c2cd 215 Thread::wait(400);
tylerjw 19:1cfe22ef30e2 216 continue; // don't do anything
tylerjw 21:8799ee63c2cd 217 } else if(course_diff > 180.0 || course_diff < 0.0) {
tylerjw 28:032d55fa57b8 218 left_turn_led = 1;
tylerjw 28:032d55fa57b8 219 right_s = right_pos = 0.0; // NOT TESTED!!!
tylerjw 28:032d55fa57b8 220 left_s = left_pos = 1.0;
tylerjw 21:8799ee63c2cd 221 Thread::wait(400); // turn left
tylerjw 25:81c3696ba2c9 222 left_s = 0.0;
tylerjw 22:becb67846755 223 } else {
tylerjw 28:032d55fa57b8 224 right_turn_led = 1;
tylerjw 28:032d55fa57b8 225 left_s = left_pos = 0.0;
tylerjw 28:032d55fa57b8 226 right_s = right_pos = 1.0;
tylerjw 22:becb67846755 227 Thread::wait(400); // turn righ
tylerjw 28:032d55fa57b8 228 right_s = right_pos = 0.0;
tylerjw 21:8799ee63c2cd 229 }
tylerjw 18:29b19a25a963 230 }
tylerjw 17:4927053e120f 231 }
tylerjw 17:4927053e120f 232
tylerjw 7:d8ecabe16c9e 233 int main()
tylerjw 7:d8ecabe16c9e 234 {
tylerjw 10:b13416bbb4cd 235 pc.baud(9600);
tylerjw 9:4debfbc1fb3e 236 Thread thread(gps_thread, NULL, osPriorityHigh);
tylerjw 13:db6af0620264 237 Thread thread2(log_thread, NULL, osPriorityNormal);
tylerjw 14:dce4d8c29b17 238 Thread thread3(sensor_thread, NULL, osPriorityNormal);
tylerjw 17:4927053e120f 239 Thread thread4(parachute_thread, NULL, osPriorityRealtime);
tylerjw 25:81c3696ba2c9 240
tylerjw 25:81c3696ba2c9 241 Watchdog wdt; // NOT TESTED!!!
tylerjw 25:81c3696ba2c9 242
tylerjw 25:81c3696ba2c9 243 wdt.kick(2.0);
tylerjw 13:db6af0620264 244
tylerjw 25:81c3696ba2c9 245 while(true) {
tylerjw 25:81c3696ba2c9 246 wdt.kick();
tylerjw 25:81c3696ba2c9 247 Thread::wait(500);
tylerjw 25:81c3696ba2c9 248 }
tylerjw 0:ce5f06c3895f 249 }