1 files changed, 38 insertions, 152 deletions

diff --git a/test_fw/src/main.cpp b/test_fw/src/main.cpp
index 1ea9d09..cae2994 100644
--- a/test_fw/src/main.cpp
+++ b/test_fw/src/main.cpp
@@ -17,6 +17,11 @@
 #include "board.h"
 #include "bringup.h"
 #include "log.h"
+#include "channel.h"
+
+QueueHandle_t MEAS_CHANNEL;
+static StaticQueue_t MEAS_QUEUE;
+static uint8_t MEAS_STORAGE[MEAS_CHAN_LEN * sizeof(Message)];
 
 #define USE_BSEC 0
 
@@ -38,12 +43,6 @@ static VEML lux(Wire1);
 static Adafruit_LSM6DSL lsm;
 
 #define SCAN_I2C 0
-#define ENABLE_SD 1
-#define DEBUG_SD 0
-
-#define SDCARD_SPI SD_SPI
-#include <SD.h>
-static SDClass sd;
 
 static bringup::init_check STARTUP_CHECKS[] = {
     {
@@ -138,6 +137,8 @@ void setup() {
     xTaskCreateStatic(bme_task, "bme", 2048, nullptr, tskIDLE_PRIORITY, bsec_stack, &bsec_task_buffer);
 #endif
 
+    MEAS_CHANNEL = xQueueCreateStatic(32, sizeof(Message), MEAS_STORAGE, &MEAS_QUEUE);
+
     bringup::boot_animation();
 }
 
@@ -170,7 +171,13 @@ void setup() {
 }
 #endif
 
+constexpr float ODR_HZ = 25;
+constexpr float ODR_PERIOD = 1.0 / ODR_HZ;
+constexpr uint32_t ODR_PERIOD_MILLIS = static_cast<uint32_t>(ODR_PERIOD * 1000.f);
+
 void loop() {
+    static uint32_t loop_top = 0;
+
 #if USE_BSEC
     static bme_data bme_data;
     // do not wait
@@ -196,9 +203,6 @@ void loop() {
     const auto lux_value = lux.lux();
     analogWrite(LED_STORAGE, map(static_cast<int>(lux_value), 200, 1000, 0, 255));
 
-    const bool card_detected = !digitalRead(CARD_DETECT);
-    analogWrite(LED_CAPTURING, card_detected ? 24 : 0);
-
     static sensors_vec_t accel, gyro;
 
     if (lsm.gyroscopeAvailable())
@@ -234,162 +238,44 @@ void loop() {
 
         const auto pin_val = etl::clamp(static_cast<int>(compensated * 255), 0, 255);
 
-        // LOGGER.printf("norm: %.2f, compensated: %.2f, pin_val: %d\n", norm, compensated, pin_val);
-
         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_connected)
-    {
-        sd_last_connect = millis();
-
-#if 0
-        Sd2Card card;
-        auto result = card.init(3, SD_CS);
-
-        if (!result)
-        {
-            LOGGER.printf("failed to connect to sd card, code: %d, data: %d", card.errorCode(), card.errorData());
-        }
+    const Message msg = {
+        .uptime = millis(),
+        .measurement = {
+            lux_value,
+            bme_data_source.temperature,
+            bme_data_source.humidity,
+#if USE_BSEC
+            bme_data_source.gas,
 #else
-        if (!sd.begin(SD_CS))
-        {
-            LOGGER.println("failed connection to sd card");
-            sd_connected = false;
-        } else
-        {
-            sd_connected = true;
-        }
-#endif
-    }
+            static_cast<float>(bme.gas_resistance),
 #endif
 
-    struct
-    {
-        const char* name;
-        float value;
-    } readings[] = {
-        {
-            .name = "lux",
-            .value = lux_value,
-        },
+            accel.x,
+            accel.y,
+            accel.z,
+
+            gyro.x,
+            gyro.y,
+            gyro.z,
 
-        {
-            .name = "temp",
-            .value = bme_data_source.temperature,
-        },
-        {
-            .name = "hum",
-            .value = bme_data_source.humidity,
-        },
-        {
-            .name = "pres",
-            .value = static_cast<float>(bme_data_source.pressure),
-        },
 #if USE_BSEC
-        {
-            .name = "iaq",
-            .value = bme_data_source.iaq,
-        },
-        {
-            .name = "co2",
-            .value = bme_data_source.co2,
-        },
-        {
-            .name = "gas",
-            .value = bme_data_source.gas,
-        },
-#else
-        {
-            .name = "gas",
-            .value = static_cast<float>(bme.gas_resistance),
-        },
+            bme_data_source.iaq,
+            bme_data_source.co2,
 #endif
-
-        {
-            .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,
         },
     };
 
-
-#if !DEBUG_SD
-    if (LOGGER)
+    if (!xQueueSend(MEAS_CHANNEL, &msg, 0))
     {
-        for (const auto &[name, value] : readings)
-        {
-            // This awkwardness is to support the fact that the RTT implementation
-            // doesn't support formatting floats and instead silently fails -- manually
-            // format first, then printf the string.
-            char buf[32];
-            snprintf(buf, sizeof(buf), "%s:%f,", name, value);
-
-            LOGGER.printf("%s", buf);
-        }
-
-        LOGGER.println();
+        LOGGER.println("queue was full, dropping measurement!");
     }
-#endif
-
-#if ENABLE_SD
-    if (sd_connected) {
-        auto data = sd.open("data.csv", O_CREAT | O_WRITE | O_APPEND);
-        if (data.size() == 0)
-        {
-            data.write("uptime_ms,");
-
-            for (const auto &[name, _] : readings)
-            {
-                data.printf("%s,", name);
-            }
-
-            data.print('\n');
-        }
-
-        data.printf("%d,", millis());
-
-        for (const auto &[_, value] : readings)
-        {
-            data.printf("%f,", value);
-        }
 
-        data.print('\n');
-        data.close();
-
-#if DEBUG_SD
-        LOGGER.println("wrote entry to sd");
-#endif
-    }
-#endif
+    const auto now = millis();
+    const auto next = loop_top + ODR_PERIOD_MILLIS;
+    loop_top = next;
+    if (next < now) return;
+    delay(next - now);
 }