405 lines
13 KiB
C++
405 lines
13 KiB
C++
/// © 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;
|
|
p[num_total++].index = 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);
|
|
}
|
|
} |