Michael Shimniok / Mbed OS DataBus2018

2018 revision to classic DataBus AVC code.

Dependencies:   LSM303DLM Servo SerialGraphicLCD L3G4200D IncrementalEncoder SimpleShell

main.cpp@24:a7f92dfc5310, 2018-12-21 (annotated)

Committer:
shimniok
Date:
Fri Dec 21 20:04:09 2018 +0000
Revision:
24:a7f92dfc5310
Parent:
23:5e61cf4a8c34
Child:
25:b8176ebb96c6
thread changes, added stats command, added sd filesystem, other minor changes

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