/// © MiroZ 2024 #include "app_config.h" #include "TaskMgr.h" #include "SensorService.h" #include "Buffers.h" #include "SensorData.h" #include "Settings.h" #include "Led.h" static const char *TAG = "sensors"; #define ms_to_us(ms) ((ms)*1000) #define LIGHT_SENSOR_PIN 36 struct BMP_DATA m_bmp_data; struct BME_DATA m_bme_data; SensorService::SensorService(AppIF & app_if) : m_app_if(app_if) { // Initialize light measurement structure memset(&m_light_measurement, 0, sizeof(m_light_measurement)); } void SensorService::start() { ESP_LOGW(TAG, "Starting sensor service..."); esp_log_level_set("gpio", ESP_LOG_WARN); memset(&m_bmp_data, 0, sizeof(m_bmp_data)); memset(&m_bme_data, 0, sizeof(m_bme_data)); m_bmp280 = new Bmp280(Wire); m_bme68x = new Bme68x(Wire); m_ld2410 = new LD2410(); bool hw_fault = false; if(!m_bmp280->init()) { hw_fault = true; ESP_LOGE(TAG, "bmp280 sensor error"); } if(!m_bme68x->init()) { hw_fault = true; ESP_LOGE(TAG, "bme68x sensor error"); } if(!m_ld2410->init()) { hw_fault = true; ESP_LOGE(TAG, "ld2410 sensor error"); } if(hw_fault) m_app_if.getLed()->setColor(255, 0, 0); assert(m_i2c1_task = TaskMgr::getInstance().createTask(std::bind(&SensorService::run_i2c_1, this), I2C1_TASK_NAME, I2C1_TASK_STACK_SIZE, I2C1_TASK_PRIORITY, I2C1_TASK_CORE)); assert(m_i2c2_task = TaskMgr::getInstance().createTask(std::bind(&SensorService::run_uart, this), UART_TASK_NAME, UART_TASK_STACK_SIZE, UART_TASK_PRIORITY, UART_TASK_CORE)); pinMode(LIGHT_SENSOR_PIN, INPUT); } // Kalman filter variables static double kp_q = 0.5; // process noise static double kp_r = 32; // sensor noise static double kp_p = 1023; // estimation error static double kp_x = 0; // initial value // Pressure monitoring variables static uint64_t pressure_period_started = 0; static float pressure_filtered_min = 0; static float pressure_filtered_max = 0; static const uint64_t pressure_window_ms = 1000; // 1000ms = 1 second double getFilteredValue(double m) { if(kp_x == 0) kp_x=m; double k_k; kp_p = kp_p + kp_q; k_k = kp_p / (kp_p + kp_r); kp_x = kp_x + k_k * (m - kp_x); kp_p = (1 - k_k) * kp_p; return kp_x; } void::SensorService::processPressure(float pressure) { //static uint64_t previous = esp_timer_get_time(); // Get filtered pressure value using Kalman filter double filtered = getFilteredValue((double)pressure); double PRESSURE_THRESHOLD = 14.0; uint64_t now = esp_timer_get_time(); uint64_t now_ms = now / 1000; // Convert to milliseconds // Initialize pressure monitoring period if (pressure_period_started == 0) { pressure_period_started = now_ms; pressure_filtered_min = filtered; pressure_filtered_max = filtered; } else { // Track min and max during the window if (filtered > pressure_filtered_max) { pressure_filtered_max = filtered; //ESP_LOGE(TAG, "pressure_filtered_max: %0.3f", filtered); } if (filtered < pressure_filtered_min) { pressure_filtered_min = filtered; //ESP_LOGE(TAG, "pressure_filtered_min: %0.3f", filtered); } // Check if window period has elapsed if (now_ms > (pressure_period_started + pressure_window_ms)) { double change = 1000*abs(pressure_filtered_max - pressure_filtered_min); ESP_LOGE(TAG, "@P: %0.3f", change); // Check for significant pressure change (door detection) if (change > PRESSURE_THRESHOLD) { // Threshold of 1 //ESP_LOGE(TAG, "Door detected - pressure change: %0.3f", change); // Send MQTT notification struct MESSAGE_NOTIFY_PRESSURE msg; MQTT_MESSAGE_NOTIFY_PRESSURE(&msg); msg.value = change; // or you could send the change value m_app_if.getBuffer()->putBlock((uint8_t*)&msg, sizeof(msg)); //ESP_LOGE(TAG, "Door detected - pressure change: %0.3f, time: %u.%06u", change, msg.header.sec, msg.header.usec); // Convert epoch time to human-readable format time_t timestamp = msg.header.sec; struct tm *timeinfo = localtime(×tamp); char time_str[64]; strftime(time_str, sizeof(time_str), "%Y-%m-%d %H:%M:%S", timeinfo); ESP_LOGE(TAG, "Door detected - pressure change: %0.3f, time: %s.%06u", change, time_str, msg.header.usec); } // Reset for next window pressure_period_started = now_ms; pressure_filtered_min = filtered; pressure_filtered_max = filtered; } } //previous = now; // Optional: store current filtered value for other uses //m_pressure_value = filtered; } void SensorService::postBme68xData(float pressure, float temp) { uint8_t msg_buffer[sizeof(struct MESSAGE_SENSORS_BLOCK) + 10*sizeof(struct GAS_DATA)]; struct MESSAGE_SENSORS_BLOCK * msg = (struct MESSAGE_SENSORS_BLOCK *)msg_buffer; MQTT_MESSAGE_BLOCK_SENSOR(msg); msg->humidity = m_bme_data.humidity; msg->light = m_light_value; msg->pressure = pressure; msg->temperature = temp + SETTINGS.sensors.temperature.temp_offset; int num_total = 0; struct GAS_DATA * p = msg->data; for(int n = 0; n < 10; n++) { if(m_bme_data.measurement_bitmask & (1 << n)) { p[num_total].resistance = m_bme_data.measurement[n].resistance; // Use index 100 + (profile * 10) + measurement_index // Profile 0: indexes 100-109, Profile 1: indexes 110-119, etc. p[num_total++].index = 100 + (m_bme_data.current_profile * 10) + n; } } msg->num_data = num_total; m_app_if.getBuffer()->putBlock((uint8_t*)msg, sizeof(*msg) + num_total * sizeof(struct GAS_DATA)); // clear the blackboard memset(&m_bme_data, 0, sizeof(m_bme_data)); } void SensorService::synchronizedLightMeasurement(uint16_t light_value) { uint64_t now = esp_timer_get_time() / 1000; // Convert to milliseconds // Initialize measurement window if (m_light_measurement.window_start == 0) { m_light_measurement.window_start = now; m_light_measurement.sample_count = 0; m_light_measurement.leds_were_controlled = false; } // Add sample to current window (if we have space) if (m_light_measurement.sample_count < MAX_LIGHT_SAMPLES) { m_light_measurement.samples[m_light_measurement.sample_count] = light_value; m_light_measurement.sample_count++; } // Check if LEDs are controlled (for method selection) bool leds_off = m_app_if.getLed()->areLedsCurrentlyOff(); if (!leds_off) { m_light_measurement.leds_were_controlled = true; } // Process window when complete if (now >= (m_light_measurement.window_start + LIGHT_WINDOW_MS)) { uint16_t processed_value; static uint16_t last_processed_value = 0; if (m_light_measurement.leds_were_controlled) { // LEDs were on during this period - use minimum (original method) processed_value = findMinimum(m_light_measurement.samples, m_light_measurement.sample_count); ESP_LOGI(TAG, "Light (min method): %u, samples: %zu", processed_value, m_light_measurement.sample_count); } else { // LEDs were off - use filtered average double avg = calculateMovingAverage(m_light_measurement.samples, m_light_measurement.sample_count); processed_value = (uint16_t)avg; ESP_LOGI(TAG, "Light (avg method): %u, samples: %zu", processed_value, m_light_measurement.sample_count); } // Check for significant change if (last_processed_value > 0) { double change = abs((int)processed_value - (int)last_processed_value); double change_percent = (change / last_processed_value) * 100.0; double absolute_threshold = 4096 * 2.0 / 100.0; // 2% of ADC range (~82) double relative_threshold = 5.0; // 5% relative change bool significant_absolute = change > absolute_threshold; bool significant_relative = change_percent > relative_threshold; ESP_LOGI(TAG, "Light change: %0.1f (%0.2f%%) [abs_thresh:%0.1f, rel_thresh:%0.1f%%]", change, change_percent, absolute_threshold, relative_threshold); // Trigger notification for significant changes // Adjust thresholds based on your requirements if (significant_absolute && significant_relative) { ESP_LOGI(TAG, "Significant light change detected (both thresholds met)"); struct MESSAGE_NOTIFY_LIGHT msg; MQTT_MESSAGE_NOTIFY_LIGHT(&msg); msg.value = change; m_app_if.getBuffer()->putBlock((uint8_t*)&msg, sizeof(msg)); } else { ESP_LOGD(TAG, "Change not significant: abs=%s, rel=%s", significant_absolute ? "YES" : "NO", significant_relative ? "YES" : "NO"); } } last_processed_value = processed_value; m_light_value = processed_value; // Reset for next window m_light_measurement.window_start = now; m_light_measurement.sample_count = 0; m_light_measurement.leds_were_controlled = false; } } uint16_t SensorService::findMinimum(const uint16_t* samples, size_t count) { if (count == 0) return 0; uint16_t min_val = samples[0]; for (size_t i = 1; i < count; i++) { if (samples[i] < min_val) { min_val = samples[i]; } } return min_val; } double SensorService::calculateMovingAverage(const uint16_t* samples, size_t count, size_t avg_count) { if (count == 0) return 0.0; // Use the last 'avg_count' samples or all samples if fewer available size_t n = (avg_count < count) ? avg_count : count; size_t start_idx = count - n; uint32_t sum = 0; for (size_t i = start_idx; i < count; i++) { sum += samples[i]; } return (double)sum / n; } /// @brief Actual light value is minimum in 2s window /// @param light_value void SensorService::processLight(int light_value) { synchronizedLightMeasurement((uint16_t)light_value); } // handles pressure and voc sensor // Additional noise reduction techniques void SensorService::run_i2c_1() { // ADC configuration for better noise performance analogSetAttenuation(ADC_11db); // For 0-3.3V range analogSetWidth(12); // 12-bit resolution while(true) { if(m_bmp280->read(m_bmp_data.temp, m_bmp_data.pressure)) processPressure(m_bmp_data.pressure); bool bme_cycle_finished = m_bme68x->read(&m_bme_data); // Multiple ADC readings for noise reduction uint32_t light_sum = 0; const int num_readings = 4; for (int i = 0; i < num_readings; i++) { light_sum += analogRead(LIGHT_SENSOR_PIN); delayMicroseconds(100); // Small delay between readings } uint16_t read_light_val = light_sum / num_readings; if(bme_cycle_finished) postBme68xData(m_bmp_data.pressure, m_bmp_data.temp); processLight(read_light_val); delay(10); } } // handles radar only void SensorService::run_uart() { int64_t last_read = esp_timer_get_time(); while(true) { bool has_read = m_ld2410->read(); if(has_read && esp_timer_get_time() - last_read >= ms_to_us(10000-50)) { int64_t now = esp_timer_get_time(); // ESP_LOGI(TAG, "count %d", (int)m_ld2410->stationary_energy[0]); if(m_ld2410->stationary_energy[0] != 0) { struct MESSAGE_RADAR_BLOCK msg; MQTT_MESSAGE_BLOCK_RADAR(&msg); for(int n = 0; n < 24; n++) { if(n < 14) msg.vals[n] = m_ld2410->motion_energy[n] > 0xffff ? 0xffff : (uint16_t)m_ld2410->motion_energy[n]; else msg.vals[n] = m_ld2410->stationary_energy[n-14] > 0xffff ? 0xffff : (uint16_t)m_ld2410->stationary_energy[n-14]; } m_app_if.getBuffer()->putBlock((uint8_t*)&msg, sizeof(msg)); // ESP_LOGI(TAG, "delta t: %lld", (now - last_read)/1000); last_read = now; #if 0 ESP_LOGI("stationary energy", "%0.0f %0.0f %0.0f %0.0f %0.0f %0.0f %0.0f", m_ld2410->stationary_energy[3]/m_ld2410->stationary_energy[0], m_ld2410->stationary_energy[4]/m_ld2410->stationary_energy[0], m_ld2410->stationary_energy[5]/m_ld2410->stationary_energy[0], m_ld2410->stationary_energy[6]/m_ld2410->stationary_energy[0], m_ld2410->stationary_energy[7]/m_ld2410->stationary_energy[0], m_ld2410->stationary_energy[8]/m_ld2410->stationary_energy[0], m_ld2410->stationary_energy[9]/m_ld2410->stationary_energy[0]); ESP_LOGW("motion energy", "%0.0f %0.0f %0.0f %0.0f %0.0f %0.0f %0.0f %0.0f %0.0f", m_ld2410->motion_energy[5]/m_ld2410->motion_energy[0], m_ld2410->motion_energy[6]/m_ld2410->motion_energy[0], m_ld2410->motion_energy[7]/m_ld2410->motion_energy[0], m_ld2410->motion_energy[8]/m_ld2410->motion_energy[0], m_ld2410->motion_energy[9]/m_ld2410->motion_energy[0], m_ld2410->motion_energy[10]/m_ld2410->motion_energy[0], m_ld2410->motion_energy[11]/m_ld2410->motion_energy[0], m_ld2410->motion_energy[12]/m_ld2410->motion_energy[0], m_ld2410->motion_energy[13]/m_ld2410->motion_energy[0]); #endif m_ld2410->resetGates(); } } if(has_read) // next read will happen in 100ms. sleep untill just before then. delay(95); } }