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
main.cpp@30:b81274979e73, 2013-01-17 (annotated)
- Committer:
- tylerjw
- Date:
- Thu Jan 17 01:44:32 2013 +0000
- Revision:
- 30:b81274979e73
- Parent:
- 29:8cdb56a0fe57
- Child:
- 31:b9ac7d61b15b
Implemented release, watchdog improvements, and test routine - NOT TESTED; ; TODO: Calibrate release servo.
Who changed what in which revision?
User | Revision | Line number | New 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 | 30:b81274979e73 | 150 | |
tylerjw | 30:b81274979e73 | 151 | pc.baud(9600); |
tylerjw | 29:8cdb56a0fe57 | 152 | |
tylerjw | 29:8cdb56a0fe57 | 153 | while( sd_sem.wait(50) <= 0) // wait for the sd card to initialize and open files |
tylerjw | 29:8cdb56a0fe57 | 154 | sensor_wd = true; // kick the dog |
tylerjw | 19:1cfe22ef30e2 | 155 | |
tylerjw | 19:1cfe22ef30e2 | 156 | if(WAIT_FOR_LOCK) { |
tylerjw | 29:8cdb56a0fe57 | 157 | while(!nmea.date()) { |
tylerjw | 29:8cdb56a0fe57 | 158 | Thread::wait(50); // wait for lock |
tylerjw | 29:8cdb56a0fe57 | 159 | sensor_wd = true; // kick the dog |
tylerjw | 29:8cdb56a0fe57 | 160 | } |
tylerjw | 19:1cfe22ef30e2 | 161 | } |
tylerjw | 19:1cfe22ef30e2 | 162 | |
tylerjw | 17:4927053e120f | 163 | t.start(); // start timer after lock |
tylerjw | 19:1cfe22ef30e2 | 164 | |
tylerjw | 14:dce4d8c29b17 | 165 | sensor_line *message = mpool_sensor_line.alloc(); |
tylerjw | 29:8cdb56a0fe57 | 166 | 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 | 167 | queue_sensor_line.put(message); |
tylerjw | 29:8cdb56a0fe57 | 168 | |
tylerjw | 29:8cdb56a0fe57 | 169 | sensor_wd = true; // kick the dog |
tylerjw | 29:8cdb56a0fe57 | 170 | |
tylerjw | 26:85cdb1031eb1 | 171 | message = mpool_sensor_line.alloc(); |
tylerjw | 29:8cdb56a0fe57 | 172 | sprintf(message->line, "Temperature,GPS Altitude,GPS Course,"); |
tylerjw | 29:8cdb56a0fe57 | 173 | queue_sensor_line.put(message); |
tylerjw | 29:8cdb56a0fe57 | 174 | |
tylerjw | 29:8cdb56a0fe57 | 175 | sensor_wd = true; // kick the dog |
tylerjw | 29:8cdb56a0fe57 | 176 | |
tylerjw | 29:8cdb56a0fe57 | 177 | message = mpool_sensor_line.alloc(); |
tylerjw | 29:8cdb56a0fe57 | 178 | sprintf(message->line, "Course to Target,Distance,Left Servo,Right Servo \r\n"); |
tylerjw | 14:dce4d8c29b17 | 179 | queue_sensor_line.put(message); |
tylerjw | 19:1cfe22ef30e2 | 180 | |
tylerjw | 19:1cfe22ef30e2 | 181 | while(true) { |
tylerjw | 13:db6af0620264 | 182 | //get sensor line memory |
tylerjw | 18:29b19a25a963 | 183 | sensor_led = !sensor_led; |
tylerjw | 19:1cfe22ef30e2 | 184 | |
tylerjw | 16:653df0cfe6ee | 185 | //timestamp |
tylerjw | 24:7477105103e5 | 186 | time = nmea.utc_time(); |
tylerjw | 19:1cfe22ef30e2 | 187 | |
tylerjw | 13:db6af0620264 | 188 | //gps battery |
tylerjw | 18:29b19a25a963 | 189 | gps_battery_voltage = gps_battery.read()*BAT_GPS_MUL; |
tylerjw | 19:1cfe22ef30e2 | 190 | |
tylerjw | 16:653df0cfe6ee | 191 | //mbed battery |
tylerjw | 18:29b19a25a963 | 192 | mbed_battery_voltage = mbed_battery.read()*BAT_MBED_MUL; |
tylerjw | 19:1cfe22ef30e2 | 193 | |
tylerjw | 26:85cdb1031eb1 | 194 | //temperature |
tylerjw | 26:85cdb1031eb1 | 195 | temp = temperature.sample_f(); |
tylerjw | 29:8cdb56a0fe57 | 196 | |
tylerjw | 27:24fd8e32511c | 197 | //gps |
tylerjw | 27:24fd8e32511c | 198 | distance = nmea.calc_dist_to_km(target_lat, target_lon); |
tylerjw | 27:24fd8e32511c | 199 | course_to = nmea.calc_initial_bearing(target_lat, target_lon); |
tylerjw | 27:24fd8e32511c | 200 | course = nmea.track(); |
tylerjw | 30:b81274979e73 | 201 | |
tylerjw | 30:b81274979e73 | 202 | pc.printf("course: %4.1f course_to: %4.1f distance: %f\r\n", course, course_to, distance); |
tylerjw | 19:1cfe22ef30e2 | 203 | |
tylerjw | 29:8cdb56a0fe57 | 204 | sensor_line *message = mpool_sensor_line.alloc(); |
tylerjw | 29:8cdb56a0fe57 | 205 | sprintf(message->line, "%f,%f,%f,%f,%f,", time,gps_battery_voltage,mbed_battery_voltage,temp,nmea.calc_altitude_ft()); |
tylerjw | 14:dce4d8c29b17 | 206 | queue_sensor_line.put(message); |
tylerjw | 29:8cdb56a0fe57 | 207 | |
tylerjw | 29:8cdb56a0fe57 | 208 | message = mpool_sensor_line.alloc(); |
tylerjw | 29:8cdb56a0fe57 | 209 | sprintf(message->line, "%f,%f,%f,%f,%f\r\n", course,course_to,distance,left_pos,right_pos); |
tylerjw | 29:8cdb56a0fe57 | 210 | queue_sensor_line.put(message); |
tylerjw | 29:8cdb56a0fe57 | 211 | |
tylerjw | 29:8cdb56a0fe57 | 212 | sensor_wd = true; // kick the dog |
tylerjw | 30:b81274979e73 | 213 | Thread::wait(500); |
tylerjw | 0:ce5f06c3895f | 214 | } |
tylerjw | 7:d8ecabe16c9e | 215 | } |
tylerjw | 7:d8ecabe16c9e | 216 | |
tylerjw | 17:4927053e120f | 217 | void parachute_thread(const void *args) |
tylerjw | 17:4927053e120f | 218 | { |
tylerjw | 28:032d55fa57b8 | 219 | DigitalOut left_turn_led(LED4); |
tylerjw | 28:032d55fa57b8 | 220 | DigitalOut right_turn_led(LED1); |
tylerjw | 29:8cdb56a0fe57 | 221 | bool is_released = false; |
tylerjw | 29:8cdb56a0fe57 | 222 | |
tylerjw | 25:81c3696ba2c9 | 223 | // servos ////////////////////////// NOT TESTED!!! /////////////////////////// |
tylerjw | 25:81c3696ba2c9 | 224 | Servo left_s(p21); |
tylerjw | 25:81c3696ba2c9 | 225 | Servo right_s(p22); |
tylerjw | 29:8cdb56a0fe57 | 226 | Servo release_s(p23); |
tylerjw | 25:81c3696ba2c9 | 227 | |
tylerjw | 29:8cdb56a0fe57 | 228 | left_s.calibrate_max(SERVO_L_MAX); |
tylerjw | 29:8cdb56a0fe57 | 229 | left_s.calibrate_min(SERVO_L_MIN); |
tylerjw | 29:8cdb56a0fe57 | 230 | right_s.calibrate_max(SERVO_R_MAX); |
tylerjw | 29:8cdb56a0fe57 | 231 | right_s.calibrate_min(SERVO_R_MIN); |
tylerjw | 29:8cdb56a0fe57 | 232 | |
tylerjw | 29:8cdb56a0fe57 | 233 | // TODO: Calibrate release servo |
tylerjw | 29:8cdb56a0fe57 | 234 | |
tylerjw | 25:81c3696ba2c9 | 235 | left_s = 0.0; |
tylerjw | 25:81c3696ba2c9 | 236 | right_s = 0.0; |
tylerjw | 29:8cdb56a0fe57 | 237 | |
tylerjw | 29:8cdb56a0fe57 | 238 | // TODO: Move release servo in and out to allow attachment on startup |
tylerjw | 30:b81274979e73 | 239 | release_s = 0.0; // NOT TESTED! |
tylerjw | 30:b81274979e73 | 240 | Thread::wait(1000); |
tylerjw | 30:b81274979e73 | 241 | parachute_wd = true; // kick the dog |
tylerjw | 30:b81274979e73 | 242 | Thread::wait(1000); |
tylerjw | 30:b81274979e73 | 243 | release_s = 1.0; |
tylerjw | 29:8cdb56a0fe57 | 244 | |
tylerjw | 25:81c3696ba2c9 | 245 | //////////////////////////////////////////////////////////////////////////////// |
tylerjw | 19:1cfe22ef30e2 | 246 | |
tylerjw | 30:b81274979e73 | 247 | //right_turn_led = 1; // remove with watchdog on - test to determine which led is which |
tylerjw | 30:b81274979e73 | 248 | //Thread::wait(1000); |
tylerjw | 30:b81274979e73 | 249 | //left_turn_led = 1; |
tylerjw | 30:b81274979e73 | 250 | //Thread::wait(1000); |
tylerjw | 30:b81274979e73 | 251 | |
tylerjw | 30:b81274979e73 | 252 | int counter = 0; |
tylerjw | 30:b81274979e73 | 253 | |
tylerjw | 19:1cfe22ef30e2 | 254 | while(true) { |
tylerjw | 30:b81274979e73 | 255 | parachute_wd = true; // kick the dog |
tylerjw | 28:032d55fa57b8 | 256 | right_turn_led = left_turn_led = 0; |
tylerjw | 29:8cdb56a0fe57 | 257 | while( parachute_sem.wait(50) <= 0) // didn't get it (timeout) |
tylerjw | 29:8cdb56a0fe57 | 258 | parachute_wd = true; // kick the dog |
tylerjw | 29:8cdb56a0fe57 | 259 | |
tylerjw | 30:b81274979e73 | 260 | if(!is_released) |
tylerjw | 30:b81274979e73 | 261 | release_s = 0.0; // let go of the balloon (NOT Tested) |
tylerjw | 30:b81274979e73 | 262 | |
tylerjw | 30:b81274979e73 | 263 | if(counter < test_length) // test flight -- (NOT Tested) |
tylerjw | 30:b81274979e73 | 264 | { |
tylerjw | 30:b81274979e73 | 265 | left_s = test_left[counter]; |
tylerjw | 30:b81274979e73 | 266 | right_s = test_right[counter]; |
tylerjw | 30:b81274979e73 | 267 | Thread::wait(1000); |
tylerjw | 30:b81274979e73 | 268 | parachute_wd = true; // kick the watchdog |
tylerjw | 30:b81274979e73 | 269 | Thread::wait(1000); |
tylerjw | 30:b81274979e73 | 270 | counter++; |
tylerjw | 30:b81274979e73 | 271 | continue; |
tylerjw | 30:b81274979e73 | 272 | } |
tylerjw | 30:b81274979e73 | 273 | |
tylerjw | 21:8799ee63c2cd | 274 | float distance = nmea.calc_dist_to_km(target_lat, target_lon); |
tylerjw | 19:1cfe22ef30e2 | 275 | |
tylerjw | 21:8799ee63c2cd | 276 | if(distance < distance_fudge_km) |
tylerjw | 30:b81274979e73 | 277 | { |
tylerjw | 30:b81274979e73 | 278 | alarm = 1; // sound the alarm |
tylerjw | 19:1cfe22ef30e2 | 279 | continue; // dont do anything |
tylerjw | 30:b81274979e73 | 280 | } |
tylerjw | 19:1cfe22ef30e2 | 281 | |
tylerjw | 24:7477105103e5 | 282 | float course = nmea.track(); |
tylerjw | 24:7477105103e5 | 283 | float course_to = nmea.calc_initial_bearing(target_lat, target_lon); |
tylerjw | 19:1cfe22ef30e2 | 284 | float course_diff = course_to - course; |
tylerjw | 29:8cdb56a0fe57 | 285 | |
tylerjw | 22:becb67846755 | 286 | if(course == 0.0) // not moving fast enough |
tylerjw | 22:becb67846755 | 287 | continue; // do nothing |
tylerjw | 19:1cfe22ef30e2 | 288 | |
tylerjw | 21:8799ee63c2cd | 289 | if(course_diff < course_fudge && course_diff > neg_course_fudge) { |
tylerjw | 28:032d55fa57b8 | 290 | right_turn_led = left_turn_led = 1; |
tylerjw | 21:8799ee63c2cd | 291 | Thread::wait(400); |
tylerjw | 19:1cfe22ef30e2 | 292 | continue; // don't do anything |
tylerjw | 21:8799ee63c2cd | 293 | } else if(course_diff > 180.0 || course_diff < 0.0) { |
tylerjw | 28:032d55fa57b8 | 294 | left_turn_led = 1; |
tylerjw | 30:b81274979e73 | 295 | right_s = right_pos = 0.0; |
tylerjw | 28:032d55fa57b8 | 296 | left_s = left_pos = 1.0; |
tylerjw | 21:8799ee63c2cd | 297 | Thread::wait(400); // turn left |
tylerjw | 25:81c3696ba2c9 | 298 | left_s = 0.0; |
tylerjw | 22:becb67846755 | 299 | } else { |
tylerjw | 28:032d55fa57b8 | 300 | right_turn_led = 1; |
tylerjw | 28:032d55fa57b8 | 301 | left_s = left_pos = 0.0; |
tylerjw | 28:032d55fa57b8 | 302 | right_s = right_pos = 1.0; |
tylerjw | 22:becb67846755 | 303 | Thread::wait(400); // turn righ |
tylerjw | 28:032d55fa57b8 | 304 | right_s = right_pos = 0.0; |
tylerjw | 21:8799ee63c2cd | 305 | } |
tylerjw | 29:8cdb56a0fe57 | 306 | } |
tylerjw | 29:8cdb56a0fe57 | 307 | } |
tylerjw | 29:8cdb56a0fe57 | 308 | |
tylerjw | 29:8cdb56a0fe57 | 309 | void watchdog_thread(const void *args) |
tylerjw | 29:8cdb56a0fe57 | 310 | { |
tylerjw | 29:8cdb56a0fe57 | 311 | Watchdog wdt; // NOT TESTED!!! |
tylerjw | 29:8cdb56a0fe57 | 312 | |
tylerjw | 29:8cdb56a0fe57 | 313 | wdt.kick(2.0); |
tylerjw | 29:8cdb56a0fe57 | 314 | |
tylerjw | 29:8cdb56a0fe57 | 315 | while(true) { |
tylerjw | 29:8cdb56a0fe57 | 316 | if(gps_wd && sensor_wd && parachute_wd && log_wd) { |
tylerjw | 29:8cdb56a0fe57 | 317 | wdt.kick(); |
tylerjw | 29:8cdb56a0fe57 | 318 | gps_wd = sensor_wd = parachute_wd = log_wd = false; |
tylerjw | 29:8cdb56a0fe57 | 319 | } else { |
tylerjw | 29:8cdb56a0fe57 | 320 | Thread::wait(50); |
tylerjw | 29:8cdb56a0fe57 | 321 | } |
tylerjw | 18:29b19a25a963 | 322 | } |
tylerjw | 17:4927053e120f | 323 | } |
tylerjw | 17:4927053e120f | 324 | |
tylerjw | 7:d8ecabe16c9e | 325 | int main() |
tylerjw | 7:d8ecabe16c9e | 326 | { |
tylerjw | 9:4debfbc1fb3e | 327 | Thread thread(gps_thread, NULL, osPriorityHigh); |
tylerjw | 13:db6af0620264 | 328 | Thread thread2(log_thread, NULL, osPriorityNormal); |
tylerjw | 14:dce4d8c29b17 | 329 | Thread thread3(sensor_thread, NULL, osPriorityNormal); |
tylerjw | 17:4927053e120f | 330 | Thread thread4(parachute_thread, NULL, osPriorityRealtime); |
tylerjw | 29:8cdb56a0fe57 | 331 | Thread thread5(watchdog_thread, NULL, osPriorityHigh); |
tylerjw | 13:db6af0620264 | 332 | |
tylerjw | 25:81c3696ba2c9 | 333 | while(true) { |
tylerjw | 29:8cdb56a0fe57 | 334 | Thread::wait(osWaitForever); |
tylerjw | 25:81c3696ba2c9 | 335 | } |
tylerjw | 0:ce5f06c3895f | 336 | } |