Diff: main.cpp

Revision:

24:a7f92dfc5310

Parent:

23:5e61cf4a8c34

Child:

25:b8176ebb96c6

--- a/main.cpp	Tue Dec 18 17:09:38 2018 +0000
+++ b/main.cpp	Fri Dec 21 20:04:09 2018 +0000
@@ -14,52 +14,74 @@
 #include "Config.h"
 #include "Updater.h"
 #include "Ublox6.h"
+#include "Logger.h"
+#include "SystemState.h"
 
-Serial pc(USBTX, USBRX);
-//SDBlockDevice bd(p5, p6, p7, p8); // MOSI, MISO, CLK, CS
-//FATFileSystem ffs("log", &bd);
+///////////////////////////////////////////////////////////////////////////////
+// Config
+Config config;
+
+///////////////////////////////////////////////////////////////////////////////
+// Devices
 LocalFileSystem lfs("etc");
-Config config;
-SimpleShell sh;
-Ublox6 ublox;
-
+SDBlockDevice bd(p5, p6, p7, p8); // MOSI, MISO, CLK, CS
+FATFileSystem ffs("log", &bd);
+Serial pc(USBTX, USBRX);
 DigitalOut led1(LED1);
 DigitalOut led3(LED3);
-
 RawSerial s(UART1TX, UART1RX, 38400);
 
-EventQueue gpsQueue(32 * EVENTS_EVENT_SIZE);
+///////////////////////////////////////////////////////////////////////////////
+// Idle hook
+void idler() {
+    while(1) {
+        led1 = !led1;
+        wait(0.1);
+    }
+}
 
+///////////////////////////////////////////////////////////////////////////////
+// Logging
+EventQueue logQueue(8 * EVENTS_EVENT_SIZE);
+Logger logger("/etc/test.log");
+
+///////////////////////////////////////////////////////////////////////////////
+// GPS
+Ublox6 ublox;
+EventQueue gpsQueue(8 * EVENTS_EVENT_SIZE);
 
 // Callback for gps parse data ready
 void gps_callback() {
+    GpsData d;
+    
     led3 = !led3;
+    ublox.read(d.latitude, d.longitude, d.course, d.speed, d.hdop, d.svcount);   
+    //logQueue.call(&logger, &Logger::log_gps, d);
 }
     
-
-// thread / eventqueue wrapper for gps parse
-void gps_parse(int c) {
-    if (ublox.parse(c)) {
-        led3 = !led3;
-    }
-}
-
 // ISR for GPS serial, passes off to thread
 void gps_handler() {
     while (s.readable()) {
         int c = s.getc();
-        //gpsQueue.call(gps_parse, c);
         gpsQueue.call(&ublox, &Ublox6::parse, c);
     }
 }
 
-/******** SHELL COMMANDS ********/
+///////////////////////////////////////////////////////////////////////////////
+// Shell
+SimpleShell sh;
 
-void test() {
+void test(int argc, char **argv) {
     printf("Hello world!\n");
 }
 
-void read_gps()
+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;
@@ -74,7 +96,7 @@
     printf("%d %f\n", svcount, hdop);
 }
 
-void read_gyro() 
+void read_gyro(int argc, char **argv) 
 {
     int g[3];
     float dt;
@@ -86,14 +108,14 @@
     printf("Gyro: %d, %d, %d - dt: %f\n", g[0], g[1], g[2], dt);
 }
 
-void read_enc()
+void read_enc(int argc, char **argv)
 {
     Updater *u = Updater::instance();
     
     printf("Encoder: %d\n", u->encoder());
 }
 
-void bridge_uart1() {
+void bridge_uart1(int argc, char **argv) {
     RawSerial s(UART1TX, UART1RX, 38400);
     while (1) {
         if (pc.readable()) s.putc(pc.getc());
@@ -101,18 +123,20 @@
     }
 }
 
-
-void reset()
+void reset(int argc, char **argv)
 {
     NVIC_SystemReset();
 }
 
-/******** MAIN ********/
+///////////////////////////////////////////////////////////////////////////////
+// MAIN
 
 // main() runs in its own thread in the OS
 int main()
 {   
     //bridge_uart1();
+
+    Kernel::attach_idle_hook(idler);
     
     printf("Bootup...\n");
     fflush(stdout);
@@ -144,27 +168,33 @@
         printf("error loading config\n");
     }
 
+    printf("Starting updater...\n");
+    Updater *u = Updater::instance();
+    u->setInterval(20);
+    Thread updaterThread(osPriorityRealtime, 512, 0, "updater");
+    updaterThread.start(callback(u, &Updater::start));
+    
+    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");
-    Thread shellThread;
-    shellThread.start(callback(&sh, &SimpleShell::run));
-
-    printf("Starting updater...\n");
-    Updater *u = Updater::instance();
-    u->setInterval(20);
-    Thread updaterThread(osPriorityRealtime);
-    updaterThread.start(callback(u, &Updater::start));
+    sh.attach(stats, "stats");
+    //Thread shellThread(osPriorityNormal, 1024, 0, "shell");
+    //shellThread.start(callback(&sh, &SimpleShell::run));
+    sh.run();
     
-    printf("Starting gps...\n");
-    Thread gpsThread(osPriorityHigh);
-    gpsThread.start(callback(&gpsQueue, &EventQueue::dispatch_forever));
-    ublox.subscribe(gps_callback);
-    s.attach(gps_handler);
-
     /*
     FILE *fp;
     char buf[128];