File content as of revision 27:bfe141fad082:

/* 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"
#include "Logger.h"
#include "SystemState.h"

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

///////////////////////////////////////////////////////////////////////////////
// Devices
LocalFileSystem lfs("etc");
SDBlockDevice bd(p5, p6, p7, p8); // MOSI, MISO, CLK, CS
FATFileSystem ffs("log", &bd);
Serial pc(USBTX, USBRX);
DigitalOut led1(LED1);
DigitalOut led2(LED2);
RawSerial s(UART1TX, UART1RX, 38400);

///////////////////////////////////////////////////////////////////////////////
// Idle hook
void idler() {
    while(1) {
        led1 = !led1;
        wait(0.1);
    }
}

///////////////////////////////////////////////////////////////////////////////
// Logging
EventQueue logQueue(8 * EVENTS_EVENT_SIZE);
Logger logger("/etc/test.log");


///////////////////////////////////////////////////////////////////////////////
// Updater
Updater *u = Updater::instance();
EventQueue updaterQueue(8 * EVENTS_EVENT_SIZE);

void updater_callback() {
    led1 = !led1;
}

///////////////////////////////////////////////////////////////////////////////
// GPS
Ublox6 ublox;
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);   
    //logQueue.call(&logger, &Logger::log_gps, d);
}
    
// ISR for GPS serial, passes off to thread
void gps_handler() {
    while (s.readable()) {
        int c = s.getc();
        gpsQueue.call(&ublox, &Ublox6::parse, c);
    }
}

///////////////////////////////////////////////////////////////////////////////
// Shell
SimpleShell sh;

void test(int argc, char **argv) {
    printf("Hello world!\n");
}

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;
    
    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 argc, char **argv) 
{
    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(int argc, char **argv)
{
    Updater *u = Updater::instance();
    
    printf("Encoder: %d\n", u->encoder());
}

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 reset(int argc, char **argv)
{
    NVIC_SystemReset();
}

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

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

    //Kernel::attach_idle_hook(idler);
    
    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 updater...\n");
    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");
    Thread gpsThread(osPriorityHigh, 256, 0, "gps");
    gpsThread.start(callback(&gpsQueue, &EventQueue::dispatch_forever));
    ublox.subscribe(gps_callback);
    s.attach(gps_handler);
    
    printf("Starting logging...\n");    
    Thread logThread(osPriorityNormal, 256, 0, "log");
    logThread.start(callback(&logQueue, &EventQueue::dispatch_forever));

    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");
    sh.attach(stats, "stats");
    sh.run();
    
    while (true) {
        // Blink LED and wait 0.5 seconds
        led1 = !led1;
        wait(0.5f);
    }
}