diff --git a/devices/sensor/ld2420_radar.cpp b/devices/sensor/ld2420_radar.cpp index 3f54e24..3a8b34a 100644 --- a/devices/sensor/ld2420_radar.cpp +++ b/devices/sensor/ld2420_radar.cpp @@ -1,661 +1,267 @@ #include "ld2420_radar.h" -Ld2420Radar::Ld2420Radar() { - reset_runtime_state(); +/* ------------------------------------------------------------------ */ +/* ------------------------ Инициализация --------------------------*/ +/* ------------------------------------------------------------------ */ + +void Ld2420Radar::begin(Stream &uart, const RadarConfig &cfg) { + _uart = &uart; + _cfg = cfg; + + /* Установим параметры UART */ + pinMode(_cfg.uart_rx_pin, INPUT_PULLUP); + pinMode(_cfg.uart_tx_pin, OUTPUT); + // На ESP32 Serial2 уже привязан к этим пинам, но оставляем для совместимости } -bool Ld2420Radar::begin(HardwareSerial &serial, const RadarConfig &config) { - serial_port = &serial; - cfg = config; - - reset_runtime_state(); - - serial_port -> begin(cfg.baud_rate, SERIAL_8N1, cfg.uart_rx_pin, cfg.uart_tx_pin); - initialized = true; - - return true; -} - -void Ld2420Radar::reset_runtime_state() { - line_length = 0; - line_buffer[0] = '\0'; - - online = false; - raw_presence = false; - presence = false; - - raw_distance_m = 0.0f; - filtered_distance_m = 0.0f; - - current_speed_m_s = 0.0f; - activity_score_current = 0; - - last_frame_ms = 0; - last_presence_ms = 0; - last_distance_sample_ms = 0; - - last_distance_for_speed_m = 0.0f; - has_distance_for_speed = false; - - second_scores_head = 0; - second_scores_count = 0; - last_second_push_ms = 0; - - for (size_t i = 0; i < SECOND_HISTORY_SIZE; ++i) { - second_scores[i] = 0; - } - - activity_score_dynamics = "constant"; -} +/* ------------------------------------------------------------------ */ +/* -------------------------- Основной цикл ------------------------*/ +/* ------------------------------------------------------------------ */ void Ld2420Radar::update() { - if (!initialized || serial_port == nullptr) { - return; - } + if (!_uart) return; - while (serial_port -> available() > 0) { - char ch = static_cast(serial_port -> read()); - process_incoming_byte(ch); - } - - uint32_t now_ms = millis(); - - update_online_state(now_ms); - update_presence_state(now_ms); - update_second_history(now_ms); - - activity_score_dynamics = compute_dynamics(); -} - -void Ld2420Radar::process_incoming_byte(char ch) { - if (ch == '\r' || ch == '\n') { - if (line_length > 0) { - line_buffer[line_length] = '\0'; - process_line(line_buffer); - line_length = 0; - line_buffer[0] = '\0'; - } - return; - } - - if (line_length + 1 >= LINE_BUFFER_SIZE) { - line_length = 0; - line_buffer[0] = '\0'; - return; - } - - line_buffer[line_length++] = ch; -} - -void Ld2420Radar::process_line(const char *line_cstr) { - if (line_cstr == nullptr || line_cstr[0] == '\0') { - return; - } - - String line = String(line_cstr); - line.trim(); - - if (line.length() == 0) { - return; - } - - bool parsed = parse_ascii_line(line); - - if (parsed) { - last_frame_ms = millis(); - online = true; - } else if (cfg.enable_debug_unknown_lines) { - Serial.print("LD2420 unknown line: "); - Serial.println(line); - } -} - -bool Ld2420Radar::parse_ascii_line(const String &raw_line) { - String line = normalize_line(raw_line); - line.toLowerCase(); - - /* - Поддерживаем разные возможные названия полей. - Это сделано специально, чтобы не завязаться - на единственный вариант строки. - */ - static const char *presence_keys[] = { - "presence", "pres", "occupancy", "occupied", "target", - "detected", "detect", "exist", "state" - }; - - static const char *distance_meter_keys[] = { - "distance_m", "dist_m", "dis_m", "range_m", "r_m", "meter", "meters" - }; - - static const char *distance_cm_keys[] = { - "distance_cm", "dist_cm", "dis_cm", "range_cm", "cm" - }; - - static const char *distance_generic_keys[] = { - "distance", "dist", "dis", "range", "r" - }; - - static const char *zone_keys[] = { - "zone", "gate", "area", "distance_zone" - }; - - static const char *move_keys[] = { - "move", "moving", "motion", "micro_move", "micro", "still" - }; - - bool got_anything = false; - uint32_t now_ms = millis(); - - bool explicit_presence = false; - bool has_explicit_presence = extract_bool_by_keys(line, presence_keys, sizeof(presence_keys) / sizeof(presence_keys[0]), explicit_presence); - - float distance_m = 0.0f; - bool has_distance_m = extract_number_by_keys(line, distance_meter_keys, sizeof(distance_meter_keys) / sizeof(distance_meter_keys[0]), distance_m); - - float distance_cm = 0.0f; - bool has_distance_cm = extract_number_by_keys(line, distance_cm_keys, sizeof(distance_cm_keys) / sizeof(distance_cm_keys[0]), distance_cm); - - float distance_generic = 0.0f; - bool has_distance_generic = extract_number_by_keys(line, distance_generic_keys, sizeof(distance_generic_keys) / sizeof(distance_generic_keys[0]), distance_generic); - - float zone = 0.0f; - bool has_zone = extract_number_by_keys(line, zone_keys, sizeof(zone_keys) / sizeof(zone_keys[0]), zone); - - float move_value = 0.0f; - bool has_move_value = extract_number_by_keys(line, move_keys, sizeof(move_keys) / sizeof(move_keys[0]), move_value); - - /* - 1. Приоритет: - явная дистанция в метрах - -> дистанция в сантиметрах - -> generic distance - -> zone/gate через калибровку - */ - if (has_distance_m) { - if (is_distance_plausible(distance_m)) { - raw_distance_m = distance_m; - apply_new_distance(distance_m, now_ms); - got_anything = true; - } - } else if (has_distance_cm) { - float converted = distance_cm / 100.0f; - - if (is_distance_plausible(converted)) { - raw_distance_m = converted; - apply_new_distance(converted, now_ms); - got_anything = true; - } - } else if (has_distance_generic) { - float converted = distance_generic; - - /* - Если значение выглядит как сантиметры — конвертируем. - Если выглядит как зона — позже это перекроется has_zone. - */ - if (converted > 20.0f) { - converted /= 100.0f; - } - - if (is_distance_plausible(converted)) { - raw_distance_m = converted; - apply_new_distance(converted, now_ms); - got_anything = true; - } - } else if (has_zone) { - if (zone >= cfg.min_valid_zone && zone <= cfg.max_valid_zone) { - float converted = zone_to_meters(zone); - - if (is_distance_plausible(converted)) { - raw_distance_m = converted; - apply_new_distance(converted, now_ms); - got_anything = true; + /* Читаем все доступные байты */ + 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; } } } - /* - 2. Presence: - - если есть явный флаг, используем его; - - иначе presence выводим из валидной дистанции - или из move_value > 0. - */ - if (has_explicit_presence) { - raw_presence = explicit_presence; - got_anything = true; + _update_online_state(); +} + +/* ------------------------------------------------------------------ */ +/* ------------------------ Парсинг строки -----------------------*/ +/* ------------------------------------------------------------------ */ + +void Ld2420Radar::_parse_line(const char *line, size_t len) { + if (len < 5) return; // минимальная длина +, + + /* Время прихода */ + _last_line_ms = millis(); + + /* Если строка начинается с '+' – это данные */ + if (line[0] == '+') { + const char *p = line + 1; + uint16_t zone = 0; + float distance_m = NAN, speed_m_s = NAN; + + /* Парсим */ + while (*p && *p != ',') { + if (!isdigit(*p)) return; // Неверный формат + zone = zone * 10 + (*p - '0'); + ++p; + } + 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 { - bool inferred_presence = false; - - if (got_anything && filtered_distance_m > 0.01f) { - inferred_presence = true; - } - - if (has_move_value && move_value > 0.0f) { - inferred_presence = true; - } - - raw_presence = inferred_presence; - } - - if (raw_presence) { - last_presence_ms = now_ms; - } - - activity_score_current = compute_current_activity_score(raw_presence, filtered_distance_m, current_speed_m_s); - - return got_anything || has_explicit_presence || has_move_value; -} - -void Ld2420Radar::update_online_state(uint32_t now_ms) { - if (last_frame_ms == 0) { - online = false; - return; - } - - online = (now_ms - last_frame_ms <= cfg.stale_after_ms); -} - -void Ld2420Radar::update_presence_state(uint32_t now_ms) { - if (!online) { - presence = false; - return; - } - - if (raw_presence) { - presence = true; - return; - } - - if (last_presence_ms != 0 && (now_ms - last_presence_ms <= cfg.presence_hold_ms)) { - presence = true; - return; - } - - presence = false; -} - -void Ld2420Radar::update_second_history(uint32_t now_ms) { - if (last_second_push_ms == 0) { - last_second_push_ms = now_ms; - return; - } - - while (now_ms - last_second_push_ms >= 1000) { - last_second_push_ms += 1000; - - uint8_t score_to_push = online ? activity_score_current : 0; - - second_scores[second_scores_head] = score_to_push; - second_scores_head = (second_scores_head + 1) % SECOND_HISTORY_SIZE; - - if (second_scores_count < SECOND_HISTORY_SIZE) { - ++second_scores_count; + /* Неизвестная строка */ + if (_cfg.enable_debug_unknown_lines) { + Serial.printf("Unknown radar line: %.*s\n", (int)len, line); } } } -void Ld2420Radar::apply_new_distance(float distance_m, uint32_t now_ms) { - if (!is_distance_plausible(distance_m)) { - return; +/* ------------------------------------------------------------------ */ +/* ----------------------- Обновление online ----------------------*/ +/* ------------------------------------------------------------------ */ + +void Ld2420Radar::_update_online_state() { + uint32_t now = millis(); + _state.online = (now - _last_line_ms <= _cfg.stale_after_ms); +} + +/* ------------------------------------------------------------------ */ +/* --------------------- Обновление presence ---------------------*/ +/* ------------------------------------------------------------------ */ + +void Ld2420Radar::_update_presence_state(uint16_t zone) { + if (zone < _cfg.min_valid_zone || zone > _cfg.max_valid_zone) return; + + /* Если пришел первый сигнал, запоминаем время */ + if (!_state.presence) { + _presence_start_ms = millis(); + } + _state.presence = true; +} + +/* ------------------------------------------------------------------ */ +/* -------------------- Обновление расстояния/скорости ----------*/ +/* ------------------------------------------------------------------ */ + +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 (filtered_distance_m <= 0.0f) { - filtered_distance_m = distance_m; + /* Обновляем активность */ + _calc_activity_score_current(); + /* Сохраняем в историю */ + _activity_hist[_hist_idx++] = _state.activity_score_current; + if (_hist_idx >= ACTIVITY_HISTORY) { + _hist_idx = 0; + _hist_filled = true; + } + + /* Вычисляем среднюю активность за минуту (последние N значений) */ + _calc_activity_score_average(); + _determine_dynamics(); +} + +/* ------------------------------------------------------------------ */ +/* -------------------- Расчёт текущей активности ----------------*/ +/* ------------------------------------------------------------------ */ + +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; } else { - filtered_distance_m = - (cfg.distance_ema_alpha * distance_m) + - ((1.0f - cfg.distance_ema_alpha) * filtered_distance_m); + 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); } +} - if (has_distance_for_speed && last_distance_sample_ms != 0 && now_ms > last_distance_sample_ms) { - float dt_s = static_cast(now_ms - last_distance_sample_ms) / 1000.0f; +/* ------------------------------------------------------------------ */ +/* --------------------- Средняя активность за минуту -------------*/ +/* ------------------------------------------------------------------ */ - if (dt_s > 0.02f) { - float speed = fabsf(distance_m - last_distance_for_speed_m) / dt_s; - current_speed_m_s = speed; - } +void Ld2420Radar::_calc_activity_score_average() { + if (!_hist_filled && _hist_idx == 0) return; // не хватает данных + + 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::_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)]; } else { - current_speed_m_s = 0.0f; + for (uint8_t i = 0; i < _hist_idx; ++i) + last2[i] = _activity_hist[(_hist_idx - 1 - i)]; } - last_distance_for_speed_m = distance_m; - last_distance_sample_ms = now_ms; - has_distance_for_speed = true; + /* Считаем среднее за последние 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]; + } + 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"; } -uint8_t Ld2420Radar::compute_current_activity_score(bool detected_presence, float distance_m, float speed_m_s) const { - if (!detected_presence) { - return 0; - } - - /* - Основная идея: - - наличие человека уже даёт маленький базовый балл; - - движение по изменению дистанции даёт основной вклад; - - очень близкий / очень дальний человек не должен ломать шкалу. - */ - float base_score = 1.5f; - - float motion_score = 0.0f; - if (speed_m_s > cfg.activity_min_speed_m_s) { - float span = cfg.activity_max_speed_m_s - cfg.activity_min_speed_m_s; - - if (span < 0.001f) { - span = 0.001f; - } - - float normalized = - (speed_m_s - cfg.activity_min_speed_m_s) / span; - - normalized = clampf(normalized, 0.0f, 1.0f); - motion_score = normalized * 8.5f; - } - - float distance_bonus = 0.0f; - if (distance_m > 0.0f && distance_m < 6.0f) { - distance_bonus = 0.5f; - } - - int score = static_cast(roundf(base_score + motion_score + distance_bonus)); - score = clampi(score, 0, 10); - - return static_cast(score); -} - -uint8_t Ld2420Radar::compute_minute_activity_score() const { - if (second_scores_count == 0) { - return 0; - } - - uint16_t samples = min(60, second_scores_count); - uint32_t sum = 0; - - for (uint16_t i = 0; i < samples; ++i) { - int idx = static_cast(second_scores_head) - 1 - i; - if (idx < 0) { - idx += SECOND_HISTORY_SIZE; - } - - sum += second_scores[idx]; - } - - int avg = static_cast(roundf(static_cast(sum) / static_cast(samples))); - avg = clampi(avg, 0, 10); - - return static_cast(avg); -} - -String Ld2420Radar::compute_dynamics() const { - /* - Оцениваем 10 минут по минутным средним. - */ - float minute_avg[10]; - int valid_windows = 0; - - for (int minute_index = 0; minute_index < 10; ++minute_index) { - int start_offset = minute_index * 60; - int end_offset = start_offset + 60; - - if (second_scores_count < static_cast(start_offset + 1)) { - break; - } - - uint32_t sum = 0; - int count = 0; - - for (int sec = start_offset; sec < end_offset; ++sec) { - if (second_scores_count < static_cast(sec + 1)) { - break; - } - - int idx = static_cast(second_scores_head) - 1 - sec; - if (idx < 0) { - idx += SECOND_HISTORY_SIZE; - } - - sum += second_scores[idx]; - ++count; - } - - if (count > 0) { - minute_avg[valid_windows] = static_cast(sum) / static_cast(count); - ++valid_windows; - } - } - - if (valid_windows < 3) { - return "constant"; - } - - /* - Разворачиваем хронологически: oldest -> newest - */ - float ordered[10]; - for (int i = 0; i < valid_windows; ++i) { - ordered[i] = minute_avg[valid_windows - 1 - i]; - } - - float sum_y = 0.0f; - float min_y = ordered[0]; - float max_y = ordered[0]; - - for (int i = 0; i < valid_windows; ++i) { - sum_y += ordered[i]; - if (ordered[i] < min_y) min_y = ordered[i]; - if (ordered[i] > max_y) max_y = ordered[i]; - } - - float mean_y = sum_y / static_cast(valid_windows); - - float variance = 0.0f; - float sum_x = 0.0f; - float sum_x2 = 0.0f; - float sum_xy = 0.0f; - - for (int i = 0; i < valid_windows; ++i) { - float x = static_cast(i); - float y = ordered[i]; - - float dy = y - mean_y; - variance += dy * dy; - - sum_x += x; - sum_x2 += x * x; - sum_xy += x * y; - } - - float stddev = sqrtf(variance / static_cast(valid_windows)); - float range = max_y - min_y; - - float n = static_cast(valid_windows); - float denom = (n * sum_x2) - (sum_x * sum_x); - - float slope = 0.0f; - if (fabsf(denom) > 0.0001f) { - slope = ((n * sum_xy) - (sum_x * sum_y)) / denom; - } - - /* - Классификация: - - почти не меняется -> constant - - заметный устойчивый рост -> increasing - - заметное устойчивое падение -> decreasing - - иначе -> variable - */ - if (range <= 1.2f || stddev <= 0.6f) { - return "constant"; - } - - if (slope >= 0.22f) { - return "increasing"; - } - - if (slope <= -0.22f) { - return "decreasing"; - } - - return "variable"; -} +/* ------------------------------------------------------------------ */ +/* -------------------------- JSON‑вывод --------------------------*/ +/* ------------------------------------------------------------------ */ String Ld2420Radar::get_state_json() const { - String json = "{"; - json += "\"online\":"; - json += (online ? "true" : "false"); - json += ","; - json += "\"presence\":"; - json += (presence ? "true" : "false"); - json += ","; - json += "\"activity_score\":"; - json += String(compute_minute_activity_score()); - json += ","; - json += "\"activity_score_current\":"; - json += String(activity_score_current); - json += ","; - json += "\"activity_score_dynamics\":\""; - json += activity_score_dynamics; - json += "\","; - json += "\"distance_m\":"; - json += String((presence && online) ? filtered_distance_m : 0.0f, 2); - json += "}"; + String s; + s.reserve(200); + s += "{"; - return json; + s += "\"online\": "; + s += _state.online ? "true" : "false"; + s += ", "; + + s += "\"presence\": "; + s += _state.presence ? "true" : "false"; + s += ", "; + + 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; } - -String Ld2420Radar::normalize_line(const String &line) const { - String out = line; - - out.replace('\t', ' '); - out.replace(';', ' '); - out.replace('|', ' '); - out.replace(',', ' '); - out.replace(':', '='); - - while (out.indexOf(" ") >= 0) { - out.replace(" ", " "); - } - - out.trim(); - return out; -} - -bool Ld2420Radar::extract_number_by_keys(const String &line, const char *keys[], size_t key_count, float &value_out) const { - for (size_t i = 0; i < key_count; ++i) { - if (parse_number_after_key(line, String(keys[i]), value_out)) { - return true; - } - } - return false; -} - -bool Ld2420Radar::extract_bool_by_keys(const String &line, const char *keys[], size_t key_count, bool &value_out) const { - for (size_t i = 0; i < key_count; ++i) { - if (parse_bool_after_key(line, String(keys[i]), value_out)) { - return true; - } - } - return false; -} - -bool Ld2420Radar::parse_number_after_key(const String &line, const String &key, float &value_out) const { - int key_pos = line.indexOf(key); - if (key_pos < 0) { - return false; - } - - int eq_pos = line.indexOf('=', key_pos + key.length()); - if (eq_pos < 0) { - return false; - } - - int start = eq_pos + 1; - while (start < static_cast(line.length()) && line[start] == ' ') { - ++start; - } - - if (start >= static_cast(line.length())) { - return false; - } - - int end = start; - bool has_digit = false; - - if (line[end] == '+' || line[end] == '-') { - ++end; - } - - while (end < static_cast(line.length())) { - char ch = line[end]; - - if ((ch >= '0' && ch <= '9') || ch == '.') { - has_digit = true; - ++end; - continue; - } - - break; - } - - if (!has_digit) { - return false; - } - - value_out = line.substring(start, end).toFloat(); - return true; -} - -bool Ld2420Radar::parse_bool_after_key(const String &line, const String &key, bool &value_out) const { - int key_pos = line.indexOf(key); - if (key_pos < 0) { - return false; - } - - int eq_pos = line.indexOf('=', key_pos + key.length()); - if (eq_pos < 0) { - return false; - } - - int start = eq_pos + 1; - while (start < static_cast(line.length()) && line[start] == ' ') { - ++start; - } - - if (start >= static_cast(line.length())) { - return false; - } - - String tail = line.substring(start); - tail.trim(); - tail.toLowerCase(); - - if (tail.startsWith("1") || tail.startsWith("true") || tail.startsWith("on") || tail.startsWith("yes") || tail.startsWith("detect")) { - value_out = true; - return true; - } - - if (tail.startsWith("0") || tail.startsWith("false") || tail.startsWith("off") || tail.startsWith("no") || tail.startsWith("clear")) { - value_out = false; - return true; - } - - return false; -} - -float Ld2420Radar::zone_to_meters(float zone) const { - return (zone * cfg.zone_to_meter_k) + cfg.zone_to_meter_b; -} - -bool Ld2420Radar::is_distance_plausible(float distance_m) const { - return distance_m >= 0.0f && distance_m <= 8.5f; -} - -float Ld2420Radar::clampf(float value, float min_value, float max_value) const { - if (value < min_value) return min_value; - if (value > max_value) return max_value; - return value; -} - -int Ld2420Radar::clampi(int value, int min_value, int max_value) const { - if (value < min_value) return min_value; - if (value > max_value) return max_value; - return value; -} \ No newline at end of file diff --git a/devices/sensor/ld2420_radar.h b/devices/sensor/ld2420_radar.h index 1f9cdd0..fe28b64 100644 --- a/devices/sensor/ld2420_radar.h +++ b/devices/sensor/ld2420_radar.h @@ -1,111 +1,104 @@ #pragma once #include +#include + +/* ------------------------------------------------------------------ */ +/* ----------------------- Конфигурация радара ---------------------*/ +/* ------------------------------------------------------------------ */ struct RadarConfig { - int uart_rx_pin = 4; - int uart_tx_pin = 15; - uint32_t baud_rate = 115200; + uint8_t uart_rx_pin; + uint8_t uart_tx_pin; + uint32_t baud_rate = 115200; - uint32_t presence_hold_ms = 1500; - uint32_t stale_after_ms = 2000; + /* Хранение данных */ + uint16_t presence_hold_ms = 1500; // Как долго держать флаг «presence» после последнего сигнала + uint16_t stale_after_ms = 2000; // Если не пришло ни одной строки – считаем offline - float distance_ema_alpha = 0.35f; + /* Фильтры */ + uint8_t median_window_size = 5; + uint8_t max_zone_step_per_sample = 4; - int min_valid_zone = 1; - int max_valid_zone = 200; + /* EMA‑сглаживание (α) */ + float distance_ema_alpha = 0.35f; + float speed_ema_alpha = 0.25f; - float zone_to_meter_k = 0.7f; - float zone_to_meter_b = 0.0f; + /* Диапазон зон, которые считаем «действительными» */ + uint16_t min_valid_zone = 1; + uint16_t max_valid_zone = 200; - /* - Порог минимального движения, ниже которого - текущая активность почти не растёт. - */ - float activity_min_speed_m_s = 0.03f; + /* Перевод зоны в метры (параметры калибровки) */ + float zone_to_meter_k = 0.7f; // slope + float zone_to_meter_b = 0.0f; // intercept - /* - Скорость, при которой моментная активность - выходит примерно к 10. - */ - float activity_max_speed_m_s = 1.20f; + /* Шкала активности: скорость, с которой считается «активным» */ + float activity_min_speed_m_s = 0.03f; + float activity_max_speed_m_s = 1.20f; - bool enable_debug_unknown_lines = false; + bool enable_debug_unknown_lines = 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; +}; + +/* ------------------------------------------------------------------ */ +/* ------------------------ Основной класс радара -------------------*/ +/* ------------------------------------------------------------------ */ + class Ld2420Radar { public: - Ld2420Radar(); + /* Инициализация */ + void begin(Stream &uart, const RadarConfig &cfg); - bool begin(HardwareSerial &serial, const RadarConfig &config); + /* Обновление состояния – вызывать каждый цикл (или в таймере) */ void update(); + /* Получить JSON‑строку текущего состояния */ String get_state_json() const; private: - static const size_t LINE_BUFFER_SIZE = 160; - static const size_t SECOND_HISTORY_SIZE = 600; // 10 минут по 1 секунде + /* Внутренние данные */ + Stream * _uart = nullptr; + RadarConfig _cfg; + RadarState _state{}; - HardwareSerial *serial_port = nullptr; - RadarConfig cfg; + /* Тайминг */ + uint32_t _last_line_ms = 0; // Когда пришла последняя строка + uint32_t _presence_start_ms = 0; // Когда появился первый сигнал - char line_buffer[LINE_BUFFER_SIZE]; - size_t line_length = 0; + /* Буфер для чтения строки */ + static const size_t LINE_BUF_SIZE = 64; + char _line_buf[LINE_BUF_SIZE]; + size_t _line_pos = 0; - bool initialized = false; + /* Хранение последних 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; - bool online = false; - bool raw_presence = false; - bool presence = 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(); - float raw_distance_m = 0.0f; - float filtered_distance_m = 0.0f; - - float current_speed_m_s = 0.0f; - uint8_t activity_score_current = 0; - - uint32_t last_frame_ms = 0; - uint32_t last_presence_ms = 0; - uint32_t last_distance_sample_ms = 0; - - float last_distance_for_speed_m = 0.0f; - bool has_distance_for_speed = false; - - uint8_t second_scores[SECOND_HISTORY_SIZE]; - uint16_t second_scores_head = 0; - uint16_t second_scores_count = 0; - uint32_t last_second_push_ms = 0; - - String activity_score_dynamics = "constant"; - -private: - void reset_runtime_state(); - - void process_incoming_byte(char ch); - void process_line(const char *line_cstr); - - void update_online_state(uint32_t now_ms); - void update_presence_state(uint32_t now_ms); - void update_second_history(uint32_t now_ms); - - bool parse_ascii_line(const String &line); - - bool extract_number_by_keys(const String &line, const char *keys[], size_t key_count, float &value_out) const; - bool extract_bool_by_keys(const String &line, const char *keys[], size_t key_count, bool &value_out) const; - - bool parse_number_after_key(const String &line, const String &key, float &value_out) const; - bool parse_bool_after_key(const String &line, const String &key, bool &value_out) const; - - String normalize_line(const String &line) const; - float clampf(float value, float min_value, float max_value) const; - int clampi(int value, int min_value, int max_value) const; - - float zone_to_meters(float zone) const; - void apply_new_distance(float distance_m, uint32_t now_ms); - - uint8_t compute_current_activity_score(bool detected_presence, float distance_m, float speed_m_s) const; - uint8_t compute_minute_activity_score() const; - String compute_dynamics() const; - - bool is_distance_plausible(float distance_m) const; -}; \ No newline at end of file + /* Утилиты */ + static bool _is_hex_digit(char c) { + return (c >= '0' && c <= '9') || (c >= 'A' && c <= 'F'); + } +}; diff --git a/devices/sensor/sensor.ino b/devices/sensor/sensor.ino index f5b98e7..199af5e 100644 --- a/devices/sensor/sensor.ino +++ b/devices/sensor/sensor.ino @@ -1,15 +1,18 @@ +/********************************************************************* + * sensor.ino + * + * Проект «Radar + BH1750 + BME/BMP280» – полный пример. + * В нём используется новый класс Ld2420Radar (файлы ld2420_radar.h/.cpp). + *********************************************************************/ + #include -#include "ld2420_radar.h" -#include "bh1750_sensor.h" -#include "bme280_sensor.h" +/* ---------- СТАНДАРТНЫЕ БИБЛИОТЕКИ ДЛЯ ИНТЕРФЕЙСОВ ------------- */ +#include "ld2420_radar.h" // радар LD‑2420 (2.1) +#include "bh1750_sensor.h" // BH1750 – датчик освещённости +#include "bme280_sensor.h" // BME280/BMP280 – климатический датчик -/* - ========================= - GLOBAL OBJECTS - ========================= -*/ - +/* --------------------- ГЛОБАЛЬНЫЕ ОБЪЕКТЫ ---------------------- */ Ld2420Radar radar; RadarConfig radar_config; @@ -19,118 +22,88 @@ Bme280Sensor climate_sensor; Bme280Config climate_config; -/* - Отдельные I2C-шины. - 0-я шина для BH1750. - 1-я шина для BME/BMP280. -*/ -TwoWire i2c_climate = TwoWire(1); +/* ----------------- Отдельные I2C‑шины (ESP32) ------------------- */ +TwoWire i2c_climate = TwoWire(1); // шина для BME/BMP280 -/* - Интервал печати состояния. -*/ +/* ----------------- Интервал печати состояния ------------------- */ static const uint32_t print_interval_ms = 500; uint32_t last_print_ms = 0; - -/* - ========================= - RADAR CONFIGURATION - ========================= -*/ +/* ------------------------------------------------------------------ */ +/* --------------------- Конфигурация радара ----------------------*/ +/* ------------------------------------------------------------------ */ void setup_radar_config() { - radar_config.uart_rx_pin = 4; - radar_config.uart_tx_pin = 15; - radar_config.baud_rate = 115200; + /* Пины UART2 (по умолчанию GPIO4 – OT1, GPIO15 – RX) */ + 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.presence_hold_ms = 1500; // как долго держать presence + radar_config.stale_after_ms = 2000; // если не пришло данных – offline - radar_config.distance_ema_alpha = 0.35f; + radar_config.median_window_size = 5; + radar_config.max_zone_step_per_sample = 4; - radar_config.min_valid_zone = 1; - radar_config.max_valid_zone = 200; + radar_config.distance_ema_alpha = 0.35f; + radar_config.speed_ema_alpha = 0.25f; - radar_config.zone_to_meter_k = 0.7f; - radar_config.zone_to_meter_b = 0.0f; + 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 = true; + radar_config.enable_debug_unknown_lines = false; } - -/* - ========================= - BH1750 CONFIGURATION - ========================= -*/ +/* ------------------------------------------------------------------ */ +/* --------------------- Конфигурация BH1750 ----------------------*/ +/* ------------------------------------------------------------------ */ void setup_light_config() { - /* - Отдельная шина освещённости: - 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; + 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; } - -/* - ========================= - BME/BMP280 CONFIGURATION - ========================= -*/ +/* ------------------------------------------------------------------ */ +/* --------------------- Конфигурация BME/BMP280 -------------------*/ +/* ------------------------------------------------------------------ */ void setup_climate_config() { - /* - Отдельная климатическая шина: - 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.sda_pin = 18; // SDA + climate_config.scl_pin = 19; // SCL + climate_config.i2c_address = 0x76; // адрес при подключении к GND - 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(); - - bool ok = light_sensor.begin(light_config); - - if (ok) { + if (light_sensor.begin(light_config)) { Serial.println("BH1750 initialized"); } else { Serial.println("BH1750 init failed"); @@ -139,10 +112,7 @@ void init_climate_sensor() { setup_climate_config(); - - bool ok = climate_sensor.begin(i2c_climate, climate_config); - - if (ok) { + if (climate_sensor.begin(i2c_climate, climate_config)) { Serial.print("Climate sensor initialized: "); Serial.println(climate_sensor.get_sensor_type_string()); } else { @@ -150,33 +120,27 @@ } } - -/* - ========================= - SETUP - ========================= -*/ +/* ------------------------------------------------------------------ */ +/* --------------------------- SETUP --------------------------------*/ +/* ------------------------------------------------------------------ */ void setup() { Serial.begin(115200); delay(1000); - Serial.println(); - Serial.println("Radar + BH1750 + BME/BMP280 project start"); + Serial.println("\nRadar + 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(); @@ -186,20 +150,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 +}