Diff: main.cpp

Revision:

30:ed791f1f7f7d

Parent:

29:cb2f55fbfe9c

Child:

31:20a95043adb0

--- a/main.cpp	Sat Dec 22 20:28:52 2018 +0000
+++ b/main.cpp	Wed Dec 26 19:34:17 2018 +0000
@@ -23,13 +23,18 @@
 
 ///////////////////////////////////////////////////////////////////////////////
 // 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);
-DigitalOut led1(LED1);
-DigitalOut led2(LED2);
 RawSerial s(UART1TX, UART1RX, 38400);
+InterruptIn lbutton(LBUTTON, PullUp); // left button
+InterruptIn cbutton(CBUTTON, PullUp); // center button
+InterruptIn rbutton(RBUTTON, PullUp); // right button
 
 ///////////////////////////////////////////////////////////////////////////////
 // Idle hook
@@ -51,10 +56,40 @@
 EventQueue updaterQueue(8 * EVENTS_EVENT_SIZE);
 
 void updater_callback() {
+    SensorData d;
+    float dt;
+    
     led1 = !led1;
+    d.timestamp = Kernel::get_ms_count();
+    d.encoder = u->encoder();
+    u->gyro(d.gyro, dt);
+    logQueue.call(&logger, &Logger::log_sensors, d);
 }
 
 ///////////////////////////////////////////////////////////////////////////////
+// Buttons
+
+//enum button_mask { LEFT = 0x01, CENTER = 0x02, RIGHT = 0x04 };
+EventQueue buttonQueue(16 * EVENTS_EVENT_SIZE);
+int last = 0;
+const int BUTTON_DEBOUNCE_MS = 100;
+
+void button_event() {
+    int now = Kernel::get_ms_count();
+    if ((now - last) > BUTTON_DEBOUNCE_MS) {
+        if (logger.enabled()) {
+            logQueue.call(&logger, &Logger::stop);
+            led3 = 0;
+        } else {
+            logQueue.call(&logger, &Logger::start);
+            led3 = 1;
+        }
+    }
+    last = now;
+}
+
+
+///////////////////////////////////////////////////////////////////////////////
 // GPS
 Ublox6 ublox;
 EventQueue gpsQueue(8 * EVENTS_EVENT_SIZE);
@@ -65,6 +100,7 @@
     
     led2 = !led2;
     ublox.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);
 }
 
@@ -78,7 +114,7 @@
 
 ///////////////////////////////////////////////////////////////////////////////
 // Shell
-SimpleShell sh;
+SimpleShell sh("/log");
 
 void test(int argc, char **argv) {
     printf("Hello world!\n");
@@ -202,6 +238,13 @@
         printf("error loading config\n");
     }
 
+    printf("Starting buttons...\n");
+    lbutton.rise(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");
     u->attach(updater_callback);
     Thread updaterThread(osPriorityRealtime, 512, 0, "updater");
@@ -219,13 +262,13 @@
     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.attach(log, "log");
+    sh.command(test, "test");
+    sh.command(read_gyro, "gyro");
+    sh.command(read_enc, "enc");
+    sh.command(read_gps, "gps");
+    sh.command(reset, "reset");
+    sh.command(stats, "stats");
+    sh.command(log, "log");
     sh.run();
     
     while (true) {