Revision 24:a7f92dfc5310, committed 2018-12-21

Comitter:

shimniok

Date:

Fri Dec 21 20:04:09 2018 +0000

Parent:

23:5e61cf4a8c34

Child:

25:b8176ebb96c6

Commit message:

thread changes, added stats command, added sd filesystem, other minor changes

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Logger.cpp	Fri Dec 21 20:04:09 2018 +0000
@@ -0,0 +1,38 @@
+#include "Logger.h"
+
+Logger::Logger(const char *file) {
+    _file = file;
+}
+
+
+void Logger::log_gps(GpsData gd) {
+    _open();
+    fprintf(_fp, "G,%3.7f,%3.7f,%3.1f,%2.1f,%2.1f,%d\n",
+        gd.latitude, 
+        gd.longitude,
+        gd.course,
+        gd.speed,
+        gd.hdop,
+        gd.svcount
+    );
+    _close();
+}
+
+
+void Logger::log_estimation() {
+    _open();
+    
+    _close();
+}
+
+
+void Logger::_open() {
+    _fp = fopen(_file, "a");
+}
+
+
+void Logger::_close() {
+    if (_fp) {
+        fclose(_fp);
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Logger.h	Fri Dec 21 20:04:09 2018 +0000
@@ -0,0 +1,20 @@
+#ifndef __Logger_H
+#define __Logger_H
+
+#include "mbed.h"
+#include "SystemState.h"
+
+class Logger {
+public:
+    Logger(const char *file);
+    void log_gps(GpsData gd);
+    void log_estimation();
+
+private:
+    const char *_file;
+    FILE *_fp;
+    void _open();
+    void _close();
+};
+
+#endif
\ No newline at end of file

--- a/SimpleShell.lib	Tue Dec 18 17:09:38 2018 +0000
+++ b/SimpleShell.lib	Fri Dec 21 20:04:09 2018 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/users/shimniok/code/SimpleShell/#a29fb89018e1
+https://os.mbed.com/users/shimniok/code/SimpleShell/#2b5ed529ab37

--- a/SystemState.cpp	Tue Dec 18 17:09:38 2018 +0000
+++ b/SystemState.cpp	Fri Dec 21 20:04:09 2018 +0000
@@ -10,6 +10,8 @@
 #include <string.h>
 #include "SystemState.h"
 
+#if 0 == 1
+
 volatile int inState = 0; // push pointer
 volatile int outState = 0; // pull pointer
 SystemState *state;
@@ -94,3 +96,5 @@
 int fifo_getOutState() {
     return outState;
 }
+
+#endif

--- a/SystemState.h	Tue Dec 18 17:09:38 2018 +0000
+++ b/SystemState.h	Fri Dec 21 20:04:09 2018 +0000
@@ -1,8 +1,6 @@
 #ifndef _SYSTEMSTATE_H
 #define _SYSTEMSTATE_H
 
-#define SSBUF 32 // must be 2^n
-
 /** System State is the main mechanism for communicating current realtime system state to
  * the rest of the system for logging, data display, etc.
  */
@@ -53,42 +51,17 @@
  * throttle             raw servo setting(units?)
  * steering             raw servo setting(units?)
  */
+
 typedef struct {
-    unsigned int millis;
-    float current, voltage;
-    int g[3];
-    float gyro[3];
-    int gTemp;
-    int a[3];
-    int m[3];
-    float gHeading;
-    float cHeading;
-    //float roll, pitch, yaw;
-    double gpsLatitude;
-    double gpsLongitude;
-    float gpsCourse_deg;
-    float gpsSpeed_mps;
-    float gpsHDOP;
-    int gpsSats;
-    float lrEncDistance, rrEncDistance;
-    float lrEncSpeed, rrEncSpeed;
-    float encHeading;
-    float estHeading;
-    float estLagHeading;
-    double estLatitude, estLongitude;
-    //double estNorthing, estEasting;
-    float estX, estY;
-    unsigned short nextWaypoint;
-    float bearing;
-    float distance;
-    float gbias;
-    float errHeading;
-    float steerAngle;
-    float LABrg;
-    float LAx;
-    float LAy;
-} SystemState;
+    double latitude;
+    double longitude;
+    float course;
+    float speed;
+    float hdop;
+    int svcount;
+} GpsData;
 
+#if 0 == 1
 void state_clear( SystemState *s );
 bool fifo_init(void);
 void fifo_reset(void);
@@ -99,5 +72,6 @@
 SystemState *fifo_pull(void);
 int fifo_getInState(void);
 int fifo_getOutState(void);
+#endif
 
 #endif

--- a/Ublox6.cpp	Tue Dec 18 17:09:38 2018 +0000
+++ b/Ublox6.cpp	Fri Dec 21 20:04:09 2018 +0000
@@ -184,6 +184,7 @@
     return status;
 }
 
+
 void Ublox6::read(double& lat, double& lon, float& course, float& speed, float& hdop, int& svcount) {
     lat = latest.lat;
     lon = latest.lon;

--- 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];

--- a/stats_report.h	Tue Dec 18 17:09:38 2018 +0000
+++ b/stats_report.h	Fri Dec 21 20:04:09 2018 +0000
@@ -36,11 +36,13 @@
         // Collect the static system information
         mbed_stats_sys_get(&sys_stats);
 
+        /*
         printf("=============================== SYSTEM INFO  ================================\r\n");
         printf("Mbed OS Version: %ld \r\n", sys_stats.os_version);
         printf("CPU ID: 0x%lx \r\n", sys_stats.cpu_id);
         printf("Compiler ID: %d \r\n", sys_stats.compiler_id);
         printf("Compiler Version: %ld \r\n", sys_stats.compiler_version);
+        */
     }
 
     ~SystemReport(void)
@@ -53,7 +55,7 @@
      */
     void report_state(void)
     {
-        report_cpu_stats();
+        //report_cpu_stats();
         report_heap_stats();
         report_thread_stats();
 
@@ -68,8 +70,6 @@
     {
         static uint64_t prev_idle_time = 0;
 
-        printf("================= CPU STATS =================\r\n");
-
         // Collect and print cpu stats
         mbed_stats_cpu_get(&cpu_stats);
 
@@ -78,6 +78,7 @@
         uint8_t usage = 100 - ((diff * 100) / (sample_time_ms * 1000));  // usec;;
         prev_idle_time = cpu_stats.idle_time;
 
+        puts("");
         printf("Idle: %d%% Usage: %d%% \r\n", idle, usage);
     }
 
@@ -88,11 +89,9 @@
      */
     void report_heap_stats(void)
     {
-        printf("================ HEAP STATS =================\r\n");
-
         // Collect and print heap stats
         mbed_stats_heap_get(&heap_stats);
-
+        puts("");
         printf("Current heap: %lu\r\n", heap_stats.current_size);
         printf("Max heap size: %lu\r\n", heap_stats.max_size);
     }
@@ -102,18 +101,20 @@
      */
     void report_thread_stats(void)
     {
-        printf("================ THREAD STATS ===============\r\n");
-
         // Collect and print running thread stats
         int count = mbed_stats_thread_get_each(thread_stats, max_thread_count);
 
+        puts("");
+        puts("ID        state  prio stack  free   name");
+        puts("========  =====  ==== ====== ====== ==============");
         for (int i = 0; i < count; i++) {
-            printf("ID: 0x%lx \r\n",        thread_stats[i].id);
-            printf("Name: %s \r\n",         thread_stats[i].name);
-            printf("State: %ld \r\n",       thread_stats[i].state);
-            printf("Priority: %ld \r\n",    thread_stats[i].priority);
-            printf("Stack Size: %ld \r\n",  thread_stats[i].stack_size);
-            printf("Stack Space: %ld \r\n", thread_stats[i].stack_space);
+            printf(" 0x%04x ", thread_stats[i].id & 0xFFFF);
+            printf(" %4d ",    thread_stats[i].state);
+            printf(" %4d ",    thread_stats[i].priority);
+            printf(" %5d ",    thread_stats[i].stack_size);
+            printf(" %5d ",    thread_stats[i].stack_space);
+            printf(" %s  ",    thread_stats[i].name);
+            puts("");
         }
     }
 };