diff --git a/DOWA&DST/platformio.ini b/DOWA&DST/platformio.ini new file mode 100644 index 0000000..f17a417 --- /dev/null +++ b/DOWA&DST/platformio.ini @@ -0,0 +1,12 @@ +[env:esp32-s3-wroom-1-n16r8] +platform = espressif32 +board = esp32-s3-devkitc-1 +framework = arduino +monitor_speed = 115200 +board_build.arduino.memory_type = qio_opi +build_flags = + -DBOARD_HAS_PSRAM + -DARDUINO_USB_CDC_ON_BOOT=1 ; Route Serial to USB-C + +lib_deps = + adafruit/Adafruit SGP30 Sensor @ ^2.0.2 \ No newline at end of file diff --git a/DOWA&DST/src/dowa_dst_fusion_template.cpp b/DOWA&DST/src/dowa_dst_fusion_template.cpp new file mode 100644 index 0000000..441467b --- /dev/null +++ b/DOWA&DST/src/dowa_dst_fusion_template.cpp @@ -0,0 +1,331 @@ +#include +#include +#include "Adafruit_SGP30.h" + +/* + MOD/DOWA Fusion Template - Functional Implementation + Integrates real sensor reads: + - mmWave LD2410 radar (UART) + - PIR sensor (GPIO digital) + - SGP30 air quality (I2C) +*/ + +// ============================================================================ +// HARDWARE CONFIGURATION +// ============================================================================ + +// mmWave LD2410 (UART) +#define RADAR_RX_PIN 16 +#define RADAR_TX_PIN 17 +HardwareSerial radarSerial(1); + +// PIR Sensor (GPIO) +#define PIR_PIN 13 + +// SGP30 (I2C) +#define I2C_SDA_PIN 8 +#define I2C_SCL_PIN 9 +Adafruit_SGP30 sgp30; + +// mmWave packet parsing state +const uint8_t RADAR_HEADER[4] = {0xF4, 0xF3, 0xF2, 0xF1}; +int radarHeaderCount = 0; +uint8_t radarPacket[64]; +int radarPacketLen = 0; +bool radarReadingPacket = false; +uint8_t lastRadarPresence = 0; // LD2410 state: 0=None, 1=Moving, 2=Stationary, 3=Both +unsigned long lastRadarReadTime = 0; + +// SGP30 state +unsigned long lastSGP30ReadTime = 0; +uint16_t lastTVOC = 0; +uint16_t lasteCO2 = 400; + +// PIR State +unsigned long lastPirTriggerTime = 0; + +struct MassFunction { + float occupied; + float empty; + float unknown; +}; + +struct SensorEvidence { + uint8_t radarState; // 0=none, 1=stationary, 2=moving + bool pirMotion; + uint32_t msSinceLastPirTrigger; + uint16_t tvoc; + uint16_t eco2; +}; + +struct FusionResult { + float occupiedBelief; + float emptyBelief; + float confidence; + float conflictK; + bool occupied; +}; + +static constexpr float TVOC_OCCUPIED_THRESHOLD = 200.0f; +static constexpr float TVOC_WEAK_THRESHOLD = 100.0f; +static constexpr float DOWA_DECISION_THRESHOLD = 0.50f; + +// Sensor weight coefficients (w_i) +static constexpr float w_radar = 0.50f; // mmWave radar weight +static constexpr float w_pir = 0.35f; // PIR sensor weight +static constexpr float w_air = 0.15f; // Air quality weight + +static MassFunction makeRadarMass(uint8_t radarState) { + // radarState: 0=none, 1=stationary, 2=moving + switch(radarState) { + case 2: return {0.95f, 0.00f, 0.05f}; // moving + case 1: return {0.85f, 0.00f, 0.15f}; // stationary/breathing + default: return {0.05f, 0.70f, 0.25f}; // none + } +} + +static MassFunction makePirMass(bool motion, uint32_t msSinceLastTrigger) { + if (motion) return {0.90f, 0.00f, 0.10f}; + if (msSinceLastTrigger < 30000) return {0.75f, 0.00f, 0.25f}; // hold + if (msSinceLastTrigger < 120000) { + // linear decay of m(O) toward 0, m(U) toward 1.0 + float ratio = 1.0f - ((msSinceLastTrigger - 30000.0f) / 90000.0f); + return {0.75f * ratio, 0.00f, 1.0f - (0.75f * ratio)}; + } + return {0.00f, 0.30f, 0.70f}; // silence > 120s +} + +static MassFunction makeAirMass(uint16_t tvoc, uint16_t eco2) { + if (tvoc >= TVOC_OCCUPIED_THRESHOLD || eco2 >= 1000) { + return {0.75f, 0.10f, 0.15f}; + } + if (tvoc >= TVOC_WEAK_THRESHOLD || eco2 >= 800) { + return {0.45f, 0.25f, 0.30f}; + } + return {0.15f, 0.70f, 0.15f}; +} + +static MassFunction combineTwo(const MassFunction &a, const MassFunction &b, float &outK) { + const float k = (a.occupied * b.empty) + (a.empty * b.occupied); + outK = k; + + const float safeDenominator = 1.0f - k; + + if (safeDenominator <= 0.0001f) { + return {0.0f, 0.0f, 1.0f}; + } + + MassFunction result; + result.occupied = ((a.occupied * b.occupied) + (a.occupied * b.unknown) + (a.unknown * b.occupied)) / safeDenominator; + result.empty = ((a.empty * b.empty) + (a.empty * b.unknown) + (a.unknown * b.empty)) / safeDenominator; + result.unknown = ((a.unknown * b.unknown)) / safeDenominator; + return result; +} + +static MassFunction combineEvidence(const SensorEvidence &evidence, float &outMaxK) { + const MassFunction radarMass = makeRadarMass(evidence.radarState); + const MassFunction pirMass = makePirMass(evidence.pirMotion, evidence.msSinceLastPirTrigger); + const MassFunction airMass = makeAirMass(evidence.tvoc, evidence.eco2); + + float totalWeight = w_radar + w_pir + w_air; + float norm_radar = w_radar / totalWeight; + float norm_pir = w_pir / totalWeight; + float norm_air = w_air / totalWeight; + + MassFunction m_dowa; + m_dowa.occupied = (norm_radar * radarMass.occupied) + (norm_pir * pirMass.occupied) + (norm_air * airMass.occupied); + m_dowa.empty = (norm_radar * radarMass.empty) + (norm_pir * pirMass.empty) + (norm_air * airMass.empty); + m_dowa.unknown = (norm_radar * radarMass.unknown) + (norm_pir * pirMass.unknown) + (norm_air * airMass.unknown); + + float k = 0.0f; + float maxK = 0.0f; + + MassFunction combined = m_dowa; + + combined = combineTwo(combined, radarMass, k); + maxK = max(maxK, k); + + combined = combineTwo(combined, pirMass, k); + maxK = max(maxK, k); + + combined = combineTwo(combined, airMass, k); + maxK = max(maxK, k); + + outMaxK = maxK; + return combined; +} + +static float computeDOWAConfidence(const MassFunction &mass) { + const float signalStrength = mass.occupied + (0.5f * mass.unknown); + return constrain(signalStrength, 0.0f, 1.0f); +} + +static FusionResult runDOWA_DST_Fusion(const SensorEvidence &evidence) { + float maxK = 0.0f; + const MassFunction fusedMass = combineEvidence(evidence, maxK); + const float confidence = computeDOWAConfidence(fusedMass); + + FusionResult result; + result.occupiedBelief = fusedMass.occupied; + result.emptyBelief = fusedMass.empty; + result.confidence = confidence; + result.conflictK = maxK; + result.occupied = (fusedMass.occupied >= DOWA_DECISION_THRESHOLD); + return result; +} + +static void processRadarData() { + while(radarSerial.available()) { + uint8_t b = radarSerial.read(); + + if(!radarReadingPacket) { + if(b == RADAR_HEADER[radarHeaderCount]) { + radarHeaderCount++; + if(radarHeaderCount == 4) { + radarReadingPacket = true; + radarPacket[0] = 0xF4; + radarPacket[1] = 0xF3; + radarPacket[2] = 0xF2; + radarPacket[3] = 0xF1; + radarPacketLen = 4; + radarHeaderCount = 0; + } + } else { + radarHeaderCount = 0; + } + } else { + radarPacket[radarPacketLen] = b; + radarPacket_len++; + + if(radarPacket_len >= 4 && + radarPacket[radarPacket_len-4] == 0xF8 && + radarPacket[radarPacket_len-3] == 0xF7 && + radarPacket[radarPacket_len-2] == 0xF6 && + radarPacket[radarPacket_len-1] == 0xF5) { + + lastRadarPresence = radarPacket[6]; // LD2410 byte 6: 0=None, 1=Moving, 2=Stationary, 3=Both + lastRadarReadTime = millis(); + radarReadingPacket = false; + } + + if(radarPacket_len >= 64) { + radarReadingPacket = false; + } + } + } +} + +static void processSGP30Data() { + unsigned long now = millis(); + + if (now - lastSGP30ReadTime < 1000) { + return; + } + lastSGP30ReadTime = now; + + if (sgp30.IAQmeasure()) { + lastTVOC = sgp30.TVOC; + lasteCO2 = sgp30.eCO2; + } +} + +static SensorEvidence readSensorsTemplate() { + processRadarData(); + processSGP30Data(); + + SensorEvidence evidence; + + evidence.pirMotion = digitalRead(PIR_PIN); + if (evidence.pirMotion) { + lastPirTriggerTime = millis(); + } + evidence.msSinceLastPirTrigger = millis() - (lastPirTriggerTime > 0 ? lastPirTriggerTime : millis()); // Prevent overflow on startup buffer + + // LD2410 parsing logic maps nicely: + if (lastRadarPresence == 1 || lastRadarPresence == 3) { + evidence.radarState = 2; // Moving/Both -> 2 (Moving) + } else if (lastRadarPresence == 2) { + evidence.radarState = 1; // Stationary -> 1 (Stationary) + } else { + evidence.radarState = 0; // None -> 0 + } + + evidence.tvoc = lastTVOC; + evidence.eco2 = lasteCO2; + + return evidence; +} + +void setup() { + Serial.begin(115200); + delay(2000); + Serial.println("\n=== MOD/DOWA Fusion Engine - Functional ===\n"); + + pinMode(PIR_PIN, INPUT); + Serial.println("[PIR] Initialized on GPIO " + String(PIR_PIN)); + + radarSerial.begin(115200, SERIAL_8N1, RADAR_RX_PIN, RADAR_TX_PIN); + delay(500); + + const byte REPORT_MODE_CMD[] = { + 0xFD, 0xFC, 0xFB, 0xFA, + 0x08, 0x00, + 0x12, 0x00, 0x00, 0x00, + 0x04, 0x00, 0x00, 0x00, + 0x04, 0x03, 0x02, 0x01 + }; + radarSerial.write(REPORT_MODE_CMD, sizeof(REPORT_MODE_CMD)); + delay(100); + Serial.println("[mmWave] Initialized on UART (RX=" + String(RADAR_RX_PIN) + ", TX=" + String(RADAR_TX_PIN) + ")"); + + Wire.begin(I2C_SDA_PIN, I2C_SCL_PIN); + delay(500); + + if (!sgp30.begin(&Wire)) { + Serial.println("[SGP30] ERROR: Sensor not found!"); + while(1) { delay(100); } + } + + Serial.print("[SGP30] Found serial #"); + Serial.print(sgp30.serialnumber[0], HEX); + Serial.print(sgp30.serialnumber[1], HEX); + Serial.println(sgp30.serialnumber[2], HEX); + + Serial.println("\n[FUSION] Warming up... (1Hz measurement cycle)\n"); + Serial.println("========================================"); +} + +void loop() { + static unsigned long lastFusionMs = 0; + + if (millis() - lastFusionMs < 1000) { + return; + } + lastFusionMs = millis(); + + const SensorEvidence evidence = readSensorsTemplate(); + const FusionResult result = runDOWA_DST_Fusion(evidence); + + Serial.print("["); + Serial.print(millis() / 1000); + Serial.print("s] "); + + Serial.print("RADAR="); + Serial.print(evidence.radarState); + Serial.print(" PIR="); + Serial.print(evidence.pirMotion ? "1" : "0"); + Serial.print(" TVOC="); + Serial.print(evidence.tvoc); + Serial.print("ppb eCO2="); + Serial.print(evidence.eco2); + Serial.print("ppm | "); + + Serial.print("m(O)="); + Serial.print(result.occupiedBelief, 3); + Serial.print(" m(E)="); + Serial.print(result.emptyBelief, 3); + Serial.print(" | K="); + Serial.print(result.conflictK, 3); + Serial.print(" | "); + Serial.println(result.occupied ? ">>> OCCUPIED <<<" : "EMPTY"); +}