![](/media/cache/profiles/2012_sf_avc_095.jpg.50x50_q85.jpg)
2018 revision to classic DataBus AVC code.
Dependencies: LSM303DLM Servo SerialGraphicLCD L3G4200D IncrementalEncoder SimpleShell
main.cpp@20:043987d06f8d, 2018-12-16 (annotated)
- Committer:
- shimniok
- Date:
- Sun Dec 16 04:18:42 2018 +0000
- Revision:
- 20:043987d06f8d
- Parent:
- 19:0d1728091519
- Child:
- 21:020f4ebbb13a
Added gps interrupt handler, fixed command line gps query
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 | 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 | 19:0d1728091519 | 31 | void read_gps(); |
shimniok | 19:0d1728091519 | 32 | |
shimniok | 20:043987d06f8d | 33 | void gps_handler() { |
shimniok | 20:043987d06f8d | 34 | if (s.readable()) { |
shimniok | 20:043987d06f8d | 35 | if (ublox.parse(s.getc())) { |
shimniok | 19:0d1728091519 | 36 | led3 = !led3; |
shimniok | 19:0d1728091519 | 37 | } |
shimniok | 16:eb28d0f64a9b | 38 | } |
shimniok | 19:0d1728091519 | 39 | } |
shimniok | 16:eb28d0f64a9b | 40 | |
shimniok | 9:fc3575d2cbbf | 41 | /******** SHELL COMMANDS ********/ |
shimniok | 9:fc3575d2cbbf | 42 | |
shimniok | 1:7019a60fd585 | 43 | void test() { |
shimniok | 1:7019a60fd585 | 44 | printf("Hello world!\n"); |
shimniok | 1:7019a60fd585 | 45 | } |
shimniok | 1:7019a60fd585 | 46 | |
shimniok | 18:3f8a8f6e3cc1 | 47 | void read_gps() |
shimniok | 18:3f8a8f6e3cc1 | 48 | { |
shimniok | 18:3f8a8f6e3cc1 | 49 | double lat=0; |
shimniok | 18:3f8a8f6e3cc1 | 50 | double lon=0; |
shimniok | 18:3f8a8f6e3cc1 | 51 | float course=0; |
shimniok | 18:3f8a8f6e3cc1 | 52 | float speed=0; |
shimniok | 18:3f8a8f6e3cc1 | 53 | float hdop=0.0; |
shimniok | 18:3f8a8f6e3cc1 | 54 | int svcount=0; |
shimniok | 18:3f8a8f6e3cc1 | 55 | |
shimniok | 18:3f8a8f6e3cc1 | 56 | ublox.read(lat, lon, course, speed, hdop, svcount); |
shimniok | 18:3f8a8f6e3cc1 | 57 | printf("%3.7f %3.7f\n", lat, lon); |
shimniok | 18:3f8a8f6e3cc1 | 58 | printf("hdg=%03.1f deg spd=%3.1f m/s\n", course, speed); |
shimniok | 18:3f8a8f6e3cc1 | 59 | printf("%d %f\n", svcount, hdop); |
shimniok | 18:3f8a8f6e3cc1 | 60 | } |
shimniok | 18:3f8a8f6e3cc1 | 61 | |
shimniok | 9:fc3575d2cbbf | 62 | void read_gyro() |
shimniok | 9:fc3575d2cbbf | 63 | { |
shimniok | 12:3cd91e150d9c | 64 | int g[3]; |
shimniok | 13:5566df1250f1 | 65 | float dt; |
shimniok | 12:3cd91e150d9c | 66 | |
shimniok | 11:8ec858b7c6d1 | 67 | Updater *u = Updater::instance(); |
shimniok | 11:8ec858b7c6d1 | 68 | |
shimniok | 13:5566df1250f1 | 69 | u->gyro(g, dt); |
shimniok | 11:8ec858b7c6d1 | 70 | |
shimniok | 13:5566df1250f1 | 71 | printf("Gyro: %d, %d, %d - dt: %f\n", g[0], g[1], g[2], dt); |
shimniok | 12:3cd91e150d9c | 72 | } |
shimniok | 12:3cd91e150d9c | 73 | |
shimniok | 14:1dd83e626153 | 74 | void read_enc() |
shimniok | 14:1dd83e626153 | 75 | { |
shimniok | 14:1dd83e626153 | 76 | Updater *u = Updater::instance(); |
shimniok | 14:1dd83e626153 | 77 | |
shimniok | 14:1dd83e626153 | 78 | printf("Encoder: %d\n", u->encoder()); |
shimniok | 14:1dd83e626153 | 79 | } |
shimniok | 14:1dd83e626153 | 80 | |
shimniok | 20:043987d06f8d | 81 | void bridge_uart1() { |
shimniok | 20:043987d06f8d | 82 | RawSerial s(UART1TX, UART1RX, 38400); |
shimniok | 20:043987d06f8d | 83 | while (1) { |
shimniok | 20:043987d06f8d | 84 | if (pc.readable()) s.putc(pc.getc()); |
shimniok | 20:043987d06f8d | 85 | if (s.readable()) pc.putc(s.getc()); |
shimniok | 20:043987d06f8d | 86 | } |
shimniok | 20:043987d06f8d | 87 | } |
shimniok | 20:043987d06f8d | 88 | |
shimniok | 20:043987d06f8d | 89 | |
shimniok | 12:3cd91e150d9c | 90 | void reset() |
shimniok | 12:3cd91e150d9c | 91 | { |
shimniok | 12:3cd91e150d9c | 92 | NVIC_SystemReset(); |
shimniok | 9:fc3575d2cbbf | 93 | } |
shimniok | 9:fc3575d2cbbf | 94 | |
shimniok | 9:fc3575d2cbbf | 95 | /******** MAIN ********/ |
shimniok | 1:7019a60fd585 | 96 | |
shimniok | 0:7e98bbfd102a | 97 | // main() runs in its own thread in the OS |
shimniok | 0:7e98bbfd102a | 98 | int main() |
shimniok | 11:8ec858b7c6d1 | 99 | { |
shimniok | 20:043987d06f8d | 100 | //bridge_uart1(); |
shimniok | 20:043987d06f8d | 101 | |
shimniok | 0:7e98bbfd102a | 102 | printf("Bootup...\n"); |
shimniok | 0:7e98bbfd102a | 103 | fflush(stdout); |
shimniok | 1:7019a60fd585 | 104 | |
shimniok | 4:de7feb458652 | 105 | printf("Loading config...\n"); |
shimniok | 7:1f2661b840ed | 106 | config.add("intercept_distance", Config::DOUBLE); |
shimniok | 7:1f2661b840ed | 107 | config.add("waypoint_threshold", Config::DOUBLE); |
shimniok | 7:1f2661b840ed | 108 | config.add("minimum_turning_radius", Config::DOUBLE); |
shimniok | 7:1f2661b840ed | 109 | config.add("wheelbase", Config::DOUBLE); |
shimniok | 7:1f2661b840ed | 110 | config.add("track_width", Config::DOUBLE); |
shimniok | 7:1f2661b840ed | 111 | config.add("tire_circumference", Config::DOUBLE); |
shimniok | 7:1f2661b840ed | 112 | config.add("encoder_stripes", Config::INT); |
shimniok | 7:1f2661b840ed | 113 | config.add("esc_brake", Config::INT); |
shimniok | 7:1f2661b840ed | 114 | config.add("esc_off", Config::INT); |
shimniok | 7:1f2661b840ed | 115 | config.add("esc_max", Config::INT); |
shimniok | 7:1f2661b840ed | 116 | config.add("turn_speed", Config::DOUBLE); |
shimniok | 7:1f2661b840ed | 117 | config.add("turn_distance", Config::DOUBLE); |
shimniok | 7:1f2661b840ed | 118 | config.add("start_speed", Config::DOUBLE); |
shimniok | 7:1f2661b840ed | 119 | config.add("cruise_speed", Config::DOUBLE); |
shimniok | 7:1f2661b840ed | 120 | config.add("speed_kp", Config::DOUBLE); |
shimniok | 7:1f2661b840ed | 121 | config.add("speed_ki", Config::DOUBLE); |
shimniok | 7:1f2661b840ed | 122 | config.add("speed_kd", Config::DOUBLE); |
shimniok | 7:1f2661b840ed | 123 | config.add("steer_center", Config::DOUBLE); |
shimniok | 7:1f2661b840ed | 124 | config.add("steer_scale", Config::DOUBLE); |
shimniok | 7:1f2661b840ed | 125 | config.add("gyro_scale", Config::DOUBLE); |
shimniok | 7:1f2661b840ed | 126 | config.add("gps_valid_speed", Config::DOUBLE); |
shimniok | 7:1f2661b840ed | 127 | |
shimniok | 4:de7feb458652 | 128 | if (config.load("/etc/2018cfg.txt")) { |
shimniok | 4:de7feb458652 | 129 | printf("error loading config\n"); |
shimniok | 4:de7feb458652 | 130 | } |
shimniok | 10:9fb3feb38746 | 131 | |
shimniok | 13:5566df1250f1 | 132 | printf("Starting shell...\n"); |
shimniok | 1:7019a60fd585 | 133 | sh.attach(test, "test"); |
shimniok | 9:fc3575d2cbbf | 134 | sh.attach(read_gyro, "gyro"); |
shimniok | 14:1dd83e626153 | 135 | sh.attach(read_enc, "enc"); |
shimniok | 18:3f8a8f6e3cc1 | 136 | sh.attach(read_gps, "gps"); |
shimniok | 12:3cd91e150d9c | 137 | sh.attach(reset, "reset"); |
shimniok | 15:35c40765f7c3 | 138 | Thread shellThread; |
shimniok | 15:35c40765f7c3 | 139 | shellThread.start(callback(&sh, &SimpleShell::run)); |
shimniok | 0:7e98bbfd102a | 140 | |
shimniok | 13:5566df1250f1 | 141 | printf("Starting updater...\n"); |
shimniok | 11:8ec858b7c6d1 | 142 | Updater *u = Updater::instance(); |
shimniok | 15:35c40765f7c3 | 143 | u->setInterval(50); |
shimniok | 15:35c40765f7c3 | 144 | Thread updaterThread; |
shimniok | 15:35c40765f7c3 | 145 | updaterThread.set_priority(osPriorityRealtime); |
shimniok | 15:35c40765f7c3 | 146 | updaterThread.start(callback(u, &Updater::start)); |
shimniok | 16:eb28d0f64a9b | 147 | |
shimniok | 16:eb28d0f64a9b | 148 | printf("Starting gps...\n"); |
shimniok | 20:043987d06f8d | 149 | s.attach(&gps_handler); |
shimniok | 20:043987d06f8d | 150 | |
shimniok | 20:043987d06f8d | 151 | /* |
shimniok | 16:eb28d0f64a9b | 152 | Thread gpsThread; |
shimniok | 19:0d1728091519 | 153 | gpsThread.set_priority(osPriorityHigh); |
shimniok | 16:eb28d0f64a9b | 154 | gpsThread.start(&do_gps); |
shimniok | 20:043987d06f8d | 155 | */ |
shimniok | 10:9fb3feb38746 | 156 | |
shimniok | 20:043987d06f8d | 157 | /* |
shimniok | 0:7e98bbfd102a | 158 | FILE *fp; |
shimniok | 0:7e98bbfd102a | 159 | char buf[128]; |
shimniok | 0:7e98bbfd102a | 160 | printf("Initializing the block device... "); |
shimniok | 0:7e98bbfd102a | 161 | fflush(stdout); |
shimniok | 0:7e98bbfd102a | 162 | int err = bd.init(); |
shimniok | 0:7e98bbfd102a | 163 | printf("%s\n", (err ? "Fail :(" : "OK")); |
shimniok | 0:7e98bbfd102a | 164 | |
shimniok | 0:7e98bbfd102a | 165 | printf("Opening sdtest.txt..."); |
shimniok | 0:7e98bbfd102a | 166 | fp = fopen("/log/sdtest.txt", "r"); |
shimniok | 0:7e98bbfd102a | 167 | if(fp) { |
shimniok | 0:7e98bbfd102a | 168 | while (!feof(fp)) { |
shimniok | 0:7e98bbfd102a | 169 | fgets(buf, 127, fp); |
shimniok | 0:7e98bbfd102a | 170 | printf(buf); |
shimniok | 0:7e98bbfd102a | 171 | } |
shimniok | 0:7e98bbfd102a | 172 | fclose(fp); |
shimniok | 0:7e98bbfd102a | 173 | } |
shimniok | 0:7e98bbfd102a | 174 | |
shimniok | 0:7e98bbfd102a | 175 | printf("Opening config.txt..."); |
shimniok | 0:7e98bbfd102a | 176 | fp = fopen("/etc/config.txt", "r"); |
shimniok | 0:7e98bbfd102a | 177 | if(fp) { |
shimniok | 0:7e98bbfd102a | 178 | while (!feof(fp)) { |
shimniok | 0:7e98bbfd102a | 179 | fgets(buf, 127, fp); |
shimniok | 0:7e98bbfd102a | 180 | printf(buf); |
shimniok | 0:7e98bbfd102a | 181 | } |
shimniok | 0:7e98bbfd102a | 182 | fclose(fp); |
shimniok | 0:7e98bbfd102a | 183 | } |
shimniok | 0:7e98bbfd102a | 184 | */ |
shimniok | 0:7e98bbfd102a | 185 | |
shimniok | 0:7e98bbfd102a | 186 | //SystemReport sys_state(500); |
shimniok | 0:7e98bbfd102a | 187 | |
shimniok | 0:7e98bbfd102a | 188 | while (true) { |
shimniok | 0:7e98bbfd102a | 189 | // Blink LED and wait 0.5 seconds |
shimniok | 0:7e98bbfd102a | 190 | led1 = !led1; |
shimniok | 0:7e98bbfd102a | 191 | wait(0.5f); |
shimniok | 0:7e98bbfd102a | 192 | |
shimniok | 0:7e98bbfd102a | 193 | // Following the main thread wait, report on the current system status |
shimniok | 0:7e98bbfd102a | 194 | //sys_state.report_state(); |
shimniok | 0:7e98bbfd102a | 195 | } |
shimniok | 0:7e98bbfd102a | 196 | } |