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?
User | Revision | Line number | New 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 | } |