Michael Shimniok / Mbed OS DataBus2018

2018 revision to classic DataBus AVC code.

Dependencies:   LSM303DLM Servo SerialGraphicLCD L3G4200D IncrementalEncoder SimpleShell

Updater.cpp@11:8ec858b7c6d1, 2018-12-12 (annotated)

Committer:
shimniok
Date:
Wed Dec 12 17:20:16 2018 +0000
Revision:
11:8ec858b7c6d1
Child:
12:3cd91e150d9c
RTOS periodic sensor update; values available to other threads (rudimentary)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimniok 11:8ec858b7c6d1 1 #include "Updater.h"
shimniok 11:8ec858b7c6d1 2
shimniok 11:8ec858b7c6d1 3
shimniok 11:8ec858b7c6d1 4 Updater *Updater::instance() {
shimniok 11:8ec858b7c6d1 5 static Updater instance;
shimniok 11:8ec858b7c6d1 6
shimniok 11:8ec858b7c6d1 7 return &instance;
shimniok 11:8ec858b7c6d1 8 }
shimniok 11:8ec858b7c6d1 9
shimniok 11:8ec858b7c6d1 10
shimniok 11:8ec858b7c6d1 11 void Updater::start(int interval_ms)
shimniok 11:8ec858b7c6d1 12 {
shimniok 11:8ec858b7c6d1 13 Timer timer;
shimniok 11:8ec858b7c6d1 14
shimniok 11:8ec858b7c6d1 15 thisTime = 0;
shimniok 11:8ec858b7c6d1 16 lastTime = 0;
shimniok 11:8ec858b7c6d1 17 t = &timer;
shimniok 11:8ec858b7c6d1 18 t->start();
shimniok 11:8ec858b7c6d1 19
shimniok 11:8ec858b7c6d1 20 EventQueue *queue = mbed_highprio_event_queue();
shimniok 11:8ec858b7c6d1 21 Event<void()> event(queue, callback(this, &Updater::update));
shimniok 11:8ec858b7c6d1 22 event.period(interval_ms);
shimniok 11:8ec858b7c6d1 23 event.post();
shimniok 11:8ec858b7c6d1 24 queue->dispatch_forever();
shimniok 11:8ec858b7c6d1 25 }
shimniok 11:8ec858b7c6d1 26
shimniok 11:8ec858b7c6d1 27
shimniok 11:8ec858b7c6d1 28 void Updater::update()
shimniok 11:8ec858b7c6d1 29 {
shimniok 11:8ec858b7c6d1 30 static DigitalOut led2(LED2);
shimniok 11:8ec858b7c6d1 31 static L3G4200D gyro(p9, p10); // TODO parameterize
shimniok 11:8ec858b7c6d1 32
shimniok 11:8ec858b7c6d1 33 // Compute dt
shimniok 11:8ec858b7c6d1 34 thisTime = t->read_us();
shimniok 11:8ec858b7c6d1 35 //data.dt = (lastTime < 0) ? 0 : ((float) thisTime - (float) lastTime) / 1000.0; // first pass let dt=0
shimniok 11:8ec858b7c6d1 36 lastTime = thisTime;
shimniok 11:8ec858b7c6d1 37
shimniok 11:8ec858b7c6d1 38 // Read encoders
shimniok 11:8ec858b7c6d1 39 // Read gyro
shimniok 11:8ec858b7c6d1 40 int g_a[3];
shimniok 11:8ec858b7c6d1 41 gyro.read(g_a);
shimniok 11:8ec858b7c6d1 42 g = g_a[2];
shimniok 11:8ec858b7c6d1 43
shimniok 11:8ec858b7c6d1 44 //gyro[_z_] = (g_sign[_z_] * (g[_z_] - g_offset[_z_])) / g_scale[_z_];
shimniok 11:8ec858b7c6d1 45
shimniok 11:8ec858b7c6d1 46 // Save current data into history fifo to use 1 second from now
shimniok 11:8ec858b7c6d1 47 //history[now].dist = (sensors.lrEncDistance + sensors.rrEncDistance) / 2.0; // current distance traveled
shimniok 11:8ec858b7c6d1 48 //history[now].gyro = sensors.gyro[_z_]; // current raw gyro data
shimniok 11:8ec858b7c6d1 49 //history[now].dt = dt; // current dt, to address jitter
shimniok 11:8ec858b7c6d1 50 //history[now].hdg = clamp360( history[prev].hdg + dt*(history[now].gyro) ); // compute current heading from current gyro
shimniok 11:8ec858b7c6d1 51 //float r = PI/180 * history[now].hdg;
shimniok 11:8ec858b7c6d1 52 //history[now].x = history[prev].x + history[now].dist * sin(r); // update current position
shimniok 11:8ec858b7c6d1 53 //history[now].y = history[prev].y + history[now].dist * cos(r);
shimniok 11:8ec858b7c6d1 54
shimniok 11:8ec858b7c6d1 55 // Convert this into some kind of status message/event thingy
shimniok 11:8ec858b7c6d1 56 led2 = !led2;
shimniok 11:8ec858b7c6d1 57
shimniok 11:8ec858b7c6d1 58 return;
shimniok 11:8ec858b7c6d1 59 }