2018 revision to classic DataBus AVC code.
Dependencies: LSM303DLM Servo SerialGraphicLCD L3G4200D IncrementalEncoder SimpleShell
main.cpp@25:b8176ebb96c6, 2018-12-21 (annotated)
- Committer:
- shimniok
- Date:
- Fri Dec 21 20:38:55 2018 +0000
- Revision:
- 25:b8176ebb96c6
- Parent:
- 24:a7f92dfc5310
- Child:
- 26:2dc31a801cc8
Extracted eventqueue from Updater, implemented callback
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 | 0:7e98bbfd102a | 13 | #include "SimpleShell.h" |
shimniok | 4:de7feb458652 | 14 | #include "Config.h" |
shimniok | 11:8ec858b7c6d1 | 15 | #include "Updater.h" |
shimniok | 16:eb28d0f64a9b | 16 | #include "Ublox6.h" |
shimniok | 24:a7f92dfc5310 | 17 | #include "Logger.h" |
shimniok | 24:a7f92dfc5310 | 18 | #include "SystemState.h" |
shimniok | 0:7e98bbfd102a | 19 | |
shimniok | 24:a7f92dfc5310 | 20 | /////////////////////////////////////////////////////////////////////////////// |
shimniok | 24:a7f92dfc5310 | 21 | // Config |
shimniok | 24:a7f92dfc5310 | 22 | Config config; |
shimniok | 24:a7f92dfc5310 | 23 | |
shimniok | 24:a7f92dfc5310 | 24 | /////////////////////////////////////////////////////////////////////////////// |
shimniok | 24:a7f92dfc5310 | 25 | // Devices |
shimniok | 0:7e98bbfd102a | 26 | LocalFileSystem lfs("etc"); |
shimniok | 24:a7f92dfc5310 | 27 | SDBlockDevice bd(p5, p6, p7, p8); // MOSI, MISO, CLK, CS |
shimniok | 24:a7f92dfc5310 | 28 | FATFileSystem ffs("log", &bd); |
shimniok | 24:a7f92dfc5310 | 29 | Serial pc(USBTX, USBRX); |
shimniok | 0:7e98bbfd102a | 30 | DigitalOut led1(LED1); |
shimniok | 25:b8176ebb96c6 | 31 | DigitalOut led2(LED2); |
shimniok | 20:043987d06f8d | 32 | RawSerial s(UART1TX, UART1RX, 38400); |
shimniok | 20:043987d06f8d | 33 | |
shimniok | 24:a7f92dfc5310 | 34 | /////////////////////////////////////////////////////////////////////////////// |
shimniok | 24:a7f92dfc5310 | 35 | // Idle hook |
shimniok | 24:a7f92dfc5310 | 36 | void idler() { |
shimniok | 24:a7f92dfc5310 | 37 | while(1) { |
shimniok | 24:a7f92dfc5310 | 38 | led1 = !led1; |
shimniok | 24:a7f92dfc5310 | 39 | wait(0.1); |
shimniok | 24:a7f92dfc5310 | 40 | } |
shimniok | 24:a7f92dfc5310 | 41 | } |
shimniok | 19:0d1728091519 | 42 | |
shimniok | 24:a7f92dfc5310 | 43 | /////////////////////////////////////////////////////////////////////////////// |
shimniok | 24:a7f92dfc5310 | 44 | // Logging |
shimniok | 24:a7f92dfc5310 | 45 | EventQueue logQueue(8 * EVENTS_EVENT_SIZE); |
shimniok | 24:a7f92dfc5310 | 46 | Logger logger("/etc/test.log"); |
shimniok | 24:a7f92dfc5310 | 47 | |
shimniok | 25:b8176ebb96c6 | 48 | |
shimniok | 25:b8176ebb96c6 | 49 | /////////////////////////////////////////////////////////////////////////////// |
shimniok | 25:b8176ebb96c6 | 50 | // Updater |
shimniok | 25:b8176ebb96c6 | 51 | Updater *u = Updater::instance(); |
shimniok | 25:b8176ebb96c6 | 52 | |
shimniok | 25:b8176ebb96c6 | 53 | void updater_callback() { |
shimniok | 25:b8176ebb96c6 | 54 | led1 = !led1; |
shimniok | 25:b8176ebb96c6 | 55 | } |
shimniok | 25:b8176ebb96c6 | 56 | |
shimniok | 24:a7f92dfc5310 | 57 | /////////////////////////////////////////////////////////////////////////////// |
shimniok | 24:a7f92dfc5310 | 58 | // GPS |
shimniok | 24:a7f92dfc5310 | 59 | Ublox6 ublox; |
shimniok | 24:a7f92dfc5310 | 60 | EventQueue gpsQueue(8 * EVENTS_EVENT_SIZE); |
shimniok | 23:5e61cf4a8c34 | 61 | |
shimniok | 23:5e61cf4a8c34 | 62 | // Callback for gps parse data ready |
shimniok | 23:5e61cf4a8c34 | 63 | void gps_callback() { |
shimniok | 24:a7f92dfc5310 | 64 | GpsData d; |
shimniok | 24:a7f92dfc5310 | 65 | |
shimniok | 25:b8176ebb96c6 | 66 | led2 = !led2; |
shimniok | 24:a7f92dfc5310 | 67 | ublox.read(d.latitude, d.longitude, d.course, d.speed, d.hdop, d.svcount); |
shimniok | 24:a7f92dfc5310 | 68 | //logQueue.call(&logger, &Logger::log_gps, d); |
shimniok | 23:5e61cf4a8c34 | 69 | } |
shimniok | 23:5e61cf4a8c34 | 70 | |
shimniok | 22:4d62bd16f037 | 71 | // ISR for GPS serial, passes off to thread |
shimniok | 20:043987d06f8d | 72 | void gps_handler() { |
shimniok | 22:4d62bd16f037 | 73 | while (s.readable()) { |
shimniok | 22:4d62bd16f037 | 74 | int c = s.getc(); |
shimniok | 22:4d62bd16f037 | 75 | gpsQueue.call(&ublox, &Ublox6::parse, c); |
shimniok | 16:eb28d0f64a9b | 76 | } |
shimniok | 19:0d1728091519 | 77 | } |
shimniok | 16:eb28d0f64a9b | 78 | |
shimniok | 24:a7f92dfc5310 | 79 | /////////////////////////////////////////////////////////////////////////////// |
shimniok | 24:a7f92dfc5310 | 80 | // Shell |
shimniok | 24:a7f92dfc5310 | 81 | SimpleShell sh; |
shimniok | 9:fc3575d2cbbf | 82 | |
shimniok | 24:a7f92dfc5310 | 83 | void test(int argc, char **argv) { |
shimniok | 1:7019a60fd585 | 84 | printf("Hello world!\n"); |
shimniok | 1:7019a60fd585 | 85 | } |
shimniok | 1:7019a60fd585 | 86 | |
shimniok | 24:a7f92dfc5310 | 87 | void stats(int argc, char **argv) { |
shimniok | 24:a7f92dfc5310 | 88 | SystemReport r(200); |
shimniok | 24:a7f92dfc5310 | 89 | r.report_state(); |
shimniok | 24:a7f92dfc5310 | 90 | return; |
shimniok | 24:a7f92dfc5310 | 91 | } |
shimniok | 24:a7f92dfc5310 | 92 | |
shimniok | 24:a7f92dfc5310 | 93 | void read_gps(int argc, char **argv) |
shimniok | 18:3f8a8f6e3cc1 | 94 | { |
shimniok | 18:3f8a8f6e3cc1 | 95 | double lat=0; |
shimniok | 18:3f8a8f6e3cc1 | 96 | double lon=0; |
shimniok | 18:3f8a8f6e3cc1 | 97 | float course=0; |
shimniok | 18:3f8a8f6e3cc1 | 98 | float speed=0; |
shimniok | 18:3f8a8f6e3cc1 | 99 | float hdop=0.0; |
shimniok | 18:3f8a8f6e3cc1 | 100 | int svcount=0; |
shimniok | 18:3f8a8f6e3cc1 | 101 | |
shimniok | 18:3f8a8f6e3cc1 | 102 | ublox.read(lat, lon, course, speed, hdop, svcount); |
shimniok | 18:3f8a8f6e3cc1 | 103 | printf("%3.7f %3.7f\n", lat, lon); |
shimniok | 18:3f8a8f6e3cc1 | 104 | printf("hdg=%03.1f deg spd=%3.1f m/s\n", course, speed); |
shimniok | 18:3f8a8f6e3cc1 | 105 | printf("%d %f\n", svcount, hdop); |
shimniok | 18:3f8a8f6e3cc1 | 106 | } |
shimniok | 18:3f8a8f6e3cc1 | 107 | |
shimniok | 24:a7f92dfc5310 | 108 | void read_gyro(int argc, char **argv) |
shimniok | 9:fc3575d2cbbf | 109 | { |
shimniok | 12:3cd91e150d9c | 110 | int g[3]; |
shimniok | 13:5566df1250f1 | 111 | float dt; |
shimniok | 12:3cd91e150d9c | 112 | |
shimniok | 11:8ec858b7c6d1 | 113 | Updater *u = Updater::instance(); |
shimniok | 11:8ec858b7c6d1 | 114 | |
shimniok | 13:5566df1250f1 | 115 | u->gyro(g, dt); |
shimniok | 11:8ec858b7c6d1 | 116 | |
shimniok | 13:5566df1250f1 | 117 | printf("Gyro: %d, %d, %d - dt: %f\n", g[0], g[1], g[2], dt); |
shimniok | 12:3cd91e150d9c | 118 | } |
shimniok | 12:3cd91e150d9c | 119 | |
shimniok | 24:a7f92dfc5310 | 120 | void read_enc(int argc, char **argv) |
shimniok | 14:1dd83e626153 | 121 | { |
shimniok | 14:1dd83e626153 | 122 | Updater *u = Updater::instance(); |
shimniok | 14:1dd83e626153 | 123 | |
shimniok | 14:1dd83e626153 | 124 | printf("Encoder: %d\n", u->encoder()); |
shimniok | 14:1dd83e626153 | 125 | } |
shimniok | 14:1dd83e626153 | 126 | |
shimniok | 24:a7f92dfc5310 | 127 | void bridge_uart1(int argc, char **argv) { |
shimniok | 20:043987d06f8d | 128 | RawSerial s(UART1TX, UART1RX, 38400); |
shimniok | 20:043987d06f8d | 129 | while (1) { |
shimniok | 20:043987d06f8d | 130 | if (pc.readable()) s.putc(pc.getc()); |
shimniok | 20:043987d06f8d | 131 | if (s.readable()) pc.putc(s.getc()); |
shimniok | 20:043987d06f8d | 132 | } |
shimniok | 20:043987d06f8d | 133 | } |
shimniok | 20:043987d06f8d | 134 | |
shimniok | 24:a7f92dfc5310 | 135 | void reset(int argc, char **argv) |
shimniok | 12:3cd91e150d9c | 136 | { |
shimniok | 12:3cd91e150d9c | 137 | NVIC_SystemReset(); |
shimniok | 9:fc3575d2cbbf | 138 | } |
shimniok | 9:fc3575d2cbbf | 139 | |
shimniok | 24:a7f92dfc5310 | 140 | /////////////////////////////////////////////////////////////////////////////// |
shimniok | 24:a7f92dfc5310 | 141 | // MAIN |
shimniok | 1:7019a60fd585 | 142 | |
shimniok | 0:7e98bbfd102a | 143 | // main() runs in its own thread in the OS |
shimniok | 0:7e98bbfd102a | 144 | int main() |
shimniok | 11:8ec858b7c6d1 | 145 | { |
shimniok | 20:043987d06f8d | 146 | //bridge_uart1(); |
shimniok | 24:a7f92dfc5310 | 147 | |
shimniok | 25:b8176ebb96c6 | 148 | //Kernel::attach_idle_hook(idler); |
shimniok | 20:043987d06f8d | 149 | |
shimniok | 0:7e98bbfd102a | 150 | printf("Bootup...\n"); |
shimniok | 0:7e98bbfd102a | 151 | fflush(stdout); |
shimniok | 1:7019a60fd585 | 152 | |
shimniok | 4:de7feb458652 | 153 | printf("Loading config...\n"); |
shimniok | 7:1f2661b840ed | 154 | config.add("intercept_distance", Config::DOUBLE); |
shimniok | 7:1f2661b840ed | 155 | config.add("waypoint_threshold", Config::DOUBLE); |
shimniok | 7:1f2661b840ed | 156 | config.add("minimum_turning_radius", Config::DOUBLE); |
shimniok | 7:1f2661b840ed | 157 | config.add("wheelbase", Config::DOUBLE); |
shimniok | 7:1f2661b840ed | 158 | config.add("track_width", Config::DOUBLE); |
shimniok | 7:1f2661b840ed | 159 | config.add("tire_circumference", Config::DOUBLE); |
shimniok | 7:1f2661b840ed | 160 | config.add("encoder_stripes", Config::INT); |
shimniok | 7:1f2661b840ed | 161 | config.add("esc_brake", Config::INT); |
shimniok | 7:1f2661b840ed | 162 | config.add("esc_off", Config::INT); |
shimniok | 7:1f2661b840ed | 163 | config.add("esc_max", Config::INT); |
shimniok | 7:1f2661b840ed | 164 | config.add("turn_speed", Config::DOUBLE); |
shimniok | 7:1f2661b840ed | 165 | config.add("turn_distance", Config::DOUBLE); |
shimniok | 7:1f2661b840ed | 166 | config.add("start_speed", Config::DOUBLE); |
shimniok | 7:1f2661b840ed | 167 | config.add("cruise_speed", Config::DOUBLE); |
shimniok | 7:1f2661b840ed | 168 | config.add("speed_kp", Config::DOUBLE); |
shimniok | 7:1f2661b840ed | 169 | config.add("speed_ki", Config::DOUBLE); |
shimniok | 7:1f2661b840ed | 170 | config.add("speed_kd", Config::DOUBLE); |
shimniok | 7:1f2661b840ed | 171 | config.add("steer_center", Config::DOUBLE); |
shimniok | 7:1f2661b840ed | 172 | config.add("steer_scale", Config::DOUBLE); |
shimniok | 7:1f2661b840ed | 173 | config.add("gyro_scale", Config::DOUBLE); |
shimniok | 7:1f2661b840ed | 174 | config.add("gps_valid_speed", Config::DOUBLE); |
shimniok | 7:1f2661b840ed | 175 | |
shimniok | 4:de7feb458652 | 176 | if (config.load("/etc/2018cfg.txt")) { |
shimniok | 4:de7feb458652 | 177 | printf("error loading config\n"); |
shimniok | 4:de7feb458652 | 178 | } |
shimniok | 10:9fb3feb38746 | 179 | |
shimniok | 24:a7f92dfc5310 | 180 | printf("Starting updater...\n"); |
shimniok | 25:b8176ebb96c6 | 181 | u->attach(updater_callback); |
shimniok | 24:a7f92dfc5310 | 182 | Thread updaterThread(osPriorityRealtime, 512, 0, "updater"); |
shimniok | 25:b8176ebb96c6 | 183 | EventQueue *updaterQueue = mbed_highprio_event_queue(); |
shimniok | 25:b8176ebb96c6 | 184 | Event<void()> event(updaterQueue, callback(u, &Updater::update)); |
shimniok | 25:b8176ebb96c6 | 185 | event.period(20); |
shimniok | 25:b8176ebb96c6 | 186 | event.post(); |
shimniok | 25:b8176ebb96c6 | 187 | updaterThread.start(callback(updaterQueue, &EventQueue::dispatch_forever)); |
shimniok | 24:a7f92dfc5310 | 188 | |
shimniok | 24:a7f92dfc5310 | 189 | printf("Starting gps...\n"); |
shimniok | 24:a7f92dfc5310 | 190 | Thread gpsThread(osPriorityHigh, 256, 0, "gps"); |
shimniok | 24:a7f92dfc5310 | 191 | gpsThread.start(callback(&gpsQueue, &EventQueue::dispatch_forever)); |
shimniok | 24:a7f92dfc5310 | 192 | ublox.subscribe(gps_callback); |
shimniok | 24:a7f92dfc5310 | 193 | s.attach(gps_handler); |
shimniok | 24:a7f92dfc5310 | 194 | |
shimniok | 24:a7f92dfc5310 | 195 | printf("Starting logging...\n"); |
shimniok | 24:a7f92dfc5310 | 196 | Thread logThread(osPriorityNormal, 256, 0, "log"); |
shimniok | 24:a7f92dfc5310 | 197 | logThread.start(callback(&logQueue, &EventQueue::dispatch_forever)); |
shimniok | 24:a7f92dfc5310 | 198 | |
shimniok | 13:5566df1250f1 | 199 | printf("Starting shell...\n"); |
shimniok | 1:7019a60fd585 | 200 | sh.attach(test, "test"); |
shimniok | 9:fc3575d2cbbf | 201 | sh.attach(read_gyro, "gyro"); |
shimniok | 14:1dd83e626153 | 202 | sh.attach(read_enc, "enc"); |
shimniok | 18:3f8a8f6e3cc1 | 203 | sh.attach(read_gps, "gps"); |
shimniok | 12:3cd91e150d9c | 204 | sh.attach(reset, "reset"); |
shimniok | 24:a7f92dfc5310 | 205 | sh.attach(stats, "stats"); |
shimniok | 24:a7f92dfc5310 | 206 | //Thread shellThread(osPriorityNormal, 1024, 0, "shell"); |
shimniok | 24:a7f92dfc5310 | 207 | //shellThread.start(callback(&sh, &SimpleShell::run)); |
shimniok | 24:a7f92dfc5310 | 208 | sh.run(); |
shimniok | 22:4d62bd16f037 | 209 | |
shimniok | 20:043987d06f8d | 210 | /* |
shimniok | 0:7e98bbfd102a | 211 | FILE *fp; |
shimniok | 0:7e98bbfd102a | 212 | char buf[128]; |
shimniok | 0:7e98bbfd102a | 213 | printf("Initializing the block device... "); |
shimniok | 0:7e98bbfd102a | 214 | fflush(stdout); |
shimniok | 0:7e98bbfd102a | 215 | int err = bd.init(); |
shimniok | 0:7e98bbfd102a | 216 | printf("%s\n", (err ? "Fail :(" : "OK")); |
shimniok | 0:7e98bbfd102a | 217 | |
shimniok | 0:7e98bbfd102a | 218 | printf("Opening sdtest.txt..."); |
shimniok | 0:7e98bbfd102a | 219 | fp = fopen("/log/sdtest.txt", "r"); |
shimniok | 0:7e98bbfd102a | 220 | if(fp) { |
shimniok | 0:7e98bbfd102a | 221 | while (!feof(fp)) { |
shimniok | 0:7e98bbfd102a | 222 | fgets(buf, 127, fp); |
shimniok | 0:7e98bbfd102a | 223 | printf(buf); |
shimniok | 0:7e98bbfd102a | 224 | } |
shimniok | 0:7e98bbfd102a | 225 | fclose(fp); |
shimniok | 0:7e98bbfd102a | 226 | } |
shimniok | 0:7e98bbfd102a | 227 | |
shimniok | 0:7e98bbfd102a | 228 | printf("Opening config.txt..."); |
shimniok | 0:7e98bbfd102a | 229 | fp = fopen("/etc/config.txt", "r"); |
shimniok | 0:7e98bbfd102a | 230 | if(fp) { |
shimniok | 0:7e98bbfd102a | 231 | while (!feof(fp)) { |
shimniok | 0:7e98bbfd102a | 232 | fgets(buf, 127, fp); |
shimniok | 0:7e98bbfd102a | 233 | printf(buf); |
shimniok | 0:7e98bbfd102a | 234 | } |
shimniok | 0:7e98bbfd102a | 235 | fclose(fp); |
shimniok | 0:7e98bbfd102a | 236 | } |
shimniok | 0:7e98bbfd102a | 237 | */ |
shimniok | 0:7e98bbfd102a | 238 | |
shimniok | 0:7e98bbfd102a | 239 | //SystemReport sys_state(500); |
shimniok | 0:7e98bbfd102a | 240 | |
shimniok | 0:7e98bbfd102a | 241 | while (true) { |
shimniok | 0:7e98bbfd102a | 242 | // Blink LED and wait 0.5 seconds |
shimniok | 0:7e98bbfd102a | 243 | led1 = !led1; |
shimniok | 0:7e98bbfd102a | 244 | wait(0.5f); |
shimniok | 0:7e98bbfd102a | 245 | |
shimniok | 0:7e98bbfd102a | 246 | // Following the main thread wait, report on the current system status |
shimniok | 0:7e98bbfd102a | 247 | //sys_state.report_state(); |
shimniok | 0:7e98bbfd102a | 248 | } |
shimniok | 0:7e98bbfd102a | 249 | } |