Michael Shimniok / Mbed OS DataBus2018

2018 revision to classic DataBus AVC code.

Dependencies:   LSM303DLM Servo SerialGraphicLCD L3G4200D IncrementalEncoder SimpleShell

main.cpp@29:cb2f55fbfe9c, 2018-12-22 (annotated)

Committer:
shimniok
Date:
Sat Dec 22 20:28:52 2018 +0000
Revision:
29:cb2f55fbfe9c
Parent:
27:bfe141fad082
Child:
30:ed791f1f7f7d
Updates to shell. Implemented eventqueue-based data logging for gps. added start/stop/enabled functions to Logger and shell commands to start/stop/status 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 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 29:cb2f55fbfe9c 45 EventQueue logQueue(16 * EVENTS_EVENT_SIZE);
shimniok 29:cb2f55fbfe9c 46 Logger logger("/log/test.csv");
shimniok 25:b8176ebb96c6 47
shimniok 25:b8176ebb96c6 48 ///////////////////////////////////////////////////////////////////////////////
shimniok 25:b8176ebb96c6 49 // Updater
shimniok 25:b8176ebb96c6 50 Updater *u = Updater::instance();
shimniok 26:2dc31a801cc8 51 EventQueue updaterQueue(8 * EVENTS_EVENT_SIZE);
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 29:cb2f55fbfe9c 68 logQueue.call(&logger, &Logger::log_gps, d);
shimniok 23:5e61cf4a8c34 69 }
shimniok 29:cb2f55fbfe9c 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 29:cb2f55fbfe9c 127 void log(int argc, char **argv)
shimniok 29:cb2f55fbfe9c 128 {
shimniok 29:cb2f55fbfe9c 129 char *usage = "usage: log [start|stop]";
shimniok 29:cb2f55fbfe9c 130
shimniok 29:cb2f55fbfe9c 131 if (argc == 1) {
shimniok 29:cb2f55fbfe9c 132 printf("logging ");
shimniok 29:cb2f55fbfe9c 133 if (logger.enabled())
shimniok 29:cb2f55fbfe9c 134 printf("enabled");
shimniok 29:cb2f55fbfe9c 135 else
shimniok 29:cb2f55fbfe9c 136 printf("disabled");
shimniok 29:cb2f55fbfe9c 137 printf("\n");
shimniok 29:cb2f55fbfe9c 138 } else if (argc == 2) {
shimniok 29:cb2f55fbfe9c 139 if (!strcmp("start", argv[1])) {
shimniok 29:cb2f55fbfe9c 140 logger.start();
shimniok 29:cb2f55fbfe9c 141 } else if (!strcmp("stop", argv[1])) {
shimniok 29:cb2f55fbfe9c 142 logger.stop();
shimniok 29:cb2f55fbfe9c 143 } else {
shimniok 29:cb2f55fbfe9c 144 puts(usage);
shimniok 29:cb2f55fbfe9c 145 }
shimniok 29:cb2f55fbfe9c 146 } else {
shimniok 29:cb2f55fbfe9c 147 puts(usage);
shimniok 29:cb2f55fbfe9c 148 }
shimniok 29:cb2f55fbfe9c 149 }
shimniok 29:cb2f55fbfe9c 150
shimniok 29:cb2f55fbfe9c 151
shimniok 24:a7f92dfc5310 152 void bridge_uart1(int argc, char **argv) {
shimniok 20:043987d06f8d 153 RawSerial s(UART1TX, UART1RX, 38400);
shimniok 20:043987d06f8d 154 while (1) {
shimniok 20:043987d06f8d 155 if (pc.readable()) s.putc(pc.getc());
shimniok 20:043987d06f8d 156 if (s.readable()) pc.putc(s.getc());
shimniok 20:043987d06f8d 157 }
shimniok 20:043987d06f8d 158 }
shimniok 20:043987d06f8d 159
shimniok 24:a7f92dfc5310 160 void reset(int argc, char **argv)
shimniok 12:3cd91e150d9c 161 {
shimniok 12:3cd91e150d9c 162 NVIC_SystemReset();
shimniok 9:fc3575d2cbbf 163 }
shimniok 9:fc3575d2cbbf 164
shimniok 24:a7f92dfc5310 165 ///////////////////////////////////////////////////////////////////////////////
shimniok 24:a7f92dfc5310 166 // MAIN
shimniok 1:7019a60fd585 167
shimniok 0:7e98bbfd102a 168 // main() runs in its own thread in the OS
shimniok 0:7e98bbfd102a 169 int main()
shimniok 11:8ec858b7c6d1 170 {
shimniok 20:043987d06f8d 171 //bridge_uart1();
shimniok 24:a7f92dfc5310 172
shimniok 25:b8176ebb96c6 173 //Kernel::attach_idle_hook(idler);
shimniok 20:043987d06f8d 174
shimniok 0:7e98bbfd102a 175 printf("Bootup...\n");
shimniok 0:7e98bbfd102a 176 fflush(stdout);
shimniok 1:7019a60fd585 177
shimniok 4:de7feb458652 178 printf("Loading config...\n");
shimniok 7:1f2661b840ed 179 config.add("intercept_distance", Config::DOUBLE);
shimniok 7:1f2661b840ed 180 config.add("waypoint_threshold", Config::DOUBLE);
shimniok 7:1f2661b840ed 181 config.add("minimum_turning_radius", Config::DOUBLE);
shimniok 7:1f2661b840ed 182 config.add("wheelbase", Config::DOUBLE);
shimniok 7:1f2661b840ed 183 config.add("track_width", Config::DOUBLE);
shimniok 7:1f2661b840ed 184 config.add("tire_circumference", Config::DOUBLE);
shimniok 7:1f2661b840ed 185 config.add("encoder_stripes", Config::INT);
shimniok 7:1f2661b840ed 186 config.add("esc_brake", Config::INT);
shimniok 7:1f2661b840ed 187 config.add("esc_off", Config::INT);
shimniok 7:1f2661b840ed 188 config.add("esc_max", Config::INT);
shimniok 7:1f2661b840ed 189 config.add("turn_speed", Config::DOUBLE);
shimniok 7:1f2661b840ed 190 config.add("turn_distance", Config::DOUBLE);
shimniok 7:1f2661b840ed 191 config.add("start_speed", Config::DOUBLE);
shimniok 7:1f2661b840ed 192 config.add("cruise_speed", Config::DOUBLE);
shimniok 7:1f2661b840ed 193 config.add("speed_kp", Config::DOUBLE);
shimniok 7:1f2661b840ed 194 config.add("speed_ki", Config::DOUBLE);
shimniok 7:1f2661b840ed 195 config.add("speed_kd", Config::DOUBLE);
shimniok 7:1f2661b840ed 196 config.add("steer_center", Config::DOUBLE);
shimniok 7:1f2661b840ed 197 config.add("steer_scale", Config::DOUBLE);
shimniok 7:1f2661b840ed 198 config.add("gyro_scale", Config::DOUBLE);
shimniok 7:1f2661b840ed 199 config.add("gps_valid_speed", Config::DOUBLE);
shimniok 7:1f2661b840ed 200
shimniok 4:de7feb458652 201 if (config.load("/etc/2018cfg.txt")) {
shimniok 4:de7feb458652 202 printf("error loading config\n");
shimniok 4:de7feb458652 203 }
shimniok 10:9fb3feb38746 204
shimniok 24:a7f92dfc5310 205 printf("Starting updater...\n");
shimniok 25:b8176ebb96c6 206 u->attach(updater_callback);
shimniok 24:a7f92dfc5310 207 Thread updaterThread(osPriorityRealtime, 512, 0, "updater");
shimniok 26:2dc31a801cc8 208 updaterQueue.call_every(20, u, &Updater::update);
shimniok 26:2dc31a801cc8 209 updaterThread.start(callback(&updaterQueue, &EventQueue::dispatch_forever));
shimniok 24:a7f92dfc5310 210
shimniok 24:a7f92dfc5310 211 printf("Starting gps...\n");
shimniok 29:cb2f55fbfe9c 212 Thread gpsThread(osPriorityHigh, 2048, 0, "gps");
shimniok 24:a7f92dfc5310 213 gpsThread.start(callback(&gpsQueue, &EventQueue::dispatch_forever));
shimniok 24:a7f92dfc5310 214 ublox.subscribe(gps_callback);
shimniok 24:a7f92dfc5310 215 s.attach(gps_handler);
shimniok 24:a7f92dfc5310 216
shimniok 24:a7f92dfc5310 217 printf("Starting logging...\n");
shimniok 29:cb2f55fbfe9c 218 Thread logThread(osPriorityNormal, 2048, 0, "log");
shimniok 24:a7f92dfc5310 219 logThread.start(callback(&logQueue, &EventQueue::dispatch_forever));
shimniok 24:a7f92dfc5310 220
shimniok 13:5566df1250f1 221 printf("Starting shell...\n");
shimniok 1:7019a60fd585 222 sh.attach(test, "test");
shimniok 9:fc3575d2cbbf 223 sh.attach(read_gyro, "gyro");
shimniok 14:1dd83e626153 224 sh.attach(read_enc, "enc");
shimniok 18:3f8a8f6e3cc1 225 sh.attach(read_gps, "gps");
shimniok 12:3cd91e150d9c 226 sh.attach(reset, "reset");
shimniok 24:a7f92dfc5310 227 sh.attach(stats, "stats");
shimniok 29:cb2f55fbfe9c 228 sh.attach(log, "log");
shimniok 24:a7f92dfc5310 229 sh.run();
shimniok 22:4d62bd16f037 230
shimniok 0:7e98bbfd102a 231 while (true) {
shimniok 0:7e98bbfd102a 232 // Blink LED and wait 0.5 seconds
shimniok 0:7e98bbfd102a 233 led1 = !led1;
shimniok 0:7e98bbfd102a 234 wait(0.5f);
shimniok 0:7e98bbfd102a 235 }
shimniok 0:7e98bbfd102a 236 }