2018 revision to classic DataBus AVC code.
Dependencies: LSM303DLM Servo SerialGraphicLCD L3G4200D IncrementalEncoder SimpleShell
Updater.cpp@32:eb673f6f5734, 2018-12-27 (annotated)
- Committer:
- shimniok
- Date:
- Thu Dec 27 15:46:57 2018 +0000
- Revision:
- 32:eb673f6f5734
- Parent:
- 26:2dc31a801cc8
Added accel & mag to data collection and logging
Who changed what in which revision?
User | Revision | Line number | New 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 |