From 372adb02949556674dd98a65cdf26d0165cb184a Mon Sep 17 00:00:00 2001 From: MattDema Date: Wed, 10 Jun 2026 22:07:19 +0200 Subject: [PATCH 1/3] First approach DST and DOWA --- DOWA&DST/platformio.ini | 12 + DOWA&DST/src/dowa_dst_fusion_full.cpp | 474 ++++++++++++++++++++++ DOWA&DST/src/dowa_dst_fusion_template.cpp | 195 +++++++++ DOWA&DST/src/main.cpp | 0 4 files changed, 681 insertions(+) create mode 100644 DOWA&DST/platformio.ini create mode 100644 DOWA&DST/src/dowa_dst_fusion_full.cpp create mode 100644 DOWA&DST/src/dowa_dst_fusion_template.cpp create mode 100644 DOWA&DST/src/main.cpp diff --git a/DOWA&DST/platformio.ini b/DOWA&DST/platformio.ini new file mode 100644 index 00000000..f17a417c --- /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_full.cpp b/DOWA&DST/src/dowa_dst_fusion_full.cpp new file mode 100644 index 00000000..fdf7a82f --- /dev/null +++ b/DOWA&DST/src/dowa_dst_fusion_full.cpp @@ -0,0 +1,474 @@ +#include +#include + +/* + MOD/DOWA Fusion Engine - Full Implementation + True Presence Detection using Dempster-Shafer Theory + + Sensors: + - PIR (0.25 day / 0.10 night) + - mmWave LD2410 (0.35 day / 0.50 night) + - CO₂ SCD41 (0.20 day / 0.25 night) + - Temperature SHT41 (0.10 day / 0.08 night) + - Humidity SHT41 (0.10 day / 0.07 night) +*/ + +// ============================================================================ +// STRUCTURES +// ============================================================================ + +struct MassFunction { + float occupied; + float empty; + float unknown; +}; + +struct SensorReading { + // PIR + bool pirMotion; + unsigned long pirLastTriggerTime; + uint32_t pirTriggerCount60s; + + // mmWave LD2410 + uint8_t mmWaveState; // 0=None, 1=Moving, 2=Stationary, 3=? + unsigned long mmWaveLastReadTime; + + // CO₂ + uint16_t co2Current; + uint16_t co2Baseline; + unsigned long lastStateChangeTime; + + // Temperature + float tempCurrent; + float tempBaseline; + + // Humidity + float humCurrent; + float humBaseline; +}; + +struct FusionState { + MassFunction dowa; + float conflictK; + bool occupied; + uint8_t cyclesAboveThreshold; + uint8_t cyclesBelowThreshold; + bool lastOccupancyOutput; +}; + +// ============================================================================ +// CONSTANTS +// ============================================================================ + +// Day/Night mode +#define NIGHT_START_HOUR 23 +#define NIGHT_END_HOUR 7 + +// Weights (day mode) +static constexpr float w_pir_day = 0.25f; +static constexpr float w_mmwave_day = 0.35f; +static constexpr float w_co2_day = 0.20f; +static constexpr float w_temp_day = 0.10f; +static constexpr float w_hum_day = 0.10f; + +// Weights (night mode) +static constexpr float w_pir_night = 0.10f; +static constexpr float w_mmwave_night = 0.50f; +static constexpr float w_co2_night = 0.25f; +static constexpr float w_temp_night = 0.08f; +static constexpr float w_hum_night = 0.07f; + +// CO₂ thresholds (ppm delta from baseline) +static constexpr float CO2_DELTA_50 = 50.0f; +static constexpr float CO2_DELTA_150 = 150.0f; +static constexpr float CO2_DELTA_300 = 300.0f; +static constexpr float CO2_DELTA_500 = 500.0f; +static constexpr float CO2_LAG_PENALTY = 0.5f; +static constexpr unsigned long CO2_LAG_DURATION_MS = 180000; + +// Temperature thresholds (°C delta) +static constexpr float TEMP_DELTA_03 = 0.3f; +static constexpr float TEMP_DELTA_07 = 0.7f; +static constexpr float TEMP_DELTA_12 = 1.2f; + +// Humidity thresholds (%RH delta) +static constexpr float HUM_DELTA_15 = 1.5f; +static constexpr float HUM_DELTA_30 = 3.0f; +static constexpr float HUM_DELTA_60 = 6.0f; + +// PIR decay timing +static constexpr unsigned long PIR_HOLD_MS = 30000; +static constexpr unsigned long PIR_DECAY_MAX_MS = 120000; +static constexpr unsigned long PIR_NO_TRIGGER_THRESHOLD_MS = 120000; + +// Decision threshold +static constexpr float OCCUPANCY_THRESHOLD = 0.50f; + +// Hysteresis +static constexpr uint8_t HYSTERESIS_UP_CYCLES = 2; // 10s (2 × 5s) +static constexpr uint8_t HYSTERESIS_DOWN_CYCLES = 6; // 30s (6 × 5s) + +// Sensor timeouts +static constexpr unsigned long SENSOR_TIMEOUT_MS = 60000; + +// ============================================================================ +// GLOBAL STATE +// ============================================================================ + +SensorReading sensorData; +FusionState fusionState; +unsigned long lastFusionCycleTime = 0; +static constexpr unsigned long FUSION_CYCLE_MS = 5000; // 5 seconds + +// ============================================================================ +// UTILITY FUNCTIONS +// ============================================================================ + +bool isNightMode() { + time_t now = time(nullptr); + struct tm *timeinfo = localtime(&now); + int hour = timeinfo->tm_hour; + + return (hour >= NIGHT_START_HOUR || hour < NIGHT_END_HOUR); +} + +void getWeights(float &w_pir, float &w_mmwave, float &w_co2, float &w_temp, float &w_hum) { + if (isNightMode()) { + w_pir = w_pir_night; + w_mmwave = w_mmwave_night; + w_co2 = w_co2_night; + w_temp = w_temp_night; + w_hum = w_hum_night; + } else { + w_pir = w_pir_day; + w_mmwave = w_mmwave_day; + w_co2 = w_co2_day; + w_temp = w_temp_day; + w_hum = w_hum_day; + } +} + +// ============================================================================ +// MASS FUNCTION COMPUTATION +// ============================================================================ + +MassFunction computePIRMass() { + unsigned long now = millis(); + unsigned long timeSinceLastTrigger = now - sensorData.pirLastTriggerTime; + + // Active trigger (motion detected) + if (sensorData.pirMotion) { + return {0.90f, 0.00f, 0.10f}; + } + + // Hold period (0–30s after last trigger) + if (timeSinceLastTrigger <= PIR_HOLD_MS) { + return {0.75f, 0.00f, 0.25f}; + } + + // Decay period (30–120s after last trigger) + if (timeSinceLastTrigger <= PIR_DECAY_MAX_MS) { + float progress = (timeSinceLastTrigger - PIR_HOLD_MS) / (float)(PIR_DECAY_MAX_MS - PIR_HOLD_MS); + float decayedOccupied = 0.75f * (1.0f - progress); + float decayedUnknown = 0.25f + (0.75f * progress); + return {decayedOccupied, 0.00f, decayedUnknown}; + } + + // No trigger (>120s silence) + return {0.00f, 0.30f, 0.70f}; +} + +MassFunction computeMMWaveMass() { + unsigned long now = millis(); + unsigned long timeSinceRead = now - sensorData.mmWaveLastReadTime; + + // Check sensor timeout + if (timeSinceRead > SENSOR_TIMEOUT_MS) { + return {0.00f, 0.00f, 1.00f}; + } + + switch (sensorData.mmWaveState) { + case 1: // Moving + return {0.95f, 0.00f, 0.05f}; + case 2: // Stationary (breathing/heartbeat) + return {0.85f, 0.00f, 0.15f}; + default: // None (0) + return {0.05f, 0.70f, 0.25f}; + } +} + +MassFunction computeCO2Mass() { + unsigned long now = millis(); + unsigned long timeSinceStateChange = now - sensorData.lastStateChangeTime; + + float delta = (float)sensorData.co2Current - (float)sensorData.co2Baseline; + + MassFunction base; + if (delta < CO2_DELTA_50) { + base = {0.00f, 0.60f, 0.40f}; + } else if (delta < CO2_DELTA_150) { + base = {0.20f, 0.30f, 0.50f}; + } else if (delta < CO2_DELTA_300) { + base = {0.55f, 0.10f, 0.35f}; + } else if (delta < CO2_DELTA_500) { + base = {0.80f, 0.00f, 0.20f}; + } else { + base = {0.92f, 0.00f, 0.08f}; + } + + // Apply lag penalty for first 180s + if (timeSinceStateChange < CO2_LAG_DURATION_MS) { + float lagPenalty = CO2_LAG_PENALTY; + float remainder = (1.0f - lagPenalty) / 2.0f; + base.occupied = base.occupied * lagPenalty + remainder; + base.empty = base.empty * lagPenalty + remainder; + base.unknown = base.unknown * lagPenalty + remainder; + // Normalize + float sum = base.occupied + base.empty + base.unknown; + if (sum > 0.001f) { + base.occupied /= sum; + base.empty /= sum; + base.unknown /= sum; + } + } + + return base; +} + +MassFunction computeTemperatureMass() { + float deltaT = sensorData.tempCurrent - sensorData.tempBaseline; + + if (deltaT < TEMP_DELTA_03) { + return {0.00f, 0.30f, 0.70f}; + } else if (deltaT < TEMP_DELTA_07) { + return {0.20f, 0.10f, 0.70f}; + } else if (deltaT < TEMP_DELTA_12) { + return {0.45f, 0.00f, 0.55f}; + } else { + return {0.65f, 0.00f, 0.35f}; + } +} + +MassFunction computeHumidityMass() { + float deltaRH = sensorData.humCurrent - sensorData.humBaseline; + + if (deltaRH < HUM_DELTA_15) { + return {0.00f, 0.25f, 0.75f}; + } else if (deltaRH < HUM_DELTA_30) { + return {0.15f, 0.10f, 0.75f}; + } else if (deltaRH < HUM_DELTA_60) { + return {0.40f, 0.00f, 0.60f}; + } else { + return {0.60f, 0.00f, 0.40f}; + } +} + +// ============================================================================ +// DEMPSTER-SHAFER COMBINATION +// ============================================================================ + +MassFunction combineTwo(const MassFunction &a, const MassFunction &b, float &outK) { + // Calculate conflict K + float k = (a.occupied * b.empty) + (a.empty * b.occupied); + outK = k; + + float safeDenominator = 1.0f - k; + + // Handle conflict near 1.0 + 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; +} + +// ============================================================================ +// FUSION ENGINE +// ============================================================================ + +void runDOWAFusion() { + unsigned long now = millis(); + + // Check fusion cycle timing + if (now - lastFusionCycleTime < FUSION_CYCLE_MS) { + return; + } + lastFusionCycleTime = now; + + // Get time-of-day weights + float w_pir, w_mmwave, w_co2, w_temp, w_hum; + getWeights(w_pir, w_mmwave, w_co2, w_temp, w_hum); + + // Compute individual sensor masses + MassFunction m_pir = computePIRMass(); + MassFunction m_mmwave = computeMMWaveMass(); + MassFunction m_co2 = computeCO2Mass(); + MassFunction m_temp = computeTemperatureMass(); + MassFunction m_hum = computeHumidityMass(); + + // DOWA Weighted Aggregation + float totalWeight = w_pir + w_mmwave + w_co2 + w_temp + w_hum; + float norm_pir = w_pir / totalWeight; + float norm_mmwave = w_mmwave / totalWeight; + float norm_co2 = w_co2 / totalWeight; + float norm_temp = w_temp / totalWeight; + float norm_hum = w_hum / totalWeight; + + MassFunction m_dowa; + m_dowa.occupied = (norm_pir * m_pir.occupied) + (norm_mmwave * m_mmwave.occupied) + + (norm_co2 * m_co2.occupied) + (norm_temp * m_temp.occupied) + (norm_hum * m_hum.occupied); + m_dowa.empty = (norm_pir * m_pir.empty) + (norm_mmwave * m_mmwave.empty) + + (norm_co2 * m_co2.empty) + (norm_temp * m_temp.empty) + (norm_hum * m_hum.empty); + m_dowa.unknown = 1.0f - m_dowa.occupied - m_dowa.empty; + + // Dempster-Shafer combination (sequential pairs with K tracking) + float maxK = 0.0f; + MassFunction combined = m_dowa; + + // Combine in order: DOWA-base → PIR → mmWave → CO2 → Temp → Hum + float tempK = 0.0f; + MassFunction temp1 = combineTwo(combined, m_pir, tempK); + maxK = max(maxK, tempK); + combined = temp1; + + MassFunction temp2 = combineTwo(combined, m_mmwave, tempK); + maxK = max(maxK, tempK); + combined = temp2; + + MassFunction temp3 = combineTwo(combined, m_co2, tempK); + maxK = max(maxK, tempK); + combined = temp3; + + MassFunction temp4 = combineTwo(combined, m_temp, tempK); + maxK = max(maxK, tempK); + combined = temp4; + + MassFunction temp5 = combineTwo(combined, m_hum, tempK); + maxK = max(maxK, tempK); + combined = temp5; + + // Store fused result and conflict + fusionState.dowa = combined; + fusionState.conflictK = maxK; + + // Decision rule with hysteresis + bool beliefAboveThreshold = (combined.occupied >= OCCUPANCY_THRESHOLD); + + if (beliefAboveThreshold) { + fusionState.cyclesAboveThreshold++; + fusionState.cyclesBelowThreshold = 0; + + if (fusionState.cyclesAboveThreshold >= HYSTERESIS_UP_CYCLES) { + fusionState.occupied = true; + } + } else { + fusionState.cyclesBelowThreshold++; + fusionState.cyclesAboveThreshold = 0; + + if (fusionState.cyclesBelowThreshold >= HYSTERESIS_DOWN_CYCLES) { + fusionState.occupied = false; + } + } +} + +// ============================================================================ +// PLACEHOLDER SENSOR READS +// ============================================================================ + +void readAllSensors() { + // PIR + sensorData.pirMotion = false; // Replace with actual read + // sensorData.pirLastTriggerTime updated on interrupt + + // mmWave + sensorData.mmWaveState = 2; // Replace with actual read (0/1/2) + sensorData.mmWaveLastReadTime = millis(); + + // CO₂ (ppm) + sensorData.co2Current = 520; // Replace with actual SCD41 read + sensorData.co2Baseline = 450; // Update dynamically during confirmed EMPTY + + // Temperature (°C) + sensorData.tempCurrent = 22.5f; // Replace with actual SHT41 read + sensorData.tempBaseline = 21.2f; + + // Humidity (%RH) + sensorData.humCurrent = 52.0f; // Replace with actual SHT41 read + sensorData.humBaseline = 48.0f; +} + +// ============================================================================ +// SETUP & LOOP +// ============================================================================ + +void setup() { + Serial.begin(115200); + delay(1000); + + // Initialize state + fusionState.occupied = false; + fusionState.cyclesAboveThreshold = 0; + fusionState.cyclesBelowThreshold = 0; + fusionState.lastOccupancyOutput = false; + fusionState.conflictK = 0.0f; + sensorData.lastStateChangeTime = millis(); + sensorData.mmWaveLastReadTime = millis(); + sensorData.co2Baseline = 450; + sensorData.tempBaseline = 21.0f; + sensorData.humBaseline = 48.0f; + + Serial.println("\n=== MOD/DOWA Fusion Engine Ready ==="); + Serial.println("Measurement cycle: 5 seconds"); + Serial.println("Night mode: 23:00–07:00"); +} + +void loop() { + // Read all sensors + readAllSensors(); + + // Run fusion + runDOWAFusion(); + + // Print diagnostics every cycle + Serial.print("["); + Serial.print(isNightMode() ? "NIGHT" : "DAY"); + Serial.print("] PIR="); + Serial.print(sensorData.pirMotion ? "1" : "0"); + Serial.print(" mmWave="); + Serial.print(sensorData.mmWaveState); + Serial.print(" CO2="); + Serial.print(sensorData.co2Current); + Serial.print("ppm(Δ"); + Serial.print((int)(sensorData.co2Current - sensorData.co2Baseline)); + Serial.print(") T="); + Serial.print(sensorData.tempCurrent, 1); + Serial.print("°C RH="); + Serial.print(sensorData.humCurrent, 1); + Serial.print("%"); + + Serial.print(" | m(O)="); + Serial.print(fusionState.dowa.occupied, 3); + Serial.print(" m(E)="); + Serial.print(fusionState.dowa.empty, 3); + Serial.print(" m(U)="); + Serial.print(fusionState.dowa.unknown, 3); + + Serial.print(" | K="); + Serial.print(fusionState.conflictK, 3); + + Serial.print(" | Hysteresis["); + Serial.print(fusionState.cyclesAboveThreshold); + Serial.print("/"); + Serial.print(fusionState.cyclesBelowThreshold); + Serial.print("]"); + + Serial.print(" → "); + Serial.println(fusionState.occupied ? "OCCUPIED" : "EMPTY"); + + delay(100); // Small delay to avoid blocking +} 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 00000000..d45479c4 --- /dev/null +++ b/DOWA&DST/src/dowa_dst_fusion_template.cpp @@ -0,0 +1,195 @@ +#include + +/* + Raw template: DOWA + Dempster-Shafer fusion model + - Radar / mmWave + - PIR + - Air quality (TVOC / eCO2) + + Fill in the sensor acquisition code and tune the mass assignments / thresholds + for your hardware and environment. +*/ + +struct MassFunction { + float occupied; + float empty; + float unknown; +}; + +struct SensorEvidence { + bool radarMotion; + bool pirMotion; + 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) +// Tune these to reflect sensor reliability and importance +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(bool motion) { + if (motion) { + return {0.90f, 0.05f, 0.05f}; + } + return {0.05f, 0.90f, 0.05f}; +} + +static MassFunction makePirMass(bool motion) { + if (motion) { + return {0.80f, 0.10f, 0.10f}; + } + return {0.10f, 0.80f, 0.10f}; +} + +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) { + // Dempster-Shafer Combination Rule (PDF 5.3) + // K = conflict coefficient: Σ m_A(X) × m_B(Y) for all X ≠ Y + const float k = (a.occupied * b.empty) + (a.empty * b.occupied); + outK = k; + + const float safeDenominator = 1.0f - k; + + if (safeDenominator <= 0.0001f) { + // High conflict: default to UNKNOWN + return {0.0f, 0.0f, 1.0f}; + } + + // m_combined(H) = [ Σ m_A(X) × m_B(Y) for all XY=H ] / (1 − K) + 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.radarMotion); + const MassFunction pirMass = makePirMass(evidence.pirMotion); + const MassFunction airMass = makeAirMass(evidence.tvoc, evidence.eco2); + + // Weight Normalization (PDF 5.1) + // Normalize weights to sum to 1.0, accounting for inactive sensors + 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; + + // DOWA Weighted Aggregation (PDF section 5.2) + // m_DOWA(H) = Σ [ w_i_normalized × m_i(H) ] for each hypothesis H + + 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); + + // Dempster-Shafer Sequential Combination (PDF 5.3) + // Combine DOWA aggregate with each sensor's mass function + float k = 0.0f; + float maxK = 0.0f; + + MassFunction combined = m_dowa; + + // Combine with radar + combined = combineTwo(combined, radarMass, k); + maxK = max(maxK, k); + + // Combine with PIR + combined = combineTwo(combined, pirMass, k); + maxK = max(maxK, k); + + // Combine with air quality + 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 SensorEvidence readSensorsTemplate() { + // Replace these placeholders with your real sensor reads. + SensorEvidence evidence; + evidence.radarMotion = false; + evidence.pirMotion = false; + evidence.tvoc = 0; + evidence.eco2 = 400; + return evidence; +} + +void setup() { + Serial.begin(115200); + delay(1000); + Serial.println("DOWA + DST fusion template ready"); +} + +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("RADAR="); + Serial.print(evidence.radarMotion ? "1" : "0"); + Serial.print(" PIR="); + Serial.print(evidence.pirMotion ? "1" : "0"); + Serial.print(" TVOC="); + + Serial.print(evidence.tvoc); + Serial.print(" eCO2="); + Serial.print(evidence.eco2); + 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(" | DOWA="); + Serial.println(result.occupied ? "OCCUPIED" : "EMPTY"); +} diff --git a/DOWA&DST/src/main.cpp b/DOWA&DST/src/main.cpp new file mode 100644 index 00000000..e69de29b From a1fb7cc9d7c7837597b95db28d6217d2081a76ed Mon Sep 17 00:00:00 2001 From: MattDema Date: Thu, 11 Jun 2026 20:12:21 +0200 Subject: [PATCH 2/3] First approach of DOWA and DST, three sensors, works well --- DOWA&DST/src/dowa_dst_fusion_full.cpp | 474 ---------------------- DOWA&DST/src/dowa_dst_fusion_template.cpp | 180 +++++++- DOWA&DST/src/main.cpp | 0 3 files changed, 161 insertions(+), 493 deletions(-) delete mode 100644 DOWA&DST/src/dowa_dst_fusion_full.cpp delete mode 100644 DOWA&DST/src/main.cpp diff --git a/DOWA&DST/src/dowa_dst_fusion_full.cpp b/DOWA&DST/src/dowa_dst_fusion_full.cpp deleted file mode 100644 index fdf7a82f..00000000 --- a/DOWA&DST/src/dowa_dst_fusion_full.cpp +++ /dev/null @@ -1,474 +0,0 @@ -#include -#include - -/* - MOD/DOWA Fusion Engine - Full Implementation - True Presence Detection using Dempster-Shafer Theory - - Sensors: - - PIR (0.25 day / 0.10 night) - - mmWave LD2410 (0.35 day / 0.50 night) - - CO₂ SCD41 (0.20 day / 0.25 night) - - Temperature SHT41 (0.10 day / 0.08 night) - - Humidity SHT41 (0.10 day / 0.07 night) -*/ - -// ============================================================================ -// STRUCTURES -// ============================================================================ - -struct MassFunction { - float occupied; - float empty; - float unknown; -}; - -struct SensorReading { - // PIR - bool pirMotion; - unsigned long pirLastTriggerTime; - uint32_t pirTriggerCount60s; - - // mmWave LD2410 - uint8_t mmWaveState; // 0=None, 1=Moving, 2=Stationary, 3=? - unsigned long mmWaveLastReadTime; - - // CO₂ - uint16_t co2Current; - uint16_t co2Baseline; - unsigned long lastStateChangeTime; - - // Temperature - float tempCurrent; - float tempBaseline; - - // Humidity - float humCurrent; - float humBaseline; -}; - -struct FusionState { - MassFunction dowa; - float conflictK; - bool occupied; - uint8_t cyclesAboveThreshold; - uint8_t cyclesBelowThreshold; - bool lastOccupancyOutput; -}; - -// ============================================================================ -// CONSTANTS -// ============================================================================ - -// Day/Night mode -#define NIGHT_START_HOUR 23 -#define NIGHT_END_HOUR 7 - -// Weights (day mode) -static constexpr float w_pir_day = 0.25f; -static constexpr float w_mmwave_day = 0.35f; -static constexpr float w_co2_day = 0.20f; -static constexpr float w_temp_day = 0.10f; -static constexpr float w_hum_day = 0.10f; - -// Weights (night mode) -static constexpr float w_pir_night = 0.10f; -static constexpr float w_mmwave_night = 0.50f; -static constexpr float w_co2_night = 0.25f; -static constexpr float w_temp_night = 0.08f; -static constexpr float w_hum_night = 0.07f; - -// CO₂ thresholds (ppm delta from baseline) -static constexpr float CO2_DELTA_50 = 50.0f; -static constexpr float CO2_DELTA_150 = 150.0f; -static constexpr float CO2_DELTA_300 = 300.0f; -static constexpr float CO2_DELTA_500 = 500.0f; -static constexpr float CO2_LAG_PENALTY = 0.5f; -static constexpr unsigned long CO2_LAG_DURATION_MS = 180000; - -// Temperature thresholds (°C delta) -static constexpr float TEMP_DELTA_03 = 0.3f; -static constexpr float TEMP_DELTA_07 = 0.7f; -static constexpr float TEMP_DELTA_12 = 1.2f; - -// Humidity thresholds (%RH delta) -static constexpr float HUM_DELTA_15 = 1.5f; -static constexpr float HUM_DELTA_30 = 3.0f; -static constexpr float HUM_DELTA_60 = 6.0f; - -// PIR decay timing -static constexpr unsigned long PIR_HOLD_MS = 30000; -static constexpr unsigned long PIR_DECAY_MAX_MS = 120000; -static constexpr unsigned long PIR_NO_TRIGGER_THRESHOLD_MS = 120000; - -// Decision threshold -static constexpr float OCCUPANCY_THRESHOLD = 0.50f; - -// Hysteresis -static constexpr uint8_t HYSTERESIS_UP_CYCLES = 2; // 10s (2 × 5s) -static constexpr uint8_t HYSTERESIS_DOWN_CYCLES = 6; // 30s (6 × 5s) - -// Sensor timeouts -static constexpr unsigned long SENSOR_TIMEOUT_MS = 60000; - -// ============================================================================ -// GLOBAL STATE -// ============================================================================ - -SensorReading sensorData; -FusionState fusionState; -unsigned long lastFusionCycleTime = 0; -static constexpr unsigned long FUSION_CYCLE_MS = 5000; // 5 seconds - -// ============================================================================ -// UTILITY FUNCTIONS -// ============================================================================ - -bool isNightMode() { - time_t now = time(nullptr); - struct tm *timeinfo = localtime(&now); - int hour = timeinfo->tm_hour; - - return (hour >= NIGHT_START_HOUR || hour < NIGHT_END_HOUR); -} - -void getWeights(float &w_pir, float &w_mmwave, float &w_co2, float &w_temp, float &w_hum) { - if (isNightMode()) { - w_pir = w_pir_night; - w_mmwave = w_mmwave_night; - w_co2 = w_co2_night; - w_temp = w_temp_night; - w_hum = w_hum_night; - } else { - w_pir = w_pir_day; - w_mmwave = w_mmwave_day; - w_co2 = w_co2_day; - w_temp = w_temp_day; - w_hum = w_hum_day; - } -} - -// ============================================================================ -// MASS FUNCTION COMPUTATION -// ============================================================================ - -MassFunction computePIRMass() { - unsigned long now = millis(); - unsigned long timeSinceLastTrigger = now - sensorData.pirLastTriggerTime; - - // Active trigger (motion detected) - if (sensorData.pirMotion) { - return {0.90f, 0.00f, 0.10f}; - } - - // Hold period (0–30s after last trigger) - if (timeSinceLastTrigger <= PIR_HOLD_MS) { - return {0.75f, 0.00f, 0.25f}; - } - - // Decay period (30–120s after last trigger) - if (timeSinceLastTrigger <= PIR_DECAY_MAX_MS) { - float progress = (timeSinceLastTrigger - PIR_HOLD_MS) / (float)(PIR_DECAY_MAX_MS - PIR_HOLD_MS); - float decayedOccupied = 0.75f * (1.0f - progress); - float decayedUnknown = 0.25f + (0.75f * progress); - return {decayedOccupied, 0.00f, decayedUnknown}; - } - - // No trigger (>120s silence) - return {0.00f, 0.30f, 0.70f}; -} - -MassFunction computeMMWaveMass() { - unsigned long now = millis(); - unsigned long timeSinceRead = now - sensorData.mmWaveLastReadTime; - - // Check sensor timeout - if (timeSinceRead > SENSOR_TIMEOUT_MS) { - return {0.00f, 0.00f, 1.00f}; - } - - switch (sensorData.mmWaveState) { - case 1: // Moving - return {0.95f, 0.00f, 0.05f}; - case 2: // Stationary (breathing/heartbeat) - return {0.85f, 0.00f, 0.15f}; - default: // None (0) - return {0.05f, 0.70f, 0.25f}; - } -} - -MassFunction computeCO2Mass() { - unsigned long now = millis(); - unsigned long timeSinceStateChange = now - sensorData.lastStateChangeTime; - - float delta = (float)sensorData.co2Current - (float)sensorData.co2Baseline; - - MassFunction base; - if (delta < CO2_DELTA_50) { - base = {0.00f, 0.60f, 0.40f}; - } else if (delta < CO2_DELTA_150) { - base = {0.20f, 0.30f, 0.50f}; - } else if (delta < CO2_DELTA_300) { - base = {0.55f, 0.10f, 0.35f}; - } else if (delta < CO2_DELTA_500) { - base = {0.80f, 0.00f, 0.20f}; - } else { - base = {0.92f, 0.00f, 0.08f}; - } - - // Apply lag penalty for first 180s - if (timeSinceStateChange < CO2_LAG_DURATION_MS) { - float lagPenalty = CO2_LAG_PENALTY; - float remainder = (1.0f - lagPenalty) / 2.0f; - base.occupied = base.occupied * lagPenalty + remainder; - base.empty = base.empty * lagPenalty + remainder; - base.unknown = base.unknown * lagPenalty + remainder; - // Normalize - float sum = base.occupied + base.empty + base.unknown; - if (sum > 0.001f) { - base.occupied /= sum; - base.empty /= sum; - base.unknown /= sum; - } - } - - return base; -} - -MassFunction computeTemperatureMass() { - float deltaT = sensorData.tempCurrent - sensorData.tempBaseline; - - if (deltaT < TEMP_DELTA_03) { - return {0.00f, 0.30f, 0.70f}; - } else if (deltaT < TEMP_DELTA_07) { - return {0.20f, 0.10f, 0.70f}; - } else if (deltaT < TEMP_DELTA_12) { - return {0.45f, 0.00f, 0.55f}; - } else { - return {0.65f, 0.00f, 0.35f}; - } -} - -MassFunction computeHumidityMass() { - float deltaRH = sensorData.humCurrent - sensorData.humBaseline; - - if (deltaRH < HUM_DELTA_15) { - return {0.00f, 0.25f, 0.75f}; - } else if (deltaRH < HUM_DELTA_30) { - return {0.15f, 0.10f, 0.75f}; - } else if (deltaRH < HUM_DELTA_60) { - return {0.40f, 0.00f, 0.60f}; - } else { - return {0.60f, 0.00f, 0.40f}; - } -} - -// ============================================================================ -// DEMPSTER-SHAFER COMBINATION -// ============================================================================ - -MassFunction combineTwo(const MassFunction &a, const MassFunction &b, float &outK) { - // Calculate conflict K - float k = (a.occupied * b.empty) + (a.empty * b.occupied); - outK = k; - - float safeDenominator = 1.0f - k; - - // Handle conflict near 1.0 - 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; -} - -// ============================================================================ -// FUSION ENGINE -// ============================================================================ - -void runDOWAFusion() { - unsigned long now = millis(); - - // Check fusion cycle timing - if (now - lastFusionCycleTime < FUSION_CYCLE_MS) { - return; - } - lastFusionCycleTime = now; - - // Get time-of-day weights - float w_pir, w_mmwave, w_co2, w_temp, w_hum; - getWeights(w_pir, w_mmwave, w_co2, w_temp, w_hum); - - // Compute individual sensor masses - MassFunction m_pir = computePIRMass(); - MassFunction m_mmwave = computeMMWaveMass(); - MassFunction m_co2 = computeCO2Mass(); - MassFunction m_temp = computeTemperatureMass(); - MassFunction m_hum = computeHumidityMass(); - - // DOWA Weighted Aggregation - float totalWeight = w_pir + w_mmwave + w_co2 + w_temp + w_hum; - float norm_pir = w_pir / totalWeight; - float norm_mmwave = w_mmwave / totalWeight; - float norm_co2 = w_co2 / totalWeight; - float norm_temp = w_temp / totalWeight; - float norm_hum = w_hum / totalWeight; - - MassFunction m_dowa; - m_dowa.occupied = (norm_pir * m_pir.occupied) + (norm_mmwave * m_mmwave.occupied) + - (norm_co2 * m_co2.occupied) + (norm_temp * m_temp.occupied) + (norm_hum * m_hum.occupied); - m_dowa.empty = (norm_pir * m_pir.empty) + (norm_mmwave * m_mmwave.empty) + - (norm_co2 * m_co2.empty) + (norm_temp * m_temp.empty) + (norm_hum * m_hum.empty); - m_dowa.unknown = 1.0f - m_dowa.occupied - m_dowa.empty; - - // Dempster-Shafer combination (sequential pairs with K tracking) - float maxK = 0.0f; - MassFunction combined = m_dowa; - - // Combine in order: DOWA-base → PIR → mmWave → CO2 → Temp → Hum - float tempK = 0.0f; - MassFunction temp1 = combineTwo(combined, m_pir, tempK); - maxK = max(maxK, tempK); - combined = temp1; - - MassFunction temp2 = combineTwo(combined, m_mmwave, tempK); - maxK = max(maxK, tempK); - combined = temp2; - - MassFunction temp3 = combineTwo(combined, m_co2, tempK); - maxK = max(maxK, tempK); - combined = temp3; - - MassFunction temp4 = combineTwo(combined, m_temp, tempK); - maxK = max(maxK, tempK); - combined = temp4; - - MassFunction temp5 = combineTwo(combined, m_hum, tempK); - maxK = max(maxK, tempK); - combined = temp5; - - // Store fused result and conflict - fusionState.dowa = combined; - fusionState.conflictK = maxK; - - // Decision rule with hysteresis - bool beliefAboveThreshold = (combined.occupied >= OCCUPANCY_THRESHOLD); - - if (beliefAboveThreshold) { - fusionState.cyclesAboveThreshold++; - fusionState.cyclesBelowThreshold = 0; - - if (fusionState.cyclesAboveThreshold >= HYSTERESIS_UP_CYCLES) { - fusionState.occupied = true; - } - } else { - fusionState.cyclesBelowThreshold++; - fusionState.cyclesAboveThreshold = 0; - - if (fusionState.cyclesBelowThreshold >= HYSTERESIS_DOWN_CYCLES) { - fusionState.occupied = false; - } - } -} - -// ============================================================================ -// PLACEHOLDER SENSOR READS -// ============================================================================ - -void readAllSensors() { - // PIR - sensorData.pirMotion = false; // Replace with actual read - // sensorData.pirLastTriggerTime updated on interrupt - - // mmWave - sensorData.mmWaveState = 2; // Replace with actual read (0/1/2) - sensorData.mmWaveLastReadTime = millis(); - - // CO₂ (ppm) - sensorData.co2Current = 520; // Replace with actual SCD41 read - sensorData.co2Baseline = 450; // Update dynamically during confirmed EMPTY - - // Temperature (°C) - sensorData.tempCurrent = 22.5f; // Replace with actual SHT41 read - sensorData.tempBaseline = 21.2f; - - // Humidity (%RH) - sensorData.humCurrent = 52.0f; // Replace with actual SHT41 read - sensorData.humBaseline = 48.0f; -} - -// ============================================================================ -// SETUP & LOOP -// ============================================================================ - -void setup() { - Serial.begin(115200); - delay(1000); - - // Initialize state - fusionState.occupied = false; - fusionState.cyclesAboveThreshold = 0; - fusionState.cyclesBelowThreshold = 0; - fusionState.lastOccupancyOutput = false; - fusionState.conflictK = 0.0f; - sensorData.lastStateChangeTime = millis(); - sensorData.mmWaveLastReadTime = millis(); - sensorData.co2Baseline = 450; - sensorData.tempBaseline = 21.0f; - sensorData.humBaseline = 48.0f; - - Serial.println("\n=== MOD/DOWA Fusion Engine Ready ==="); - Serial.println("Measurement cycle: 5 seconds"); - Serial.println("Night mode: 23:00–07:00"); -} - -void loop() { - // Read all sensors - readAllSensors(); - - // Run fusion - runDOWAFusion(); - - // Print diagnostics every cycle - Serial.print("["); - Serial.print(isNightMode() ? "NIGHT" : "DAY"); - Serial.print("] PIR="); - Serial.print(sensorData.pirMotion ? "1" : "0"); - Serial.print(" mmWave="); - Serial.print(sensorData.mmWaveState); - Serial.print(" CO2="); - Serial.print(sensorData.co2Current); - Serial.print("ppm(Δ"); - Serial.print((int)(sensorData.co2Current - sensorData.co2Baseline)); - Serial.print(") T="); - Serial.print(sensorData.tempCurrent, 1); - Serial.print("°C RH="); - Serial.print(sensorData.humCurrent, 1); - Serial.print("%"); - - Serial.print(" | m(O)="); - Serial.print(fusionState.dowa.occupied, 3); - Serial.print(" m(E)="); - Serial.print(fusionState.dowa.empty, 3); - Serial.print(" m(U)="); - Serial.print(fusionState.dowa.unknown, 3); - - Serial.print(" | K="); - Serial.print(fusionState.conflictK, 3); - - Serial.print(" | Hysteresis["); - Serial.print(fusionState.cyclesAboveThreshold); - Serial.print("/"); - Serial.print(fusionState.cyclesBelowThreshold); - Serial.print("]"); - - Serial.print(" → "); - Serial.println(fusionState.occupied ? "OCCUPIED" : "EMPTY"); - - delay(100); // Small delay to avoid blocking -} diff --git a/DOWA&DST/src/dowa_dst_fusion_template.cpp b/DOWA&DST/src/dowa_dst_fusion_template.cpp index d45479c4..193db046 100644 --- a/DOWA&DST/src/dowa_dst_fusion_template.cpp +++ b/DOWA&DST/src/dowa_dst_fusion_template.cpp @@ -1,15 +1,46 @@ #include +#include +#include "Adafruit_SGP30.h" /* - Raw template: DOWA + Dempster-Shafer fusion model - - Radar / mmWave - - PIR - - Air quality (TVOC / eCO2) - - Fill in the sensor acquisition code and tune the mass assignments / thresholds - for your hardware and environment. + 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; +unsigned long lastRadarReadTime = 0; + +// SGP30 state +unsigned long lastSGP30ReadTime = 0; +uint16_t lastTVOC = 0; +uint16_t lasteCO2 = 400; + struct MassFunction { float occupied; float empty; @@ -148,20 +179,126 @@ static FusionResult runDOWA_DST_Fusion(const SensorEvidence &evidence) { return result; } +static void processRadarData() { + // Parse incoming mmWave LD2410 packets + 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; + radarPacketLen++; + + // Check for footer (F8 F7 F6 F5) + if(radarPacketLen >= 4 && + radarPacket[radarPacketLen-4] == 0xF8 && + radarPacket[radarPacketLen-3] == 0xF7 && + radarPacket[radarPacketLen-2] == 0xF6 && + radarPacket[radarPacketLen-1] == 0xF5) { + + lastRadarPresence = radarPacket[6]; // 0=Empty, 1=Occupied + lastRadarReadTime = millis(); + radarReadingPacket = false; + } + + if(radarPacketLen >= 64) { + radarReadingPacket = false; + } + } + } +} + +static void processSGP30Data() { + unsigned long now = millis(); + + // SGP30 requires exactly 1 second between measurements + if (now - lastSGP30ReadTime < 1000) { + return; + } + lastSGP30ReadTime = now; + + if (sgp30.IAQmeasure()) { + lastTVOC = sgp30.TVOC; + lasteCO2 = sgp30.eCO2; + } +} + static SensorEvidence readSensorsTemplate() { - // Replace these placeholders with your real sensor reads. + // Process incoming sensor data continuously + processRadarData(); + processSGP30Data(); + SensorEvidence evidence; - evidence.radarMotion = false; - evidence.pirMotion = false; - evidence.tvoc = 0; - evidence.eco2 = 400; + + // PIR: simple GPIO read + evidence.pirMotion = digitalRead(PIR_PIN); + + // mmWave: parsed from packet + // (lastRadarPresence is updated in processRadarData) + evidence.radarMotion = (lastRadarPresence > 0); + + // SGP30: air quality + evidence.tvoc = lastTVOC; + evidence.eco2 = lasteCO2; + return evidence; } void setup() { Serial.begin(115200); - delay(1000); - Serial.println("DOWA + DST fusion template ready"); + delay(2000); + Serial.println("\n=== MOD/DOWA Fusion Engine - Functional ===\n"); + + // Initialize PIR pin + pinMode(PIR_PIN, INPUT); + Serial.println("[PIR] Initialized on GPIO " + String(PIR_PIN)); + + // Initialize mmWave UART + radarSerial.begin(115200, SERIAL_8N1, RADAR_RX_PIN, RADAR_TX_PIN); + delay(500); + + // Send wakeup command to radar + 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) + ")"); + + // Initialize I2C and SGP30 + 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() { @@ -175,21 +312,26 @@ void loop() { 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.radarMotion ? "1" : "0"); Serial.print(" PIR="); Serial.print(evidence.pirMotion ? "1" : "0"); Serial.print(" TVOC="); - Serial.print(evidence.tvoc); - Serial.print(" eCO2="); + Serial.print("ppb eCO2="); Serial.print(evidence.eco2); - Serial.print(" | m(O)="); + 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(" | DOWA="); - Serial.println(result.occupied ? "OCCUPIED" : "EMPTY"); + Serial.print(" | "); + Serial.println(result.occupied ? ">>> OCCUPIED <<<" : "EMPTY"); } diff --git a/DOWA&DST/src/main.cpp b/DOWA&DST/src/main.cpp deleted file mode 100644 index e69de29b..00000000 From a5484211e64ee8f6c399f4f13e7fc7a3c1808079 Mon Sep 17 00:00:00 2001 From: jagorev Date: Fri, 12 Jun 2026 17:11:46 +0200 Subject: [PATCH 3/3] modifiche di matt --- DOWA&DST/src/dowa_dst_fusion_template.cpp | 94 +++++++++++------------ 1 file changed, 44 insertions(+), 50 deletions(-) diff --git a/DOWA&DST/src/dowa_dst_fusion_template.cpp b/DOWA&DST/src/dowa_dst_fusion_template.cpp index 193db046..441467b4 100644 --- a/DOWA&DST/src/dowa_dst_fusion_template.cpp +++ b/DOWA&DST/src/dowa_dst_fusion_template.cpp @@ -33,7 +33,7 @@ int radarHeaderCount = 0; uint8_t radarPacket[64]; int radarPacketLen = 0; bool radarReadingPacket = false; -uint8_t lastRadarPresence = 0; +uint8_t lastRadarPresence = 0; // LD2410 state: 0=None, 1=Moving, 2=Stationary, 3=Both unsigned long lastRadarReadTime = 0; // SGP30 state @@ -41,6 +41,9 @@ unsigned long lastSGP30ReadTime = 0; uint16_t lastTVOC = 0; uint16_t lasteCO2 = 400; +// PIR State +unsigned long lastPirTriggerTime = 0; + struct MassFunction { float occupied; float empty; @@ -48,8 +51,9 @@ struct MassFunction { }; struct SensorEvidence { - bool radarMotion; + uint8_t radarState; // 0=none, 1=stationary, 2=moving bool pirMotion; + uint32_t msSinceLastPirTrigger; uint16_t tvoc; uint16_t eco2; }; @@ -67,23 +71,28 @@ static constexpr float TVOC_WEAK_THRESHOLD = 100.0f; static constexpr float DOWA_DECISION_THRESHOLD = 0.50f; // Sensor weight coefficients (w_i) -// Tune these to reflect sensor reliability and importance 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(bool motion) { - if (motion) { - return {0.90f, 0.05f, 0.05f}; +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 } - return {0.05f, 0.90f, 0.05f}; } -static MassFunction makePirMass(bool motion) { - if (motion) { - return {0.80f, 0.10f, 0.10f}; +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.10f, 0.80f, 0.10f}; + return {0.00f, 0.30f, 0.70f}; // silence > 120s } static MassFunction makeAirMass(uint16_t tvoc, uint16_t eco2) { @@ -97,19 +106,15 @@ static MassFunction makeAirMass(uint16_t tvoc, uint16_t eco2) { } static MassFunction combineTwo(const MassFunction &a, const MassFunction &b, float &outK) { - // Dempster-Shafer Combination Rule (PDF 5.3) - // K = conflict coefficient: Σ m_A(X) × m_B(Y) for all X ≠ Y const float k = (a.occupied * b.empty) + (a.empty * b.occupied); outK = k; const float safeDenominator = 1.0f - k; if (safeDenominator <= 0.0001f) { - // High conflict: default to UNKNOWN return {0.0f, 0.0f, 1.0f}; } - // m_combined(H) = [ Σ m_A(X) × m_B(Y) for all XY=H ] / (1 − K) 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; @@ -118,41 +123,31 @@ static MassFunction combineTwo(const MassFunction &a, const MassFunction &b, flo } static MassFunction combineEvidence(const SensorEvidence &evidence, float &outMaxK) { - const MassFunction radarMass = makeRadarMass(evidence.radarMotion); - const MassFunction pirMass = makePirMass(evidence.pirMotion); + const MassFunction radarMass = makeRadarMass(evidence.radarState); + const MassFunction pirMass = makePirMass(evidence.pirMotion, evidence.msSinceLastPirTrigger); const MassFunction airMass = makeAirMass(evidence.tvoc, evidence.eco2); - // Weight Normalization (PDF 5.1) - // Normalize weights to sum to 1.0, accounting for inactive sensors 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; - // DOWA Weighted Aggregation (PDF section 5.2) - // m_DOWA(H) = Σ [ w_i_normalized × m_i(H) ] for each hypothesis H - 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); - // Dempster-Shafer Sequential Combination (PDF 5.3) - // Combine DOWA aggregate with each sensor's mass function float k = 0.0f; float maxK = 0.0f; MassFunction combined = m_dowa; - // Combine with radar combined = combineTwo(combined, radarMass, k); maxK = max(maxK, k); - // Combine with PIR combined = combineTwo(combined, pirMass, k); maxK = max(maxK, k); - // Combine with air quality combined = combineTwo(combined, airMass, k); maxK = max(maxK, k); @@ -180,7 +175,6 @@ static FusionResult runDOWA_DST_Fusion(const SensorEvidence &evidence) { } static void processRadarData() { - // Parse incoming mmWave LD2410 packets while(radarSerial.available()) { uint8_t b = radarSerial.read(); @@ -201,21 +195,20 @@ static void processRadarData() { } } else { radarPacket[radarPacketLen] = b; - radarPacketLen++; - - // Check for footer (F8 F7 F6 F5) - if(radarPacketLen >= 4 && - radarPacket[radarPacketLen-4] == 0xF8 && - radarPacket[radarPacketLen-3] == 0xF7 && - radarPacket[radarPacketLen-2] == 0xF6 && - radarPacket[radarPacketLen-1] == 0xF5) { + 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]; // 0=Empty, 1=Occupied + lastRadarPresence = radarPacket[6]; // LD2410 byte 6: 0=None, 1=Moving, 2=Stationary, 3=Both lastRadarReadTime = millis(); radarReadingPacket = false; } - if(radarPacketLen >= 64) { + if(radarPacket_len >= 64) { radarReadingPacket = false; } } @@ -225,7 +218,6 @@ static void processRadarData() { static void processSGP30Data() { unsigned long now = millis(); - // SGP30 requires exactly 1 second between measurements if (now - lastSGP30ReadTime < 1000) { return; } @@ -238,20 +230,26 @@ static void processSGP30Data() { } static SensorEvidence readSensorsTemplate() { - // Process incoming sensor data continuously processRadarData(); processSGP30Data(); SensorEvidence evidence; - // PIR: simple GPIO read evidence.pirMotion = digitalRead(PIR_PIN); + if (evidence.pirMotion) { + lastPirTriggerTime = millis(); + } + evidence.msSinceLastPirTrigger = millis() - (lastPirTriggerTime > 0 ? lastPirTriggerTime : millis()); // Prevent overflow on startup buffer - // mmWave: parsed from packet - // (lastRadarPresence is updated in processRadarData) - evidence.radarMotion = (lastRadarPresence > 0); + // 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 + } - // SGP30: air quality evidence.tvoc = lastTVOC; evidence.eco2 = lasteCO2; @@ -263,15 +261,12 @@ void setup() { delay(2000); Serial.println("\n=== MOD/DOWA Fusion Engine - Functional ===\n"); - // Initialize PIR pin pinMode(PIR_PIN, INPUT); Serial.println("[PIR] Initialized on GPIO " + String(PIR_PIN)); - // Initialize mmWave UART radarSerial.begin(115200, SERIAL_8N1, RADAR_RX_PIN, RADAR_TX_PIN); delay(500); - // Send wakeup command to radar const byte REPORT_MODE_CMD[] = { 0xFD, 0xFC, 0xFB, 0xFA, 0x08, 0x00, @@ -283,7 +278,6 @@ void setup() { delay(100); Serial.println("[mmWave] Initialized on UART (RX=" + String(RADAR_RX_PIN) + ", TX=" + String(RADAR_TX_PIN) + ")"); - // Initialize I2C and SGP30 Wire.begin(I2C_SDA_PIN, I2C_SCL_PIN); delay(500); @@ -317,7 +311,7 @@ void loop() { Serial.print("s] "); Serial.print("RADAR="); - Serial.print(evidence.radarMotion ? "1" : "0"); + Serial.print(evidence.radarState); Serial.print(" PIR="); Serial.print(evidence.pirMotion ? "1" : "0"); Serial.print(" TVOC=");