diff --git a/devices/sensor/ld2420_radar.cpp b/devices/sensor/ld2420_radar.cpp index dffd0f3..3f54e24 100644 --- a/devices/sensor/ld2420_radar.cpp +++ b/devices/sensor/ld2420_radar.cpp @@ -1,345 +1,661 @@ #include "ld2420_radar.h" -#include - -/* - ========================= - UTILS - ========================= -*/ - -static uint8_t clamp_score_0_10(int value) { - if (value < 0) return 0; - if (value > 10) return 10; - return (uint8_t)value; -} - - -/* - ========================= - CTOR - ========================= -*/ - Ld2420Radar::Ld2420Radar() { - memset(&state, 0, sizeof(state)); + reset_runtime_state(); } +bool Ld2420Radar::begin(HardwareSerial &serial, const RadarConfig &config) { + serial_port = &serial; + cfg = config; -/* - ========================= - INIT - ========================= -*/ + reset_runtime_state(); -void Ld2420Radar::begin(HardwareSerial &uart_port, const RadarConfig &cfg) { - uart = &uart_port; - config = cfg; + serial_port -> begin(cfg.baud_rate, SERIAL_8N1, cfg.uart_rx_pin, cfg.uart_tx_pin); + initialized = true; - uart -> begin(config.baud_rate, SERIAL_8N1, config.uart_rx_pin, config.uart_tx_pin); - - last_data_time_ms = 0; - last_presence_time_ms = 0; - - last_distance_m = 0.0f; - last_update_time_ms = millis(); - - memset(activity_history, 0, sizeof(activity_history)); - activity_history_index = 0; - activity_history_filled = false; + return true; } +void Ld2420Radar::reset_runtime_state() { + line_length = 0; + line_buffer[0] = '\0'; -/* - ========================= - MAIN UPDATE - ========================= -*/ + 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() { - read_uart(); - update_online_state(); - update_presence(); - update_motion(); - update_activity(); + if (!initialized || serial_port == nullptr) { + 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(); } - -/* - ========================= - UART PARSING - ========================= -*/ - -void Ld2420Radar::read_uart() { - while (uart -> available()) { - char c = uart -> read(); - - if (c == '\n') { +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_buffer = ""; - } else if (c != '\r') { - line_buffer += c; + 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(); -void Ld2420Radar::process_line(const String &line) { /* - Ожидаем формат типа: - "Range 42" + Поддерживаем разные возможные названия полей. + Это сделано специально, чтобы не завязаться + на единственный вариант строки. */ + static const char *presence_keys[] = { + "presence", "pres", "occupancy", "occupied", "target", + "detected", "detect", "exist", "state" + }; - if (line.startsWith("Range ")) { - int zone = line.substring(6).toInt(); + static const char *distance_meter_keys[] = { + "distance_m", "dist_m", "dis_m", "range_m", "r_m", "meter", "meters" + }; - if (zone >= config.min_valid_zone && zone <= config.max_valid_zone) { - handle_range(zone); + 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; } - last_data_time_ms = millis(); + 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; + } + } } -} - - -/* - ========================= - RANGE HANDLING - ========================= -*/ - -void Ld2420Radar::handle_range(int zone) { - float distance = config.zone_to_meter_k * zone + config.zone_to_meter_b; /* - EMA сглаживание дистанции + 2. Presence: + - если есть явный флаг, используем его; + - иначе presence выводим из валидной дистанции + или из move_value > 0. */ - state.distance_m = - config.distance_ema_alpha * distance + - (1.0f - config.distance_ema_alpha) * state.distance_m; - - last_presence_time_ms = millis(); - state.presence = true; -} - - -/* - ========================= - ONLINE / PRESENCE - ========================= -*/ - -void Ld2420Radar::update_online_state() { - uint32_t now = millis(); - - state.online = (now - last_data_time_ms <= config.stale_after_ms); -} - - -void Ld2420Radar::update_presence() { - uint32_t now = millis(); - - if (now - last_presence_time_ms > config.presence_hold_ms) { - state.presence = false; - } -} - - -/* - ========================= - MOTION / SPEED - ========================= -*/ - -void Ld2420Radar::update_motion() { - uint32_t now = millis(); - float dt = (now - last_update_time_ms) / 1000.0f; - - if (dt <= 0.0f) return; - - float speed = (state.distance_m - last_distance_m) / dt; - - /* - EMA по скорости - */ - state.speed_m_s = - config.speed_ema_alpha * speed + - (1.0f - config.speed_ema_alpha) * state.speed_m_s; - - last_distance_m = state.distance_m; - last_update_time_ms = now; -} - - -/* - ========================= - ACTIVITY - ========================= -*/ - -uint8_t Ld2420Radar::speed_to_activity_score(float abs_speed) const { - if (!state.presence) return 0; - - if (abs_speed <= config.activity_min_speed_m_s) return 1; - if (abs_speed >= config.activity_max_speed_m_s) return 10; - - float norm = - (abs_speed - config.activity_min_speed_m_s) / - (config.activity_max_speed_m_s - config.activity_min_speed_m_s); - - int score = 1 + (int)(norm * 9.0f + 0.5f); - - return clamp_score_0_10(score); -} - - -void Ld2420Radar::push_activity_sample(uint8_t score) { - activity_history[activity_history_index] = score; - - activity_history_index++; - if (activity_history_index >= ACTIVITY_HISTORY_SIZE) { - activity_history_index = 0; - activity_history_filled = true; - } -} - - -uint8_t Ld2420Radar::calculate_activity_average() const { - uint32_t sum = 0; - uint16_t count = 0; - - uint16_t max_count = activity_history_filled - ? ACTIVITY_HISTORY_SIZE - : activity_history_index; - - for (uint16_t i = 0; i < max_count; i++) { - sum += activity_history[i]; - count++; - } - - if (count == 0) return 0; - - return clamp_score_0_10(sum / count); -} - - -ActivityDynamics Ld2420Radar::calculate_activity_dynamics() const { - uint16_t max_count = activity_history_filled - ? ACTIVITY_HISTORY_SIZE - : activity_history_index; - - if (max_count < 20) { - return ACTIVITY_DYNAMICS_CONSTANT; - } - - uint16_t half = max_count / 2; - - uint32_t sum_old = 0; - uint32_t sum_new = 0; - - for (uint16_t i = 0; i < half; i++) { - sum_old += activity_history[i]; - } - - for (uint16_t i = half; i < max_count; i++) { - sum_new += activity_history[i]; - } - - float avg_old = (float)sum_old / half; - float avg_new = (float)sum_new / (max_count - half); - - float diff = avg_new - avg_old; - - /* - variance → детекция "variable" - */ - float variance = 0; - float avg_total = (avg_old + avg_new) * 0.5f; - - for (uint16_t i = 0; i < max_count; i++) { - float d = activity_history[i] - avg_total; - variance += d * d; - } - variance /= max_count; - - if (variance > 6.0f) { - return ACTIVITY_DYNAMICS_VARIABLE; - } - - if (fabs(diff) < 0.5f) { - return ACTIVITY_DYNAMICS_CONSTANT; - } - - if (diff > 0) { - return ACTIVITY_DYNAMICS_INCREASING; + if (has_explicit_presence) { + raw_presence = explicit_presence; + got_anything = true; } else { - return ACTIVITY_DYNAMICS_DECREASING; + 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; + } } } +void Ld2420Radar::apply_new_distance(float distance_m, uint32_t now_ms) { + if (!is_distance_plausible(distance_m)) { + return; + } -void Ld2420Radar::update_activity() { - float abs_speed = fabs(state.speed_m_s); + if (filtered_distance_m <= 0.0f) { + filtered_distance_m = distance_m; + } else { + filtered_distance_m = + (cfg.distance_ema_alpha * distance_m) + + ((1.0f - cfg.distance_ema_alpha) * filtered_distance_m); + } - state.activity_score_current = speed_to_activity_score(abs_speed); + 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; - push_activity_sample(state.activity_score_current); + if (dt_s > 0.02f) { + float speed = fabsf(distance_m - last_distance_for_speed_m) / dt_s; + current_speed_m_s = speed; + } + } else { + current_speed_m_s = 0.0f; + } - state.activity_score = calculate_activity_average(); - - state.activity_score_dynamics = calculate_activity_dynamics(); + last_distance_for_speed_m = distance_m; + last_distance_sample_ms = now_ms; + has_distance_for_speed = true; } +uint8_t Ld2420Radar::compute_current_activity_score(bool detected_presence, float distance_m, float speed_m_s) const { + if (!detected_presence) { + return 0; + } -/* - ========================= - JSON OUTPUT - ========================= -*/ + /* + Основная идея: + - наличие человека уже даёт маленький базовый балл; + - движение по изменению дистанции даёт основной вклад; + - очень близкий / очень дальний человек не должен ломать шкалу. + */ + 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"; +} String Ld2420Radar::get_state_json() const { - String json; - json.reserve(192); - - json += "{"; - + String json = "{"; json += "\"online\":"; - json += (state.online ? "true" : "false"); - - json += ",\"presence\":"; - json += (state.presence ? "true" : "false"); - - json += ",\"activity_score\":"; - json += String(clamp_score_0_10(state.activity_score)); - - json += ",\"activity_score_current\":"; - json += String(clamp_score_0_10(state.activity_score_current)); - - json += ",\"activity_score_dynamics\":\""; - json += activity_dynamics_to_string(state.activity_score_dynamics); - json += "\""; - - json += ",\"distance_m\":"; - json += String(state.distance_m, 2); - + 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 += "}"; return json; } +String Ld2420Radar::normalize_line(const String &line) const { + String out = line; -/* - ========================= - HELPERS - ========================= -*/ + out.replace('\t', ' '); + out.replace(';', ' '); + out.replace('|', ' '); + out.replace(',', ' '); + out.replace(':', '='); -const char *Ld2420Radar::activity_dynamics_to_string(ActivityDynamics d) const { - switch (d) { - case ACTIVITY_DYNAMICS_INCREASING: return "increasing"; - case ACTIVITY_DYNAMICS_DECREASING: return "decreasing"; - case ACTIVITY_DYNAMICS_VARIABLE: return "variable"; - default: return "constant"; + 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 e8dc8ec..1f9cdd0 100644 --- a/devices/sensor/ld2420_radar.h +++ b/devices/sensor/ld2420_radar.h @@ -1,270 +1,111 @@ -#ifndef LD2420_RADAR_H -#define LD2420_RADAR_H +#pragma once #include -/* - Упрощённая динамика активности. - - constant: - Активность в среднем не растёт и не падает. - - increasing: - Во второй половине 10-минутного окна активность заметно выше, - чем в первой. - - decreasing: - Во второй половине 10-минутного окна активность заметно ниже, - чем в первой. - - variable: - Активность слишком рваная/хаотичная, простая линейная трактовка - не даёт хорошего описания. -*/ -enum RadarActivityDynamics { - RADAR_ACTIVITY_DYNAMICS_CONSTANT = 0, - RADAR_ACTIVITY_DYNAMICS_INCREASING, - RADAR_ACTIVITY_DYNAMICS_DECREASING, - RADAR_ACTIVITY_DYNAMICS_VARIABLE -}; - -/* - Конфигурация радара и алгоритмов обработки. - - Сейчас это именно практичная конфигурация под первый рабочий слой, - без лишней "умности", которую можно нарастить позже. -*/ struct RadarConfig { - /* - UART-настройки для LD2420. - Под текущую схему: - OT1 радара -> RX ESP32 - RX радара -> TX ESP32 - */ int uart_rx_pin = 4; int uart_tx_pin = 15; uint32_t baud_rate = 115200; - /* - Если сырое присутствие перестало приходить, presence ещё немного - удерживается, чтобы не дёргаться от кратких провалов. - */ uint32_t presence_hold_ms = 1500; - - /* - Если слишком давно не было новых строк по UART, считаем радар - неактуальным и помечаем online=false. - */ uint32_t stale_after_ms = 2000; - /* - Базовые фильтры Range. - */ - uint8_t median_window_size = 5; - uint16_t max_zone_step_per_sample = 4; - - /* - EMA для дистанции и скорости. - */ float distance_ema_alpha = 0.35f; - float speed_ema_alpha = 0.25f; - /* - Границы допустимых зон. - Пока можно держать широкий диапазон, а потом сузить. - */ - uint16_t min_valid_zone = 1; - uint16_t max_valid_zone = 200; + int min_valid_zone = 1; + int max_valid_zone = 200; - /* - Преобразование Range -> distance_m - Пока это приближённая калибровка. - Позже подгонишь по реальным замерам. - */ float zone_to_meter_k = 0.7f; float zone_to_meter_b = 0.0f; /* - Настройка расчёта текущей активности из скорости. - Ниже этого значения считаем, что активность почти отсутствует. + Порог минимального движения, ниже которого + текущая активность почти не растёт. */ float activity_min_speed_m_s = 0.03f; /* - При такой скорости current activity уже выходит примерно к 10. - Это не "физический предел", а просто верхняя точка шкалы. + Скорость, при которой моментная активность + выходит примерно к 10. */ float activity_max_speed_m_s = 1.20f; - /* - Если нужно логировать строки, которые не распарсились. - */ bool enable_debug_unknown_lines = false; }; -/* - Итоговое упрощённое состояние, которое сейчас действительно нужно - устройству и системе умного дома. - - online: - Есть актуальные данные от радара. - - presence: - Есть присутствие после фильтрации и hold-механизма. - - activity_score: - Средняя активность за последнюю минуту, шкала 0..10. - - activity_score_current: - Текущая активность, шкала 0..10. - - activity_score_dynamics: - Характер динамики активности за последние 10 минут. - - distance_m: - Отфильтрованная текущая дистанция до цели. -*/ -struct RadarState { - bool online = false; - bool presence = false; - - uint8_t activity_score = 0; - uint8_t activity_score_current = 0; - RadarActivityDynamics activity_score_dynamics = RADAR_ACTIVITY_DYNAMICS_CONSTANT; - - float distance_m = -1.0f; - - /* - Ниже — служебные поля, полезные для внутренней логики - и отладки, но их не обязательно отдавать наружу. - */ - bool raw_presence = false; - bool stale = true; - - uint16_t raw_distance_zone = 0; - uint16_t filtered_distance_zone = 0; - - float radial_speed_m_s = 0.0f; - - uint32_t last_on_ms = 0; - uint32_t last_off_ms = 0; - uint32_t last_range_ms = 0; - uint32_t last_update_ms = 0; - - uint32_t line_counter = 0; - uint32_t parse_error_counter = 0; - uint32_t ignored_range_counter = 0; -}; - -/* - Первый рабочий слой для LD2420 в текстовом UART-режиме. - - Что умеет: - - читать ON / OFF / Range N - - фильтровать Range - - считать distance_m - - считать текущую активность - - считать усреднённую активность за минуту - - оценивать динамику активности за 10 минут - - отдавать данные как JSON-строку - - Что пока не умеет: - - несколько целей - - кошки/люди - - углы и направления по комнате - - fusion нескольких радаров -*/ class Ld2420Radar { public: Ld2420Radar(); - void begin(HardwareSerial &uart_port, const RadarConfig &config); + bool begin(HardwareSerial &serial, const RadarConfig &config); void update(); - const RadarState &get_state() const; String get_state_json() const; - void set_config(const RadarConfig &config); - const RadarConfig &get_config() const; +private: + static const size_t LINE_BUFFER_SIZE = 160; + static const size_t SECOND_HISTORY_SIZE = 600; // 10 минут по 1 секунде + + HardwareSerial *serial_port = nullptr; + RadarConfig cfg; + + char line_buffer[LINE_BUFFER_SIZE]; + size_t line_length = 0; + + bool initialized = false; + + bool online = false; + bool raw_presence = false; + bool presence = false; + + 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: - static const uint8_t max_supported_median_window = 9; - - /* - 10-минутная история активности: - 60 бакетов по 10 секунд. - Этого достаточно, чтобы: - - получить последнюю минуту как последние 6 бакетов - - получить динамику за 10 минут как анализ всех 60 бакетов - */ - static const uint8_t activity_bucket_count = 60; - static const uint32_t activity_bucket_duration_ms = 10000UL; - - static const uint16_t ACTIVITY_HISTORY_SIZE = 120; // если update ~2 раза в секунду - - uint8_t activity_history[ACTIVITY_HISTORY_SIZE]; - uint16_t activity_history_index = 0; - bool activity_history_filled = false; - - HardwareSerial *uart; - RadarConfig config; - RadarState state; - - String line_buffer; - - uint16_t zone_window[max_supported_median_window]; - uint8_t zone_window_count; - uint8_t zone_window_index; - - bool has_filtered_zone; - uint16_t last_filtered_zone; - - bool has_smoothed_distance; - float smoothed_distance_m; - - bool has_smoothed_speed; - float smoothed_speed_m_s; - - /* - История активности. - В каждом бакете храним усреднённую активность за 10 секунд. - */ - uint8_t activity_buckets[activity_bucket_count]; - uint32_t activity_bucket_start_ms; - uint32_t activity_bucket_accum_sum; - uint16_t activity_bucket_accum_count; - uint8_t activity_bucket_index; - bool activity_history_filled; - void reset_runtime_state(); - void read_uart_lines(); - void handle_line(const String &line); - void handle_on_line(); - void handle_off_line(); - void handle_range_line(const String &line); + void process_incoming_byte(char ch); + void process_line(const char *line_cstr); - void push_zone_sample(uint16_t zone); - uint16_t get_median_zone() const; - uint16_t apply_zone_step_limit(uint16_t candidate_zone); + void update_online_state(uint32_t now_ms); + void update_presence_state(uint32_t now_ms); + void update_second_history(uint32_t now_ms); - float zone_to_distance_m(uint16_t zone) const; - float apply_distance_smoothing(float new_distance_m); - void update_speed(float filtered_distance_m, uint32_t now_ms); + bool parse_ascii_line(const String &line); - void apply_presence_hold(); - void update_online_state(); + 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; - uint8_t speed_to_activity_score(float abs_speed_m_s) 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; - void update_activity_history(); - void commit_current_activity_bucket(uint32_t now_ms); - void update_activity_outputs(); + 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; - RadarActivityDynamics calculate_activity_dynamics() const; - const char *activity_dynamics_to_string(RadarActivityDynamics dynamics) const; + float zone_to_meters(float zone) const; + void apply_new_distance(float distance_m, uint32_t now_ms); - bool is_zone_valid(uint16_t zone) const; -}; + 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; -#endif \ No newline at end of file + bool is_distance_plausible(float distance_m) const; +}; \ No newline at end of file diff --git a/devices/sensor/sensor.ino b/devices/sensor/sensor.ino index 6c0c5cd..f5b98e7 100644 --- a/devices/sensor/sensor.ino +++ b/devices/sensor/sensor.ino @@ -40,60 +40,25 @@ */ void setup_radar_config() { - /* - Подключение радара: - OT1 радара -> GPIO4 ESP32 (RX) - RX радара -> GPIO15 ESP32 (TX) - */ radar_config.uart_rx_pin = 4; radar_config.uart_tx_pin = 15; radar_config.baud_rate = 115200; - /* - Удержание presence после кратковременного исчезновения сырых данных. - */ radar_config.presence_hold_ms = 1500; - - /* - Если давно не было новых строк от радара, online станет false. - */ radar_config.stale_after_ms = 2000; - /* - Базовые фильтры Range. - */ - radar_config.median_window_size = 5; - radar_config.max_zone_step_per_sample = 4; - - /* - EMA-сглаживание дистанции и скорости. - */ radar_config.distance_ema_alpha = 0.35f; - radar_config.speed_ema_alpha = 0.25f; - /* - Пока широкий диапазон допустимых зон. - Позже подрежешь под корпус, сектор и реальную геометрию установки. - */ 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; - /* - Шкала текущей активности. - Ниже activity_min_speed_m_s движение почти не учитывается. - Около activity_max_speed_m_s текущая активность выходит к 10. - */ 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_unknown_lines = true; }