Michael Shimniok / Mbed OS DataBus2018

2018 revision to classic DataBus AVC code.

Dependencies:   LSM303DLM Servo SerialGraphicLCD L3G4200D IncrementalEncoder SimpleShell

main.cpp@23:5e61cf4a8c34, 2018-12-18 (annotated)

Committer:
shimniok
Date:
Tue Dec 18 17:09:38 2018 +0000
Revision:
23:5e61cf4a8c34
Parent:
22:4d62bd16f037
Child:
24:a7f92dfc5310
implemented callback for gps data ready

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 0:7e98bbfd102a 17
shimniok 0:7e98bbfd102a 18 Serial pc(USBTX, USBRX);
shimniok 0:7e98bbfd102a 19 //SDBlockDevice bd(p5, p6, p7, p8); // MOSI, MISO, CLK, CS
shimniok 0:7e98bbfd102a 20 //FATFileSystem ffs("log", &bd);
shimniok 0:7e98bbfd102a 21 LocalFileSystem lfs("etc");
shimniok 4:de7feb458652 22 Config config;
shimniok 0:7e98bbfd102a 23 SimpleShell sh;
shimniok 18:3f8a8f6e3cc1 24 Ublox6 ublox;
shimniok 0:7e98bbfd102a 25
shimniok 0:7e98bbfd102a 26 DigitalOut led1(LED1);
shimniok 16:eb28d0f64a9b 27 DigitalOut led3(LED3);
shimniok 16:eb28d0f64a9b 28
shimniok 20:043987d06f8d 29 RawSerial s(UART1TX, UART1RX, 38400);
shimniok 20:043987d06f8d 30
shimniok 22:4d62bd16f037 31 EventQueue gpsQueue(32 * EVENTS_EVENT_SIZE);
shimniok 19:0d1728091519 32
shimniok 23:5e61cf4a8c34 33
shimniok 23:5e61cf4a8c34 34 // Callback for gps parse data ready
shimniok 23:5e61cf4a8c34 35 void gps_callback() {
shimniok 23:5e61cf4a8c34 36 led3 = !led3;
shimniok 23:5e61cf4a8c34 37 }
shimniok 23:5e61cf4a8c34 38
shimniok 23:5e61cf4a8c34 39
shimniok 22:4d62bd16f037 40 // thread / eventqueue wrapper for gps parse
shimniok 22:4d62bd16f037 41 void gps_parse(int c) {
shimniok 22:4d62bd16f037 42 if (ublox.parse(c)) {
shimniok 22:4d62bd16f037 43 led3 = !led3;
shimniok 22:4d62bd16f037 44 }
shimniok 22:4d62bd16f037 45 }
shimniok 22:4d62bd16f037 46
shimniok 22:4d62bd16f037 47 // ISR for GPS serial, passes off to thread
shimniok 20:043987d06f8d 48 void gps_handler() {
shimniok 22:4d62bd16f037 49 while (s.readable()) {
shimniok 22:4d62bd16f037 50 int c = s.getc();
shimniok 22:4d62bd16f037 51 //gpsQueue.call(gps_parse, c);
shimniok 22:4d62bd16f037 52 gpsQueue.call(&ublox, &Ublox6::parse, c);
shimniok 16:eb28d0f64a9b 53 }
shimniok 19:0d1728091519 54 }
shimniok 16:eb28d0f64a9b 55
shimniok 9:fc3575d2cbbf 56 /******** SHELL COMMANDS ********/
shimniok 9:fc3575d2cbbf 57
shimniok 1:7019a60fd585 58 void test() {
shimniok 1:7019a60fd585 59 printf("Hello world!\n");
shimniok 1:7019a60fd585 60 }
shimniok 1:7019a60fd585 61
shimniok 18:3f8a8f6e3cc1 62 void read_gps()
shimniok 18:3f8a8f6e3cc1 63 {
shimniok 18:3f8a8f6e3cc1 64 double lat=0;
shimniok 18:3f8a8f6e3cc1 65 double lon=0;
shimniok 18:3f8a8f6e3cc1 66 float course=0;
shimniok 18:3f8a8f6e3cc1 67 float speed=0;
shimniok 18:3f8a8f6e3cc1 68 float hdop=0.0;
shimniok 18:3f8a8f6e3cc1 69 int svcount=0;
shimniok 18:3f8a8f6e3cc1 70
shimniok 18:3f8a8f6e3cc1 71 ublox.read(lat, lon, course, speed, hdop, svcount);
shimniok 18:3f8a8f6e3cc1 72 printf("%3.7f %3.7f\n", lat, lon);
shimniok 18:3f8a8f6e3cc1 73 printf("hdg=%03.1f deg spd=%3.1f m/s\n", course, speed);
shimniok 18:3f8a8f6e3cc1 74 printf("%d %f\n", svcount, hdop);
shimniok 18:3f8a8f6e3cc1 75 }
shimniok 18:3f8a8f6e3cc1 76
shimniok 9:fc3575d2cbbf 77 void read_gyro()
shimniok 9:fc3575d2cbbf 78 {
shimniok 12:3cd91e150d9c 79 int g[3];
shimniok 13:5566df1250f1 80 float dt;
shimniok 12:3cd91e150d9c 81
shimniok 11:8ec858b7c6d1 82 Updater *u = Updater::instance();
shimniok 11:8ec858b7c6d1 83
shimniok 13:5566df1250f1 84 u->gyro(g, dt);
shimniok 11:8ec858b7c6d1 85
shimniok 13:5566df1250f1 86 printf("Gyro: %d, %d, %d - dt: %f\n", g[0], g[1], g[2], dt);
shimniok 12:3cd91e150d9c 87 }
shimniok 12:3cd91e150d9c 88
shimniok 14:1dd83e626153 89 void read_enc()
shimniok 14:1dd83e626153 90 {
shimniok 14:1dd83e626153 91 Updater *u = Updater::instance();
shimniok 14:1dd83e626153 92
shimniok 14:1dd83e626153 93 printf("Encoder: %d\n", u->encoder());
shimniok 14:1dd83e626153 94 }
shimniok 14:1dd83e626153 95
shimniok 20:043987d06f8d 96 void bridge_uart1() {
shimniok 20:043987d06f8d 97 RawSerial s(UART1TX, UART1RX, 38400);
shimniok 20:043987d06f8d 98 while (1) {
shimniok 20:043987d06f8d 99 if (pc.readable()) s.putc(pc.getc());
shimniok 20:043987d06f8d 100 if (s.readable()) pc.putc(s.getc());
shimniok 20:043987d06f8d 101 }
shimniok 20:043987d06f8d 102 }
shimniok 20:043987d06f8d 103
shimniok 20:043987d06f8d 104
shimniok 12:3cd91e150d9c 105 void reset()
shimniok 12:3cd91e150d9c 106 {
shimniok 12:3cd91e150d9c 107 NVIC_SystemReset();
shimniok 9:fc3575d2cbbf 108 }
shimniok 9:fc3575d2cbbf 109
shimniok 9:fc3575d2cbbf 110 /******** MAIN ********/
shimniok 1:7019a60fd585 111
shimniok 0:7e98bbfd102a 112 // main() runs in its own thread in the OS
shimniok 0:7e98bbfd102a 113 int main()
shimniok 11:8ec858b7c6d1 114 {
shimniok 20:043987d06f8d 115 //bridge_uart1();
shimniok 20:043987d06f8d 116
shimniok 0:7e98bbfd102a 117 printf("Bootup...\n");
shimniok 0:7e98bbfd102a 118 fflush(stdout);
shimniok 1:7019a60fd585 119
shimniok 4:de7feb458652 120 printf("Loading config...\n");
shimniok 7:1f2661b840ed 121 config.add("intercept_distance", Config::DOUBLE);
shimniok 7:1f2661b840ed 122 config.add("waypoint_threshold", Config::DOUBLE);
shimniok 7:1f2661b840ed 123 config.add("minimum_turning_radius", Config::DOUBLE);
shimniok 7:1f2661b840ed 124 config.add("wheelbase", Config::DOUBLE);
shimniok 7:1f2661b840ed 125 config.add("track_width", Config::DOUBLE);
shimniok 7:1f2661b840ed 126 config.add("tire_circumference", Config::DOUBLE);
shimniok 7:1f2661b840ed 127 config.add("encoder_stripes", Config::INT);
shimniok 7:1f2661b840ed 128 config.add("esc_brake", Config::INT);
shimniok 7:1f2661b840ed 129 config.add("esc_off", Config::INT);
shimniok 7:1f2661b840ed 130 config.add("esc_max", Config::INT);
shimniok 7:1f2661b840ed 131 config.add("turn_speed", Config::DOUBLE);
shimniok 7:1f2661b840ed 132 config.add("turn_distance", Config::DOUBLE);
shimniok 7:1f2661b840ed 133 config.add("start_speed", Config::DOUBLE);
shimniok 7:1f2661b840ed 134 config.add("cruise_speed", Config::DOUBLE);
shimniok 7:1f2661b840ed 135 config.add("speed_kp", Config::DOUBLE);
shimniok 7:1f2661b840ed 136 config.add("speed_ki", Config::DOUBLE);
shimniok 7:1f2661b840ed 137 config.add("speed_kd", Config::DOUBLE);
shimniok 7:1f2661b840ed 138 config.add("steer_center", Config::DOUBLE);
shimniok 7:1f2661b840ed 139 config.add("steer_scale", Config::DOUBLE);
shimniok 7:1f2661b840ed 140 config.add("gyro_scale", Config::DOUBLE);
shimniok 7:1f2661b840ed 141 config.add("gps_valid_speed", Config::DOUBLE);
shimniok 7:1f2661b840ed 142
shimniok 4:de7feb458652 143 if (config.load("/etc/2018cfg.txt")) {
shimniok 4:de7feb458652 144 printf("error loading config\n");
shimniok 4:de7feb458652 145 }
shimniok 10:9fb3feb38746 146
shimniok 13:5566df1250f1 147 printf("Starting shell...\n");
shimniok 1:7019a60fd585 148 sh.attach(test, "test");
shimniok 9:fc3575d2cbbf 149 sh.attach(read_gyro, "gyro");
shimniok 14:1dd83e626153 150 sh.attach(read_enc, "enc");
shimniok 18:3f8a8f6e3cc1 151 sh.attach(read_gps, "gps");
shimniok 12:3cd91e150d9c 152 sh.attach(reset, "reset");
shimniok 15:35c40765f7c3 153 Thread shellThread;
shimniok 15:35c40765f7c3 154 shellThread.start(callback(&sh, &SimpleShell::run));
shimniok 0:7e98bbfd102a 155
shimniok 13:5566df1250f1 156 printf("Starting updater...\n");
shimniok 11:8ec858b7c6d1 157 Updater *u = Updater::instance();
shimniok 21:020f4ebbb13a 158 u->setInterval(20);
shimniok 22:4d62bd16f037 159 Thread updaterThread(osPriorityRealtime);
shimniok 15:35c40765f7c3 160 updaterThread.start(callback(u, &Updater::start));
shimniok 22:4d62bd16f037 161
shimniok 16:eb28d0f64a9b 162 printf("Starting gps...\n");
shimniok 22:4d62bd16f037 163 Thread gpsThread(osPriorityHigh);
shimniok 22:4d62bd16f037 164 gpsThread.start(callback(&gpsQueue, &EventQueue::dispatch_forever));
shimniok 23:5e61cf4a8c34 165 ublox.subscribe(gps_callback);
shimniok 22:4d62bd16f037 166 s.attach(gps_handler);
shimniok 23:5e61cf4a8c34 167
shimniok 20:043987d06f8d 168 /*
shimniok 0:7e98bbfd102a 169 FILE *fp;
shimniok 0:7e98bbfd102a 170 char buf[128];
shimniok 0:7e98bbfd102a 171 printf("Initializing the block device... ");
shimniok 0:7e98bbfd102a 172 fflush(stdout);
shimniok 0:7e98bbfd102a 173 int err = bd.init();
shimniok 0:7e98bbfd102a 174 printf("%s\n", (err ? "Fail :(" : "OK"));
shimniok 0:7e98bbfd102a 175
shimniok 0:7e98bbfd102a 176 printf("Opening sdtest.txt...");
shimniok 0:7e98bbfd102a 177 fp = fopen("/log/sdtest.txt", "r");
shimniok 0:7e98bbfd102a 178 if(fp) {
shimniok 0:7e98bbfd102a 179 while (!feof(fp)) {
shimniok 0:7e98bbfd102a 180 fgets(buf, 127, fp);
shimniok 0:7e98bbfd102a 181 printf(buf);
shimniok 0:7e98bbfd102a 182 }
shimniok 0:7e98bbfd102a 183 fclose(fp);
shimniok 0:7e98bbfd102a 184 }
shimniok 0:7e98bbfd102a 185
shimniok 0:7e98bbfd102a 186 printf("Opening config.txt...");
shimniok 0:7e98bbfd102a 187 fp = fopen("/etc/config.txt", "r");
shimniok 0:7e98bbfd102a 188 if(fp) {
shimniok 0:7e98bbfd102a 189 while (!feof(fp)) {
shimniok 0:7e98bbfd102a 190 fgets(buf, 127, fp);
shimniok 0:7e98bbfd102a 191 printf(buf);
shimniok 0:7e98bbfd102a 192 }
shimniok 0:7e98bbfd102a 193 fclose(fp);
shimniok 0:7e98bbfd102a 194 }
shimniok 0:7e98bbfd102a 195 */
shimniok 0:7e98bbfd102a 196
shimniok 0:7e98bbfd102a 197 //SystemReport sys_state(500);
shimniok 0:7e98bbfd102a 198
shimniok 0:7e98bbfd102a 199 while (true) {
shimniok 0:7e98bbfd102a 200 // Blink LED and wait 0.5 seconds
shimniok 0:7e98bbfd102a 201 led1 = !led1;
shimniok 0:7e98bbfd102a 202 wait(0.5f);
shimniok 0:7e98bbfd102a 203
shimniok 0:7e98bbfd102a 204 // Following the main thread wait, report on the current system status
shimniok 0:7e98bbfd102a 205 //sys_state.report_state();
shimniok 0:7e98bbfd102a 206 }
shimniok 0:7e98bbfd102a 207 }