2018 revision to classic DataBus AVC code.
Dependencies: LSM303DLM Servo SerialGraphicLCD L3G4200D IncrementalEncoder SimpleShell
Diff: Updater.cpp
- Revision:
- 32:eb673f6f5734
- Parent:
- 26:2dc31a801cc8
--- a/Updater.cpp Thu Dec 27 00:42:35 2018 +0000 +++ b/Updater.cpp Thu Dec 27 15:46:57 2018 +0000 @@ -1,5 +1,7 @@ #include "Updater.h" #include "pinouts.h" +#include "L3G4200D.h" +#include "LSM303DLM.h" #include "IncrementalEncoder.h" Updater::Updater() { @@ -27,8 +29,9 @@ void Updater::update() { - static L3G4200D gyro(I2CSDA, I2CSCL); // TODO parameterize - static IncrementalEncoder enc(ALEFT); + static L3G4200D gyro(I2CSDA, I2CSCL); + static LSM303DLM accel(I2CSDA, I2CSCL); + static IncrementalEncoder encoder(ALEFT); // Compute dt thisTime = t->read_us(); @@ -36,12 +39,10 @@ if (lastTime > thisTime) _dt = -_dt; // attempt to fix rollover lastTime = thisTime; - // Read encoders - _ecount += enc.read(); - - // Read gyro + _ecount += encoder.read(); gyro.read(_gyro); - + accel.read(_accel, _mag); + //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 @@ -61,6 +62,18 @@ return; } +void Updater::imu(int g[3], int a[3], int m[3], float& dt) +{ + for (int i=0; i < 3; i++) { + g[i] = _gyro[i]; + a[i] = _accel[i]; + m[i] = _mag[i]; + } + dt = _dt; + + return; +} + void Updater::gyro(int g[3], float& dt) { @@ -73,6 +86,28 @@ } +void Updater::accel(int a[3], float& dt) +{ + for (int i=0; i < 3; i++) { + a[i] = _accel[i]; + } + dt = _dt; + + return; +} + + +void Updater::mag(int m[3], float& dt) +{ + for (int i=0; i < 3; i++) { + m[i] = _mag[i]; + } + dt = _dt; + + return; +} + + int Updater::encoder() { int result=_ecount;