Michael Shimniok / Mbed OS DataBus2018

2018 revision to classic DataBus AVC code.

Dependencies:   LSM303DLM Servo SerialGraphicLCD L3G4200D IncrementalEncoder SimpleShell

Updater.cpp@44:0d72a8a1288a, 2019-01-07 (annotated)

Committer:
shimniok
Date:
Mon Jan 07 16:47:33 2019 +0000
Revision:
44:0d72a8a1288a
Parent:
32:eb673f6f5734 
Rewrote TinyGPS -> NMEA, GPS::read() now returns struct

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimniok 11:8ec858b7c6d1 1 #include "Updater.h"
shimniok 14:1dd83e626153 2 #include "pinouts.h"
shimniok 32:eb673f6f5734 3 #include "L3G4200D.h"
shimniok 32:eb673f6f5734 4 #include "LSM303DLM.h"
shimniok 14:1dd83e626153 5 #include "IncrementalEncoder.h"
shimniok 11:8ec858b7c6d1 6
shimniok 25:b8176ebb96c6 7 Updater::Updater() {
shimniok 25:b8176ebb96c6 8 thisTime = 0;
shimniok 26:2dc31a801cc8 9 lastTime = 0;
shimniok 25:b8176ebb96c6 10 t = new Timer;
shimniok 25:b8176ebb96c6 11 t->start();
shimniok 25:b8176ebb96c6 12 _callback = 0;
shimniok 25:b8176ebb96c6 13 }
shimniok 25:b8176ebb96c6 14
shimniok 12:3cd91e150d9c 15 Updater *Updater::instance()
shimniok 12:3cd91e150d9c 16 {
shimniok 11:8ec858b7c6d1 17 static Updater instance;
shimniok 11:8ec858b7c6d1 18
shimniok 11:8ec858b7c6d1 19 return &instance;
shimniok 11:8ec858b7c6d1 20 }
shimniok 11:8ec858b7c6d1 21
shimniok 11:8ec858b7c6d1 22
shimniok 25:b8176ebb96c6 23 void Updater::attach(Callback<void()> cb) {
shimniok 25:b8176ebb96c6 24 _callback = cb;
shimniok 15:35c40765f7c3 25
shimniok 15:35c40765f7c3 26 return;
shimniok 11:8ec858b7c6d1 27 }
shimniok 11:8ec858b7c6d1 28
shimniok 11:8ec858b7c6d1 29
shimniok 11:8ec858b7c6d1 30 void Updater::update()
shimniok 11:8ec858b7c6d1 31 {
shimniok 32:eb673f6f5734 32 static L3G4200D gyro(I2CSDA, I2CSCL);
shimniok 32:eb673f6f5734 33 static LSM303DLM accel(I2CSDA, I2CSCL);
shimniok 32:eb673f6f5734 34 static IncrementalEncoder encoder(ALEFT);
shimniok 11:8ec858b7c6d1 35
shimniok 11:8ec858b7c6d1 36 // Compute dt
shimniok 11:8ec858b7c6d1 37 thisTime = t->read_us();
shimniok 26:2dc31a801cc8 38 _dt = ((float) thisTime - (float) lastTime) / 1000.0;
shimniok 26:2dc31a801cc8 39 if (lastTime > thisTime) _dt = -_dt; // attempt to fix rollover
shimniok 11:8ec858b7c6d1 40 lastTime = thisTime;
shimniok 26:2dc31a801cc8 41
shimniok 32:eb673f6f5734 42 _ecount += encoder.read();
shimniok 12:3cd91e150d9c 43 gyro.read(_gyro);
shimniok 32:eb673f6f5734 44 accel.read(_accel, _mag);
shimniok 32:eb673f6f5734 45
shimniok 11:8ec858b7c6d1 46 //gyro[_z_] = (g_sign[_z_] * (g[_z_] - g_offset[_z_])) / g_scale[_z_];
shimniok 11:8ec858b7c6d1 47
shimniok 11:8ec858b7c6d1 48 // Save current data into history fifo to use 1 second from now
shimniok 11:8ec858b7c6d1 49 //history[now].dist = (sensors.lrEncDistance + sensors.rrEncDistance) / 2.0; // current distance traveled
shimniok 11:8ec858b7c6d1 50 //history[now].gyro = sensors.gyro[_z_]; // current raw gyro data
shimniok 11:8ec858b7c6d1 51 //history[now].dt = dt; // current dt, to address jitter
shimniok 11:8ec858b7c6d1 52 //history[now].hdg = clamp360( history[prev].hdg + dt*(history[now].gyro) ); // compute current heading from current gyro
shimniok 11:8ec858b7c6d1 53 //float r = PI/180 * history[now].hdg;
shimniok 11:8ec858b7c6d1 54 //history[now].x = history[prev].x + history[now].dist * sin(r); // update current position
shimniok 11:8ec858b7c6d1 55 //history[now].y = history[prev].y + history[now].dist * cos(r);
shimniok 11:8ec858b7c6d1 56
shimniok 11:8ec858b7c6d1 57 // Convert this into some kind of status message/event thingy
shimniok 25:b8176ebb96c6 58
shimniok 25:b8176ebb96c6 59 if (_callback)
shimniok 25:b8176ebb96c6 60 _callback();
shimniok 11:8ec858b7c6d1 61
shimniok 11:8ec858b7c6d1 62 return;
shimniok 15:35c40765f7c3 63 }
shimniok 15:35c40765f7c3 64
shimniok 32:eb673f6f5734 65 void Updater::imu(int g[3], int a[3], int m[3], float& dt)
shimniok 32:eb673f6f5734 66 {
shimniok 32:eb673f6f5734 67 for (int i=0; i < 3; i++) {
shimniok 32:eb673f6f5734 68 g[i] = _gyro[i];
shimniok 32:eb673f6f5734 69 a[i] = _accel[i];
shimniok 32:eb673f6f5734 70 m[i] = _mag[i];
shimniok 32:eb673f6f5734 71 }
shimniok 32:eb673f6f5734 72 dt = _dt;
shimniok 32:eb673f6f5734 73
shimniok 32:eb673f6f5734 74 return;
shimniok 32:eb673f6f5734 75 }
shimniok 32:eb673f6f5734 76
shimniok 15:35c40765f7c3 77
shimniok 15:35c40765f7c3 78 void Updater::gyro(int g[3], float& dt)
shimniok 15:35c40765f7c3 79 {
shimniok 15:35c40765f7c3 80 for (int i=0; i < 3; i++) {
shimniok 15:35c40765f7c3 81 g[i] = _gyro[i];
shimniok 15:35c40765f7c3 82 }
shimniok 15:35c40765f7c3 83 dt = _dt;
shimniok 15:35c40765f7c3 84
shimniok 15:35c40765f7c3 85 return;
shimniok 15:35c40765f7c3 86 }
shimniok 15:35c40765f7c3 87
shimniok 15:35c40765f7c3 88
shimniok 32:eb673f6f5734 89 void Updater::accel(int a[3], float& dt)
shimniok 32:eb673f6f5734 90 {
shimniok 32:eb673f6f5734 91 for (int i=0; i < 3; i++) {
shimniok 32:eb673f6f5734 92 a[i] = _accel[i];
shimniok 32:eb673f6f5734 93 }
shimniok 32:eb673f6f5734 94 dt = _dt;
shimniok 32:eb673f6f5734 95
shimniok 32:eb673f6f5734 96 return;
shimniok 32:eb673f6f5734 97 }
shimniok 32:eb673f6f5734 98
shimniok 32:eb673f6f5734 99
shimniok 32:eb673f6f5734 100 void Updater::mag(int m[3], float& dt)
shimniok 32:eb673f6f5734 101 {
shimniok 32:eb673f6f5734 102 for (int i=0; i < 3; i++) {
shimniok 32:eb673f6f5734 103 m[i] = _mag[i];
shimniok 32:eb673f6f5734 104 }
shimniok 32:eb673f6f5734 105 dt = _dt;
shimniok 32:eb673f6f5734 106
shimniok 32:eb673f6f5734 107 return;
shimniok 32:eb673f6f5734 108 }
shimniok 32:eb673f6f5734 109
shimniok 32:eb673f6f5734 110
shimniok 15:35c40765f7c3 111 int Updater::encoder()
shimniok 15:35c40765f7c3 112 {
shimniok 15:35c40765f7c3 113 int result=_ecount;
shimniok 15:35c40765f7c3 114 _ecount = 0;
shimniok 15:35c40765f7c3 115
shimniok 15:35c40765f7c3 116 return result;
shimniok 15:35c40765f7c3 117 }
shimniok 15:35c40765f7c3 118