File content as of revision 10:9fb3feb38746:

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

#include "mbed.h"
#include <stdio.h>
#include <errno.h>
#include "stats_report.h"
#include "SDBlockDevice.h"
#include "FATFileSystem.h"
#include "SimpleShell.h"
#include "Config.h"
#include "L3G4200D.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;

DigitalOut led1(LED1);
DigitalOut led2(LED2);
Thread thread;

typedef struct {
    int g[3]; // gyro
    float dt;  // delta time
} sensor_data_t;

sensor_data_t data;

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

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

void read_gyro() 
{
    printf("Gyro: %d, %d, %d %f\n", data.g[0], data.g[1], data.g[2], data.dt);
}

/********** PERIODIC FUNCTIONS ***********/
L3G4200D gyro(p9, p10);
Timer timer;
int thisTime = 0;
int lastTime = -1;

void periodic()
{
    // Compute dt
    thisTime = timer.read_us();
    data.dt = (lastTime < 0) ? 0 : ((float) thisTime - (float) lastTime) / 1000.0; // first pass let dt=0
    lastTime = thisTime;

    // Read encoders
    // Read gyro
    gyro.read(data.g);
    //gyro[_z_] = (g_sign[_z_] * (g[_z_] - g_offset[_z_])) / g_scale[_z_];

    // Save current data into history fifo to use 1 second from now
    //history[now].dist = (sensors.lrEncDistance + sensors.rrEncDistance) / 2.0; // current distance traveled
    //history[now].gyro = sensors.gyro[_z_];  // current raw gyro data
    //history[now].dt = dt; // current dt, to address jitter
    //history[now].hdg = clamp360( history[prev].hdg + dt*(history[now].gyro) ); // compute current heading from current gyro
    //float r = PI/180 * history[now].hdg;
    //history[now].x = history[prev].x + history[now].dist * sin(r);    // update current position
    //history[now].y = history[prev].y + history[now].dist * cos(r);

    led2 = !led2;

    return;
}


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

// main() runs in its own thread in the OS
int main()
{
    EventQueue *queue = mbed_highprio_event_queue();
    
    timer.start();
    
    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");
    }

    // Startup shell    
    sh.attach(test, "test");
    sh.attach(read_gyro, "gyro");
    thread.start(callback(&sh, &SimpleShell::run));

    //ticker.attach(callback(periodic), 0.010); // call periodic every 10ms

    // schedule calls to periodic at 20 Hz
    Event<void()> event(queue, periodic);
    event.period(50);
    event.post();
    queue->dispatch_forever();
    
/*
    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();
    }
}