diff --git a/devices/sensor/ld2420_radar.cpp b/devices/sensor/ld2420_radar.cpp index 034c6d4..dc43f43 100644 --- a/devices/sensor/ld2420_radar.cpp +++ b/devices/sensor/ld2420_radar.cpp @@ -6,52 +6,253 @@ /* ========================================================= LD2420 Radar — реализация - Протокол: Energy mode (бинарный фрейм, fw >= v1.5.4) - - Фрейм данных, 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 - - Источник формата: ESPHome ld2420.cpp source + Energy mode + конфигурация через командный протокол ========================================================= */ -/* -------- constexpr definitions ------------------------- */ +/* ---- constexpr definitions ----------------------------- */ -constexpr uint8_t Ld2420Radar::HDR[4]; -constexpr uint8_t Ld2420Radar::FTR[4]; +constexpr uint8_t Ld2420Radar::CMD_HDR[4]; +constexpr uint8_t Ld2420Radar::CMD_FTR[4]; +constexpr uint8_t Ld2420Radar::DATA_HDR[4]; +constexpr uint8_t Ld2420Radar::DATA_FTR[4]; -/* -------- begin ----------------------------------------- */ +/* ==================================================== + ВСПОМОГАТЕЛЬНЫЕ ФУНКЦИИ КОМАНДНОГО ПРОТОКОЛА + ==================================================== */ -void Ld2420Radar::begin(HardwareSerial& serial, const RadarConfig& config) { +/* + Формат команды: + CMD_HDR (4) | length (2, LE) | payload (N) | CMD_FTR (4) + length = кол-во байт payload +*/ +bool Ld2420Radar::_send_cmd(const uint8_t* payload, uint8_t payload_len) { + /* Заголовок */ + _serial->write(CMD_HDR, 4); + + /* Длина payload (uint16 LE) */ + _serial->write((uint8_t)(payload_len & 0xFF)); + _serial->write((uint8_t)(payload_len >> 8)); + + /* Payload */ + _serial->write(payload, payload_len); + + /* Футер */ + _serial->write(CMD_FTR, 4); + _serial->flush(); + + return _wait_ack(); +} + +/* + Ждём ACK — фрейм начинающийся с FD FC FB FA. + Не парсим полностью, просто проверяем что он пришёл. +*/ +bool Ld2420Radar::_wait_ack() { + uint32_t deadline = millis() + ACK_TIMEOUT_MS; + uint8_t hdr_match = 0; + + while (millis() < deadline) { + if (!_serial->available()) { delay(1); continue; } + + uint8_t b = _serial->read(); + + if (b == CMD_HDR[hdr_match]) { + hdr_match++; + if (hdr_match == 4) { + /* Заголовок найден — ACK получен, читаем остаток */ + uint32_t t = millis() + 50; + while (millis() < t) { + if (_serial->available()) _serial->read(); + } + return true; + } + } else { + hdr_match = (b == CMD_HDR[0]) ? 1 : 0; + } + } + return false; +} + +void Ld2420Radar::_flush_rx() { + uint32_t t = millis() + 100; + while (millis() < t) { + if (_serial->available()) _serial->read(); + else delay(1); + } +} + + +/* ==================================================== + КОНФИГУРАЦИЯ РАДАРА + ==================================================== */ + +bool Ld2420Radar::_configure() { + Serial.println("[LD2420] Starting configuration..."); + + /* ---- 1. Войти в config mode ---- */ + /* Payload: CMD(2) + protocol_version(2) = FF 00 02 00 */ + { + uint8_t p[] = {0xFF, 0x00, 0x02, 0x00}; + bool ok = false; + for (uint8_t i = 0; i < MAX_RETRIES && !ok; i++) { + _flush_rx(); + ok = _send_cmd(p, sizeof(p)); + } + if (!ok) { + Serial.println("[LD2420] ERROR: cannot enter config mode"); + return false; + } + Serial.println("[LD2420] Config mode entered"); + } + + /* ---- 2. Установить Energy mode ---- */ + /* + CMD_WRITE_SYS_PARAM = 0x0012 + Payload: 12 00 | param_id(2) | param_value(4) + param_id для mode = 0x0000 + Energy mode value = 0x0004 + */ + { + uint8_t p[] = { + 0x12, 0x00, // команда SET_SYS_PARAM + 0x00, 0x00, // param: system mode + 0x04, 0x00, 0x00, 0x00 // value: Energy mode + }; + bool ok = false; + for (uint8_t i = 0; i < MAX_RETRIES && !ok; i++) ok = _send_cmd(p, sizeof(p)); + if (!ok) Serial.println("[LD2420] WARN: set Energy mode failed"); + else Serial.println("[LD2420] Energy mode set"); + } + + /* ---- 3. Установить max_gate ---- */ + /* + CMD_WRITE_SYS_PARAM = 0x0012 + param: 0x0001 = max gate + */ + { + uint8_t g = _cfg.max_gate; + if (g > 15) g = 15; + uint8_t p[] = { + 0x12, 0x00, + 0x01, 0x00, // param: max gate + g, 0x00, 0x00, 0x00 + }; + bool ok = false; + for (uint8_t i = 0; i < MAX_RETRIES && !ok; i++) ok = _send_cmd(p, sizeof(p)); + if (!ok) Serial.println("[LD2420] WARN: set max_gate failed"); + else { + Serial.print("[LD2420] max_gate set to "); + Serial.println(g); + } + } + + /* ---- 4. Установить таймаут ---- */ + /* + param: 0x0004 = timeout + */ + { + uint16_t t = _cfg.radar_timeout_s; + uint8_t p[] = { + 0x12, 0x00, + 0x04, 0x00, // param: timeout + (uint8_t)(t & 0xFF), (uint8_t)(t >> 8), 0x00, 0x00 + }; + bool ok = false; + for (uint8_t i = 0; i < MAX_RETRIES && !ok; i++) ok = _send_cmd(p, sizeof(p)); + if (!ok) Serial.println("[LD2420] WARN: set timeout failed"); + else { + Serial.print("[LD2420] timeout set to "); + Serial.print(t); + Serial.println(" s"); + } + } + + /* ---- 5. Записать пороги для каждого ворота ---- */ + /* + CMD_WRITE_ABD_PARAM = 0x0007 + Payload: 07 00 | gate_move_reg(2) | move_val(4) | gate_still_reg(2) | still_val(4) + Записываем оба порога одной командой на каждое ворото. + */ + Serial.println("[LD2420] Writing gate thresholds..."); + bool thresh_ok = true; + + for (uint8_t gate = 0; gate <= _cfg.max_gate; gate++) { + uint16_t move_reg = (uint16_t)(GATE_MOVE_REG_BASE + gate); + uint16_t still_reg = (uint16_t)(GATE_STILL_REG_BASE + gate); + uint32_t mv = _cfg.move_threshold; + uint32_t sv = _cfg.still_threshold; + + uint8_t p[] = { + 0x07, 0x00, + (uint8_t)(move_reg & 0xFF), (uint8_t)(move_reg >> 8), + (uint8_t)(mv & 0xFF), (uint8_t)((mv >> 8) & 0xFF), + (uint8_t)((mv >> 16) & 0xFF), (uint8_t)((mv >> 24) & 0xFF), + (uint8_t)(still_reg & 0xFF), (uint8_t)(still_reg >> 8), + (uint8_t)(sv & 0xFF), (uint8_t)((sv >> 8) & 0xFF), + (uint8_t)((sv >> 16) & 0xFF), (uint8_t)((sv >> 24) & 0xFF), + }; + + bool ok = false; + for (uint8_t i = 0; i < MAX_RETRIES && !ok; i++) ok = _send_cmd(p, sizeof(p)); + if (!ok) { + Serial.print("[LD2420] WARN: threshold gate "); + Serial.print(gate); + Serial.println(" failed"); + thresh_ok = false; + } + } + + if (thresh_ok) Serial.println("[LD2420] Thresholds written"); + + /* ---- 6. Выйти из config mode и перезапустить ---- */ + { + uint8_t p[] = {0xFE, 0x00}; + bool ok = false; + for (uint8_t i = 0; i < MAX_RETRIES && !ok; i++) ok = _send_cmd(p, sizeof(p)); + if (!ok) Serial.println("[LD2420] WARN: disable config failed"); + else Serial.println("[LD2420] Config mode exited"); + } + + /* Небольшая пауза — радар применяет настройки */ + delay(500); + + Serial.println("[LD2420] Configuration complete"); + return true; +} + + +/* ==================================================== + BEGIN + ==================================================== */ + +bool 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_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)); + if (_cfg.total_energy_max == 0) _cfg.total_energy_max = 1; serial.begin(_cfg.baud_rate, SERIAL_8N1, (int)_cfg.uart_rx_pin, (int)_cfg.uart_tx_pin); + + /* Даём радару время проснуться */ + delay(1000); + _flush_rx(); + + _configured = _configure(); + return _configured; } -/* -------- update ---------------------------------------- */ +/* ==================================================== + UPDATE + ==================================================== */ void Ld2420Radar::update() { if (!_serial) return; @@ -62,150 +263,136 @@ 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; - } + _presence = (_last_on_ms > 0) && + (now_ms - _last_on_ms < _cfg.presence_hold_ms); _tick_activity_avg(now_ms); _tick_trend(now_ms); } -/* -------- _feed_byte ------------------------------------- */ - -/* - Стратегия синхронизации: - 1. Ищем заголовок F4 F3 F2 F1 побайтово. - 2. После его захвата накапливаем оставшиеся (FRAME_SIZE - 4) байт. - 3. Проверяем футер. Если неверен — сбрасываем и ищем снова. -*/ +/* ==================================================== + ПАРСЕР ENERGY-ФРЕЙМОВ + ==================================================== */ 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; - if (_rx_buf[0] == HDR[0] && - _rx_buf[1] == HDR[1] && - _rx_buf[2] == HDR[2] && - _rx_buf[3] == HDR[3]) { + if (_rx_buf[0] == DATA_HDR[0] && + _rx_buf[1] == DATA_HDR[1] && + _rx_buf[2] == DATA_HDR[2] && + _rx_buf[3] == DATA_HDR[3]) { _synced = true; - _rx_idx = 4; // заголовок уже в буфере + _rx_idx = 4; } return; } - /* Накапливаем остаток фрейма */ _rx_buf[_rx_idx++] = b; if (_rx_idx >= FRAME_SIZE) { - if (_validate_footer()) { + if (_validate_data_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(); - } + } else if (_cfg.enable_debug_frames) { + Serial.print("[LD2420] bad footer at: "); + Serial.printf("%02X %02X %02X %02X\n", + _rx_buf[FRAME_FOOTER_OFF], _rx_buf[FRAME_FOOTER_OFF+1], + _rx_buf[FRAME_FOOTER_OFF+2], _rx_buf[FRAME_FOOTER_OFF+3]); } - /* В любом случае ищем следующий заголовок */ _synced = false; _rx_idx = 0; - memset(_rx_buf, 0, sizeof(_rx_buf)); } } - -/* -------- _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]; +bool Ld2420Radar::_validate_data_footer() const { + return _rx_buf[FRAME_FOOTER_OFF + 0] == DATA_FTR[0] && + _rx_buf[FRAME_FOOTER_OFF + 1] == DATA_FTR[1] && + _rx_buf[FRAME_FOOTER_OFF + 2] == DATA_FTR[2] && + _rx_buf[FRAME_FOOTER_OFF + 3] == DATA_FTR[3]; } - -/* -------- _process_frame -------------------------------- */ - void Ld2420Radar::_process_frame() { uint32_t now_ms = millis(); - _last_frame_ms = now_ms; + _last_frame_ms = now_ms; - /* Поле Presence */ - uint8_t presence_raw = _rx_buf[FRAME_PRESENCE_OFF]; - bool has_target = (presence_raw == 0x01 || presence_raw == 0x02); + /* Presence — поле из фрейма только для debug, решение принимаем сами */ + uint8_t pres_raw = _rx_buf[FRAME_PRESENCE]; - if (has_target) { - _last_on_ms = now_ms; - } + /* Distance */ + uint16_t dist_cm = (uint16_t)_rx_buf[FRAME_DIST] | + (uint16_t)_rx_buf[FRAME_DIST + 1] << 8; + float raw_m = (float)dist_cm * 0.01f; + /* дистанция обновляется ниже после вычисления has_target */ - /* Дистанция (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; - } - - /* Энергии ворот Gate[0..15] */ + /* Gate energies + activity_score_current */ + /* Каждое ворото — uint16_t LE, 2 байта */ + /* Суммируем только от min_gate до max_gate — исключаем шум ближней зоны */ uint32_t total_energy = 0; + uint8_t gate_min = _cfg.min_gate; + uint8_t gate_max = (_cfg.max_gate < LD2420_TOTAL_GATES - 1) + ? _cfg.max_gate : LD2420_TOTAL_GATES - 1; for (uint8_t g = 0; g < LD2420_TOTAL_GATES; g++) { - _gate_energy[g] = _rx_buf[FRAME_GATE_OFFSET + g]; - total_energy += _gate_energy[g]; + uint8_t lo = _rx_buf[FRAME_GATES + g * 2]; + uint8_t hi = _rx_buf[FRAME_GATES + g * 2 + 1]; + _gate_energy[g] = (uint16_t)lo | ((uint16_t)hi << 8); + if (g >= gate_min && g <= gate_max) { + total_energy += _gate_energy[g]; + } } /* - activity_score_current: - Суммарная энергия по всем воротам нормируется к 0–10. - total_energy_max соответствует score = 10. + Вычисляем presence самостоятельно по энергиям ворот. + Ворото активно если его энергия > presence_thresholds[g]. + Пороги g0 и g1 = 0 — они всегда исключены. */ - if (total_energy == 0) { - _activity_score_current = 0; - } else { - 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); + uint8_t active_gates = 0; + for (uint8_t g = _cfg.min_gate; g <= gate_max; g++) { + if (_cfg.presence_thresholds[g] > 0 && + _gate_energy[g] > _cfg.presence_thresholds[g]) { + active_gates++; + } + } + bool has_target = (active_gates >= _cfg.presence_min_active_gates); + if (has_target) _last_on_ms = now_ms; + + /* Дистанция: обновляем только если has_target и dist >= min_gate */ + { + float min_dist_m = (float)_cfg.min_gate * 0.70f; + bool dist_valid = has_target && dist_cm > 0 && raw_m >= min_dist_m; + if (dist_valid) { + 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; + } + /* has_target=true но dist невалидна — держим последнее значение */ } + 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); + Serial.printf("[LD2420] pres_raw=%u active_gates=%u dist_cm=%u energy=%lu score=%u\n", + pres_raw, active_gates, dist_cm, (unsigned long)total_energy, _activity_score_current); } } -/* -------- _tick_activity_avg ---------------------------- */ +/* ==================================================== + ТИКИ НАКОПЛЕНИЯ + ==================================================== */ void Ld2420Radar::_tick_activity_avg(uint32_t now_ms) { if (_act_avg_last_ms == 0) { _act_avg_last_ms = now_ms; return; } @@ -222,9 +409,6 @@ _activity_score = (uint8_t)(sum / _act_avg_count); } - -/* -------- _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; @@ -238,72 +422,49 @@ _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; + float sx = 0, sy = 0, sxx = 0, sxy = 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; + uint8_t idx = (uint8_t)((_trend_idx - n + i + win) % win); + float x = (float)i, y = (float)_trend_buf[idx]; + sx += x; sy += y; sxx += x*x; sxy += x*y; mean_y += y; } - float fn = (float)n; - float denom = fn * sum_xx - sum_x * sum_x; - + float fn = (float)n, denom = fn * sxx - sx * sx; if (fabsf(denom) < 1e-6f) { _trend_slope = 0; } else { - 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; + float slope = (fn * sxy - sx * sy) / denom; + _trend_slope = (slope > 0.30f) ? 1 : (slope < -0.30f) ? -1 : 0; } - /* Стандартное отклонение для детектирования "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; + uint8_t idx = (uint8_t)((_trend_idx - n + i + win) % win); + float d = (float)_trend_buf[idx] - mean_y; var += d * d; } _trend_std_dev = sqrtf(var / fn); } - -/* -------- get_activity_dynamics ------------------------- */ - const char* Ld2420Radar::get_activity_dynamics() const { - if (_trend_count < 2) return "constant"; - - /* - "variable" = высокий разброс без чёткого тренда. - Порог StdDev > 2.5 при нулевом или слабом слоупе. - */ + if (_trend_count < 2) return "constant"; if (_trend_std_dev > 2.5f && _trend_slope == 0) return "variable"; - - if (_trend_slope > 0) return "increasing"; - else if (_trend_slope < 0) return "decreasing"; - else return "constant"; + if (_trend_slope > 0) return "increasing"; + if (_trend_slope < 0) return "decreasing"; + return "constant"; } -/* -------- get_state_json -------------------------------- */ +/* ==================================================== + JSON + ==================================================== */ const char* Ld2420Radar::get_state_json() { snprintf(_json_buf, sizeof(_json_buf), diff --git a/devices/sensor/ld2420_radar.h b/devices/sensor/ld2420_radar.h index 27b4831..6bbe498 100644 --- a/devices/sensor/ld2420_radar.h +++ b/devices/sensor/ld2420_radar.h @@ -5,60 +5,130 @@ /* ========================================================= - LD2420 Radar — конфигурация + LD2420 Radar Модуль: LD2420 v2.1 - Режим: Energy mode (бинарный фрейм, fw >= v1.5.4) + Режим: Energy mode (бинарный фрейм) - Формат фрейма данных (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 байт + При старте ESP32 сам переводит радар в Energy mode и + записывает пороги чувствительности через командный протокол. + + Командный протокол (config mode): + Header: FD FC FB FA + Footer: 04 03 02 01 + + Фрейм данных Energy mode (31 байт): + [0..3] Header F4 F3 F2 F1 + [4..5] Length uint16 LE + [6] Presence 0=нет 1=движение 2=стационарный + [7..8] Distance uint16 LE (сантиметры) + [9..24] Gate[0..15] uint8 — энергия каждого ворота + [25..26] Padding + [27..30] Footer F8 F7 F6 F5 ========================================================= */ static constexpr uint8_t LD2420_TOTAL_GATES = 16; struct RadarConfig { - /* UART пины */ - uint8_t uart_rx_pin = 4; - uint8_t uart_tx_pin = 15; - uint32_t baud_rate = 115200; + /* ---- UART ---- */ + uint8_t uart_rx_pin = 4; + uint8_t uart_tx_pin = 15; + uint32_t baud_rate = 115200; - /* Удерживать presence после пропадания данных */ + /* ---- Присутствие и онлайн ---- */ uint32_t presence_hold_ms = 1500; + uint32_t stale_after_ms = 2000; - /* Считать online пока данные свежее stale_after_ms */ - uint32_t stale_after_ms = 2000; + /* ---- Дистанция ---- */ + float distance_ema_alpha = 0.6f; - /* EMA для дистанции (0 < α ≤ 1; больше = быстрее, меньше = плавнее) */ - float distance_ema_alpha = 0.35f; + /* ---- Параметры, записываемые в радар при старте ---- */ /* - Расстояние одних ворот в метрах. - Для LD2420 при стандартной конфигурации ≈ 0.70 м. - Подгони по реальным замерам. + Максимальная зона детекции (0–15). + Каждая зона ≈ 0.7 м, итого: + 5 ворот ≈ 3.5 м + 8 ворот ≈ 5.6 м + 12 ворот ≈ 8.4 м (максимум) */ - float gate_size_m = 0.70f; + uint8_t min_gate = 2; // игнорировать ворота ближе этого (0-14) + uint8_t max_gate = 8; /* - Максимальная суммарная энергия по всем воротам, - соответствующая activity_score_current = 10. - Калибруй под реальную комнату и типичное движение. + Таймаут исчезновения присутствия внутри самого радара (секунды). + Радар не выдаст OFF пока не пройдёт это время без движения. + Рекомендуется 5–30 с. Мы дополнительно держим presence + в прошивке через presence_hold_ms. */ - uint32_t total_energy_max = 2000; + uint16_t radar_timeout_s = 5; - /* Окно усреднения activity_score, секунды (1–60) */ - uint8_t activity_avg_window_s = 60; + /* + Пороги чувствительности — одинаковые для всех ворот. + Чем выше значение, тем менее чувствителен радар. - /* Окно тренда для dynamics, минуты (1–10) */ + move_threshold: порог обнаружения движения + still_threshold: порог обнаружения стационарного присутствия + + Заводские значения Hi-Link: + gate 0-1: move=60000, still=40000 (очень близко — шумно) + gate 2+: move=250, still=200 + + Рекомендуемый старт для комнаты без ложных срабатываний: + move=500, still=300 + Если пропускает реального человека — уменьшай. + Если ложные срабатывания — увеличивай. + */ + /* + Пороги присутствия для каждого ворота (g0–g15). + Presence вычисляется в прошивке — поле pres из фрейма игнорируется. + Ворото считается активным если его энергия > порога. + presence = true если активных ворот >= presence_min_active_gates. + + Как настраивать: + 1. Включи enable_debug_frames=true, посмотри [GATES] пустой комнаты + 2. Порог ворота = максимум_шума_этого_ворота * 2 + 3. g0 и g1 всегда 0 — не участвуют в решении + */ + uint32_t presence_thresholds[LD2420_TOTAL_GATES] = { + 0, // g0 шум антенны, игнорируем + 0, // g1 фоновый шум, игнорируем + 800, // g2 ~1.4 м + 500, // g3 ~2.1 м + 15000, // g4 ~2.8 м (высокий: помехи в этой зоне) + 25000, // g5 ~3.5 м (высокий: помехи) + 15000, // g6 ~4.2 м (высокий: помехи) + 2000, // g7 ~4.9 м + 2000, // g8 ~5.6 м + 500, // g9 + 500, // g10 + 500, // g11 + 500, // g12 + 500, // g13 + 500, // g14 + 500, // g15 + }; + + /* Минимальное кол-во активных ворот для presence=true */ + uint8_t presence_min_active_gates = 1; + + /* Пороги для записи в модуль (внутренняя логика радара) */ + uint32_t move_threshold = 10000; + uint32_t still_threshold = 8000; + + /* ---- activity_score ---- */ + + /* + Максимальная суммарная энергия по всем воротам + соответствующая score = 10. + Откалибруй: включи enable_debug_frames, посмотри + energy_sum при активном движении и подставь это значение. + */ + uint32_t total_energy_max = 3000; + + uint8_t activity_avg_window_s = 60; uint8_t activity_trend_window_min = 10; + /* ---- Отладка ---- */ bool enable_debug_frames = false; }; @@ -73,73 +143,104 @@ public: Ld2420Radar() = default; - /* Вызвать один раз в setup() */ - void begin(HardwareSerial& serial, const RadarConfig& config); + /* + Инициализация: открывает UART, входит в config mode, + записывает Energy mode + пороги + max_gate, выходит. + Возвращает true если конфигурация применена успешно. + */ + bool begin(HardwareSerial& serial, const RadarConfig& config); - /* Вызывать каждый loop() */ - void update(); - - /* JSON-снимок состояния (статический буфер, валиден до следующего вызова) */ + void update(); 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; + const char* get_activity_dynamics() const; + uint16_t get_gate_energy(uint8_t g) const { + return (g < LD2420_TOTAL_GATES) ? _gate_energy[g] : 0; } + /* true если конфигурация была успешно записана в радар */ + bool is_configured() const { return _configured; } + private: - /* ---- Конфигурация и железо ---- */ HardwareSerial* _serial = nullptr; RadarConfig _cfg; + bool _configured = false; - /* ---- Бинарный парсер фрейма ---- */ - 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; + /* ==================================================== + КОМАНДНЫЙ ПРОТОКОЛ (config mode) + ==================================================== */ - /* Байты заголовка */ - static constexpr uint8_t HDR[4] = {0xF4, 0xF3, 0xF2, 0xF1}; - /* Байты футера */ - static constexpr uint8_t FTR[4] = {0xF8, 0xF7, 0xF6, 0xF5}; + static constexpr uint8_t CMD_HDR[4] = {0xFD, 0xFC, 0xFB, 0xFA}; + static constexpr uint8_t CMD_FTR[4] = {0x04, 0x03, 0x02, 0x01}; + + static constexpr uint16_t CMD_ENABLE_CONFIG = 0x00FF; + static constexpr uint16_t CMD_DISABLE_CONFIG = 0x00FE; + static constexpr uint16_t CMD_SET_SYS_MODE = 0x0012; + static constexpr uint16_t CMD_WRITE_THRESH = 0x0007; + static constexpr uint16_t CMD_SET_MAX_GATE = 0x0012; // тот же 0x0012, param 0x0001 + static constexpr uint16_t CMD_RESTART = 0x0068; + + static constexpr uint16_t SYS_MODE_ENERGY = 0x0004; + + static constexpr uint16_t REG_MAX_GATE = 0x0001; + static constexpr uint16_t REG_TIMEOUT = 0x0004; + + static constexpr uint16_t GATE_MOVE_REG_BASE = 0x0010; // 0x0010..0x001F + static constexpr uint16_t GATE_STILL_REG_BASE = 0x0020; // 0x0020..0x002F + + static constexpr uint32_t ACK_TIMEOUT_MS = 300; + static constexpr uint8_t MAX_RETRIES = 3; + + /* Отправить команду и дождаться ACK. Возвращает true при успехе. */ + bool _send_cmd(const uint8_t* payload, uint8_t payload_len); + + /* Ждать ACK-фрейм (header FD FC FB FA) до таймаута. */ + bool _wait_ack(); + + /* Сбросить входной буфер UART. */ + void _flush_rx(); + + /* Вся последовательность конфигурации. */ + bool _configure(); + + /* ==================================================== + ПАРСЕР ENERGY-ФРЕЙМОВ + ==================================================== */ + + static constexpr uint8_t FRAME_SIZE = 45; + static constexpr uint8_t FRAME_PRESENCE = 6; + static constexpr uint8_t FRAME_DIST = 7; + static constexpr uint8_t FRAME_GATES = 9; + static constexpr uint8_t FRAME_FOOTER_OFF = 41; + + static constexpr uint8_t DATA_HDR[4] = {0xF4, 0xF3, 0xF2, 0xF1}; + static constexpr uint8_t DATA_FTR[4] = {0xF8, 0xF7, 0xF6, 0xF5}; uint8_t _rx_buf[FRAME_SIZE] = {}; uint8_t _rx_idx = 0; - bool _synced = false; // заголовок пойман, накапливаем оставшиеся байты + bool _synced = false; void _feed_byte(uint8_t b); - bool _validate_footer() const; + bool _validate_data_footer() const; void _process_frame(); - /* ---- Данные последнего фрейма ---- */ - uint8_t _gate_energy[LD2420_TOTAL_GATES] = {}; - float _dist_ema = -1.0f; - float _prev_dist_m = -1.0f; + /* ---- Состояние ---- */ + uint16_t _gate_energy[LD2420_TOTAL_GATES] = {}; + float _dist_ema = -1.0f; + float _distance_m = 0.0f; - /* ---- Онлайн / присутствие ---- */ uint32_t _last_frame_ms = 0; uint32_t _last_on_ms = 0; bool _online = false; bool _presence = false; - /* ---- Дистанция ---- */ - float _distance_m = 0.0f; + uint8_t _activity_score_current = 0; - /* ---- 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; @@ -148,17 +249,15 @@ 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 = спад + int8_t _trend_slope = 0; 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 48e2242..246291e 100644 --- a/devices/sensor/sensor.ino +++ b/devices/sensor/sensor.ino @@ -1,3 +1,5 @@ +#include + #include "ld2420_radar.h" #include "bh1750_sensor.h" #include "bme280_sensor.h" @@ -38,20 +40,71 @@ */ void setup_radar_config() { - radar_config.uart_rx_pin = 4; - radar_config.uart_tx_pin = 15; - radar_config.baud_rate = 115200; + /* ---- UART ---- */ + radar_config.uart_rx_pin = 4; + radar_config.uart_tx_pin = 15; + radar_config.baud_rate = 115200; + /* ---- Присутствие и онлайн ---- */ radar_config.presence_hold_ms = 1500; radar_config.stale_after_ms = 2000; - radar_config.distance_ema_alpha = 0.35f; + radar_config.distance_ema_alpha = 0.6f; - radar_config.gate_size_m = 0.70f; // уточни по реальным замерам - radar_config.total_energy_max = 2000; // откалибруй под помещение + /* ---- Параметры, записываемые в радар при старте ---- */ + radar_config.min_gate = 1; // игнорировать ворота < 0.7 м + radar_config.max_gate = 8; // детекция до ~5.6 м + radar_config.radar_timeout_s = 5; + /* ---- Пороги для записи в модуль (внутренняя логика радара) ---- */ + radar_config.move_threshold = 10000; + radar_config.still_threshold = 8000; + + /* + Пороги присутствия по воротам — вычисляется в прошивке, + поле pres из фрейма радара игнорируется. + + Настройка: + 1. Включи enable_debug_frames=true + 2. Посмотри [GATES] в пустой комнате + 3. Порог ворота = максимум_шума_этого_ворота * 2 + 4. g0 и g1 всегда 0 + + Текущие пороги подобраны под эту комнату: + g0-g1: шум антенны и фон — игнорируем + g2: шум до ~30 → порог 800 + g3: шум до ~220 → порог 500 (основной сигнал на 2 м) + g4-g6: высокий шум от помех в комнате → высокие пороги + g7+: малый шум → порог 2000 + */ + static const uint32_t thresholds[LD2420_TOTAL_GATES] = { + 0, // g0 ~0.0 м шум антенны + 0, // g1 ~0.7 м фоновый шум + 800, // g2 ~1.4 м + 500, // g3 ~2.1 м + 15000, // g4 ~2.8 м (помехи в комнате) + 25000, // g5 ~3.5 м (помехи в комнате) + 15000, // g6 ~4.2 м (помехи в комнате) + 2000, // g7 ~4.9 м + 2000, // g8 ~5.6 м + 500, // g9 + 500, // g10 + 500, // g11 + 500, // g12 + 500, // g13 + 500, // g14 + 500, // g15 + }; + memcpy(radar_config.presence_thresholds, thresholds, + sizeof(radar_config.presence_thresholds)); + + radar_config.presence_min_active_gates = 1; + + /* ---- activity_score ---- */ + radar_config.total_energy_max = 150000; radar_config.activity_avg_window_s = 60; radar_config.activity_trend_window_min = 10; + /* ---- Отладка ---- */ radar_config.enable_debug_frames = false; } @@ -101,8 +154,8 @@ 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; } @@ -114,9 +167,9 @@ void init_radar() { setup_radar_config(); - radar.begin(Serial2, radar_config); - - Serial.println("Radar initialized"); + bool ok = radar.begin(Serial2, radar_config); + if (ok) Serial.println("Radar configured and initialized"); + else Serial.println("Radar init failed — running with defaults"); } void init_light_sensor() { @@ -124,11 +177,8 @@ bool ok = light_sensor.begin(light_config); - if (ok) { - Serial.println("BH1750 initialized"); - } else { - Serial.println("BH1750 init failed"); - } + if (ok) Serial.println("BH1750 initialized"); + else Serial.println("BH1750 init failed"); } void init_climate_sensor() { @@ -180,20 +230,8 @@ 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 +}