File content as of revision 43:9a285515f33a:

/* 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 "Servo.h"
#include "SimpleShell.h"
#include "Config.h"
#include "Updater.h"
//#include "Ublox6.h"
#include "TinyGPS.h"
#include "Logger.h"
#include "SystemState.h"
#include "Display.h"

///////////////////////////////////////////////////////////////////////////////
// Config
Config config;


///////////////////////////////////////////////////////////////////////////////
// Devices
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
LocalFileSystem lfs("etc");
SDBlockDevice bd(p5, p6, p7, p8); // MOSI, MISO, CLK, CS
FATFileSystem ffs("log", &bd);
Serial pc(USBTX, USBRX);
//RawSerial s(UART1TX, UART1RX, 38400);
RawSerial s(UART1TX, UART1RX, 19200);
InterruptIn lbutton(LBUTTON, PullUp); // button interrupts
InterruptIn cbutton(CBUTTON, PullUp);
InterruptIn rbutton(RBUTTON, PullUp);
Servo steer(STEERING);
Servo esc(THROTTLE);


///////////////////////////////////////////////////////////////////////////////
// Logging
EventQueue logQueue(16 * EVENTS_EVENT_SIZE);
Logger logger;


///////////////////////////////////////////////////////////////////////////////
// Display
EventQueue lcdQueue(8 * EVENTS_EVENT_SIZE);
Display display(UART3TX, UART3RX);

const uint64_t sample_time_ms = 50;

void compute_cpu_stats()
{
    static uint64_t prev_idle_time = 0;
    mbed_stats_cpu_t    cpu_stats;

    // Collect and print cpu stats
    mbed_stats_cpu_get(&cpu_stats);

    uint64_t diff = (cpu_stats.idle_time - prev_idle_time);
    uint8_t idle = (diff * 100) / (sample_time_ms * 1000);  // usec;
    prev_idle_time = cpu_stats.idle_time;
    display.cpu(idle);
}
    
///////////////////////////////////////////////////////////////////////////////
// Updater
Updater *u = Updater::instance();
EventQueue updaterQueue(8 * EVENTS_EVENT_SIZE);

void updater_callback() {
    SensorData d;
    float dt;
    static int count = 0;
    
    led1 = !led1;
    d.timestamp = Kernel::get_ms_count();
    d.encoder = u->encoder();
    u->imu(d.gyro, d.accel, d.mag, dt);
    logQueue.call(&logger, &Logger::log_sensors, d);
    if (count++ > 10) {
        lcdQueue.call(&display, &Display::imu, d);
        count = 0;
    }
}


///////////////////////////////////////////////////////////////////////////////
// Buttons
EventQueue buttonQueue(16 * EVENTS_EVENT_SIZE);
const int BUTTON_DEBOUNCE_SAMPLES = 20;
const int BUTTON_DEBOUNCE_THRESH = 8;
const int BUTTON_SAMPLE_MS = 0.005;

void button_event() {
    int samples = 0;

    // disable subsequent interrupts
    lbutton.fall(NULL);
 
    // sample repeatedly to debounce
    for (int i=0; i < BUTTON_DEBOUNCE_SAMPLES; i++) {
        if (lbutton == 0) samples++;
        wait(BUTTON_SAMPLE_MS);
    }
    
    if (samples > BUTTON_DEBOUNCE_THRESH) {
        if (logger.enabled()) {
            logQueue.call(&logger, &Logger::stop);
            led3 = 0;
        } else {
            logQueue.call(&logger, &Logger::start);
            led3 = 1;
        }
    }
    
    lbutton.fall(buttonQueue.event(button_event));
}


///////////////////////////////////////////////////////////////////////////////
// GPS
//Ublox6 ublox;
TinyGPS gps;
EventQueue gpsQueue(8 * EVENTS_EVENT_SIZE);

// Callback for gps parse data ready
void gps_callback() {
    GpsData d;
    
    led2 = !led2;
    //ublox.read(d.latitude, d.longitude, d.course, d.speed, d.hdop, d.svcount);   
    gps.read(d.latitude, d.longitude, d.course, d.speed, d.hdop, d.svcount);
    d.timestamp = Kernel::get_ms_count();
    logQueue.call(&logger, &Logger::log_gps, d);
    lcdQueue.call(&display, &Display::gps, d);
}

// ISR for GPS serial, passes off to thread
void gps_handler() {
    while (s.readable()) {
        char c = s.getc();
        //gpsQueue.call(&ublox, &Ublox6::parse, c);
        gpsQueue.call(&gps, &TinyGPS::parse, c);
    }
}


///////////////////////////////////////////////////////////////////////////////
// Shell
SimpleShell sh("/log");

void test(int argc, char **argv) {
    //printf("Hello world!\n");
    char file[32];
    
    for (int i=30; i < 40; i++) {
        sprintf(file, "/log/%04d.csv", i);
        printf("removing <%s>\n", file);
        int stat = remove(file);
        printf("return: %d\n", stat);
    }
}

void stats(int argc, char **argv) {
    SystemReport r(200);
    r.report_state();
    return;   
}

void read_gps(int argc, char **argv)
{
    double lat=0;
    double lon=0;
    float course=0;
    float speed=0;
    float hdop=0.0;
    int svcount=0;
    
    gps.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_imu(int argc, char **argv) 
{
    int g[3];
    int a[3];
    int m[3];
    float dt;
    
    Updater *u = Updater::instance();
    
    u->imu(g, a, m, dt);
   
    printf("  Gyro: %d, %d, %d\n", g[0], g[1], g[2]);
    printf(" Accel: %d, %d, %d\n", a[0], a[1], a[2]);
    printf("   Mag: %d, %d, %d\n", m[0], m[1], m[2]);
    printf("    dt: %f\n", dt);
}

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

void log(int argc, char **argv)
{
    char *usage = "usage: log [start|stop]";
    
    if (argc == 1) {
        printf("logging ");
        if (logger.enabled())
            printf("enabled");
        else
            printf("disabled");
        printf("\n");
    } else if (argc == 2) {
        if (!strcmp("start", argv[1])) {
            logger.start();
        } else if (!strcmp("stop", argv[1])) {
            logger.stop();
        } else {
            puts(usage);
        }
    } else {
        puts(usage);
    }
}


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


void bridge_uart2() {
    RawSerial s(UART2TX, UART2RX, 19200);
    while (1) {
        if (pc.readable()) s.putc(pc.getc());
        if (s.readable()) pc.putc(s.getc());
    }
}


void reset(int argc, char **argv)
{
    NVIC_SystemReset();
}

///////////////////////////////////////////////////////////////////////////////
// MAIN

// main() runs in its own thread in the OS
int main()
{   
    //bridge_uart1();
    //bridge_uart2();

    //Kernel::attach_idle_hook(idler);
    
    display.status("Bootup...");
    printf("Bootup...\n");
    fflush(stdout);
    
    steer = 0.50;
    esc = 0.00;
    
    printf("Loading config...\n");
    display.status("Loading config");
    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");
        display.status("config load error");
    }

    printf("Starting display...\n");
    Thread lcdThread(osPriorityNormal, 2048, 0, "lcd");
    lcdThread.start(callback(&lcdQueue, &EventQueue::dispatch_forever));
    //lcdQueue.call_every(sample_time_ms, callback(&compute_cpu_stats));

    printf("Starting buttons...\n");
    display.status("Starting buttons");
    lbutton.fall(buttonQueue.event(button_event));
    //cbutton.rise(buttonQueue.event(button_event));
    //rbutton.rise(buttonQueue.event(button_event));
    Thread buttonThread(osPriorityNormal, 256, 0, "buttons");
    buttonThread.start(callback(&buttonQueue, &EventQueue::dispatch_forever));
    
    printf("Starting updater...\n");
    display.status("Starting updater");
    u->attach(updater_callback);
    Thread updaterThread(osPriorityRealtime, 512, 0, "updater");
    updaterQueue.call_every(20, u, &Updater::update);
    updaterThread.start(callback(&updaterQueue, &EventQueue::dispatch_forever));
    
    printf("Starting gps...\n");
    display.status("Starting gps");
    Thread gpsThread(osPriorityHigh, 2048, 0, "gps");
    gpsThread.start(callback(&gpsQueue, &EventQueue::dispatch_forever));
    gps.subscribe(gps_callback);
    s.attach(gps_handler);
    
    printf("Starting logging...\n");
    display.status("Starting logging");
    Thread logThread(osPriorityNormal, 2048, 0, "log");
    logThread.start(callback(&logQueue, &EventQueue::dispatch_forever));

    printf("Starting shell...\n");
    display.status("Starting shell");
    sh.command(test, "test");
    sh.command(read_imu, "imu");
    sh.command(read_enc, "enc");
    sh.command(read_gps, "gps");
    sh.command(reset, "reset");
    sh.command(stats, "stats");
    sh.command(log, "log");

    display.status("DataBus 2018");
    sh.run();
    
    while (true) {
        // Blink LED and wait 0.5 seconds
        led1 = !led1;
        wait(0.5f);
    }
}