Michael Shimniok / Mbed OS DataBus2018

2018 revision to classic DataBus AVC code.

Dependencies:   LSM303DLM Servo SerialGraphicLCD L3G4200D IncrementalEncoder SimpleShell

main.cpp@32:eb673f6f5734, 2018-12-27 (annotated)

Committer:
shimniok
Date:
Thu Dec 27 15:46:57 2018 +0000
Revision:
32:eb673f6f5734
Parent:
31:20a95043adb0
Child:
33:74c514aea0a1
Added accel & mag to data collection and logging

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 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 30:ed791f1f7f7d 26 DigitalOut led1(LED1);
shimniok 30:ed791f1f7f7d 27 DigitalOut led2(LED2);
shimniok 30:ed791f1f7f7d 28 DigitalOut led3(LED3);
shimniok 30:ed791f1f7f7d 29 DigitalOut led4(LED4);
shimniok 0:7e98bbfd102a 30 LocalFileSystem lfs("etc");
shimniok 24:a7f92dfc5310 31 SDBlockDevice bd(p5, p6, p7, p8); // MOSI, MISO, CLK, CS
shimniok 24:a7f92dfc5310 32 FATFileSystem ffs("log", &bd);
shimniok 24:a7f92dfc5310 33 Serial pc(USBTX, USBRX);
shimniok 20:043987d06f8d 34 RawSerial s(UART1TX, UART1RX, 38400);
shimniok 30:ed791f1f7f7d 35 InterruptIn lbutton(LBUTTON, PullUp); // left button
shimniok 30:ed791f1f7f7d 36 InterruptIn cbutton(CBUTTON, PullUp); // center button
shimniok 30:ed791f1f7f7d 37 InterruptIn rbutton(RBUTTON, PullUp); // right button
shimniok 20:043987d06f8d 38
shimniok 24:a7f92dfc5310 39 ///////////////////////////////////////////////////////////////////////////////
shimniok 24:a7f92dfc5310 40 // Idle hook
shimniok 24:a7f92dfc5310 41 void idler() {
shimniok 24:a7f92dfc5310 42 while(1) {
shimniok 24:a7f92dfc5310 43 led1 = !led1;
shimniok 24:a7f92dfc5310 44 wait(0.1);
shimniok 24:a7f92dfc5310 45 }
shimniok 24:a7f92dfc5310 46 }
shimniok 19:0d1728091519 47
shimniok 24:a7f92dfc5310 48 ///////////////////////////////////////////////////////////////////////////////
shimniok 24:a7f92dfc5310 49 // Logging
shimniok 29:cb2f55fbfe9c 50 EventQueue logQueue(16 * EVENTS_EVENT_SIZE);
shimniok 31:20a95043adb0 51 Logger logger;
shimniok 25:b8176ebb96c6 52
shimniok 25:b8176ebb96c6 53 ///////////////////////////////////////////////////////////////////////////////
shimniok 25:b8176ebb96c6 54 // Updater
shimniok 25:b8176ebb96c6 55 Updater *u = Updater::instance();
shimniok 26:2dc31a801cc8 56 EventQueue updaterQueue(8 * EVENTS_EVENT_SIZE);
shimniok 25:b8176ebb96c6 57
shimniok 25:b8176ebb96c6 58 void updater_callback() {
shimniok 30:ed791f1f7f7d 59 SensorData d;
shimniok 30:ed791f1f7f7d 60 float dt;
shimniok 30:ed791f1f7f7d 61
shimniok 25:b8176ebb96c6 62 led1 = !led1;
shimniok 30:ed791f1f7f7d 63 d.timestamp = Kernel::get_ms_count();
shimniok 30:ed791f1f7f7d 64 d.encoder = u->encoder();
shimniok 32:eb673f6f5734 65 u->imu(d.gyro, d.accel, d.mag, dt);
shimniok 30:ed791f1f7f7d 66 logQueue.call(&logger, &Logger::log_sensors, d);
shimniok 25:b8176ebb96c6 67 }
shimniok 25:b8176ebb96c6 68
shimniok 24:a7f92dfc5310 69 ///////////////////////////////////////////////////////////////////////////////
shimniok 30:ed791f1f7f7d 70 // Buttons
shimniok 30:ed791f1f7f7d 71
shimniok 30:ed791f1f7f7d 72 //enum button_mask { LEFT = 0x01, CENTER = 0x02, RIGHT = 0x04 };
shimniok 30:ed791f1f7f7d 73 EventQueue buttonQueue(16 * EVENTS_EVENT_SIZE);
shimniok 30:ed791f1f7f7d 74 int last = 0;
shimniok 30:ed791f1f7f7d 75 const int BUTTON_DEBOUNCE_MS = 100;
shimniok 30:ed791f1f7f7d 76
shimniok 30:ed791f1f7f7d 77 void button_event() {
shimniok 30:ed791f1f7f7d 78 int now = Kernel::get_ms_count();
shimniok 30:ed791f1f7f7d 79 if ((now - last) > BUTTON_DEBOUNCE_MS) {
shimniok 30:ed791f1f7f7d 80 if (logger.enabled()) {
shimniok 30:ed791f1f7f7d 81 logQueue.call(&logger, &Logger::stop);
shimniok 30:ed791f1f7f7d 82 led3 = 0;
shimniok 30:ed791f1f7f7d 83 } else {
shimniok 30:ed791f1f7f7d 84 logQueue.call(&logger, &Logger::start);
shimniok 30:ed791f1f7f7d 85 led3 = 1;
shimniok 30:ed791f1f7f7d 86 }
shimniok 30:ed791f1f7f7d 87 }
shimniok 30:ed791f1f7f7d 88 last = now;
shimniok 30:ed791f1f7f7d 89 }
shimniok 30:ed791f1f7f7d 90
shimniok 30:ed791f1f7f7d 91
shimniok 30:ed791f1f7f7d 92 ///////////////////////////////////////////////////////////////////////////////
shimniok 24:a7f92dfc5310 93 // GPS
shimniok 24:a7f92dfc5310 94 Ublox6 ublox;
shimniok 24:a7f92dfc5310 95 EventQueue gpsQueue(8 * EVENTS_EVENT_SIZE);
shimniok 23:5e61cf4a8c34 96
shimniok 23:5e61cf4a8c34 97 // Callback for gps parse data ready
shimniok 23:5e61cf4a8c34 98 void gps_callback() {
shimniok 24:a7f92dfc5310 99 GpsData d;
shimniok 24:a7f92dfc5310 100
shimniok 25:b8176ebb96c6 101 led2 = !led2;
shimniok 24:a7f92dfc5310 102 ublox.read(d.latitude, d.longitude, d.course, d.speed, d.hdop, d.svcount);
shimniok 30:ed791f1f7f7d 103 d.timestamp = Kernel::get_ms_count();
shimniok 29:cb2f55fbfe9c 104 logQueue.call(&logger, &Logger::log_gps, d);
shimniok 23:5e61cf4a8c34 105 }
shimniok 29:cb2f55fbfe9c 106
shimniok 22:4d62bd16f037 107 // ISR for GPS serial, passes off to thread
shimniok 20:043987d06f8d 108 void gps_handler() {
shimniok 22:4d62bd16f037 109 while (s.readable()) {
shimniok 22:4d62bd16f037 110 int c = s.getc();
shimniok 22:4d62bd16f037 111 gpsQueue.call(&ublox, &Ublox6::parse, c);
shimniok 16:eb28d0f64a9b 112 }
shimniok 19:0d1728091519 113 }
shimniok 16:eb28d0f64a9b 114
shimniok 24:a7f92dfc5310 115 ///////////////////////////////////////////////////////////////////////////////
shimniok 24:a7f92dfc5310 116 // Shell
shimniok 30:ed791f1f7f7d 117 SimpleShell sh("/log");
shimniok 9:fc3575d2cbbf 118
shimniok 24:a7f92dfc5310 119 void test(int argc, char **argv) {
shimniok 1:7019a60fd585 120 printf("Hello world!\n");
shimniok 1:7019a60fd585 121 }
shimniok 1:7019a60fd585 122
shimniok 24:a7f92dfc5310 123 void stats(int argc, char **argv) {
shimniok 24:a7f92dfc5310 124 SystemReport r(200);
shimniok 24:a7f92dfc5310 125 r.report_state();
shimniok 24:a7f92dfc5310 126 return;
shimniok 24:a7f92dfc5310 127 }
shimniok 24:a7f92dfc5310 128
shimniok 24:a7f92dfc5310 129 void read_gps(int argc, char **argv)
shimniok 18:3f8a8f6e3cc1 130 {
shimniok 18:3f8a8f6e3cc1 131 double lat=0;
shimniok 18:3f8a8f6e3cc1 132 double lon=0;
shimniok 18:3f8a8f6e3cc1 133 float course=0;
shimniok 18:3f8a8f6e3cc1 134 float speed=0;
shimniok 18:3f8a8f6e3cc1 135 float hdop=0.0;
shimniok 18:3f8a8f6e3cc1 136 int svcount=0;
shimniok 18:3f8a8f6e3cc1 137
shimniok 18:3f8a8f6e3cc1 138 ublox.read(lat, lon, course, speed, hdop, svcount);
shimniok 18:3f8a8f6e3cc1 139 printf("%3.7f %3.7f\n", lat, lon);
shimniok 18:3f8a8f6e3cc1 140 printf("hdg=%03.1f deg spd=%3.1f m/s\n", course, speed);
shimniok 18:3f8a8f6e3cc1 141 printf("%d %f\n", svcount, hdop);
shimniok 18:3f8a8f6e3cc1 142 }
shimniok 18:3f8a8f6e3cc1 143
shimniok 32:eb673f6f5734 144 void read_imu(int argc, char **argv)
shimniok 9:fc3575d2cbbf 145 {
shimniok 12:3cd91e150d9c 146 int g[3];
shimniok 32:eb673f6f5734 147 int a[3];
shimniok 32:eb673f6f5734 148 int m[3];
shimniok 13:5566df1250f1 149 float dt;
shimniok 12:3cd91e150d9c 150
shimniok 11:8ec858b7c6d1 151 Updater *u = Updater::instance();
shimniok 11:8ec858b7c6d1 152
shimniok 32:eb673f6f5734 153 u->imu(g, a, m, dt);
shimniok 32:eb673f6f5734 154
shimniok 32:eb673f6f5734 155 printf(" Gyro: %d, %d, %d\n", g[0], g[1], g[2]);
shimniok 32:eb673f6f5734 156 printf(" Accel: %d, %d, %d\n", a[0], a[1], a[2]);
shimniok 32:eb673f6f5734 157 printf(" Mag: %d, %d, %d\n", m[0], m[1], m[2]);
shimniok 32:eb673f6f5734 158 printf(" dt: %f\n", dt);
shimniok 12:3cd91e150d9c 159 }
shimniok 12:3cd91e150d9c 160
shimniok 24:a7f92dfc5310 161 void read_enc(int argc, char **argv)
shimniok 14:1dd83e626153 162 {
shimniok 14:1dd83e626153 163 Updater *u = Updater::instance();
shimniok 14:1dd83e626153 164
shimniok 14:1dd83e626153 165 printf("Encoder: %d\n", u->encoder());
shimniok 14:1dd83e626153 166 }
shimniok 14:1dd83e626153 167
shimniok 29:cb2f55fbfe9c 168 void log(int argc, char **argv)
shimniok 29:cb2f55fbfe9c 169 {
shimniok 29:cb2f55fbfe9c 170 char *usage = "usage: log [start|stop]";
shimniok 29:cb2f55fbfe9c 171
shimniok 29:cb2f55fbfe9c 172 if (argc == 1) {
shimniok 29:cb2f55fbfe9c 173 printf("logging ");
shimniok 29:cb2f55fbfe9c 174 if (logger.enabled())
shimniok 29:cb2f55fbfe9c 175 printf("enabled");
shimniok 29:cb2f55fbfe9c 176 else
shimniok 29:cb2f55fbfe9c 177 printf("disabled");
shimniok 29:cb2f55fbfe9c 178 printf("\n");
shimniok 29:cb2f55fbfe9c 179 } else if (argc == 2) {
shimniok 29:cb2f55fbfe9c 180 if (!strcmp("start", argv[1])) {
shimniok 29:cb2f55fbfe9c 181 logger.start();
shimniok 29:cb2f55fbfe9c 182 } else if (!strcmp("stop", argv[1])) {
shimniok 29:cb2f55fbfe9c 183 logger.stop();
shimniok 29:cb2f55fbfe9c 184 } else {
shimniok 29:cb2f55fbfe9c 185 puts(usage);
shimniok 29:cb2f55fbfe9c 186 }
shimniok 29:cb2f55fbfe9c 187 } else {
shimniok 29:cb2f55fbfe9c 188 puts(usage);
shimniok 29:cb2f55fbfe9c 189 }
shimniok 29:cb2f55fbfe9c 190 }
shimniok 29:cb2f55fbfe9c 191
shimniok 29:cb2f55fbfe9c 192
shimniok 24:a7f92dfc5310 193 void bridge_uart1(int argc, char **argv) {
shimniok 20:043987d06f8d 194 RawSerial s(UART1TX, UART1RX, 38400);
shimniok 20:043987d06f8d 195 while (1) {
shimniok 20:043987d06f8d 196 if (pc.readable()) s.putc(pc.getc());
shimniok 20:043987d06f8d 197 if (s.readable()) pc.putc(s.getc());
shimniok 20:043987d06f8d 198 }
shimniok 20:043987d06f8d 199 }
shimniok 20:043987d06f8d 200
shimniok 24:a7f92dfc5310 201 void reset(int argc, char **argv)
shimniok 12:3cd91e150d9c 202 {
shimniok 12:3cd91e150d9c 203 NVIC_SystemReset();
shimniok 9:fc3575d2cbbf 204 }
shimniok 9:fc3575d2cbbf 205
shimniok 24:a7f92dfc5310 206 ///////////////////////////////////////////////////////////////////////////////
shimniok 24:a7f92dfc5310 207 // MAIN
shimniok 1:7019a60fd585 208
shimniok 0:7e98bbfd102a 209 // main() runs in its own thread in the OS
shimniok 0:7e98bbfd102a 210 int main()
shimniok 11:8ec858b7c6d1 211 {
shimniok 20:043987d06f8d 212 //bridge_uart1();
shimniok 24:a7f92dfc5310 213
shimniok 25:b8176ebb96c6 214 //Kernel::attach_idle_hook(idler);
shimniok 20:043987d06f8d 215
shimniok 0:7e98bbfd102a 216 printf("Bootup...\n");
shimniok 0:7e98bbfd102a 217 fflush(stdout);
shimniok 1:7019a60fd585 218
shimniok 4:de7feb458652 219 printf("Loading config...\n");
shimniok 7:1f2661b840ed 220 config.add("intercept_distance", Config::DOUBLE);
shimniok 7:1f2661b840ed 221 config.add("waypoint_threshold", Config::DOUBLE);
shimniok 7:1f2661b840ed 222 config.add("minimum_turning_radius", Config::DOUBLE);
shimniok 7:1f2661b840ed 223 config.add("wheelbase", Config::DOUBLE);
shimniok 7:1f2661b840ed 224 config.add("track_width", Config::DOUBLE);
shimniok 7:1f2661b840ed 225 config.add("tire_circumference", Config::DOUBLE);
shimniok 7:1f2661b840ed 226 config.add("encoder_stripes", Config::INT);
shimniok 7:1f2661b840ed 227 config.add("esc_brake", Config::INT);
shimniok 7:1f2661b840ed 228 config.add("esc_off", Config::INT);
shimniok 7:1f2661b840ed 229 config.add("esc_max", Config::INT);
shimniok 7:1f2661b840ed 230 config.add("turn_speed", Config::DOUBLE);
shimniok 7:1f2661b840ed 231 config.add("turn_distance", Config::DOUBLE);
shimniok 7:1f2661b840ed 232 config.add("start_speed", Config::DOUBLE);
shimniok 7:1f2661b840ed 233 config.add("cruise_speed", Config::DOUBLE);
shimniok 7:1f2661b840ed 234 config.add("speed_kp", Config::DOUBLE);
shimniok 7:1f2661b840ed 235 config.add("speed_ki", Config::DOUBLE);
shimniok 7:1f2661b840ed 236 config.add("speed_kd", Config::DOUBLE);
shimniok 7:1f2661b840ed 237 config.add("steer_center", Config::DOUBLE);
shimniok 7:1f2661b840ed 238 config.add("steer_scale", Config::DOUBLE);
shimniok 7:1f2661b840ed 239 config.add("gyro_scale", Config::DOUBLE);
shimniok 7:1f2661b840ed 240 config.add("gps_valid_speed", Config::DOUBLE);
shimniok 7:1f2661b840ed 241
shimniok 4:de7feb458652 242 if (config.load("/etc/2018cfg.txt")) {
shimniok 4:de7feb458652 243 printf("error loading config\n");
shimniok 4:de7feb458652 244 }
shimniok 10:9fb3feb38746 245
shimniok 30:ed791f1f7f7d 246 printf("Starting buttons...\n");
shimniok 30:ed791f1f7f7d 247 lbutton.rise(buttonQueue.event(button_event));
shimniok 30:ed791f1f7f7d 248 //cbutton.rise(buttonQueue.event(button_event));
shimniok 30:ed791f1f7f7d 249 //rbutton.rise(buttonQueue.event(button_event));
shimniok 30:ed791f1f7f7d 250 Thread buttonThread(osPriorityNormal, 256, 0, "buttons");
shimniok 30:ed791f1f7f7d 251 buttonThread.start(callback(&buttonQueue, &EventQueue::dispatch_forever));
shimniok 30:ed791f1f7f7d 252
shimniok 24:a7f92dfc5310 253 printf("Starting updater...\n");
shimniok 25:b8176ebb96c6 254 u->attach(updater_callback);
shimniok 24:a7f92dfc5310 255 Thread updaterThread(osPriorityRealtime, 512, 0, "updater");
shimniok 26:2dc31a801cc8 256 updaterQueue.call_every(20, u, &Updater::update);
shimniok 26:2dc31a801cc8 257 updaterThread.start(callback(&updaterQueue, &EventQueue::dispatch_forever));
shimniok 24:a7f92dfc5310 258
shimniok 24:a7f92dfc5310 259 printf("Starting gps...\n");
shimniok 29:cb2f55fbfe9c 260 Thread gpsThread(osPriorityHigh, 2048, 0, "gps");
shimniok 24:a7f92dfc5310 261 gpsThread.start(callback(&gpsQueue, &EventQueue::dispatch_forever));
shimniok 24:a7f92dfc5310 262 ublox.subscribe(gps_callback);
shimniok 24:a7f92dfc5310 263 s.attach(gps_handler);
shimniok 24:a7f92dfc5310 264
shimniok 24:a7f92dfc5310 265 printf("Starting logging...\n");
shimniok 29:cb2f55fbfe9c 266 Thread logThread(osPriorityNormal, 2048, 0, "log");
shimniok 24:a7f92dfc5310 267 logThread.start(callback(&logQueue, &EventQueue::dispatch_forever));
shimniok 24:a7f92dfc5310 268
shimniok 13:5566df1250f1 269 printf("Starting shell...\n");
shimniok 30:ed791f1f7f7d 270 sh.command(test, "test");
shimniok 32:eb673f6f5734 271 sh.command(read_imu, "imu");
shimniok 30:ed791f1f7f7d 272 sh.command(read_enc, "enc");
shimniok 30:ed791f1f7f7d 273 sh.command(read_gps, "gps");
shimniok 30:ed791f1f7f7d 274 sh.command(reset, "reset");
shimniok 30:ed791f1f7f7d 275 sh.command(stats, "stats");
shimniok 30:ed791f1f7f7d 276 sh.command(log, "log");
shimniok 24:a7f92dfc5310 277 sh.run();
shimniok 22:4d62bd16f037 278
shimniok 0:7e98bbfd102a 279 while (true) {
shimniok 0:7e98bbfd102a 280 // Blink LED and wait 0.5 seconds
shimniok 0:7e98bbfd102a 281 led1 = !led1;
shimniok 0:7e98bbfd102a 282 wait(0.5f);
shimniok 0:7e98bbfd102a 283 }
shimniok 0:7e98bbfd102a 284 }