Revision 32:eb673f6f5734, committed 2018-12-27

Comitter:

shimniok

Date:

Thu Dec 27 15:46:57 2018 +0000

Parent:

31:20a95043adb0

Child:

33:74c514aea0a1

Commit message:

Added accel & mag to data collection and logging

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LSM303DLM.lib	Thu Dec 27 15:46:57 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/shimniok/code/LSM303DLM/#0fcea8569714

--- a/Logger.cpp	Thu Dec 27 00:42:35 2018 +0000
+++ b/Logger.cpp	Thu Dec 27 15:46:57 2018 +0000
@@ -73,12 +73,18 @@
 
 void Logger::log_sensors(SensorData d) {
     if (enabled()) {
-        fprintf(_fp, "S,%llu,%u,%d,%d,%d\n",
+        fprintf(_fp, "S,%llu,%u,%d,%d,%d,%d,%d,%d,%d,%d,%d\n",
             d.timestamp,
             d.encoder, 
             d.gyro[0],
             d.gyro[1],
-            d.gyro[2]
+            d.gyro[2],
+            d.accel[0],
+            d.accel[1],
+            d.accel[2],
+            d.mag[0],
+            d.mag[1],
+            d.mag[2]            
         );       
     }
 }

--- a/SystemState.h	Thu Dec 27 00:42:35 2018 +0000
+++ b/SystemState.h	Thu Dec 27 15:46:57 2018 +0000
@@ -67,6 +67,8 @@
     uint64_t timestamp;
     int encoder;
     int gyro[3];
+    int accel[3];
+    int mag[3];
 } SensorData;
 
 #endif

--- 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;

--- a/Updater.h	Thu Dec 27 00:42:35 2018 +0000
+++ b/Updater.h	Thu Dec 27 15:46:57 2018 +0000
@@ -2,7 +2,6 @@
 #define __UPDATER_H
 
 #include "mbed.h"
-#include "L3G4200D.h"
 
 /** Periodically reads sensor data
  * This class reads and updates sensor data. Intended to be called at a fixed
@@ -28,12 +27,36 @@
     /// Update all sensors
     void update(); 
     
+    /** Get imu values
+     * @param g x, y, z gyro values to return
+     * @param a x, y, z accelerometer values to return
+     * @param m x, y, z magnetometer values to return
+     * @param dt time since data last updated
+     * @return g, a, m and dt
+     */
+    void imu(int g[3], int a[3], int m[3], float& dt);
+    
     /** Get gyro values
-     * @return g array of x, y, and z gyro values
-     * @return dt time since data last updated
+     * @param g x, y, z gyro values to return
+     * @param dt time since data last updated
+     * @return g and dt
      */
     void gyro(int g[3], float& dt);
+
+    /** Get accel values
+     * @param a x, y, z accelerometer values to return
+     * @param dt time since data last updated
+     * @return a and dt
+     */
+    void accel(int g[3], float& dt);
     
+    /** Get magnetometer values
+     * @param m x, y, z magnetometer values to return
+     * @param dt time since data last updated
+     * @return m and dt
+     */
+    void mag(int g[3], float& dt);
+
     /** Get encoder count
      * @return encoder count since last call
      */
@@ -46,6 +69,8 @@
     Callback<void()> _callback; // notification callback    
     Timer *t; // timer used to measure dt
     int _gyro[3]; // gyro raw
+    int _accel[3]; // accelerometer raw
+    int _mag[3]; // magnetometer raw
     int _ecount; // encoder count
     float _dt;
     int _interval;

--- a/main.cpp	Thu Dec 27 00:42:35 2018 +0000
+++ b/main.cpp	Thu Dec 27 15:46:57 2018 +0000
@@ -62,7 +62,7 @@
     led1 = !led1;
     d.timestamp = Kernel::get_ms_count();
     d.encoder = u->encoder();
-    u->gyro(d.gyro, dt);
+    u->imu(d.gyro, d.accel, d.mag, dt);
     logQueue.call(&logger, &Logger::log_sensors, d);
 }
 
@@ -141,16 +141,21 @@
     printf("%d %f\n", svcount, hdop);
 }
 
-void read_gyro(int argc, char **argv) 
+void read_imu(int argc, char **argv) 
 {
     int g[3];
+    int a[3];
+    int m[3];
     float dt;
     
     Updater *u = Updater::instance();
     
-    u->gyro(g, dt);
-    
-    printf("Gyro: %d, %d, %d - dt: %f\n", g[0], g[1], g[2], dt);
+    u->imu(g, a, m, dt);
+   
+    printf("  Gyro: %d, %d, %d\n", g[0], g[1], g[2]);
+    printf(" Accel: %d, %d, %d\n", a[0], a[1], a[2]);
+    printf("   Mag: %d, %d, %d\n", m[0], m[1], m[2]);
+    printf("    dt: %f\n", dt);
 }
 
 void read_enc(int argc, char **argv)
@@ -263,7 +268,7 @@
 
     printf("Starting shell...\n");
     sh.command(test, "test");
-    sh.command(read_gyro, "gyro");
+    sh.command(read_imu, "imu");
     sh.command(read_enc, "enc");
     sh.command(read_gps, "gps");
     sh.command(reset, "reset");