Revision 11:8ec858b7c6d1, committed 2018-12-12

Comitter:

shimniok

Date:

Wed Dec 12 17:20:16 2018 +0000

Parent:

10:9fb3feb38746

Child:

12:3cd91e150d9c

Commit message:

RTOS periodic sensor update; values available to other threads (rudimentary)

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Updater.cpp	Wed Dec 12 17:20:16 2018 +0000
@@ -0,0 +1,59 @@
+#include "Updater.h"
+
+
+Updater *Updater::instance() {
+    static Updater instance;
+
+    return &instance;
+}
+
+
+void Updater::start(int interval_ms)
+{
+    Timer timer;
+    
+    thisTime = 0;
+    lastTime = 0; 
+    t = &timer;
+    t->start();
+   
+    EventQueue *queue = mbed_highprio_event_queue();
+    Event<void()> event(queue, callback(this, &Updater::update));
+    event.period(interval_ms);
+    event.post();
+    queue->dispatch_forever();
+}
+
+
+void Updater::update()
+{
+    static DigitalOut led2(LED2);
+    static L3G4200D gyro(p9, p10); // TODO parameterize
+
+    // Compute dt
+    thisTime = t->read_us();
+    //data.dt = (lastTime < 0) ? 0 : ((float) thisTime - (float) lastTime) / 1000.0; // first pass let dt=0
+    lastTime = thisTime;
+
+    // Read encoders
+    // Read gyro
+    int g_a[3];
+    gyro.read(g_a);
+    g = g_a[2];
+    
+    //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);
+
+    // Convert this into some kind of status message/event thingy
+    led2 = !led2;
+
+    return;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Updater.h	Wed Dec 12 17:20:16 2018 +0000
@@ -0,0 +1,29 @@
+#ifndef __UPDATER_H
+#define __UPDATER_H
+
+#include "mbed.h"
+#include "L3G4200D.h"
+
+class Updater {
+public:
+    void start(int interval_ms);
+
+    static Updater *instance();
+    
+    int get_gyro() { return g; }
+      
+private:
+    typedef struct {
+        int g[3]; // gyro
+        float dt;  // delta time
+    } sensor_data_t;
+
+    Updater() {} // parameterize
+    Timer *t;
+    void update();
+    int g;
+    int thisTime;
+    int lastTime;
+};
+
+#endif
\ No newline at end of file

--- a/main.cpp	Mon Dec 10 20:12:41 2018 +0000
+++ b/main.cpp	Wed Dec 12 17:20:16 2018 +0000
@@ -11,7 +11,7 @@
 #include "FATFileSystem.h"
 #include "SimpleShell.h"
 #include "Config.h"
-#include "L3G4200D.h"
+#include "Updater.h"
 
 Serial pc(USBTX, USBRX);
 //SDBlockDevice bd(p5, p6, p7, p8); // MOSI, MISO, CLK, CS
@@ -21,16 +21,8 @@
 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() {
@@ -39,51 +31,18 @@
 
 void read_gyro() 
 {
-    printf("Gyro: %d, %d, %d %f\n", data.g[0], data.g[1], data.g[2], data.dt);
+    Updater *u = Updater::instance();
+    
+    printf("Gyro: %d\n", u->get_gyro());
+    
+    //printf("Gyro: %d, %d, %d\n", g[0], g[1], g[2]);
 }
 
-/********** 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);
     
@@ -119,13 +78,9 @@
     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();
+    // Startup updater
+    Updater *u = Updater::instance();
+    u->start(50);
     
 /*
     FILE *fp;