port to separate core for sd

3 files changed, 232 insertions, 152 deletions

diff --git a/test_fw/src/channel.h b/test_fw/src/channel.h
new file mode 100644
index 0000000..eb38e64
--- /dev/null
+++ b/test_fw/src/channel.h
@@ -0,0 +1,36 @@
+#pragma once
+
+#include <FreeRTOS.h>
+#include <queue.h>
+
+constexpr uint8_t MEAS_CHAN_LEN = 32;
+
+enum class Measurement : uint8_t
+{
+  LUX,
+  TEMP,
+  HUM,
+  PRES,
+  GAS,
+  AX,
+  AY,
+  AZ,
+  GX,
+  GY,
+  GZ,
+
+#if USE_BSEC
+  CO2,
+  IAQ,
+#endif
+
+  MAX,
+};
+
+struct Message
+{
+  uint32_t uptime;
+  float measurement[static_cast<size_t>(Measurement::MAX)];
+};
+
+extern QueueHandle_t MEAS_CHANNEL;
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);
 }
diff --git a/test_fw/src/sd.cpp b/test_fw/src/sd.cpp
new file mode 100644
index 0000000..486c347
--- /dev/null
+++ b/test_fw/src/sd.cpp
@@ -0,0 +1,158 @@
+#include <Arduino.h>
+#include <etl/vector.h>
+
+#include "channel.h"
+#include "log.h"
+#include "board.h"
+
+#define ENABLE_SD 0
+
+#if ENABLE_SD
+#define SDCARD_SPI SD_SPI
+#include <SD.h>
+static SDClass sd;
+#endif
+
+
+void setup1() {
+    while (MEAS_CHANNEL == nullptr)
+    {
+        delay(5);
+    }
+}
+
+inline const char* name_for_meas(Measurement meas)
+{
+    switch (meas)
+    {
+    case Measurement::LUX:
+        return "lux";
+    case Measurement::TEMP:
+        return "temp";
+    case Measurement::HUM:
+        return "hum";
+    case Measurement::PRES:
+        return "pres";
+    case Measurement::GAS:
+        return "gas";
+    case Measurement::AX:
+        return "ax";
+    case Measurement::AY:
+        return "ay";
+    case Measurement::AZ:
+        return "az";
+    case Measurement::GX:
+        return "gx";
+    case Measurement::GY:
+        return "gy";
+    case Measurement::GZ:
+        return "gz";
+
+#if USE_BSEC
+    case Measurement::CO2:
+        return "co2";
+    case Measurement::IAQ:
+        return "iaq";
+#endif
+
+    default:
+        return "unknown";
+    }
+}
+
+void loop1() {
+    static etl::vector<Message, 128> chunk;
+
+    while (chunk.available() > chunk.capacity() / 2)
+    {
+        Message read;
+        if (!xQueueReceive(MEAS_CHANNEL, &read, portMAX_DELAY))
+        {
+            LOGGER.println("failed to receive message from channel");
+            continue;
+        }
+
+        chunk.push_back(read);
+
+        if (LOGGER)
+        {
+            for (int i = 0; i < static_cast<size_t>(Measurement::MAX); i++)
+            {
+                const auto name = name_for_meas(static_cast<Measurement>(i));
+                const auto& value = read.measurement[i];
+
+                // 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();
+        }
+    }
+
+#if !ENABLE_SD
+    chunk.clear();
+    return;
+#endif
+
+#if ENABLE_SD
+    constexpr auto sd_period = 50;
+    static unsigned int sd_last_connect = 0;
+    static bool sd_connected = false;
+
+    if (!digitalRead(CARD_DETECT))
+    {
+        sd_connected = false;
+        analogWrite(LED_CAPTURING, 0);
+        return;
+    }
+
+    if (millis() - sd_last_connect > sd_period && !sd_connected)
+    {
+        sd_last_connect = millis();
+
+        if (!sd.begin(SD_CS))
+        {
+            LOGGER.println("failed connection to sd card");
+            sd_connected = false;
+            analogWrite(LED_CAPTURING, 0);
+            return;
+        }
+
+        sd_connected = true;
+        analogWrite(LED_CAPTURING, 24);
+    }
+
+    auto data = sd.open("data.csv", O_CREAT | O_WRITE | O_APPEND);
+    if (data.size() == 0)
+    {
+        data.write("uptime_ms,");
+
+        for (int i = 0; i < static_cast<size_t>(Measurement::MAX); i++)
+        {
+            const auto name = name_for_meas(static_cast<Measurement>(i));
+            data.printf("%s,", name);
+        }
+
+        data.print('\n');
+    }
+
+    for (const auto& m : chunk)
+    {
+        data.printf("%d,", m.uptime);
+
+        for (const auto &value : m.measurement)
+        {
+            data.printf("%f,", value);
+        }
+
+        data.print('\n');
+    }
+
+    data.close();
+#endif
+}