try bsec library

4 files changed, 242 insertions, 14 deletions

diff --git a/test_fw/platformio.ini b/test_fw/platformio.ini
index df71bc1..6b402d8 100644
--- a/test_fw/platformio.ini
+++ b/test_fw/platformio.ini
@@ -23,9 +23,10 @@ lib_deps =
 	etlcpp/Embedded Template Library@^20.39.4
 	arduino-libraries/SD@^1.3.0
 	adafruit/Adafruit LSM6DS@^4.7.3
+	boschsensortec/BSEC Software Library@^1.8.1492
 build_flags = 
 	-std=c++2a
 	-DPIO_FRAMEWORK_ARDUINO_ENABLE_EXCEPTIONS
-	-DDEBUG_RP2040_CORE 
+	-DDEBUG_RP2040_CORE
 	-DDEBUG_RP2040_PORT=Serial
 	-fstack-protector
diff --git a/test_fw/plot.py b/test_fw/plot.py
new file mode 100644
index 0000000..53f1bc2
--- /dev/null
+++ b/test_fw/plot.py
@@ -0,0 +1,98 @@
+from datetime import datetime
+import matplotlib.pyplot as plt
+import matplotlib.animation as animation
+import serial
+import numpy as np
+import math
+
+
+SER = serial.Serial('COM14')
+
+CHANNELS = [
+    'ax',
+    'ay',
+    'az',
+    'gx',
+    'gy',
+    'gz',
+    'temp',
+    'hum',
+    'pres',
+    'gas',
+    'lux',
+]
+CHANNEL_IDX = {name: i for (i, name) in enumerate(CHANNELS)}
+
+SAMPLE_COUNT = 500
+
+
+def main():
+    fig = plt.figure()
+    
+    # subfigs = fig.subfigures(3, math.ceil(len(CHANNELS) / 3))
+    
+    elem_data = {}
+    
+    
+    for chan in CHANNELS:
+        idx = CHANNEL_IDX[chan]
+        # subfig = subfigs[idx % 3][idx // 3]
+        
+        ax = fig.add_subplot(3, math.ceil(len(CHANNELS) / 3), idx + 1)
+        ax.set_title(chan)
+        
+        xs = np.arange(SAMPLE_COUNT),
+        ys = np.zeros(SAMPLE_COUNT),
+        
+        line = ax.plot(xs, ys)[0]
+        
+        elem_data[chan] = {
+            'ys': ys,
+            # 'fig': subfig,
+            'ax': ax,
+            'line': line
+        }
+    
+    elem_data['temp']['ax'].set_ylim([15, 40])
+    elem_data['hum']['ax'].set_ylim([0, 100])
+    elem_data['pres']['ax'].set_ylim([0, 100000])
+    
+    def animate(i):
+        now = datetime.now()
+        
+        if not SER.in_waiting:
+            return []
+            
+        line = SER.readline().decode('utf-8')
+        
+        for elem in line.split(','):
+            elem = elem.strip()
+
+            if len(elem) == 0:
+                continue
+                
+            split = elem.split(':')
+
+            if len(split) != 2:
+                continue
+
+            k = split[0].strip()
+            v = float(split[1].strip())
+            
+            if k in elem_data:
+                dat = elem_data[k]
+                
+                dat['ys'] = np.roll(dat['ys'], -1)
+                dat['ys'][-1] = v
+                dat['line'].set_ydata(dat['ys'])
+        
+        yield from (item['line'] for item in elem_data.values())
+        
+        
+    ani = animation.FuncAnimation(fig, animate, interval=50, blit=True)
+    plt.show()
+    
+
+
+if __name__ == '__main__':
+    main()
diff --git a/test_fw/src/bringup.cpp b/test_fw/src/bringup.cpp
index 44a07c0..f2d2292 100644
--- a/test_fw/src/bringup.cpp
+++ b/test_fw/src/bringup.cpp
@@ -8,6 +8,7 @@ using namespace ocularium;
 void bringup::startup_checks(const etl::span<bringup::init_check> &checks)
 {
     auto success = true;
+    auto fault_indication = false;
     
     do
     {
@@ -27,8 +28,15 @@ void bringup::startup_checks(const etl::span<bringup::init_check> &checks)
             else Serial.println("bad");
         }
 
-        if (!success) delay(10);
+        if (!success)
+        {
+            fault_indication = !fault_indication;
+            digitalWrite(LED_FAULT, fault_indication);
+            delay(25);
+        }
     } while (!success);
+
+    digitalWrite(LED_FAULT, LOW);
 }
 
 void bringup::init_buses()
@@ -71,7 +79,7 @@ void bringup::boot_animation()
         LED_CAPTURING,
     };
 
-    for (auto i = 0; i < 5; i++)
+    for (auto i = 0; i < 4; i++)
     {
         for (const auto pin: chase)
         {
diff --git a/test_fw/src/main.cpp b/test_fw/src/main.cpp
index 5206c16..dd7240a 100644
--- a/test_fw/src/main.cpp
+++ b/test_fw/src/main.cpp
@@ -2,10 +2,14 @@
 #include <Wire.h>
 #include <SPI.h>
 
-#include <Adafruit_BME680.h>
 #include <Adafruit_LSM6DS.h>
 #include <Adafruit_LSM6DSL.h>
 
+#include <FreeRTOS.h>
+#include <task.h>
+#include <message_buffer.h>
+#include <queue.h>
+
 #include <hardware/pio.h>
 
 #include "i2c_scan.h"
@@ -13,9 +17,18 @@
 #include "board.h"
 #include "bringup.h"
 
-using namespace ocularium;
+#define USE_BSEC 0
 
+#if USE_BSEC
+#include <bsec.h>
+static Bsec bme;
+#else
+#include <Adafruit_BME680.h>
 static Adafruit_BME680 bme(&Wire1);
+#endif
+
+using namespace ocularium;
+
 static VEML lux(Wire1);
 static Adafruit_LSM6DSL lsm;
 
@@ -28,7 +41,25 @@ static SDClass sd;
 static bringup::init_check STARTUP_CHECKS[] = {
     {
         .name = String("bme680"),
-        .f = [] { return bme.begin(BME_ADDR); },
+        .f = []
+        {
+#if USE_BSEC
+            bme.begin(BME_ADDR, Wire1);
+
+            bsec_virtual_sensor_t sensors[] = {
+                BSEC_OUTPUT_IAQ,
+                BSEC_OUTPUT_SENSOR_HEAT_COMPENSATED_HUMIDITY,
+                BSEC_OUTPUT_SENSOR_HEAT_COMPENSATED_TEMPERATURE,
+                BSEC_OUTPUT_RAW_PRESSURE,
+                BSEC_OUTPUT_GAS_PERCENTAGE,
+                BSEC_OUTPUT_CO2_EQUIVALENT,
+            };
+            bme.updateSubscription(sensors, ETL_ARRAY_SIZE(sensors), BSEC_SAMPLE_RATE_CONT);
+            return true;
+#else
+            return bme.begin(BME_ADDR);
+#endif
+        },
         .succeeded = false,
     },
     {
@@ -58,6 +89,17 @@ static bringup::init_check STARTUP_CHECKS[] = {
     }
 };
 
+#if USE_BSEC
+struct bme_data
+{
+    float temperature, humidity, pressure, iaq, co2, gas;
+};
+
+static uint8_t bme_storage[2 * sizeof(bme_data)];
+static StaticQueue_t bme_buffer;
+static QueueHandle_t bme_buffer_handle;
+#endif
+
 void setup() {
     Serial.begin(115200);
     Serial.println("boot");
@@ -74,13 +116,57 @@ void setup() {
 
     Serial.println("sensors initialized");
 
+#if USE_BSEC
+    [[noreturn]] void bme_task(void* params);
+
+    bme_buffer_handle = xQueueCreateStatic(2, sizeof(bme_data), bme_storage, &bme_buffer);
+    
+    static StaticTask_t bsec_task_buffer;
+    static StackType_t bsec_stack[2048];
+    xTaskCreateStatic(bme_task, "bme", 2048, nullptr, tskIDLE_PRIORITY, bsec_stack, &bsec_task_buffer);
+#endif
+    
     bringup::boot_animation();
     delay(100);
+}
+
 
-    digitalWrite(LED_CAPTURING, HIGH);
+#if USE_BSEC
+[[noreturn]] void bme_task(void* params)
+{
+    while (true)
+    {
+        Serial.println("bme run start");
+        const auto status = bme.run();
+        Serial.println("bme run end");
+        
+        if (status)
+        {
+            bme_data data {
+                .temperature = bme.temperature,
+                .humidity = bme.humidity,
+                .pressure = bme.pressure,
+                .iaq = bme.iaq,
+                .co2 = bme.co2Equivalent,
+                .gas = bme.gasPercentage,
+            };
+            
+            xQueueSend(bme_buffer_handle, &data, 0);
+        }
+        
+        delay(max(bme.nextCall - millis(), 0));
+    }
 }
+#endif
 
 void loop() {
+#if USE_BSEC
+    static bme_data bme_data;
+    // do not wait
+    xQueueReceiveFromISR(bme_buffer_handle, &bme_data, nullptr);
+    auto& bme_data_source = bme_data;
+    
+#else
     static uint32_t bme_target_millis = 0;
 
     if (millis() >= bme_target_millis)
@@ -93,16 +179,36 @@ void loop() {
         bme_target_millis = bme.beginReading();
     }
 
+    auto& bme_data_source = bme;
+#endif
+
+    const auto lux_value = lux.lux();
+    analogWrite(LED_STORAGE, map(static_cast<int>(lux_value), 200, 1000, 0, 255));
+    
     static sensors_vec_t accel, gyro;
 
-    while (lsm.gyroscopeAvailable())
+    if (lsm.gyroscopeAvailable())
     {
         lsm.readGyroscope(gyro.x, gyro.y, gyro.z);
     }
 
-    while (lsm.accelerationAvailable())
+    if (lsm.accelerationAvailable())
     {
         lsm.readAcceleration(accel.x, accel.y, accel.z);
+        
+        const auto norm = sqrtf(accel.x * accel.x + accel.y * accel.y + accel.z * accel.z);
+
+        constexpr auto NEUTRAL = 1.98f;
+        constexpr auto RANGE = 8.f;
+        
+        auto compensated = abs(norm - NEUTRAL) / RANGE;
+        compensated *= 5;    // more sensitivity (heuristic)
+        
+        const auto pin_val = etl::clamp(static_cast<int>(compensated * 255), 0, 255);
+
+        // Serial.printf("norm: %.2f, compensated: %.2f, pin_val: %d\n", norm, compensated, pin_val);
+        
+        analogWrite(LED_OTHER, pin_val);
     }
 
     struct
@@ -112,25 +218,40 @@ void loop() {
     } readings[] = {
         {
             .name = "lux",
-            .value = lux.lux(),
+            .value = lux_value,
         },
         
         {
             .name = "temp",
-            .value = bme.temperature,
+            .value = bme_data_source.temperature,
         },
         {
             .name = "hum",
-            .value = bme.humidity,
+            .value = bme_data_source.humidity,
         },
         {
             .name = "pres",
-            .value = static_cast<float>(bme.pressure),
+            .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 = bme.humidity,
+            .value = static_cast<float>(bme.gas_resistance),
         },
+#endif
 
         {
             .name = "ax",