Michael Shimniok / Mbed OS DataBus2018

2018 revision to classic DataBus AVC code.

Dependencies:   LSM303DLM Servo SerialGraphicLCD L3G4200D IncrementalEncoder SimpleShell

main.cpp@44:0d72a8a1288a, 2019-01-07 (annotated)

Committer:
shimniok
Date:
Mon Jan 07 16:47:33 2019 +0000
Revision:
44:0d72a8a1288a
Parent:
43:9a285515f33a 
Rewrote TinyGPS -> NMEA, GPS::read() now returns struct

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimniok 0:7e98bbfd102a 1 /* mbed Microcontroller Library
shimniok 0:7e98bbfd102a 2 * Copyright (c) 2018 ARM Limited
shimniok 0:7e98bbfd102a 3 * SPDX-License-Identifier: Apache-2.0
shimniok 0:7e98bbfd102a 4 */
shimniok 0:7e98bbfd102a 5
shimniok 0:7e98bbfd102a 6 #include "mbed.h"
shimniok 0:7e98bbfd102a 7 #include <stdio.h>
shimniok 0:7e98bbfd102a 8 #include <errno.h>
shimniok 16:eb28d0f64a9b 9 #include "pinouts.h"
shimniok 0:7e98bbfd102a 10 #include "stats_report.h"
shimniok 0:7e98bbfd102a 11 #include "SDBlockDevice.h"
shimniok 0:7e98bbfd102a 12 #include "FATFileSystem.h"
shimniok 35:c42e7e29c3bd 13 #include "Servo.h"
shimniok 0:7e98bbfd102a 14 #include "SimpleShell.h"
shimniok 4:de7feb458652 15 #include "Config.h"
shimniok 11:8ec858b7c6d1 16 #include "Updater.h"
shimniok 43:9a285515f33a 17 //#include "Ublox6.h"
shimniok 44:0d72a8a1288a 18 #include "NMEA.h"
shimniok 24:a7f92dfc5310 19 #include "Logger.h"
shimniok 24:a7f92dfc5310 20 #include "SystemState.h"
shimniok 37:b8259500dbd3 21 #include "Display.h"
shimniok 0:7e98bbfd102a 22
shimniok 24:a7f92dfc5310 23 ///////////////////////////////////////////////////////////////////////////////
shimniok 24:a7f92dfc5310 24 // Config
shimniok 24:a7f92dfc5310 25 Config config;
shimniok 24:a7f92dfc5310 26
shimniok 42:8d99f64f5898 27
shimniok 24:a7f92dfc5310 28 ///////////////////////////////////////////////////////////////////////////////
shimniok 24:a7f92dfc5310 29 // Devices
shimniok 30:ed791f1f7f7d 30 DigitalOut led1(LED1);
shimniok 30:ed791f1f7f7d 31 DigitalOut led2(LED2);
shimniok 30:ed791f1f7f7d 32 DigitalOut led3(LED3);
shimniok 30:ed791f1f7f7d 33 DigitalOut led4(LED4);
shimniok 0:7e98bbfd102a 34 LocalFileSystem lfs("etc");
shimniok 24:a7f92dfc5310 35 SDBlockDevice bd(p5, p6, p7, p8); // MOSI, MISO, CLK, CS
shimniok 24:a7f92dfc5310 36 FATFileSystem ffs("log", &bd);
shimniok 24:a7f92dfc5310 37 Serial pc(USBTX, USBRX);
shimniok 43:9a285515f33a 38 //RawSerial s(UART1TX, UART1RX, 38400);
shimniok 43:9a285515f33a 39 RawSerial s(UART1TX, UART1RX, 19200);
shimniok 33:74c514aea0a1 40 InterruptIn lbutton(LBUTTON, PullUp); // button interrupts
shimniok 33:74c514aea0a1 41 InterruptIn cbutton(CBUTTON, PullUp);
shimniok 33:74c514aea0a1 42 InterruptIn rbutton(RBUTTON, PullUp);
shimniok 35:c42e7e29c3bd 43 Servo steer(STEERING);
shimniok 35:c42e7e29c3bd 44 Servo esc(THROTTLE);
shimniok 20:043987d06f8d 45
shimniok 19:0d1728091519 46
shimniok 24:a7f92dfc5310 47 ///////////////////////////////////////////////////////////////////////////////
shimniok 24:a7f92dfc5310 48 // Logging
shimniok 29:cb2f55fbfe9c 49 EventQueue logQueue(16 * EVENTS_EVENT_SIZE);
shimniok 31:20a95043adb0 50 Logger logger;
shimniok 25:b8176ebb96c6 51
shimniok 42:8d99f64f5898 52
shimniok 42:8d99f64f5898 53 ///////////////////////////////////////////////////////////////////////////////
shimniok 42:8d99f64f5898 54 // Display
shimniok 42:8d99f64f5898 55 EventQueue lcdQueue(8 * EVENTS_EVENT_SIZE);
shimniok 42:8d99f64f5898 56 Display display(UART3TX, UART3RX);
shimniok 42:8d99f64f5898 57
shimniok 42:8d99f64f5898 58 const uint64_t sample_time_ms = 50;
shimniok 42:8d99f64f5898 59
shimniok 42:8d99f64f5898 60 void compute_cpu_stats()
shimniok 42:8d99f64f5898 61 {
shimniok 42:8d99f64f5898 62 static uint64_t prev_idle_time = 0;
shimniok 42:8d99f64f5898 63 mbed_stats_cpu_t cpu_stats;
shimniok 42:8d99f64f5898 64
shimniok 42:8d99f64f5898 65 // Collect and print cpu stats
shimniok 42:8d99f64f5898 66 mbed_stats_cpu_get(&cpu_stats);
shimniok 42:8d99f64f5898 67
shimniok 42:8d99f64f5898 68 uint64_t diff = (cpu_stats.idle_time - prev_idle_time);
shimniok 42:8d99f64f5898 69 uint8_t idle = (diff * 100) / (sample_time_ms * 1000); // usec;
shimniok 42:8d99f64f5898 70 prev_idle_time = cpu_stats.idle_time;
shimniok 42:8d99f64f5898 71 display.cpu(idle);
shimniok 42:8d99f64f5898 72 }
shimniok 42:8d99f64f5898 73
shimniok 25:b8176ebb96c6 74 ///////////////////////////////////////////////////////////////////////////////
shimniok 25:b8176ebb96c6 75 // Updater
shimniok 25:b8176ebb96c6 76 Updater *u = Updater::instance();
shimniok 26:2dc31a801cc8 77 EventQueue updaterQueue(8 * EVENTS_EVENT_SIZE);
shimniok 25:b8176ebb96c6 78
shimniok 25:b8176ebb96c6 79 void updater_callback() {
shimniok 30:ed791f1f7f7d 80 SensorData d;
shimniok 30:ed791f1f7f7d 81 float dt;
shimniok 42:8d99f64f5898 82 static int count = 0;
shimniok 30:ed791f1f7f7d 83
shimniok 25:b8176ebb96c6 84 led1 = !led1;
shimniok 30:ed791f1f7f7d 85 d.timestamp = Kernel::get_ms_count();
shimniok 30:ed791f1f7f7d 86 d.encoder = u->encoder();
shimniok 32:eb673f6f5734 87 u->imu(d.gyro, d.accel, d.mag, dt);
shimniok 30:ed791f1f7f7d 88 logQueue.call(&logger, &Logger::log_sensors, d);
shimniok 42:8d99f64f5898 89 if (count++ > 10) {
shimniok 42:8d99f64f5898 90 lcdQueue.call(&display, &Display::imu, d);
shimniok 42:8d99f64f5898 91 count = 0;
shimniok 42:8d99f64f5898 92 }
shimniok 25:b8176ebb96c6 93 }
shimniok 25:b8176ebb96c6 94
shimniok 42:8d99f64f5898 95
shimniok 24:a7f92dfc5310 96 ///////////////////////////////////////////////////////////////////////////////
shimniok 30:ed791f1f7f7d 97 // Buttons
shimniok 30:ed791f1f7f7d 98 EventQueue buttonQueue(16 * EVENTS_EVENT_SIZE);
shimniok 33:74c514aea0a1 99 const int BUTTON_DEBOUNCE_SAMPLES = 20;
shimniok 33:74c514aea0a1 100 const int BUTTON_DEBOUNCE_THRESH = 8;
shimniok 33:74c514aea0a1 101 const int BUTTON_SAMPLE_MS = 0.005;
shimniok 30:ed791f1f7f7d 102
shimniok 30:ed791f1f7f7d 103 void button_event() {
shimniok 33:74c514aea0a1 104 int samples = 0;
shimniok 33:74c514aea0a1 105
shimniok 33:74c514aea0a1 106 // disable subsequent interrupts
shimniok 33:74c514aea0a1 107 lbutton.fall(NULL);
shimniok 33:74c514aea0a1 108
shimniok 33:74c514aea0a1 109 // sample repeatedly to debounce
shimniok 33:74c514aea0a1 110 for (int i=0; i < BUTTON_DEBOUNCE_SAMPLES; i++) {
shimniok 33:74c514aea0a1 111 if (lbutton == 0) samples++;
shimniok 33:74c514aea0a1 112 wait(BUTTON_SAMPLE_MS);
shimniok 33:74c514aea0a1 113 }
shimniok 33:74c514aea0a1 114
shimniok 33:74c514aea0a1 115 if (samples > BUTTON_DEBOUNCE_THRESH) {
shimniok 30:ed791f1f7f7d 116 if (logger.enabled()) {
shimniok 30:ed791f1f7f7d 117 logQueue.call(&logger, &Logger::stop);
shimniok 30:ed791f1f7f7d 118 led3 = 0;
shimniok 30:ed791f1f7f7d 119 } else {
shimniok 30:ed791f1f7f7d 120 logQueue.call(&logger, &Logger::start);
shimniok 30:ed791f1f7f7d 121 led3 = 1;
shimniok 30:ed791f1f7f7d 122 }
shimniok 30:ed791f1f7f7d 123 }
shimniok 33:74c514aea0a1 124
shimniok 33:74c514aea0a1 125 lbutton.fall(buttonQueue.event(button_event));
shimniok 30:ed791f1f7f7d 126 }
shimniok 30:ed791f1f7f7d 127
shimniok 30:ed791f1f7f7d 128
shimniok 30:ed791f1f7f7d 129 ///////////////////////////////////////////////////////////////////////////////
shimniok 24:a7f92dfc5310 130 // GPS
shimniok 43:9a285515f33a 131 //Ublox6 ublox;
shimniok 44:0d72a8a1288a 132 NMEA nmea;
shimniok 24:a7f92dfc5310 133 EventQueue gpsQueue(8 * EVENTS_EVENT_SIZE);
shimniok 23:5e61cf4a8c34 134
shimniok 23:5e61cf4a8c34 135 // Callback for gps parse data ready
shimniok 23:5e61cf4a8c34 136 void gps_callback() {
shimniok 44:0d72a8a1288a 137 GPS::gps_data_t data;
shimniok 24:a7f92dfc5310 138 GpsData d;
shimniok 24:a7f92dfc5310 139
shimniok 25:b8176ebb96c6 140 led2 = !led2;
shimniok 43:9a285515f33a 141 //ublox.read(d.latitude, d.longitude, d.course, d.speed, d.hdop, d.svcount);
shimniok 44:0d72a8a1288a 142 data = nmea.read();
shimniok 44:0d72a8a1288a 143 d.latitude = data.lat;
shimniok 44:0d72a8a1288a 144 d.longitude = data.lon;
shimniok 44:0d72a8a1288a 145 d.course = data.course;
shimniok 44:0d72a8a1288a 146 d.speed = data.speed;
shimniok 44:0d72a8a1288a 147 d.hdop = data.hdop;
shimniok 44:0d72a8a1288a 148 d.svcount = data.svcount;
shimniok 30:ed791f1f7f7d 149 d.timestamp = Kernel::get_ms_count();
shimniok 29:cb2f55fbfe9c 150 logQueue.call(&logger, &Logger::log_gps, d);
shimniok 39:465213249f71 151 lcdQueue.call(&display, &Display::gps, d);
shimniok 23:5e61cf4a8c34 152 }
shimniok 29:cb2f55fbfe9c 153
shimniok 22:4d62bd16f037 154 // ISR for GPS serial, passes off to thread
shimniok 20:043987d06f8d 155 void gps_handler() {
shimniok 22:4d62bd16f037 156 while (s.readable()) {
shimniok 43:9a285515f33a 157 char c = s.getc();
shimniok 43:9a285515f33a 158 //gpsQueue.call(&ublox, &Ublox6::parse, c);
shimniok 44:0d72a8a1288a 159 gpsQueue.call(&nmea, &NMEA::parse, c);
shimniok 16:eb28d0f64a9b 160 }
shimniok 19:0d1728091519 161 }
shimniok 16:eb28d0f64a9b 162
shimniok 42:8d99f64f5898 163
shimniok 24:a7f92dfc5310 164 ///////////////////////////////////////////////////////////////////////////////
shimniok 24:a7f92dfc5310 165 // Shell
shimniok 30:ed791f1f7f7d 166 SimpleShell sh("/log");
shimniok 9:fc3575d2cbbf 167
shimniok 24:a7f92dfc5310 168 void test(int argc, char **argv) {
shimniok 35:c42e7e29c3bd 169 //printf("Hello world!\n");
shimniok 35:c42e7e29c3bd 170 char file[32];
shimniok 35:c42e7e29c3bd 171
shimniok 35:c42e7e29c3bd 172 for (int i=30; i < 40; i++) {
shimniok 35:c42e7e29c3bd 173 sprintf(file, "/log/%04d.csv", i);
shimniok 35:c42e7e29c3bd 174 printf("removing <%s>\n", file);
shimniok 35:c42e7e29c3bd 175 int stat = remove(file);
shimniok 35:c42e7e29c3bd 176 printf("return: %d\n", stat);
shimniok 35:c42e7e29c3bd 177 }
shimniok 1:7019a60fd585 178 }
shimniok 1:7019a60fd585 179
shimniok 24:a7f92dfc5310 180 void stats(int argc, char **argv) {
shimniok 24:a7f92dfc5310 181 SystemReport r(200);
shimniok 24:a7f92dfc5310 182 r.report_state();
shimniok 24:a7f92dfc5310 183 return;
shimniok 24:a7f92dfc5310 184 }
shimniok 24:a7f92dfc5310 185
shimniok 24:a7f92dfc5310 186 void read_gps(int argc, char **argv)
shimniok 18:3f8a8f6e3cc1 187 {
shimniok 44:0d72a8a1288a 188 GPS::gps_data_t d;
shimniok 18:3f8a8f6e3cc1 189
shimniok 44:0d72a8a1288a 190 d = nmea.read();
shimniok 44:0d72a8a1288a 191 printf("%3.7f %3.7f\n", d.lat, d.lon);
shimniok 44:0d72a8a1288a 192 printf("hdg=%03.1f deg spd=%3.1f m/s\n", d.course, d.speed);
shimniok 44:0d72a8a1288a 193 printf("%d %f\n", d.svcount, d.hdop);
shimniok 18:3f8a8f6e3cc1 194 }
shimniok 18:3f8a8f6e3cc1 195
shimniok 32:eb673f6f5734 196 void read_imu(int argc, char **argv)
shimniok 9:fc3575d2cbbf 197 {
shimniok 12:3cd91e150d9c 198 int g[3];
shimniok 32:eb673f6f5734 199 int a[3];
shimniok 32:eb673f6f5734 200 int m[3];
shimniok 13:5566df1250f1 201 float dt;
shimniok 12:3cd91e150d9c 202
shimniok 11:8ec858b7c6d1 203 Updater *u = Updater::instance();
shimniok 11:8ec858b7c6d1 204
shimniok 32:eb673f6f5734 205 u->imu(g, a, m, dt);
shimniok 32:eb673f6f5734 206
shimniok 32:eb673f6f5734 207 printf(" Gyro: %d, %d, %d\n", g[0], g[1], g[2]);
shimniok 32:eb673f6f5734 208 printf(" Accel: %d, %d, %d\n", a[0], a[1], a[2]);
shimniok 32:eb673f6f5734 209 printf(" Mag: %d, %d, %d\n", m[0], m[1], m[2]);
shimniok 32:eb673f6f5734 210 printf(" dt: %f\n", dt);
shimniok 12:3cd91e150d9c 211 }
shimniok 12:3cd91e150d9c 212
shimniok 24:a7f92dfc5310 213 void read_enc(int argc, char **argv)
shimniok 14:1dd83e626153 214 {
shimniok 14:1dd83e626153 215 Updater *u = Updater::instance();
shimniok 14:1dd83e626153 216
shimniok 14:1dd83e626153 217 printf("Encoder: %d\n", u->encoder());
shimniok 14:1dd83e626153 218 }
shimniok 14:1dd83e626153 219
shimniok 29:cb2f55fbfe9c 220 void log(int argc, char **argv)
shimniok 29:cb2f55fbfe9c 221 {
shimniok 29:cb2f55fbfe9c 222 char *usage = "usage: log [start|stop]";
shimniok 29:cb2f55fbfe9c 223
shimniok 29:cb2f55fbfe9c 224 if (argc == 1) {
shimniok 29:cb2f55fbfe9c 225 printf("logging ");
shimniok 29:cb2f55fbfe9c 226 if (logger.enabled())
shimniok 29:cb2f55fbfe9c 227 printf("enabled");
shimniok 29:cb2f55fbfe9c 228 else
shimniok 29:cb2f55fbfe9c 229 printf("disabled");
shimniok 29:cb2f55fbfe9c 230 printf("\n");
shimniok 29:cb2f55fbfe9c 231 } else if (argc == 2) {
shimniok 29:cb2f55fbfe9c 232 if (!strcmp("start", argv[1])) {
shimniok 29:cb2f55fbfe9c 233 logger.start();
shimniok 29:cb2f55fbfe9c 234 } else if (!strcmp("stop", argv[1])) {
shimniok 29:cb2f55fbfe9c 235 logger.stop();
shimniok 29:cb2f55fbfe9c 236 } else {
shimniok 29:cb2f55fbfe9c 237 puts(usage);
shimniok 29:cb2f55fbfe9c 238 }
shimniok 29:cb2f55fbfe9c 239 } else {
shimniok 29:cb2f55fbfe9c 240 puts(usage);
shimniok 29:cb2f55fbfe9c 241 }
shimniok 29:cb2f55fbfe9c 242 }
shimniok 29:cb2f55fbfe9c 243
shimniok 29:cb2f55fbfe9c 244
shimniok 24:a7f92dfc5310 245 void bridge_uart1(int argc, char **argv) {
shimniok 20:043987d06f8d 246 RawSerial s(UART1TX, UART1RX, 38400);
shimniok 20:043987d06f8d 247 while (1) {
shimniok 20:043987d06f8d 248 if (pc.readable()) s.putc(pc.getc());
shimniok 20:043987d06f8d 249 if (s.readable()) pc.putc(s.getc());
shimniok 20:043987d06f8d 250 }
shimniok 20:043987d06f8d 251 }
shimniok 20:043987d06f8d 252
shimniok 42:8d99f64f5898 253
shimniok 42:8d99f64f5898 254 void bridge_uart2() {
shimniok 42:8d99f64f5898 255 RawSerial s(UART2TX, UART2RX, 19200);
shimniok 42:8d99f64f5898 256 while (1) {
shimniok 42:8d99f64f5898 257 if (pc.readable()) s.putc(pc.getc());
shimniok 42:8d99f64f5898 258 if (s.readable()) pc.putc(s.getc());
shimniok 42:8d99f64f5898 259 }
shimniok 42:8d99f64f5898 260 }
shimniok 42:8d99f64f5898 261
shimniok 42:8d99f64f5898 262
shimniok 24:a7f92dfc5310 263 void reset(int argc, char **argv)
shimniok 12:3cd91e150d9c 264 {
shimniok 12:3cd91e150d9c 265 NVIC_SystemReset();
shimniok 9:fc3575d2cbbf 266 }
shimniok 9:fc3575d2cbbf 267
shimniok 24:a7f92dfc5310 268 ///////////////////////////////////////////////////////////////////////////////
shimniok 24:a7f92dfc5310 269 // MAIN
shimniok 1:7019a60fd585 270
shimniok 0:7e98bbfd102a 271 // main() runs in its own thread in the OS
shimniok 0:7e98bbfd102a 272 int main()
shimniok 11:8ec858b7c6d1 273 {
shimniok 20:043987d06f8d 274 //bridge_uart1();
shimniok 43:9a285515f33a 275 //bridge_uart2();
shimniok 24:a7f92dfc5310 276
shimniok 25:b8176ebb96c6 277 //Kernel::attach_idle_hook(idler);
shimniok 20:043987d06f8d 278
shimniok 37:b8259500dbd3 279 display.status("Bootup...");
shimniok 0:7e98bbfd102a 280 printf("Bootup...\n");
shimniok 0:7e98bbfd102a 281 fflush(stdout);
shimniok 1:7019a60fd585 282
shimniok 35:c42e7e29c3bd 283 steer = 0.50;
shimniok 35:c42e7e29c3bd 284 esc = 0.00;
shimniok 35:c42e7e29c3bd 285
shimniok 4:de7feb458652 286 printf("Loading config...\n");
shimniok 37:b8259500dbd3 287 display.status("Loading config");
shimniok 7:1f2661b840ed 288 config.add("intercept_distance", Config::DOUBLE);
shimniok 7:1f2661b840ed 289 config.add("waypoint_threshold", Config::DOUBLE);
shimniok 7:1f2661b840ed 290 config.add("minimum_turning_radius", Config::DOUBLE);
shimniok 7:1f2661b840ed 291 config.add("wheelbase", Config::DOUBLE);
shimniok 7:1f2661b840ed 292 config.add("track_width", Config::DOUBLE);
shimniok 7:1f2661b840ed 293 config.add("tire_circumference", Config::DOUBLE);
shimniok 7:1f2661b840ed 294 config.add("encoder_stripes", Config::INT);
shimniok 7:1f2661b840ed 295 config.add("esc_brake", Config::INT);
shimniok 7:1f2661b840ed 296 config.add("esc_off", Config::INT);
shimniok 7:1f2661b840ed 297 config.add("esc_max", Config::INT);
shimniok 7:1f2661b840ed 298 config.add("turn_speed", Config::DOUBLE);
shimniok 7:1f2661b840ed 299 config.add("turn_distance", Config::DOUBLE);
shimniok 7:1f2661b840ed 300 config.add("start_speed", Config::DOUBLE);
shimniok 7:1f2661b840ed 301 config.add("cruise_speed", Config::DOUBLE);
shimniok 7:1f2661b840ed 302 config.add("speed_kp", Config::DOUBLE);
shimniok 7:1f2661b840ed 303 config.add("speed_ki", Config::DOUBLE);
shimniok 7:1f2661b840ed 304 config.add("speed_kd", Config::DOUBLE);
shimniok 7:1f2661b840ed 305 config.add("steer_center", Config::DOUBLE);
shimniok 7:1f2661b840ed 306 config.add("steer_scale", Config::DOUBLE);
shimniok 7:1f2661b840ed 307 config.add("gyro_scale", Config::DOUBLE);
shimniok 7:1f2661b840ed 308 config.add("gps_valid_speed", Config::DOUBLE);
shimniok 35:c42e7e29c3bd 309
shimniok 4:de7feb458652 310 if (config.load("/etc/2018cfg.txt")) {
shimniok 4:de7feb458652 311 printf("error loading config\n");
shimniok 37:b8259500dbd3 312 display.status("config load error");
shimniok 4:de7feb458652 313 }
shimniok 10:9fb3feb38746 314
shimniok 38:6fec81f85221 315 printf("Starting display...\n");
shimniok 39:465213249f71 316 Thread lcdThread(osPriorityNormal, 2048, 0, "lcd");
shimniok 38:6fec81f85221 317 lcdThread.start(callback(&lcdQueue, &EventQueue::dispatch_forever));
shimniok 42:8d99f64f5898 318 //lcdQueue.call_every(sample_time_ms, callback(&compute_cpu_stats));
shimniok 38:6fec81f85221 319
shimniok 30:ed791f1f7f7d 320 printf("Starting buttons...\n");
shimniok 37:b8259500dbd3 321 display.status("Starting buttons");
shimniok 33:74c514aea0a1 322 lbutton.fall(buttonQueue.event(button_event));
shimniok 30:ed791f1f7f7d 323 //cbutton.rise(buttonQueue.event(button_event));
shimniok 30:ed791f1f7f7d 324 //rbutton.rise(buttonQueue.event(button_event));
shimniok 30:ed791f1f7f7d 325 Thread buttonThread(osPriorityNormal, 256, 0, "buttons");
shimniok 30:ed791f1f7f7d 326 buttonThread.start(callback(&buttonQueue, &EventQueue::dispatch_forever));
shimniok 30:ed791f1f7f7d 327
shimniok 24:a7f92dfc5310 328 printf("Starting updater...\n");
shimniok 37:b8259500dbd3 329 display.status("Starting updater");
shimniok 25:b8176ebb96c6 330 u->attach(updater_callback);
shimniok 24:a7f92dfc5310 331 Thread updaterThread(osPriorityRealtime, 512, 0, "updater");
shimniok 26:2dc31a801cc8 332 updaterQueue.call_every(20, u, &Updater::update);
shimniok 26:2dc31a801cc8 333 updaterThread.start(callback(&updaterQueue, &EventQueue::dispatch_forever));
shimniok 24:a7f92dfc5310 334
shimniok 24:a7f92dfc5310 335 printf("Starting gps...\n");
shimniok 37:b8259500dbd3 336 display.status("Starting gps");
shimniok 29:cb2f55fbfe9c 337 Thread gpsThread(osPriorityHigh, 2048, 0, "gps");
shimniok 24:a7f92dfc5310 338 gpsThread.start(callback(&gpsQueue, &EventQueue::dispatch_forever));
shimniok 44:0d72a8a1288a 339 nmea.subscribe(gps_callback);
shimniok 24:a7f92dfc5310 340 s.attach(gps_handler);
shimniok 24:a7f92dfc5310 341
shimniok 37:b8259500dbd3 342 printf("Starting logging...\n");
shimniok 37:b8259500dbd3 343 display.status("Starting logging");
shimniok 29:cb2f55fbfe9c 344 Thread logThread(osPriorityNormal, 2048, 0, "log");
shimniok 24:a7f92dfc5310 345 logThread.start(callback(&logQueue, &EventQueue::dispatch_forever));
shimniok 24:a7f92dfc5310 346
shimniok 13:5566df1250f1 347 printf("Starting shell...\n");
shimniok 37:b8259500dbd3 348 display.status("Starting shell");
shimniok 30:ed791f1f7f7d 349 sh.command(test, "test");
shimniok 32:eb673f6f5734 350 sh.command(read_imu, "imu");
shimniok 30:ed791f1f7f7d 351 sh.command(read_enc, "enc");
shimniok 30:ed791f1f7f7d 352 sh.command(read_gps, "gps");
shimniok 30:ed791f1f7f7d 353 sh.command(reset, "reset");
shimniok 30:ed791f1f7f7d 354 sh.command(stats, "stats");
shimniok 30:ed791f1f7f7d 355 sh.command(log, "log");
shimniok 37:b8259500dbd3 356
shimniok 37:b8259500dbd3 357 display.status("DataBus 2018");
shimniok 24:a7f92dfc5310 358 sh.run();
shimniok 22:4d62bd16f037 359
shimniok 0:7e98bbfd102a 360 while (true) {
shimniok 0:7e98bbfd102a 361 // Blink LED and wait 0.5 seconds
shimniok 0:7e98bbfd102a 362 led1 = !led1;
shimniok 0:7e98bbfd102a 363 wait(0.5f);
shimniok 0:7e98bbfd102a 364 }
shimniok 0:7e98bbfd102a 365 }