diff --git a/devices/sensor/ld2420_radar.cpp b/devices/sensor/ld2420_radar.cpp index bbfc140..dffd0f3 100644 --- a/devices/sensor/ld2420_radar.cpp +++ b/devices/sensor/ld2420_radar.cpp @@ -1,64 +1,309 @@ #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 static_cast(value); + if (value < 0) return 0; + if (value > 10) return 10; + return (uint8_t)value; } + +/* + ========================= + CTOR + ========================= +*/ + Ld2420Radar::Ld2420Radar() { - uart = nullptr; - reset_runtime_state(); + memset(&state, 0, sizeof(state)); } -void Ld2420Radar::begin(HardwareSerial &uart_port, const RadarConfig &new_config) { + +/* + ========================= + INIT + ========================= +*/ + +void Ld2420Radar::begin(HardwareSerial &uart_port, const RadarConfig &cfg) { uart = &uart_port; - config = new_config; + config = cfg; - if (config.median_window_size == 0) { - config.median_window_size = 1; - } - if (config.median_window_size > max_supported_median_window) { - config.median_window_size = max_supported_median_window; - } + uart -> begin(config.baud_rate, SERIAL_8N1, config.uart_rx_pin, config.uart_tx_pin); - line_buffer.reserve(96); + last_data_time_ms = 0; + last_presence_time_ms = 0; - uart->begin( - config.baud_rate, - SERIAL_8N1, - config.uart_rx_pin, - config.uart_tx_pin - ); + last_distance_m = 0.0f; + last_update_time_ms = millis(); - reset_runtime_state(); - - uint32_t now_ms = millis(); - state.last_update_ms = now_ms; - activity_bucket_start_ms = now_ms; + memset(activity_history, 0, sizeof(activity_history)); + activity_history_index = 0; + activity_history_filled = false; } + +/* + ========================= + MAIN UPDATE + ========================= +*/ + void Ld2420Radar::update() { - read_uart_lines(); - apply_presence_hold(); + read_uart(); update_online_state(); - update_activity_history(); - update_activity_outputs(); + update_presence(); + update_motion(); + update_activity(); } -const RadarState &Ld2420Radar::get_state() const { - return state; + +/* + ========================= + UART PARSING + ========================= +*/ + +void Ld2420Radar::read_uart() { + while (uart -> available()) { + char c = uart -> read(); + + if (c == '\n') { + process_line(line_buffer); + line_buffer = ""; + } else if (c != '\r') { + line_buffer += c; + } + } } + +void Ld2420Radar::process_line(const String &line) { + /* + Ожидаем формат типа: + "Range 42" + */ + + if (line.startsWith("Range ")) { + int zone = line.substring(6).toInt(); + + if (zone >= config.min_valid_zone && zone <= config.max_valid_zone) { + handle_range(zone); + } + + last_data_time_ms = millis(); + } +} + + +/* + ========================= + RANGE HANDLING + ========================= +*/ + +void Ld2420Radar::handle_range(int zone) { + float distance = config.zone_to_meter_k * zone + config.zone_to_meter_b; + + /* + EMA сглаживание дистанции + */ + 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; + } else { + return ACTIVITY_DYNAMICS_DECREASING; + } +} + + +void Ld2420Radar::update_activity() { + float abs_speed = fabs(state.speed_m_s); + + state.activity_score_current = speed_to_activity_score(abs_speed); + + push_activity_sample(state.activity_score_current); + + state.activity_score = calculate_activity_average(); + + state.activity_score_dynamics = calculate_activity_dynamics(); +} + + +/* + ========================= + JSON OUTPUT + ========================= +*/ + String Ld2420Radar::get_state_json() const { String json; json.reserve(192); json += "{"; + json += "\"online\":"; json += (state.online ? "true" : "false"); @@ -83,506 +328,18 @@ return json; } -void Ld2420Radar::set_config(const RadarConfig &new_config) { - config = new_config; - if (config.median_window_size == 0) { - config.median_window_size = 1; +/* + ========================= + HELPERS + ========================= +*/ + +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"; } - if (config.median_window_size > max_supported_median_window) { - config.median_window_size = max_supported_median_window; - } -} - -const RadarConfig &Ld2420Radar::get_config() const { - return config; -} - -void Ld2420Radar::reset_runtime_state() { - state = RadarState(); - - line_buffer = ""; - - zone_window_count = 0; - zone_window_index = 0; - for (uint8_t i = 0; i < max_supported_median_window; i++) { - zone_window[i] = 0; - } - - has_filtered_zone = false; - last_filtered_zone = 0; - - has_smoothed_distance = false; - smoothed_distance_m = 0.0f; - - has_smoothed_speed = false; - smoothed_speed_m_s = 0.0f; - - for (uint8_t i = 0; i < activity_bucket_count; i++) { - activity_buckets[i] = 0; - } - - activity_bucket_start_ms = 0; - activity_bucket_accum_sum = 0; - activity_bucket_accum_count = 0; - activity_bucket_index = 0; - activity_history_filled = false; -} - -void Ld2420Radar::read_uart_lines() { - if (uart == nullptr) { - return; - } - - while (uart->available()) { - char c = static_cast(uart->read()); - - if (c == '\n') { - handle_line(line_buffer); - line_buffer = ""; - } else if (c != '\r') { - line_buffer += c; - - if (line_buffer.length() > 120) { - line_buffer = ""; - state.parse_error_counter++; - } - } - } -} - -void Ld2420Radar::handle_line(const String &line_raw) { - String line = line_raw; - line.trim(); - - if (line.length() == 0) { - return; - } - - state.line_counter++; - state.last_update_ms = millis(); - - if (line == "ON") { - handle_on_line(); - return; - } - - if (line == "OFF") { - handle_off_line(); - return; - } - - if (line.startsWith("Range ")) { - handle_range_line(line); - return; - } - - if (config.enable_debug_unknown_lines) { - Serial.print("unknown_line: "); - Serial.println(line); - } - - state.parse_error_counter++; -} - -void Ld2420Radar::handle_on_line() { - state.raw_presence = true; - state.last_on_ms = state.last_update_ms; -} - -void Ld2420Radar::handle_off_line() { - state.raw_presence = false; - state.last_off_ms = state.last_update_ms; -} - -void Ld2420Radar::handle_range_line(const String &line) { - String value_str = line.substring(6); - value_str.trim(); - - int raw_zone_signed = value_str.toInt(); - if (raw_zone_signed < 0) { - state.parse_error_counter++; - return; - } - - uint16_t raw_zone = static_cast(raw_zone_signed); - - if (!is_zone_valid(raw_zone)) { - state.ignored_range_counter++; - return; - } - - uint32_t now_ms = millis(); - - state.raw_distance_zone = raw_zone; - - push_zone_sample(raw_zone); - - uint16_t median_zone = get_median_zone(); - uint16_t filtered_zone = apply_zone_step_limit(median_zone); - - state.filtered_distance_zone = filtered_zone; - - float raw_distance_m = zone_to_distance_m(filtered_zone); - float filtered_distance_m = apply_distance_smoothing(raw_distance_m); - - update_speed(filtered_distance_m, now_ms); - - state.distance_m = filtered_distance_m; - state.last_range_ms = now_ms; - - /* - Если пришёл Range, считаем это достаточным признаком присутствия. - */ - state.raw_presence = true; - state.last_on_ms = now_ms; -} - -void Ld2420Radar::push_zone_sample(uint16_t zone) { - zone_window[zone_window_index] = zone; - zone_window_index = (zone_window_index + 1) % config.median_window_size; - - if (zone_window_count < config.median_window_size) { - zone_window_count++; - } -} - -uint16_t Ld2420Radar::get_median_zone() const { - if (zone_window_count == 0) { - return 0; - } - - uint16_t temp[max_supported_median_window]; - - for (uint8_t i = 0; i < zone_window_count; i++) { - temp[i] = zone_window[i]; - } - - for (uint8_t i = 0; i < zone_window_count; i++) { - for (uint8_t j = i + 1; j < zone_window_count; j++) { - if (temp[j] < temp[i]) { - uint16_t swap_tmp = temp[i]; - temp[i] = temp[j]; - temp[j] = swap_tmp; - } - } - } - - return temp[zone_window_count / 2]; -} - -uint16_t Ld2420Radar::apply_zone_step_limit(uint16_t candidate_zone) { - if (!has_filtered_zone) { - has_filtered_zone = true; - last_filtered_zone = candidate_zone; - return candidate_zone; - } - - int delta = static_cast(candidate_zone) - static_cast(last_filtered_zone); - - if (delta > static_cast(config.max_zone_step_per_sample)) { - candidate_zone = last_filtered_zone + config.max_zone_step_per_sample; - } else if (delta < -static_cast(config.max_zone_step_per_sample)) { - candidate_zone = last_filtered_zone - config.max_zone_step_per_sample; - } - - last_filtered_zone = candidate_zone; - return candidate_zone; -} - -float Ld2420Radar::zone_to_distance_m(uint16_t zone) const { - float distance_m = static_cast(zone) * config.zone_to_meter_k + config.zone_to_meter_b; - - if (distance_m < 0.0f) { - distance_m = 0.0f; - } - - return distance_m; -} - -float Ld2420Radar::apply_distance_smoothing(float new_distance_m) { - if (!has_smoothed_distance) { - has_smoothed_distance = true; - smoothed_distance_m = new_distance_m; - return smoothed_distance_m; - } - - smoothed_distance_m = - config.distance_ema_alpha * new_distance_m + - (1.0f - config.distance_ema_alpha) * smoothed_distance_m; - - return smoothed_distance_m; -} - -void Ld2420Radar::update_speed(float filtered_distance_m, uint32_t now_ms) { - if (state.last_range_ms == 0 || now_ms <= state.last_range_ms || state.distance_m < 0.0f) { - state.radial_speed_m_s = 0.0f; - has_smoothed_speed = false; - smoothed_speed_m_s = 0.0f; - return; - } - - float dt_s = static_cast(now_ms - state.last_range_ms) / 1000.0f; - if (dt_s <= 0.0f) { - return; - } - - float raw_speed_m_s = (filtered_distance_m - state.distance_m) / dt_s; - - if (!has_smoothed_speed) { - has_smoothed_speed = true; - smoothed_speed_m_s = raw_speed_m_s; - } else { - smoothed_speed_m_s = - config.speed_ema_alpha * raw_speed_m_s + - (1.0f - config.speed_ema_alpha) * smoothed_speed_m_s; - } - - state.radial_speed_m_s = smoothed_speed_m_s; -} - -void Ld2420Radar::apply_presence_hold() { - uint32_t now_ms = millis(); - - if (state.raw_presence) { - state.presence = true; - return; - } - - if (state.last_on_ms > 0 && (now_ms - state.last_on_ms) <= config.presence_hold_ms) { - state.presence = true; - } else { - state.presence = false; - state.radial_speed_m_s = 0.0f; - } -} - -void Ld2420Radar::update_online_state() { - uint32_t now_ms = millis(); - - state.stale = !(state.last_update_ms > 0 && (now_ms - state.last_update_ms) < config.stale_after_ms); - state.online = !state.stale; -} - -uint8_t Ld2420Radar::speed_to_activity_score(float abs_speed_m_s) const { - if (!state.presence) { - return 0; - } - - if (abs_speed_m_s <= config.activity_min_speed_m_s) { - return 1; - } - - if (abs_speed_m_s >= config.activity_max_speed_m_s) { - return 10; - } - - float normalized = - (abs_speed_m_s - config.activity_min_speed_m_s) / - (config.activity_max_speed_m_s - config.activity_min_speed_m_s); - - int score = 1 + static_cast(normalized * 9.0f + 0.5f); - - return clamp_score_0_10(score); -} - -void Ld2420Radar::update_activity_history() { - uint32_t now_ms = millis(); - - if (activity_bucket_start_ms == 0) { - activity_bucket_start_ms = now_ms; - } - - /* - Каждый update складываем текущий score в накопитель текущего 10-секундного окна. - */ - uint8_t current_score = speed_to_activity_score(fabs(state.radial_speed_m_s)); - state.activity_score_current = current_score; - - activity_bucket_accum_sum += current_score; - activity_bucket_accum_count++; - - /* - Если прошло 10 секунд — фиксируем бакет. - Если прошло сразу несколько бакетов подряд, тоже корректно продвигаем окно. - */ - while ((now_ms - activity_bucket_start_ms) >= activity_bucket_duration_ms) { - commit_current_activity_bucket(activity_bucket_start_ms + activity_bucket_duration_ms); - } -} - -void Ld2420Radar::commit_current_activity_bucket(uint32_t next_bucket_start_ms) { - uint8_t average_score = 0; - - if (activity_bucket_accum_count > 0) { - uint32_t avg = activity_bucket_accum_sum / activity_bucket_accum_count; - average_score = clamp_score_0_10(static_cast(avg)); - } - - activity_buckets[activity_bucket_index] = average_score; - - activity_bucket_index++; - if (activity_bucket_index >= activity_bucket_count) { - activity_bucket_index = 0; - activity_history_filled = true; - } - - activity_bucket_accum_sum = 0; - activity_bucket_accum_count = 0; - activity_bucket_start_ms = next_bucket_start_ms; -} - -void Ld2420Radar::update_activity_outputs() { - /* - activity_score_current уже считается отдельно в update_activity_history(). - Здесь считаем усреднённую активность за последнюю минуту. - - Берём: - - текущий незавершённый бакет - - до 5 последних завершённых бакетов - И усредняем всё в диапазоне 0..10. - */ - - uint32_t sum = 0; - uint8_t count = 0; - - /* - Текущий частичный бакет. - */ - if (activity_bucket_accum_count > 0) { - uint8_t current_bucket_avg = clamp_score_0_10( - static_cast(activity_bucket_accum_sum / activity_bucket_accum_count) - ); - - sum += current_bucket_avg; - count++; - } - - /* - Последние завершённые бакеты. - Всего хотим получить окно примерно в 1 минуту: - 1 текущий + 5 завершённых = до 6 бакетов по 10 секунд. - */ - uint8_t completed_to_take = 5; - uint8_t taken = 0; - - for (uint8_t i = 0; i < completed_to_take; i++) { - int index = static_cast(activity_bucket_index) - 1 - i; - if (index < 0) { - index += activity_bucket_count; - } - - if (!activity_history_filled) { - if (index < 0 || index >= activity_bucket_index) { - continue; - } - } - - sum += clamp_score_0_10(activity_buckets[index]); - count++; - taken++; - } - - if (count > 0) { - state.activity_score = clamp_score_0_10(static_cast(sum / count)); - } else { - state.activity_score = clamp_score_0_10(state.activity_score_current); - } - - state.activity_score_dynamics = calculate_activity_dynamics(); -} - -RadarActivityDynamics Ld2420Radar::calculate_activity_dynamics() const { - /* - Для оценки динамики за 10 минут используем 60 бакетов по 10 секунд. - Делим их на две половины: - - первые 5 минут - - последние 5 минут - - Если разница средних заметна — increasing/decreasing. - Если внутри ряда слишком большой разброс — variable. - Иначе constant. - */ - uint8_t available_count = activity_history_filled ? activity_bucket_count : activity_bucket_index; - - if (available_count < 12) { - return RADAR_ACTIVITY_DYNAMICS_CONSTANT; - } - - uint8_t half = available_count / 2; - - float first_sum = 0.0f; - float second_sum = 0.0f; - - for (uint8_t i = 0; i < half; i++) { - int index = static_cast(activity_bucket_index) - available_count + i; - while (index < 0) { - index += activity_bucket_count; - } - first_sum += activity_buckets[index]; - } - - for (uint8_t i = half; i < available_count; i++) { - int index = static_cast(activity_bucket_index) - available_count + i; - while (index < 0) { - index += activity_bucket_count; - } - second_sum += activity_buckets[index]; - } - - float first_avg = first_sum / half; - float second_avg = second_sum / (available_count - half); - float delta = second_avg - first_avg; - - /* - Проверка на "variable": - если средний размах скачков между соседними бакетами большой, - динамика считается рваной. - */ - float diff_sum = 0.0f; - uint8_t diff_count = 0; - - for (uint8_t i = 1; i < available_count; i++) { - int prev_index = static_cast(activity_bucket_index) - available_count + (i - 1); - int curr_index = static_cast(activity_bucket_index) - available_count + i; - - while (prev_index < 0) prev_index += activity_bucket_count; - while (curr_index < 0) curr_index += activity_bucket_count; - - diff_sum += abs(static_cast(activity_buckets[curr_index]) - static_cast(activity_buckets[prev_index])); - diff_count++; - } - - float avg_diff = (diff_count > 0) ? (diff_sum / diff_count) : 0.0f; - - if (avg_diff >= 2.5f && fabs(delta) < 2.0f) { - return RADAR_ACTIVITY_DYNAMICS_VARIABLE; - } - - if (delta >= 1.5f) { - return RADAR_ACTIVITY_DYNAMICS_INCREASING; - } - - if (delta <= -1.5f) { - return RADAR_ACTIVITY_DYNAMICS_DECREASING; - } - - return RADAR_ACTIVITY_DYNAMICS_CONSTANT; -} - -const char *Ld2420Radar::activity_dynamics_to_string(RadarActivityDynamics dynamics) const { - switch (dynamics) { - case RADAR_ACTIVITY_DYNAMICS_INCREASING: - return "increasing"; - case RADAR_ACTIVITY_DYNAMICS_DECREASING: - return "decreasing"; - case RADAR_ACTIVITY_DYNAMICS_VARIABLE: - return "variable"; - default: - return "constant"; - } -} - -bool Ld2420Radar::is_zone_valid(uint16_t zone) const { - return zone >= config.min_valid_zone && zone <= config.max_valid_zone; } \ No newline at end of file diff --git a/devices/sensor/ld2420_radar.h b/devices/sensor/ld2420_radar.h index 0c48bdf..e8dc8ec 100644 --- a/devices/sensor/ld2420_radar.h +++ b/devices/sensor/ld2420_radar.h @@ -200,6 +200,12 @@ 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;