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;