File content as of revision 21:020f4ebbb13a:

/* mbed Microcontroller Library
 * Copyright (c) 2018 ARM Limited
 * SPDX-License-Identifier: Apache-2.0
 */

#include "mbed.h"
#include <stdio.h>
#include <errno.h>
#include "pinouts.h"
#include "stats_report.h"
#include "SDBlockDevice.h"
#include "FATFileSystem.h"
#include "SimpleShell.h"
#include "Config.h"
#include "Updater.h"
#include "Ublox6.h"

Serial pc(USBTX, USBRX);
//SDBlockDevice bd(p5, p6, p7, p8); // MOSI, MISO, CLK, CS
//FATFileSystem ffs("log", &bd);
LocalFileSystem lfs("etc");
Config config;
SimpleShell sh;
Ublox6 ublox;

DigitalOut led1(LED1);
DigitalOut led3(LED3);

RawSerial s(UART1TX, UART1RX, 38400);

void read_gps();

void gps_handler() {
    if (s.readable()) {
        if (ublox.parse(s.getc())) {
            led3 = !led3;
        }
    }
}

/******** SHELL COMMANDS ********/

void test() {
    printf("Hello world!\n");
}

void read_gps()
{
    double lat=0;
    double lon=0;
    float course=0;
    float speed=0;
    float hdop=0.0;
    int svcount=0;
    
    ublox.read(lat, lon, course, speed, hdop, svcount);
    printf("%3.7f %3.7f\n", lat, lon);
    printf("hdg=%03.1f deg spd=%3.1f m/s\n", course, speed);
    printf("%d %f\n", svcount, hdop);
}

void read_gyro() 
{
    int g[3];
    float dt;
    
    Updater *u = Updater::instance();
    
    u->gyro(g, dt);
    
    printf("Gyro: %d, %d, %d - dt: %f\n", g[0], g[1], g[2], dt);
}

void read_enc()
{
    Updater *u = Updater::instance();
    
    printf("Encoder: %d\n", u->encoder());
}

void bridge_uart1() {
    RawSerial s(UART1TX, UART1RX, 38400);
    while (1) {
        if (pc.readable()) s.putc(pc.getc());
        if (s.readable()) pc.putc(s.getc());
    }
}


void reset()
{
    NVIC_SystemReset();
}

/******** MAIN ********/

// main() runs in its own thread in the OS
int main()
{   
    //bridge_uart1();
    
    printf("Bootup...\n");
    fflush(stdout);
    
    printf("Loading config...\n");
    config.add("intercept_distance", Config::DOUBLE);
    config.add("waypoint_threshold", Config::DOUBLE);
    config.add("minimum_turning_radius", Config::DOUBLE);
    config.add("wheelbase", Config::DOUBLE);
    config.add("track_width", Config::DOUBLE);
    config.add("tire_circumference", Config::DOUBLE);
    config.add("encoder_stripes", Config::INT);
    config.add("esc_brake", Config::INT);
    config.add("esc_off", Config::INT);
    config.add("esc_max", Config::INT);
    config.add("turn_speed", Config::DOUBLE);
    config.add("turn_distance", Config::DOUBLE);
    config.add("start_speed", Config::DOUBLE);
    config.add("cruise_speed", Config::DOUBLE);
    config.add("speed_kp", Config::DOUBLE);
    config.add("speed_ki", Config::DOUBLE);
    config.add("speed_kd", Config::DOUBLE);
    config.add("steer_center", Config::DOUBLE);
    config.add("steer_scale", Config::DOUBLE);
    config.add("gyro_scale", Config::DOUBLE);
    config.add("gps_valid_speed", Config::DOUBLE);    
    
    if (config.load("/etc/2018cfg.txt")) {
        printf("error loading config\n");
    }

    printf("Starting shell...\n");
    sh.attach(test, "test");
    sh.attach(read_gyro, "gyro");
    sh.attach(read_enc, "enc");
    sh.attach(read_gps, "gps");
    sh.attach(reset, "reset");
    Thread shellThread;
    shellThread.start(callback(&sh, &SimpleShell::run));

    printf("Starting updater...\n");
    Updater *u = Updater::instance();
    u->setInterval(20);
    Thread updaterThread;
    updaterThread.set_priority(osPriorityRealtime);
    updaterThread.start(callback(u, &Updater::start));

    printf("Starting gps...\n");
    s.attach(&gps_handler);

    /*
    Thread gpsThread;
    gpsThread.set_priority(osPriorityHigh);
    gpsThread.start(&do_gps);
    */
    
    /*
    FILE *fp;
    char buf[128];
    printf("Initializing the block device... ");
    fflush(stdout);
    int err = bd.init();
    printf("%s\n", (err ? "Fail :(" : "OK"));

    printf("Opening sdtest.txt...");
    fp = fopen("/log/sdtest.txt", "r");
    if(fp) {
        while (!feof(fp)) {
            fgets(buf, 127, fp);
            printf(buf);
        }
        fclose(fp);
    }
  
    printf("Opening config.txt...");
    fp = fopen("/etc/config.txt", "r");
    if(fp) {
        while (!feof(fp)) {
            fgets(buf, 127, fp);
            printf(buf);
        }
        fclose(fp);
    }
*/  

    //SystemReport sys_state(500);

    while (true) {
        // Blink LED and wait 0.5 seconds
        led1 = !led1;
        wait(0.5f);

        // Following the main thread wait, report on the current system status
        //sys_state.report_state();
    }
}