1 files changed, 93 insertions, 9 deletions

diff --git a/test_fw/src/main.cpp b/test_fw/src/main.cpp
index 5f8e1d4..5206c16 100644
--- a/test_fw/src/main.cpp
+++ b/test_fw/src/main.cpp
@@ -1,9 +1,10 @@
 #include <Arduino.h>
 #include <Wire.h>
 #include <SPI.h>
-#include <SD.h>
 
 #include <Adafruit_BME680.h>
+#include <Adafruit_LSM6DS.h>
+#include <Adafruit_LSM6DSL.h>
 
 #include <hardware/pio.h>
 
@@ -14,13 +15,14 @@
 
 using namespace ocularium;
 
-
 static Adafruit_BME680 bme(&Wire1);
 static VEML lux(Wire1);
+static Adafruit_LSM6DSL lsm;
 
 #define SCAN_I2C 0
 
 #define SDCARD_SPI SD_SPI
+#include <SD.h>
 static SDClass sd;
 
 static bringup::init_check STARTUP_CHECKS[] = {
@@ -35,6 +37,24 @@ static bringup::init_check STARTUP_CHECKS[] = {
             .gain = veml::AmbientLightGain::Quarter,
         }); },
         .succeeded = false,
+    },
+    {
+        .name = String("lsm6dsm"),
+        .f = []
+        {
+            if (lsm.begin_SPI(LSM_CS, &LSM_SPI))
+            {
+                lsm.reset();
+                
+                lsm.setAccelDataRate(LSM6DS_RATE_52_HZ);
+                lsm.setGyroDataRate(LSM6DS_RATE_52_HZ);
+                
+                return true;
+            };
+
+            return false;
+        },
+        .succeeded = false,
     }
 };
 
@@ -72,13 +92,77 @@ void loop() {
         
         bme_target_millis = bme.beginReading();
     }
+
+    static sensors_vec_t accel, gyro;
+
+    while (lsm.gyroscopeAvailable())
+    {
+        lsm.readGyroscope(gyro.x, gyro.y, gyro.z);
+    }
+
+    while (lsm.accelerationAvailable())
+    {
+        lsm.readAcceleration(accel.x, accel.y, accel.z);
+    }
+
+    struct
+    {
+        const char* name;
+        float value;
+    } readings[] = {
+        {
+            .name = "lux",
+            .value = lux.lux(),
+        },
+        
+        {
+            .name = "temp",
+            .value = bme.temperature,
+        },
+        {
+            .name = "hum",
+            .value = bme.humidity,
+        },
+        {
+            .name = "pres",
+            .value = static_cast<float>(bme.pressure),
+        },
+        {
+            .name = "gas",
+            .value = bme.humidity,
+        },
+
+        {
+            .name = "ax",
+            .value = accel.x,
+        },
+        {
+            .name = "ay",
+            .value = accel.y,
+        },
+        {
+            .name = "az",
+            .value = accel.z,
+        },
+        
+        {
+            .name = "gx",
+            .value = gyro.x,
+        },
+        {
+            .name = "gy",
+            .value = gyro.y,
+        },
+        {
+            .name = "gz",
+            .value = gyro.z,
+        },
+    };
+
+    for (const auto &[name, value] : readings)
+    {
+        Serial.printf("%s:%f,", name, value);
+    }
     
-    Serial.print("lux: " + String(lux.lux()));
-    Serial.print(", temp: " + String(bme.temperature));
-    Serial.print(", hum: " + String(bme.humidity));
-    Serial.print(", pres: " + String(bme.pressure));
-    Serial.print(", gas: " + String(bme.gas_resistance));
     Serial.println();
-
-    delay(100);
 }
\ No newline at end of file