diff --git a/devices/sensor/ld2420_radar.cpp b/devices/sensor/ld2420_radar.cpp index 3a8b34a..034c6d4 100644 --- a/devices/sensor/ld2420_radar.cpp +++ b/devices/sensor/ld2420_radar.cpp @@ -1,267 +1,326 @@ #include "ld2420_radar.h" +#include +#include +#include -/* ------------------------------------------------------------------ */ -/* ------------------------ Инициализация --------------------------*/ -/* ------------------------------------------------------------------ */ +/* + ========================================================= + LD2420 Radar — реализация + Протокол: Energy mode (бинарный фрейм, fw >= v1.5.4) -void Ld2420Radar::begin(Stream &uart, const RadarConfig &cfg) { - _uart = &uart; - _cfg = cfg; + Фрейм данных, 31 байт: + [0..3] Header F4 F3 F2 F1 + [4..5] Length uint16_t LE + [6] Presence 0=нет 1=движение 2=стационарный + [7..8] Distance uint16_t LE (сантиметры) + [9..24] Gate[0..15] uint8_t + [25..26] Padding 00 00 + [27..30] Footer F8 F7 F6 F5 - /* Установим параметры UART */ - pinMode(_cfg.uart_rx_pin, INPUT_PULLUP); - pinMode(_cfg.uart_tx_pin, OUTPUT); - // На ESP32 Serial2 уже привязан к этим пинам, но оставляем для совместимости + Источник формата: ESPHome ld2420.cpp source + ========================================================= +*/ + +/* -------- constexpr definitions ------------------------- */ + +constexpr uint8_t Ld2420Radar::HDR[4]; +constexpr uint8_t Ld2420Radar::FTR[4]; + + +/* -------- begin ----------------------------------------- */ + +void Ld2420Radar::begin(HardwareSerial& serial, const RadarConfig& config) { + _serial = &serial; + _cfg = config; + + /* Ограничиваем окна */ + if (_cfg.activity_avg_window_s < 1) _cfg.activity_avg_window_s = 1; + if (_cfg.activity_avg_window_s > ACT_AVG_MAX) _cfg.activity_avg_window_s = ACT_AVG_MAX; + if (_cfg.activity_trend_window_min < 1) _cfg.activity_trend_window_min = 1; + if (_cfg.activity_trend_window_min > TREND_MAX) _cfg.activity_trend_window_min = TREND_MAX; + if (_cfg.total_energy_max == 0) _cfg.total_energy_max = 1; + + memset(_gate_energy, 0, sizeof(_gate_energy)); + memset(_act_avg_buf, 0, sizeof(_act_avg_buf)); + memset(_trend_buf, 0, sizeof(_trend_buf)); + memset(_rx_buf, 0, sizeof(_rx_buf)); + + serial.begin(_cfg.baud_rate, SERIAL_8N1, + (int)_cfg.uart_rx_pin, + (int)_cfg.uart_tx_pin); } -/* ------------------------------------------------------------------ */ -/* -------------------------- Основной цикл ------------------------*/ -/* ------------------------------------------------------------------ */ + +/* -------- update ---------------------------------------- */ void Ld2420Radar::update() { - if (!_uart) return; + if (!_serial) return; - /* Читаем все доступные байты */ - while (_uart->available()) { - char c = _uart->read(); - if (c == '\n' || c == '\r') { - if (_line_pos > 0) { - _parse_line(_line_buf, _line_pos); - _line_pos = 0; - } - } else { - if (_line_pos < LINE_BUF_SIZE - 1) { - _line_buf[_line_pos++] = c; - } else { - /* Буфер переполнен – сбрасываем */ - _line_pos = 0; - } - } + while (_serial->available()) { + _feed_byte((uint8_t)_serial->read()); } - _update_online_state(); + uint32_t now_ms = millis(); + + /* online */ + _online = (_last_frame_ms > 0) && + (now_ms - _last_frame_ms < _cfg.stale_after_ms); + + /* presence hold */ + if (_last_on_ms > 0 && (now_ms - _last_on_ms < _cfg.presence_hold_ms)) { + _presence = true; + } else { + _presence = false; + } + + _tick_activity_avg(now_ms); + _tick_trend(now_ms); } -/* ------------------------------------------------------------------ */ -/* ------------------------ Парсинг строки -----------------------*/ -/* ------------------------------------------------------------------ */ -void Ld2420Radar::_parse_line(const char *line, size_t len) { - if (len < 5) return; // минимальная длина +, +/* -------- _feed_byte ------------------------------------- */ - /* Время прихода */ - _last_line_ms = millis(); +/* + Стратегия синхронизации: + 1. Ищем заголовок F4 F3 F2 F1 побайтово. + 2. После его захвата накапливаем оставшиеся (FRAME_SIZE - 4) байт. + 3. Проверяем футер. Если неверен — сбрасываем и ищем снова. +*/ - /* Если строка начинается с '+' – это данные */ - if (line[0] == '+') { - const char *p = line + 1; - uint16_t zone = 0; - float distance_m = NAN, speed_m_s = NAN; +void Ld2420Radar::_feed_byte(uint8_t b) { + if (!_synced) { + /* Сдвигаем первые 4 байта в поисках заголовка */ + _rx_buf[0] = _rx_buf[1]; + _rx_buf[1] = _rx_buf[2]; + _rx_buf[2] = _rx_buf[3]; + _rx_buf[3] = b; - /* Парсим */ - while (*p && *p != ',') { - if (!isdigit(*p)) return; // Неверный формат - zone = zone * 10 + (*p - '0'); - ++p; + if (_rx_buf[0] == HDR[0] && + _rx_buf[1] == HDR[1] && + _rx_buf[2] == HDR[2] && + _rx_buf[3] == HDR[3]) { + _synced = true; + _rx_idx = 4; // заголовок уже в буфере } - if (*p != ',') return; - ++p; - - /* Парсим (число с плавающей точкой) */ - char dist_str[16]; - size_t i = 0; - while (*p && *p != ',' && i < sizeof(dist_str)-1) { - dist_str[i++] = *p++; - } - if (*p != ',') return; - ++p; - dist_str[i] = '\0'; - distance_m = atof(dist_str); - - /* Парсим (число с плавающей точкой) */ - char speed_str[16]; - i = 0; - while (*p && i < sizeof(speed_str)-1) { - speed_str[i++] = *p++; - } - speed_str[i] = '\0'; - speed_m_s = atof(speed_str); - - /* Обновляем состояние */ - _update_presence_state(zone); - _update_distance_speed(distance_m, speed_m_s); - } else if (line[0] == '-') { - /* Ошибка от радара – просто игнорируем */ return; - } else { - /* Неизвестная строка */ - if (_cfg.enable_debug_unknown_lines) { - Serial.printf("Unknown radar line: %.*s\n", (int)len, line); + } + + /* Накапливаем остаток фрейма */ + _rx_buf[_rx_idx++] = b; + + if (_rx_idx >= FRAME_SIZE) { + if (_validate_footer()) { + _process_frame(); + } else { + if (_cfg.enable_debug_frames) { + Serial.print("[LD2420] bad footer: "); + for (uint8_t i = 0; i < FRAME_SIZE; i++) { + char hex[4]; + snprintf(hex, sizeof(hex), "%02X ", _rx_buf[i]); + Serial.print(hex); + } + Serial.println(); + } } + /* В любом случае ищем следующий заголовок */ + _synced = false; + _rx_idx = 0; + memset(_rx_buf, 0, sizeof(_rx_buf)); } } -/* ------------------------------------------------------------------ */ -/* ----------------------- Обновление online ----------------------*/ -/* ------------------------------------------------------------------ */ -void Ld2420Radar::_update_online_state() { - uint32_t now = millis(); - _state.online = (now - _last_line_ms <= _cfg.stale_after_ms); +/* -------- _validate_footer ------------------------------ */ + +bool Ld2420Radar::_validate_footer() const { + return _rx_buf[FRAME_FOOTER_OFF + 0] == FTR[0] && + _rx_buf[FRAME_FOOTER_OFF + 1] == FTR[1] && + _rx_buf[FRAME_FOOTER_OFF + 2] == FTR[2] && + _rx_buf[FRAME_FOOTER_OFF + 3] == FTR[3]; } -/* ------------------------------------------------------------------ */ -/* --------------------- Обновление presence ---------------------*/ -/* ------------------------------------------------------------------ */ -void Ld2420Radar::_update_presence_state(uint16_t zone) { - if (zone < _cfg.min_valid_zone || zone > _cfg.max_valid_zone) return; +/* -------- _process_frame -------------------------------- */ - /* Если пришел первый сигнал, запоминаем время */ - if (!_state.presence) { - _presence_start_ms = millis(); - } - _state.presence = true; -} +void Ld2420Radar::_process_frame() { + uint32_t now_ms = millis(); + _last_frame_ms = now_ms; -/* ------------------------------------------------------------------ */ -/* -------------------- Обновление расстояния/скорости ----------*/ -/* ------------------------------------------------------------------ */ + /* Поле Presence */ + uint8_t presence_raw = _rx_buf[FRAME_PRESENCE_OFF]; + bool has_target = (presence_raw == 0x01 || presence_raw == 0x02); -void Ld2420Radar::_update_distance_speed(float distance_m, float speed_m_s) { - /* Сглаживаем дистанцию */ - if (isnan(_state.distance_m)) _state.distance_m = distance_m; - else - _state.distance_m = - _cfg.distance_ema_alpha * distance_m + - (1.0f - _cfg.distance_ema_alpha) * _state.distance_m; - - /* Сглаживаем скорость */ - float speed_smooth = 0.0f; - if (!isnan(speed_m_s)) { - static float prev_speed = 0.0f; - speed_smooth = - _cfg.speed_ema_alpha * speed_m_s + - (1.0f - _cfg.speed_ema_alpha) * prev_speed; - prev_speed = speed_smooth; + if (has_target) { + _last_on_ms = now_ms; } - /* Обновляем активность */ - _calc_activity_score_current(); - /* Сохраняем в историю */ - _activity_hist[_hist_idx++] = _state.activity_score_current; - if (_hist_idx >= ACTIVITY_HISTORY) { - _hist_idx = 0; - _hist_filled = true; + /* Дистанция (uint16_t LE, сантиметры → метры) */ + uint16_t dist_cm = (uint16_t)(_rx_buf[FRAME_DIST_OFFSET]) | + (uint16_t)(_rx_buf[FRAME_DIST_OFFSET + 1]) << 8; + float raw_m = (float)dist_cm / 100.0f; + + if (has_target && dist_cm > 0) { + if (_dist_ema < 0.0f) { + _dist_ema = raw_m; + } else { + _dist_ema = _cfg.distance_ema_alpha * raw_m + + (1.0f - _cfg.distance_ema_alpha) * _dist_ema; + } + _distance_m = _dist_ema; + } else if (!has_target) { + _dist_ema = -1.0f; + _distance_m = 0.0f; } - /* Вычисляем среднюю активность за минуту (последние N значений) */ - _calc_activity_score_average(); - _determine_dynamics(); -} + /* Энергии ворот Gate[0..15] */ + uint32_t total_energy = 0; + for (uint8_t g = 0; g < LD2420_TOTAL_GATES; g++) { + _gate_energy[g] = _rx_buf[FRAME_GATE_OFFSET + g]; + total_energy += _gate_energy[g]; + } -/* ------------------------------------------------------------------ */ -/* -------------------- Расчёт текущей активности ----------------*/ -/* ------------------------------------------------------------------ */ - -void Ld2420Radar::_calc_activity_score_current() { - /* Прямое линейное преобразование скорости к шкале 0…10 */ - float speed = _cfg.speed_ema_alpha * _state.distance_m; // placeholder - if (speed < _cfg.activity_min_speed_m_s) { - _state.activity_score_current = 0; - } else if (speed >= _cfg.activity_max_speed_m_s) { - _state.activity_score_current = 10; + /* + activity_score_current: + Суммарная энергия по всем воротам нормируется к 0–10. + total_energy_max соответствует score = 10. + */ + if (total_energy == 0) { + _activity_score_current = 0; } else { - float v = (speed - _cfg.activity_min_speed_m_s) / - (_cfg.activity_max_speed_m_s - _cfg.activity_min_speed_m_s); - _state.activity_score_current = round(v * 10.0f); + float score_f = (float)total_energy / (float)_cfg.total_energy_max * 10.0f; + if (score_f > 10.0f) score_f = 10.0f; + _activity_score_current = (uint8_t)(score_f + 0.5f); + } + + if (_cfg.enable_debug_frames) { + Serial.print("[LD2420] pres="); + Serial.print(presence_raw); + Serial.print(" dist_cm="); + Serial.print(dist_cm); + Serial.print(" energy_sum="); + Serial.print(total_energy); + Serial.print(" score_cur="); + Serial.println(_activity_score_current); } } -/* ------------------------------------------------------------------ */ -/* --------------------- Средняя активность за минуту -------------*/ -/* ------------------------------------------------------------------ */ -void Ld2420Radar::_calc_activity_score_average() { - if (!_hist_filled && _hist_idx == 0) return; // не хватает данных +/* -------- _tick_activity_avg ---------------------------- */ - uint8_t sum = 0; - uint8_t cnt = _hist_filled ? ACTIVITY_HISTORY : _hist_idx; - for (uint8_t i = 0; i < cnt; ++i) - sum += _activity_hist[i]; - _state.activity_score = round((float)sum / cnt); +void Ld2420Radar::_tick_activity_avg(uint32_t now_ms) { + if (_act_avg_last_ms == 0) { _act_avg_last_ms = now_ms; return; } + if (now_ms - _act_avg_last_ms < 1000) return; + _act_avg_last_ms = now_ms; + + uint8_t win = _cfg.activity_avg_window_s; + _act_avg_buf[_act_avg_idx] = _activity_score_current; + _act_avg_idx = (_act_avg_idx + 1) % win; + if (_act_avg_count < win) _act_avg_count++; + + uint32_t sum = 0; + for (uint8_t i = 0; i < _act_avg_count; i++) sum += _act_avg_buf[i]; + _activity_score = (uint8_t)(sum / _act_avg_count); } -/* ------------------------------------------------------------------ */ -/* ----------------------- Динамика активности --------------------*/ -/* ------------------------------------------------------------------ */ -void Ld2420Radar::_determine_dynamics() { - /* Сравниваем последние два значения (или средние) */ - uint8_t last2[ACTIVITY_HISTORY]; - if (_hist_filled) { - for (int i = 0; i < ACTIVITY_HISTORY; ++i) - last2[i] = _activity_hist[(ACTIVITY_HISTORY - 1 - i)]; +/* -------- _tick_trend ----------------------------------- */ + +void Ld2420Radar::_tick_trend(uint32_t now_ms) { + if (_trend_last_ms == 0) { _trend_last_ms = now_ms; return; } + if (now_ms - _trend_last_ms < 60000u) return; + _trend_last_ms = now_ms; + + uint8_t win = _cfg.activity_trend_window_min; + _trend_buf[_trend_idx] = _activity_score; + _trend_idx = (_trend_idx + 1) % win; + if (_trend_count < win) _trend_count++; + + _compute_trend(); +} + + +/* -------- _compute_trend -------------------------------- */ + +void Ld2420Radar::_compute_trend() { + uint8_t n = _trend_count; + if (n < 2) { _trend_slope = 0; _trend_std_dev = 0.0f; return; } + + uint8_t win = _cfg.activity_trend_window_min; + + float sum_x = 0, sum_y = 0, sum_xx = 0, sum_xy = 0, mean_y = 0; + + for (uint8_t i = 0; i < n; i++) { + /* Хронологический порядок: старый → новый */ + uint8_t buf_i = (uint8_t)((_trend_idx - n + i + win) % win); + float x = (float)i; + float y = (float)_trend_buf[buf_i]; + sum_x += x; + sum_y += y; + sum_xx += x * x; + sum_xy += x * y; + mean_y += y; + } + + float fn = (float)n; + float denom = fn * sum_xx - sum_x * sum_x; + + if (fabsf(denom) < 1e-6f) { + _trend_slope = 0; } else { - for (uint8_t i = 0; i < _hist_idx; ++i) - last2[i] = _activity_hist[(_hist_idx - 1 - i)]; + float slope = (fn * sum_xy - sum_x * sum_y) / denom; + /* Значимый порог: ±0.30 балл/мин */ + if (slope > 0.30f) _trend_slope = 1; + else if (slope < -0.30f) _trend_slope = -1; + else _trend_slope = 0; } - /* Считаем среднее за последние 5 значений */ - uint8_t cnt = min(5, ACTIVITY_HISTORY); - uint8_t avg_prev = 0, avg_curr = 0; - for (uint8_t i = 0; i < cnt; ++i) { - avg_prev += last2[i]; - if (i >= cnt/2) avg_curr += last2[i]; + /* Стандартное отклонение для детектирования "variable" */ + mean_y /= fn; + float var = 0.0f; + for (uint8_t i = 0; i < n; i++) { + uint8_t buf_i = (uint8_t)((_trend_idx - n + i + win) % win); + float d = (float)_trend_buf[buf_i] - mean_y; + var += d * d; } - avg_prev /= cnt; - avg_curr /= cnt; - - /* Выбираем динамику */ - if (avg_curr > avg_prev + 1) - _state.activity_score_dynamics = "increasing"; - else if (avg_curr < avg_prev - 1) - _state.activity_score_dynamics = "decreasing"; - else - _state.activity_score_dynamics = "constant"; + _trend_std_dev = sqrtf(var / fn); } -/* ------------------------------------------------------------------ */ -/* -------------------------- JSON‑вывод --------------------------*/ -/* ------------------------------------------------------------------ */ -String Ld2420Radar::get_state_json() const { - String s; - s.reserve(200); - s += "{"; +/* -------- get_activity_dynamics ------------------------- */ - s += "\"online\": "; - s += _state.online ? "true" : "false"; - s += ", "; +const char* Ld2420Radar::get_activity_dynamics() const { + if (_trend_count < 2) return "constant"; - s += "\"presence\": "; - s += _state.presence ? "true" : "false"; - s += ", "; + /* + "variable" = высокий разброс без чёткого тренда. + Порог StdDev > 2.5 при нулевом или слабом слоупе. + */ + if (_trend_std_dev > 2.5f && _trend_slope == 0) return "variable"; - s += "\"activity_score\": "; - s += String(_state.activity_score); - s += ", "; - - s += "\"activity_score_current\": "; - s += String(_state.activity_score_current); - s += ", "; - - s += "\"activity_score_dynamics\": \""; - s += _state.activity_score_dynamics; - s += "\", "; - - s += "\"distance_m\": "; - if (isnan(_state.distance_m)) { - s += "null"; - } else { - s += String(_state.distance_m, 2); // 2 знака после запятой - } - - s += "}"; - return s; + if (_trend_slope > 0) return "increasing"; + else if (_trend_slope < 0) return "decreasing"; + else return "constant"; } + + +/* -------- get_state_json -------------------------------- */ + +const char* Ld2420Radar::get_state_json() { + snprintf(_json_buf, sizeof(_json_buf), + "{" + "\"online\":%s," + "\"presence\":%s," + "\"activity_score\":%u," + "\"activity_score_current\":%u," + "\"activity_score_dynamics\":\"%s\"," + "\"distance_m\":%.2f" + "}", + _online ? "true" : "false", + _presence ? "true" : "false", + (unsigned)_activity_score, + (unsigned)_activity_score_current, + get_activity_dynamics(), + _distance_m + ); + return _json_buf; +} \ No newline at end of file diff --git a/devices/sensor/ld2420_radar.h b/devices/sensor/ld2420_radar.h index fe28b64..27b4831 100644 --- a/devices/sensor/ld2420_radar.h +++ b/devices/sensor/ld2420_radar.h @@ -1,104 +1,164 @@ #pragma once #include -#include +#include -/* ------------------------------------------------------------------ */ -/* ----------------------- Конфигурация радара ---------------------*/ -/* ------------------------------------------------------------------ */ +/* + ========================================================= + LD2420 Radar — конфигурация + Модуль: LD2420 v2.1 + Режим: Energy mode (бинарный фрейм, fw >= v1.5.4) + + Формат фрейма данных (Energy mode, 31 байт): + Offset Size Поле + 0 4 Header: F4 F3 F2 F1 + 4 2 Length: uint16_t LE (обычно 0x0023) + 6 1 Presence: 0=нет, 1=движение, 2=стационарный + 7 2 Distance: uint16_t LE, сантиметры + 9 16 Gate[0..15]: uint8_t, энергия каждых ворот + 25 2 Padding: 00 00 + 27 4 Footer: F8 F7 F6 F5 + Итого: 31 байт + ========================================================= +*/ + +static constexpr uint8_t LD2420_TOTAL_GATES = 16; struct RadarConfig { - uint8_t uart_rx_pin; - uint8_t uart_tx_pin; - uint32_t baud_rate = 115200; + /* UART пины */ + uint8_t uart_rx_pin = 4; + uint8_t uart_tx_pin = 15; + uint32_t baud_rate = 115200; - /* Хранение данных */ - uint16_t presence_hold_ms = 1500; // Как долго держать флаг «presence» после последнего сигнала - uint16_t stale_after_ms = 2000; // Если не пришло ни одной строки – считаем offline + /* Удерживать presence после пропадания данных */ + uint32_t presence_hold_ms = 1500; - /* Фильтры */ - uint8_t median_window_size = 5; - uint8_t max_zone_step_per_sample = 4; + /* Считать online пока данные свежее stale_after_ms */ + uint32_t stale_after_ms = 2000; - /* EMA‑сглаживание (α) */ - float distance_ema_alpha = 0.35f; - float speed_ema_alpha = 0.25f; + /* EMA для дистанции (0 < α ≤ 1; больше = быстрее, меньше = плавнее) */ + float distance_ema_alpha = 0.35f; - /* Диапазон зон, которые считаем «действительными» */ - uint16_t min_valid_zone = 1; - uint16_t max_valid_zone = 200; + /* + Расстояние одних ворот в метрах. + Для LD2420 при стандартной конфигурации ≈ 0.70 м. + Подгони по реальным замерам. + */ + float gate_size_m = 0.70f; - /* Перевод зоны в метры (параметры калибровки) */ - float zone_to_meter_k = 0.7f; // slope - float zone_to_meter_b = 0.0f; // intercept + /* + Максимальная суммарная энергия по всем воротам, + соответствующая activity_score_current = 10. + Калибруй под реальную комнату и типичное движение. + */ + uint32_t total_energy_max = 2000; - /* Шкала активности: скорость, с которой считается «активным» */ - float activity_min_speed_m_s = 0.03f; - float activity_max_speed_m_s = 1.20f; + /* Окно усреднения activity_score, секунды (1–60) */ + uint8_t activity_avg_window_s = 60; - bool enable_debug_unknown_lines = false; + /* Окно тренда для dynamics, минуты (1–10) */ + uint8_t activity_trend_window_min = 10; + + bool enable_debug_frames = false; }; -/* ------------------------------------------------------------------ */ -/* -------------------------- Состояние радара ---------------------*/ -/* ------------------------------------------------------------------ */ -struct RadarState { - bool online = false; // Приём данных - bool presence = false; // Присутствует человек - uint8_t activity_score = 0; // Средняя активность за минуту (0…10) - uint8_t activity_score_current = 0; // Активность в момент запроса - const char *activity_score_dynamics = "constant"; - float distance_m = NAN; -}; - -/* ------------------------------------------------------------------ */ -/* ------------------------ Основной класс радара -------------------*/ -/* ------------------------------------------------------------------ */ +/* + ========================================================= + Ld2420Radar — публичный интерфейс + ========================================================= +*/ class Ld2420Radar { public: - /* Инициализация */ - void begin(Stream &uart, const RadarConfig &cfg); + Ld2420Radar() = default; - /* Обновление состояния – вызывать каждый цикл (или в таймере) */ + /* Вызвать один раз в setup() */ + void begin(HardwareSerial& serial, const RadarConfig& config); + + /* Вызывать каждый loop() */ void update(); - /* Получить JSON‑строку текущего состояния */ - String get_state_json() const; + /* JSON-снимок состояния (статический буфер, валиден до следующего вызова) */ + const char* get_state_json(); + + /* ---- Прямой доступ к полям ---- */ + bool is_online() const { return _online; } + bool is_presence() const { return _presence; } + uint8_t get_activity_score() const { return _activity_score; } + uint8_t get_activity_score_current() const { return _activity_score_current; } + float get_distance_m() const { return _distance_m; } + + /* "constant" | "increasing" | "decreasing" | "variable" */ + const char* get_activity_dynamics() const; + + /* Сырые уровни энергии ворот 0–15 из последнего фрейма */ + uint8_t get_gate_energy(uint8_t gate) const { + return (gate < LD2420_TOTAL_GATES) ? _gate_energy[gate] : 0; + } private: - /* Внутренние данные */ - Stream * _uart = nullptr; - RadarConfig _cfg; - RadarState _state{}; + /* ---- Конфигурация и железо ---- */ + HardwareSerial* _serial = nullptr; + RadarConfig _cfg; - /* Тайминг */ - uint32_t _last_line_ms = 0; // Когда пришла последняя строка - uint32_t _presence_start_ms = 0; // Когда появился первый сигнал + /* ---- Бинарный парсер фрейма ---- */ + static constexpr uint8_t FRAME_SIZE = 31; + static constexpr uint8_t FRAME_PRESENCE_OFF = 6; + static constexpr uint8_t FRAME_DIST_OFFSET = 7; + static constexpr uint8_t FRAME_GATE_OFFSET = 9; + static constexpr uint8_t FRAME_FOOTER_OFF = 27; - /* Буфер для чтения строки */ - static const size_t LINE_BUF_SIZE = 64; - char _line_buf[LINE_BUF_SIZE]; - size_t _line_pos = 0; + /* Байты заголовка */ + static constexpr uint8_t HDR[4] = {0xF4, 0xF3, 0xF2, 0xF1}; + /* Байты футера */ + static constexpr uint8_t FTR[4] = {0xF8, 0xF7, 0xF6, 0xF5}; - /* Хранение последних N оценок активности (N=10) */ - static const uint8_t ACTIVITY_HISTORY = 10; - uint8_t _activity_hist[ACTIVITY_HISTORY] = {0}; - uint8_t _hist_idx = 0; - bool _hist_filled = false; + uint8_t _rx_buf[FRAME_SIZE] = {}; + uint8_t _rx_idx = 0; + bool _synced = false; // заголовок пойман, накапливаем оставшиеся байты - /* Публичные вспомогательные функции (для тестов) */ - void _parse_line(const char *line, size_t len); - void _update_online_state(); - void _update_presence_state(uint16_t zone); - void _update_distance_speed(float distance_m, float speed_m_s); - void _calc_activity_score_current(); - void _calc_activity_score_average(); - void _determine_dynamics(); + void _feed_byte(uint8_t b); + bool _validate_footer() const; + void _process_frame(); - /* Утилиты */ - static bool _is_hex_digit(char c) { - return (c >= '0' && c <= '9') || (c >= 'A' && c <= 'F'); - } -}; + /* ---- Данные последнего фрейма ---- */ + uint8_t _gate_energy[LD2420_TOTAL_GATES] = {}; + float _dist_ema = -1.0f; + float _prev_dist_m = -1.0f; + + /* ---- Онлайн / присутствие ---- */ + uint32_t _last_frame_ms = 0; + uint32_t _last_on_ms = 0; + bool _online = false; + bool _presence = false; + + /* ---- Дистанция ---- */ + float _distance_m = 0.0f; + + /* ---- activity_score_current ---- */ + uint8_t _activity_score_current = 0; + + /* ---- activity_score (скользящее среднее 1 сэмпл/с, окно 60 с) ---- */ + static constexpr uint8_t ACT_AVG_MAX = 60; + uint8_t _act_avg_buf[ACT_AVG_MAX] = {}; + uint8_t _act_avg_idx = 0; + uint8_t _act_avg_count = 0; + uint32_t _act_avg_last_ms = 0; + uint8_t _activity_score = 0; + void _tick_activity_avg(uint32_t now_ms); + + /* ---- Тренд активности (1 точка/мин, окно 10 мин) ---- */ + static constexpr uint8_t TREND_MAX = 10; + uint8_t _trend_buf[TREND_MAX] = {}; + uint8_t _trend_idx = 0; + uint8_t _trend_count = 0; + uint32_t _trend_last_ms = 0; + int8_t _trend_slope = 0; // 1 = рост, 0 = плоско, -1 = спад + float _trend_std_dev = 0.0f; + void _tick_trend(uint32_t now_ms); + void _compute_trend(); + + /* ---- JSON-буфер ---- */ + char _json_buf[200]; +}; \ No newline at end of file diff --git a/devices/sensor/sensor.ino b/devices/sensor/sensor.ino index 199af5e..48e2242 100644 --- a/devices/sensor/sensor.ino +++ b/devices/sensor/sensor.ino @@ -1,18 +1,13 @@ -/********************************************************************* - * sensor.ino - * - * Проект «Radar + BH1750 + BME/BMP280» – полный пример. - * В нём используется новый класс Ld2420Radar (файлы ld2420_radar.h/.cpp). - *********************************************************************/ +#include "ld2420_radar.h" +#include "bh1750_sensor.h" +#include "bme280_sensor.h" -#include +/* + ========================= + GLOBAL OBJECTS + ========================= +*/ -/* ---------- СТАНДАРТНЫЕ БИБЛИОТЕКИ ДЛЯ ИНТЕРФЕЙСОВ ------------- */ -#include "ld2420_radar.h" // радар LD‑2420 (2.1) -#include "bh1750_sensor.h" // BH1750 – датчик освещённости -#include "bme280_sensor.h" // BME280/BMP280 – климатический датчик - -/* --------------------- ГЛОБАЛЬНЫЕ ОБЪЕКТЫ ---------------------- */ Ld2420Radar radar; RadarConfig radar_config; @@ -22,88 +17,114 @@ Bme280Sensor climate_sensor; Bme280Config climate_config; -/* ----------------- Отдельные I2C‑шины (ESP32) ------------------- */ -TwoWire i2c_climate = TwoWire(1); // шина для BME/BMP280 +/* + Отдельные I2C-шины. + 0-я шина для BH1750. + 1-я шина для BME/BMP280. +*/ +TwoWire i2c_climate = TwoWire(1); -/* ----------------- Интервал печати состояния ------------------- */ +/* + Интервал печати состояния. +*/ static const uint32_t print_interval_ms = 500; uint32_t last_print_ms = 0; -/* ------------------------------------------------------------------ */ -/* --------------------- Конфигурация радара ----------------------*/ -/* ------------------------------------------------------------------ */ + +/* + ========================= + RADAR CONFIGURATION + ========================= +*/ void setup_radar_config() { - /* Пины UART2 (по умолчанию GPIO4 – OT1, GPIO15 – RX) */ - radar_config.uart_rx_pin = 4; - radar_config.uart_tx_pin = 15; - radar_config.baud_rate = 115200; + radar_config.uart_rx_pin = 4; + radar_config.uart_tx_pin = 15; + radar_config.baud_rate = 115200; - radar_config.presence_hold_ms = 1500; // как долго держать presence - radar_config.stale_after_ms = 2000; // если не пришло данных – offline + radar_config.presence_hold_ms = 1500; + radar_config.stale_after_ms = 2000; + radar_config.distance_ema_alpha = 0.35f; - radar_config.median_window_size = 5; - radar_config.max_zone_step_per_sample = 4; + radar_config.gate_size_m = 0.70f; // уточни по реальным замерам + radar_config.total_energy_max = 2000; // откалибруй под помещение - radar_config.distance_ema_alpha = 0.35f; - radar_config.speed_ema_alpha = 0.25f; + radar_config.activity_avg_window_s = 60; + radar_config.activity_trend_window_min = 10; - radar_config.min_valid_zone = 1; - radar_config.max_valid_zone = 200; - - radar_config.zone_to_meter_k = 0.7f; // калибровка зоны → м - radar_config.zone_to_meter_b = 0.0f; - - radar_config.activity_min_speed_m_s = 0.03f; - radar_config.activity_max_speed_m_s = 1.20f; - - radar_config.enable_debug_unknown_lines = false; + radar_config.enable_debug_frames = false; } -/* ------------------------------------------------------------------ */ -/* --------------------- Конфигурация BH1750 ----------------------*/ -/* ------------------------------------------------------------------ */ + +/* + ========================= + BH1750 CONFIGURATION + ========================= +*/ void setup_light_config() { - light_config.sda_pin = 16; // SDA - light_config.scl_pin = 17; // SCL - light_config.i2c_address = 0x5C; - light_config.measurement_mode = 0x10; // Continuous High Resolution - light_config.stale_after_ms = 2000; - light_config.read_interval_ms = 200; - light_config.lux_ema_alpha = 0.25f; + /* + Отдельная шина освещённости: + SDA -> GPIO16 + SCL -> GPIO17 + ADDR -> VCC -> 0x5C + */ + light_config.sda_pin = 16; + light_config.scl_pin = 17; + light_config.i2c_address = 0x5C; + light_config.measurement_mode = 0x10; + light_config.stale_after_ms = 2000; + light_config.read_interval_ms = 200; + light_config.lux_ema_alpha = 0.25f; } -/* ------------------------------------------------------------------ */ -/* --------------------- Конфигурация BME/BMP280 -------------------*/ -/* ------------------------------------------------------------------ */ + +/* + ========================= + BME/BMP280 CONFIGURATION + ========================= +*/ void setup_climate_config() { - climate_config.sda_pin = 18; // SDA - climate_config.scl_pin = 19; // SCL - climate_config.i2c_address = 0x76; // адрес при подключении к GND + /* + Отдельная климатическая шина: + SDA -> GPIO18 + SCL -> GPIO19 + SDO -> GND => 0x76 + SDO -> VCC => 0x77 + */ + climate_config.sda_pin = 18; + climate_config.scl_pin = 19; + climate_config.i2c_address = 0x76; - climate_config.stale_after_ms = 3000; - climate_config.read_interval_ms = 1000; + climate_config.stale_after_ms = 3000; + climate_config.read_interval_ms = 1000; climate_config.temperature_ema_alpha = 0.25f; - climate_config.pressure_ema_alpha = 0.20f; - climate_config.humidity_ema_alpha = 0.20f; + climate_config.pressure_ema_alpha = 0.20f; + climate_config.humidity_ema_alpha = 0.20f; } -/* ------------------------------------------------------------------ */ -/* --------------------- Инициализация устройств -----------------*/ -/* ------------------------------------------------------------------ */ + +/* + ========================= + DEVICE INITIALIZATION + ========================= +*/ void init_radar() { setup_radar_config(); radar.begin(Serial2, radar_config); + Serial.println("Radar initialized"); } void init_light_sensor() { setup_light_config(); - if (light_sensor.begin(light_config)) { + + bool ok = light_sensor.begin(light_config); + + if (ok) { Serial.println("BH1750 initialized"); } else { Serial.println("BH1750 init failed"); @@ -112,7 +133,10 @@ void init_climate_sensor() { setup_climate_config(); - if (climate_sensor.begin(i2c_climate, climate_config)) { + + bool ok = climate_sensor.begin(i2c_climate, climate_config); + + if (ok) { Serial.print("Climate sensor initialized: "); Serial.println(climate_sensor.get_sensor_type_string()); } else { @@ -120,27 +144,33 @@ } } -/* ------------------------------------------------------------------ */ -/* --------------------------- SETUP --------------------------------*/ -/* ------------------------------------------------------------------ */ + +/* + ========================= + SETUP + ========================= +*/ void setup() { Serial.begin(115200); delay(1000); - Serial.println("\nRadar + BH1750 + BME/BMP280 project start"); + Serial.println(); + Serial.println("Radar + BH1750 + BME/BMP280 project start"); init_radar(); init_light_sensor(); init_climate_sensor(); } -/* ------------------------------------------------------------------ */ -/* --------------------------- LOOP --------------------------------*/ -/* ------------------------------------------------------------------ */ + +/* + ========================= + LOOP + ========================= +*/ void loop() { - /* Обновляем каждое устройство */ radar.update(); light_sensor.update(); climate_sensor.update(); @@ -150,8 +180,20 @@ if (now_ms - last_print_ms >= print_interval_ms) { last_print_ms = now_ms; + /* + Каждый сенсор отдаёт свой JSON независимо. + Радар теперь отдаёт упрощённую структуру вида: + { + "online": true, + "presence": true, + "activity_score": 5, + "activity_score_current": 4, + "activity_score_dynamics": "increasing", + "distance_m": 2.3 + } + */ Serial.println(radar.get_state_json()); Serial.println(light_sensor.get_state_json()); Serial.println(climate_sensor.get_state_json()); } -} +} \ No newline at end of file