rolling average accel

1 files changed, 63 insertions, 5 deletions

diff --git a/test_fw/src/main.cpp b/test_fw/src/main.cpp
index 0370408..0acdc82 100644
--- a/test_fw/src/main.cpp
+++ b/test_fw/src/main.cpp
@@ -33,6 +33,7 @@ static VEML lux(Wire1);
 static Adafruit_LSM6DSL lsm;
 
 #define SCAN_I2C 0
+#define ENABLE_SD 0
 
 #define SDCARD_SPI SD_SPI
 #include <SD.h>
@@ -77,8 +78,8 @@ static bringup::init_check STARTUP_CHECKS[] = {
             {
                 lsm.reset();
                 
-                lsm.setAccelDataRate(LSM6DS_RATE_52_HZ);
-                lsm.setGyroDataRate(LSM6DS_RATE_52_HZ);
+                lsm.setAccelDataRate(LSM6DS_RATE_104_HZ);
+                lsm.setGyroDataRate(LSM6DS_RATE_104_HZ);
                 
                 return true;
             };
@@ -86,7 +87,7 @@ static bringup::init_check STARTUP_CHECKS[] = {
             return false;
         },
         .succeeded = false,
-    }
+    },
 };
 
 #if USE_BSEC
@@ -185,7 +186,8 @@ void loop() {
     const auto lux_value = lux.lux();
     analogWrite(LED_STORAGE, map(static_cast<int>(lux_value), 200, 1000, 0, 255));
 
-    analogWrite(LED_CAPTURING, digitalRead(CARD_DETECT) ? 0 : 24);
+    const bool card_detected = !digitalRead(CARD_DETECT);
+    analogWrite(LED_CAPTURING, card_detected ? 24 : 0);
     
     static sensors_vec_t accel, gyro;
 
@@ -200,10 +202,24 @@ void loop() {
         
         const auto norm = sqrtf(accel.x * accel.x + accel.y * accel.y + accel.z * accel.z);
 
+        // small rolling average to reduce the effects of noise
+        constexpr auto WINDOW = 10;
+        static float norm_hist[WINDOW] = {0.0};
+        for (int i = 0; i < WINDOW - 1; i++)
+            norm_hist[i] = norm_hist[i + 1];
+        norm_hist[WINDOW - 1] = norm;
+
+        auto norm_avg = 0.f;
+        for (const float i : norm_hist)
+        {
+            norm_avg += i;
+        }
+        norm_avg /= static_cast<float>(WINDOW);
+
         constexpr auto NEUTRAL = 1.98f;
         constexpr auto RANGE = 8.f;
         
-        auto compensated = abs(norm - NEUTRAL) / RANGE;
+        auto compensated = abs(norm_avg - NEUTRAL) / RANGE;
         compensated *= 5;    // more sensitivity (heuristic)
         
         const auto pin_val = etl::clamp(static_cast<int>(compensated * 255), 0, 255);
@@ -213,6 +229,31 @@ void loop() {
         analogWrite(LED_OTHER, pin_val);
     }
 
+#if ENABLE_SD
+    constexpr auto sd_period = 50;
+    static unsigned int sd_last_connect = 0;
+    static bool sd_connected = false;
+
+    if (!card_detected)
+    {
+        sd_connected = false;
+    }
+    
+    if (millis() - sd_last_connect > sd_period && card_detected)
+    {
+        sd_last_connect = millis();
+        
+        if (!sd.begin(SD_CS))
+        {
+            Serial.println("failed connection to sd card");
+            sd_connected = false;
+        } else
+        {
+            sd_connected = true;
+        }
+    }
+#endif
+
     struct
     {
         const char* name;
@@ -288,4 +329,21 @@ void loop() {
     }
     
     Serial.println();
+
+#if ENABLE_SD
+    if (sd_connected) {
+        auto data = sd.open("data.csv", O_CREAT | O_WRITE | O_APPEND);
+        
+        data.write(String(millis()).c_str());
+        
+        for (const auto &[name, value] : readings)
+        {
+            data.write(String(value).c_str());
+            data.write(",");
+        }
+        
+        data.write("\n");
+        data.close();
+    }
+#endif
 }
\ No newline at end of file