globals: - id: last_update_ld2450 type: unsigned long restore_value: no initial_value: '0' - id: init_zone_publish type: bool restore_value: no initial_value: "false" - id: last_illuminance type: float restore_value: no initial_value: "-1" - id: last_illuminance_timestamp type: int restore_value: no initial_value: "-1" # Filtrowane pozycje – Twoje nazwy - id: p1_x_f type: float restore_value: no initial_value: '0.0' - id: p1_y_f type: float restore_value: no initial_value: '0.0' - id: p2_x_f type: float restore_value: no initial_value: '0.0' - id: p2_y_f type: float restore_value: no initial_value: '0.0' - id: p3_x_f type: float restore_value: no initial_value: '0.0' - id: p3_y_f type: float restore_value: no initial_value: '0.0' # Opcjonalnie: filtrowana prędkość (jeśli chcesz – możesz potem usunąć) - id: p1_speed_f type: float restore_value: no initial_value: '0.0' - id: p2_speed_f type: float restore_value: no initial_value: '0.0' - id: p3_speed_f type: float restore_value: no initial_value: '0.0' text_sensor: - platform: debug reset_reason: name: "ESP Reset Reason" icon: mdi:anchor disabled_by_default: True - platform: wifi_info ip_address: name: ESP IP Address entity_category: "diagnostic" disabled_by_default: True icon: mdi:ip-network mac_address: name: ESP MAC entity_category: "diagnostic" icon: mdi:ip-network disabled_by_default: True - platform: template name: "Zone1 Info" id: tips_zone1_conf icon: mdi:information-outline entity_category: config lambda: |- return {"Configure below" }; update_interval: 1000s - platform: template name: "Zone2 Info" id: tips_zone2_conf icon: mdi:information-outline entity_category: config lambda: |- return {"Configure below" }; update_interval: 1000s - platform: template name: "Zone3 Info" id: tips_zone3_conf icon: mdi:information-outline entity_category: config lambda: |- return {"Configure below" }; update_interval: 1000s - platform: template name: "Zout1 Info" id: tips_zone_ex1_conf icon: mdi:information-outline entity_category: config lambda: |- return {"Zone Exclusion 1" }; update_interval: 1000s - platform: template name: "Any-Presence Info" id: tips_any_presence_conf icon: mdi:information-outline entity_category: config lambda: |- return {"Any Presence Config" }; update_interval: 1000s - platform: template name: "Target1 Direction" id: target1_direction icon: mdi:directions - platform: template name: "Target2 Direction" id: target2_direction icon: mdi:directions - platform: template name: "Target3 Direction" id: target3_direction icon: mdi:directions - platform: template name: "Target1 Position" id: target1_position icon: mdi:directions - platform: template name: "Target2 Position" id: target2_position icon: mdi:directions - platform: template name: "Target3 Position" id: target3_position icon: mdi:directions number: - platform: template name: "Any Presence Timeout" id: any_presence_timeout min_value: 0 max_value: 1800 mode: box device_class: duration entity_category: config unit_of_measurement: s icon: mdi:timer-off step: 1 optimistic: True initial_value: 0 restore_value: True - platform: template name: "Zone1 Timeout" id: zone1_x_timeout min_value: 0 max_value: 1800 mode: box device_class: duration entity_category: config unit_of_measurement: s icon: mdi:timer-off step: 1 optimistic: True initial_value: 0 restore_value: True - platform: template name: "Zone2 Timeout" id: zone2_x_timeout min_value: 0 max_value: 1800 mode: box device_class: duration entity_category: config unit_of_measurement: s icon: mdi:timer-off step: 1 optimistic: True initial_value: 0 restore_value: True - platform: template name: "Zone3 Timeout" id: zone3_x_timeout min_value: 0 max_value: 1800 mode: box device_class: duration entity_category: config unit_of_measurement: s icon: mdi:timer-off step: 1 optimistic: True initial_value: 0 restore_value: True # Zone 1 - platform: template name: Zone1 X-Begin id: zone1_x_begin min_value: -4000 max_value: 4000 mode: box device_class: distance entity_category: config unit_of_measurement: mm icon: mdi:arrow-left-bold step: 10 optimistic: True initial_value: 0 restore_value: True on_value: then: - script.execute: check_zone1_vaild - platform: template name: Zone1 X-End id: zone1_x_end mode: box min_value: -4000 max_value: 4000 device_class: distance unit_of_measurement: mm entity_category: config icon: mdi:arrow-right-bold step: 10 initial_value: 0 optimistic: True restore_value: True on_value: then: - script.execute: check_zone1_vaild - platform: template name: Zone1 Y-Begin id: zone1_y_begin mode: box min_value: 0 max_value: 6000 device_class: distance entity_category: config icon: mdi:arrow-up-bold unit_of_measurement: mm step: 10 initial_value: 0 optimistic: True restore_value: True on_value: then: - script.execute: check_zone1_vaild - platform: template name: Zone1 Y-End id: zone1_y_end icon: mdi:arrow-down-bold mode: box min_value: 0 max_value: 6000 initial_value: 0 entity_category: config device_class: distance unit_of_measurement: mm step: 10 optimistic: True restore_value: True on_value: then: - script.execute: check_zone1_vaild # Zone 2 - platform: template name: Zone2 X-Begin id: zone2_x_begin min_value: -4000 max_value: 4000 mode: box device_class: distance entity_category: config unit_of_measurement: mm icon: mdi:arrow-left-bold step: 10 optimistic: True initial_value: 0 restore_value: True on_value: then: - script.execute: check_zone2_vaild - platform: template name: Zone2 X-End id: zone2_x_end mode: box min_value: -4000 max_value: 4000 device_class: distance unit_of_measurement: mm entity_category: config icon: mdi:arrow-right-bold step: 10 initial_value: 0 optimistic: True restore_value: True on_value: then: - script.execute: check_zone2_vaild - platform: template name: Zone2 Y-Begin id: zone2_y_begin mode: box min_value: 0 max_value: 6000 device_class: distance entity_category: config icon: mdi:arrow-up-bold unit_of_measurement: mm step: 10 initial_value: 0 optimistic: True restore_value: True on_value: then: - script.execute: check_zone2_vaild - platform: template name: Zone2 Y-End id: zone2_y_end icon: mdi:arrow-down-bold mode: box min_value: 0 max_value: 6000 initial_value: 0 entity_category: config device_class: distance unit_of_measurement: mm step: 10 optimistic: True restore_value: True on_value: then: - script.execute: check_zone2_vaild # Zone 3 - platform: template name: Zone3 X-Begin id: zone3_x_begin min_value: -4000 max_value: 4000 mode: box device_class: distance entity_category: config unit_of_measurement: mm icon: mdi:arrow-left-bold step: 10 optimistic: True initial_value: 0 restore_value: True on_value: then: - script.execute: check_zone3_vaild - platform: template name: Zone3 X-End id: zone3_x_end mode: box min_value: -4000 max_value: 4000 device_class: distance unit_of_measurement: mm entity_category: config icon: mdi:arrow-right-bold step: 10 initial_value: 0 optimistic: True restore_value: True on_value: then: - script.execute: check_zone3_vaild - platform: template name: Zone3 Y-Begin id: zone3_y_begin mode: box min_value: 0 max_value: 6000 device_class: distance entity_category: config icon: mdi:arrow-up-bold unit_of_measurement: mm step: 10 initial_value: 0 optimistic: True restore_value: True on_value: then: - script.execute: check_zone3_vaild - platform: template name: Zone3 Y-End id: zone3_y_end icon: mdi:arrow-down-bold mode: box min_value: 0 max_value: 6000 initial_value: 0 entity_category: config device_class: distance unit_of_measurement: mm step: 10 optimistic: True restore_value: True on_value: then: - script.execute: check_zone3_vaild # Zout1 - platform: template name: Zout1 X-Begin id: zone_ex1_x_begin min_value: -4000 max_value: 4000 mode: box device_class: distance entity_category: config unit_of_measurement: mm icon: mdi:arrow-left-bold step: 10 optimistic: True initial_value: 0 restore_value: True on_value: then: - script.execute: check_zout1_vaild - platform: template name: Zout1 X-End id: zone_ex1_x_end mode: box min_value: -4000 max_value: 4000 device_class: distance unit_of_measurement: mm entity_category: config icon: mdi:arrow-right-bold step: 10 initial_value: 0 optimistic: True restore_value: True on_value: then: - script.execute: check_zout1_vaild - platform: template name: Zout1 Y-Begin id: zone_ex1_y_begin mode: box min_value: 0 max_value: 6000 device_class: distance entity_category: config icon: mdi:arrow-up-bold unit_of_measurement: mm step: 10 initial_value: 0 optimistic: True restore_value: True on_value: then: - script.execute: check_zout1_vaild - platform: template name: Zout1 Y-End id: zone_ex1_y_end icon: mdi:arrow-down-bold mode: box min_value: 0 max_value: 6000 initial_value: 0 entity_category: config device_class: distance unit_of_measurement: mm step: 10 optimistic: True restore_value: True on_value: then: - script.execute: check_zout1_vaild binary_sensor: - platform: status name: Online id: ink_ha_connected - platform: template name: "Any Presence" id: any_target_exsits device_class: occupancy filters: - delayed_off: !lambda |- if (!id(init_zone_publish) || !id(zone_fn_enable).state) { return 0; } return id(any_presence_timeout).state * 1000.0; - platform: template name: "Zone1 Presence" id: zone1_target_exsits device_class: occupancy filters: - delayed_off: !lambda |- if (!id(init_zone_publish) || !id(zone_fn_enable).state) { return 0; } return id(zone1_x_timeout).state * 1000.0; - platform: template name: "Zone2 Presence" id: zone2_target_exsits device_class: occupancy filters: - delayed_off: !lambda |- if (!id(init_zone_publish) || !id(zone_fn_enable).state) { return 0; } return id(zone2_x_timeout).state * 1000.0; - platform: template name: "Zone3 Presence" id: zone3_target_exsits device_class: occupancy filters: - delayed_off: !lambda |- if (!id(init_zone_publish) || !id(zone_fn_enable).state) { return 0; } return id(zone3_x_timeout).state * 1000.0; - platform: template name: "Zout1 Presence" id: zone_ex1_target_exsits icon: mdi:account-multiple-remove device_class: occupancy script: - id: check_zone1_vaild then: - lambda: |- if (id(zone1_x_begin).state > id(zone1_x_end).state){ id(tips_zone1_conf).publish_state("Err: X-Begin > X-End"); return; } if (id(zone1_y_begin).state > id(zone1_y_end).state){ id(tips_zone1_conf).publish_state("Err: Y-Begin > Y-End"); return; } if (id(zone1_x_begin).state == 0 && id(zone1_x_end).state == 0 && id(zone1_y_begin).state == 0 && id(zone1_y_end).state == 0){ id(tips_zone1_conf).publish_state("Configure below"); return; } int x_size = id(zone1_x_end).state - id(zone1_x_begin).state; int y_size = id(zone1_y_end).state - id(zone1_y_begin).state; char combined[80]; sprintf(combined, "Curr Size: %d x %d", x_size, y_size); id(tips_zone1_conf).publish_state(combined); - id: check_zone2_vaild then: - lambda: |- if (id(zone2_x_begin).state > id(zone2_x_end).state){ id(tips_zone2_conf).publish_state("Err: X-Begin > X-End"); return; } if (id(zone2_y_begin).state > id(zone2_y_end).state){ id(tips_zone2_conf).publish_state("Err: Y-Begin > Y-End"); return; } if (id(zone2_x_begin).state == 0 && id(zone2_x_end).state == 0 && id(zone2_y_begin).state == 0 && id(zone2_y_end).state == 0){ id(tips_zone2_conf).publish_state("Configure below"); return; } int x_size = id(zone2_x_end).state - id(zone2_x_begin).state; int y_size = id(zone2_y_end).state - id(zone2_y_begin).state; char combined[80]; sprintf(combined, "Curr Size: %d x %d", x_size, y_size); id(tips_zone2_conf).publish_state(combined); - id: check_zone3_vaild then: - lambda: |- if (id(zone3_x_begin).state > id(zone3_x_end).state){ id(tips_zone3_conf).publish_state("Err: X-Begin > X-End"); return; } if (id(zone3_y_begin).state > id(zone3_y_end).state){ id(tips_zone3_conf).publish_state("Err: Y-Begin > Y-End"); return; } if (id(zone3_x_begin).state == 0 && id(zone3_x_end).state == 0 && id(zone3_y_begin).state == 0 && id(zone3_y_end).state == 0){ id(tips_zone3_conf).publish_state("Configure below"); return; } int x_size = id(zone3_x_end).state - id(zone3_x_begin).state; int y_size = id(zone3_y_end).state - id(zone3_y_begin).state; char combined[80]; sprintf(combined, "Curr Size: %d x %d", x_size, y_size); id(tips_zone3_conf).publish_state(combined); - id: check_zout1_vaild then: - lambda: |- if (id(zone_ex1_x_begin).state > id(zone_ex1_x_end).state){ id(tips_zone_ex1_conf).publish_state("Err: X-Begin > X-End"); return; } if (id(zone_ex1_y_begin).state > id(zone_ex1_y_end).state){ id(tips_zone_ex1_conf).publish_state("Err: Y-Begin > Y-End"); return; } id(tips_zone_ex1_conf).publish_state("Zone Exclusion 1"); sensor: - platform: internal_temperature id: sys_esp_temperature name: ESP Temperature unit_of_measurement: °C device_class: TEMPERATURE update_interval: 45s entity_category: "diagnostic" - platform: uptime name: ESP Uptime id: sys_uptime update_interval: 60s - platform: wifi_signal name: RSSI id: wifi_signal_db update_interval: 60s entity_category: "diagnostic" - platform: template id: esp_memory icon: mdi:memory name: ESP Free Memory lambda: return heap_caps_get_free_size(MALLOC_CAP_INTERNAL) / 1024; unit_of_measurement: 'kB' state_class: measurement entity_category: "diagnostic" update_interval: 60s # Radar counters - platform: template name: "All Target Counts" id: all_target_count accuracy_decimals: 0 icon: "mdi:counter" unit_of_measurement: "targets" - platform: template name: "Zone1 Target Counts" id: zone1_target_count accuracy_decimals: 0 icon: "mdi:counter" unit_of_measurement: "targets" - platform: template name: "Zone2 Target Counts" id: zone2_target_count accuracy_decimals: 0 icon: "mdi:counter" unit_of_measurement: "targets" - platform: template name: "Zone3 Target Counts" id: zone3_target_count accuracy_decimals: 0 icon: "mdi:counter" unit_of_measurement: "targets" - platform: template name: "Zout1 Target Counts" id: zone_ex1_target_count accuracy_decimals: 0 icon: mdi:account-multiple-minus-outline unit_of_measurement: "targets" # Target 1 - platform: template name: "Target1 X" id: target1_x accuracy_decimals: 0 unit_of_measurement: 'mm' state_class: measurement icon: mdi:focus-field-horizontal device_class: distance - platform: template name: "Target1 Y" id: target1_y accuracy_decimals: 0 unit_of_measurement: 'mm' state_class: measurement device_class: distance icon: mdi:focus-field-vertical - platform: template name: "Target1 Speed" id: target1_speed accuracy_decimals: 2 unit_of_measurement: 'm/s' state_class: measurement device_class: speed - platform: template name: "Target1 Resolution" id: target1_resolution accuracy_decimals: 0 unit_of_measurement: 'mm' state_class: measurement device_class: distance # Target 2 - platform: template name: "Target2 X" id: target2_x accuracy_decimals: 0 unit_of_measurement: 'mm' state_class: measurement device_class: distance icon: mdi:focus-field-horizontal - platform: template name: "Target2 Y" id: target2_y accuracy_decimals: 0 unit_of_measurement: 'mm' state_class: measurement device_class: distance icon: mdi:focus-field-vertical - platform: template name: "Target2 Speed" id: target2_speed accuracy_decimals: 2 unit_of_measurement: 'm/s' state_class: measurement device_class: speed - platform: template name: "Target2 Resolution" id: target2_resolution accuracy_decimals: 0 unit_of_measurement: 'mm' state_class: measurement device_class: distance # Target 3 - platform: template name: "Target3 X" id: target3_x accuracy_decimals: 0 unit_of_measurement: 'mm' state_class: measurement device_class: distance icon: mdi:focus-field-horizontal - platform: template name: "Target3 Y" id: target3_y accuracy_decimals: 0 unit_of_measurement: 'mm' state_class: measurement device_class: distance icon: mdi:focus-field-vertical - platform: template name: "Target3 Speed" id: target3_speed accuracy_decimals: 2 unit_of_measurement: 'm/s' state_class: measurement device_class: speed - platform: template name: "Target3 Resolution" id: target3_resolution accuracy_decimals: 0 unit_of_measurement: 'mm' state_class: measurement device_class: distance - platform: template name: "Target1 Angle" id: target1_angle unit_of_measurement: 'º' accuracy_decimals: 1 icon: mdi:angle-acute - platform: template name: "Target2 Angle" id: target2_angle accuracy_decimals: 1 unit_of_measurement: 'º' icon: mdi:angle-acute - platform: template name: "Target3 Angle" id: target3_angle accuracy_decimals: 1 unit_of_measurement: 'º' icon: mdi:angle-acute time: - platform: sntp id: time_now switch: - platform: factory_reset name: Factory Reset disabled_by_default: True icon: mdi:heart-broken entity_category: diagnostic - platform: template name: Zout1 Enable id: zone_ex1_enable optimistic: True icon: mdi:account-cancel entity_category: config restore_mode: RESTORE_DEFAULT_OFF - platform: template name: Zone Enable id: zone_fn_enable optimistic: True icon: mdi:target-variant entity_category: config restore_mode: RESTORE_DEFAULT_ON - platform: template name: Illuminance Fast-Update id: bh1750_fast_update optimistic: True entity_category: diagnostic restore_mode: RESTORE_DEFAULT_OFF icon: mdi:run-fast button: - platform: restart icon: mdi:power-cycle name: "ESP Reboot" entity_category: diagnostic # Poprawiony blok UART dla LD2450 z: # - obsługą 3 targetów, # - przypisywaniem do stref (zone1-3), # - obsługą wykluczeń (zout1), # - kierunkiem (approaching/away, left/right) uart: id: uart_bus tx_pin: number: GPIO22 mode: input: true pullup: true rx_pin: number: GPIO23 mode: input: true pullup: true baud_rate: 256000 parity: NONE stop_bits: 1 data_bits: 8 debug: direction: BOTH dummy_receiver: True after: delimiter: [0x55, 0xCC] sequence: - lambda: |- // ----------------------------------------- // OGRANICZENIE CZĘSTOTLIWOŚCI AKTUALIZACJI // ----------------------------------------- if ((millis() - id(last_update_ld2450)) <= 100) { return; } id(last_update_ld2450) = millis(); // ========================================================= // STABILIZACJA POZYCJI (EMA + anti-jump + hold + kwantyzacja) // ========================================================= const uint32_t HOLD_MS = 1200; // ile trzymać po zgubieniu targetu const int16_t MAX_JUMP_MM = 600; // maksymalny dopuszczalny skok (mm) const float ALPHA_POS = 0.25f; // im mniejsze, tym stabilniej static bool f1_init = false, f2_init = false, f3_init = false; static float f1x = 0, f1y = 0, f2x = 0, f2y = 0, f3x = 0, f3y = 0; static uint32_t f1_last = 0, f2_last = 0, f3_last = 0; auto smooth_pos = [&](bool valid, int16_t &x, int16_t &y, bool &init, float &fx, float &fy, uint32_t &last_ok) -> bool { uint32_t now = millis(); // Brak targetu – trzymamy ostatnią pozycję przez HOLD_MS if (!valid) { if (init && (now - last_ok) <= HOLD_MS) { x = (int16_t) fx; y = (int16_t) fy; return true; } return false; } // Pierwsza poprawna próbka if (!init) { fx = (float) x; fy = (float) y; init = true; last_ok = now; return true; } // Anti-jump – ignoruj chore skoki if (abs(x - (int16_t)fx) > MAX_JUMP_MM || abs(y - (int16_t)fy) > MAX_JUMP_MM) { x = (int16_t) fx; y = (int16_t) fy; return true; } // EMA fx = ALPHA_POS * (float)x + (1.0f - ALPHA_POS) * fx; fy = ALPHA_POS * (float)y + (1.0f - ALPHA_POS) * fy; x = (int16_t) fx; y = (int16_t) fy; last_ok = now; return true; }; auto quantize_5cm = [&](int16_t &v) { // kwantyzacja do 50 mm, żeby mapa nie drżała o pojedyncze cm int tmp = (int)v; int q = (tmp >= 0) ? ((tmp + 25) / 50) * 50 : ((tmp - 25) / 50) * 50; v = (int16_t)q; }; // ========================================================= // PARSOWANIE TARGETÓW Z RAMKI LD2450 // ========================================================= // p1 int16_t p1_x = (uint16_t((bytes[5] << 8) | bytes[4] )); if ((bytes[5] & 0x80) >> 7){ p1_x -= pow(2, 15); }else{ p1_x = 0 - p1_x; } int16_t p1_y = (uint16_t((bytes[7] << 8) | bytes[6] )); if ((bytes[7] & 0x80) >> 7){ p1_y -= pow(2, 15); }else{ p1_y = 0 - p1_y; } int p1_speed = (bytes[9] << 8 | bytes[8] ); if ((bytes[9] & 0x80) >> 7){ p1_speed -= pow(2, 15); }else{ p1_speed = 0 - p1_speed; } int16_t p1_distance_resolution = (uint16_t((bytes[11] << 8) | bytes[10] )); // p2 int16_t p2_x = (uint16_t((bytes[13] << 8) | bytes[12] )); if ((bytes[13] & 0x80) >> 7){ p2_x -= pow(2, 15); }else{ p2_x = 0 - p2_x; } int16_t p2_y = (uint16_t((bytes[15] << 8) | bytes[14] )); if ((bytes[15] & 0x80) >> 7){ p2_y -= pow(2, 15); }else{ p2_y = 0 - p2_y; } int p2_speed = (bytes[17] << 8 | bytes[16] ); if ((bytes[17] & 0x80) >> 7){ p2_speed -= pow(2, 15); }else{ p2_speed = 0 - p2_speed; } int16_t p2_distance_resolution = (uint16_t((bytes[19] << 8) | bytes[18] )); // p3 int16_t p3_x = (uint16_t((bytes[21] << 8) | bytes[20] )); if ((bytes[21] & 0x80) >> 7){ p3_x -= pow(2, 15); }else{ p3_x = 0 - p3_x; } int16_t p3_y = (uint16_t((bytes[23] << 8) | bytes[22] )); if ((bytes[23] & 0x80) >> 7){ p3_y -= pow(2, 15); }else{ p3_y = 0 - p3_y; } int p3_speed = (bytes[25] << 8 | bytes[24] ); if ((bytes[25] & 0x80) >> 7){ p3_speed -= pow(2, 15); }else{ p3_speed = 0 - p3_speed; } int16_t p3_distance_resolution = (uint16_t((bytes[27] << 8) | bytes[26] )); bool p1_vaild = (p1_x != 0 || p1_y > 0); bool p2_vaild = (p2_x != 0 || p2_y > 0); bool p3_vaild = (p3_x != 0 || p3_y > 0); // ========================================================= // ZASTOSUJ STABILIZACJĘ I KWANTYZACJĘ // ========================================================= p1_vaild = smooth_pos(p1_vaild, p1_x, p1_y, f1_init, f1x, f1y, f1_last); p2_vaild = smooth_pos(p2_vaild, p2_x, p2_y, f2_init, f2x, f2y, f2_last); p3_vaild = smooth_pos(p3_vaild, p3_x, p3_y, f3_init, f3x, f3y, f3_last); if (p1_vaild) { quantize_5cm(p1_x); quantize_5cm(p1_y); } if (p2_vaild) { quantize_5cm(p2_x); quantize_5cm(p2_y); } if (p3_vaild) { quantize_5cm(p3_x); quantize_5cm(p3_y); } // ========================================================= // DALEJ – TWÓJ ORYGINAŁ 1:1 (zony, ZOUT, kąty, publish) // ========================================================= // zone exlude 1 int16_t target_count_in_zone_ex1 = 0; int16_t zone_ex1_x_min = id(zone_ex1_x_begin).state; int16_t zone_ex1_x_max = id(zone_ex1_x_end).state; int16_t zone_ex1_y_min = id(zone_ex1_y_begin).state; int16_t zone_ex1_y_max = id(zone_ex1_y_end).state; bool p1_zone_ex_enter = false; bool p2_zone_ex_enter = false; bool p3_zone_ex_enter = false; if (id(zone_ex1_enable).state){ if (p1_vaild){ if (p1_x >= zone_ex1_x_min && p1_x <= zone_ex1_x_max && p1_y >= zone_ex1_y_min && p1_y <= zone_ex1_y_max){ p1_zone_ex_enter = true; target_count_in_zone_ex1 ++; } } if (p2_vaild){ if (p2_x >= zone_ex1_x_min && p2_x <= zone_ex1_x_max && p2_y >= zone_ex1_y_min && p2_y <= zone_ex1_y_max){ p2_zone_ex_enter = true; target_count_in_zone_ex1 ++; } } if (p3_vaild){ if (p3_x >= zone_ex1_x_min && p3_x <= zone_ex1_x_max && p3_y >= zone_ex1_y_min && p3_y <= zone_ex1_y_max){ p3_zone_ex_enter = true; target_count_in_zone_ex1 ++; } } } bool has_target_in_zone_ex1 = (target_count_in_zone_ex1 > 0); int16_t all_target_counts = 0; if (p1_vaild && !p1_zone_ex_enter){ all_target_counts ++; } if (p2_vaild && !p2_zone_ex_enter){ all_target_counts ++; } if (p3_vaild && !p3_zone_ex_enter){ all_target_counts ++; } bool has_target_in_zone_all = (all_target_counts > 0); int16_t target_count_in_zone1 = 0; bool has_target_in_zone1 = false; int16_t target_count_in_zone2 = 0; bool has_target_in_zone2 = false; int16_t target_count_in_zone3 = 0; bool has_target_in_zone3 = false; if (id(zone_fn_enable).state){ // zone 1 check int16_t zone1_x_min = id(zone1_x_begin).state; int16_t zone1_x_max = id(zone1_x_end).state; int16_t zone1_y_min = id(zone1_y_begin).state; int16_t zone1_y_max = id(zone1_y_end).state; if (p1_vaild && !p1_zone_ex_enter){ if (p1_x >= zone1_x_min && p1_x <= zone1_x_max && p1_y >= zone1_y_min && p1_y <= zone1_y_max){ target_count_in_zone1 ++; } } if (p2_vaild && !p2_zone_ex_enter){ if (p2_x >= zone1_x_min && p2_x <= zone1_x_max && p2_y >= zone1_y_min && p2_y <= zone1_y_max){ target_count_in_zone1 ++; } } if (p3_vaild && !p3_zone_ex_enter){ if (p3_x >= zone1_x_min && p3_x <= zone1_x_max && p3_y >= zone1_y_min && p3_y <= zone1_y_max){ target_count_in_zone1 ++; } } has_target_in_zone1 = (target_count_in_zone1 > 0); // zone 2 check int16_t zone2_x_min = id(zone2_x_begin).state; int16_t zone2_x_max = id(zone2_x_end).state; int16_t zone2_y_min = id(zone2_y_begin).state; int16_t zone2_y_max = id(zone2_y_end).state; if (p1_vaild && !p1_zone_ex_enter){ if (p1_x >= zone2_x_min && p1_x <= zone2_x_max && p1_y >= zone2_y_min && p1_y <= zone2_y_max){ target_count_in_zone2 ++; } } if (p2_vaild && !p2_zone_ex_enter){ if (p2_x >= zone2_x_min && p2_x <= zone2_x_max && p2_y >= zone2_y_min && p2_y <= zone2_y_max){ target_count_in_zone2 ++; } } if (p3_vaild && !p3_zone_ex_enter){ if (p3_x >= zone2_x_min && p3_x <= zone2_x_max && p3_y >= zone2_y_min && p3_y <= zone2_y_max){ target_count_in_zone2 ++; } } has_target_in_zone2 = (target_count_in_zone2 > 0); // zone 3 check int16_t zone3_x_min = id(zone3_x_begin).state; int16_t zone3_x_max = id(zone3_x_end).state; int16_t zone3_y_min = id(zone3_y_begin).state; int16_t zone3_y_max = id(zone3_y_end).state; if (p1_vaild && !p1_zone_ex_enter){ if (p1_x >= zone3_x_min && p1_x <= zone3_x_max && p1_y >= zone3_y_min && p1_y <= zone3_y_max){ target_count_in_zone3 ++; } } if (p2_vaild && !p2_zone_ex_enter){ if (p2_x >= zone3_x_min && p2_x <= zone3_x_max && p2_y >= zone3_y_min && p2_y <= zone3_y_max){ target_count_in_zone3 ++; } } if (p3_vaild && !p3_zone_ex_enter){ if (p3_x >= zone3_x_min && p3_x <= zone3_x_max && p3_y >= zone3_y_min && p3_y <= zone3_y_max){ target_count_in_zone3 ++; } } has_target_in_zone3 = (target_count_in_zone3 > 0); } // Angle, Position and Direction, idea from walberjunior. float p1_angle = 0; if (p1_vaild){ // atan2 jest stabilniejsze niż x/y p1_angle = atan2((float)p1_x, (float)p1_y) * 180.0f / M_PI; } std::basic_string p1_position = "Static"; if (p1_speed > 0) { p1_position = "Moving away"; } else if (p1_speed < 0) { p1_position = "Approaching"; } std::basic_string p1_direction = "None"; if (p1_x > 0) { p1_direction = "Right"; } else if (p1_x < 0) { p1_direction = "Left"; } else if (p1_y > 0){ p1_direction = "Middle"; } float p2_angle = 0; if (p2_vaild){ p2_angle = atan2((float)p2_x, (float)p2_y) * 180.0f / M_PI; } std::basic_string p2_position = "Static"; if (p2_speed > 0) { p2_position = "Moving away"; } else if (p2_speed < 0) { p2_position = "Approaching"; } std::basic_string p2_direction = "None"; if (p2_x > 0) { p2_direction = "Right"; } else if (p2_x < 0) { p2_direction = "Left"; } else if (p2_y > 0){ p2_direction = "Middle"; } float p3_angle = 0; if (p3_vaild){ p3_angle = atan2((float)p3_x, (float)p3_y) * 180.0f / M_PI; } std::basic_string p3_position = "Static"; if (p3_speed > 0) { p3_position = "Moving away"; } else if (p3_speed < 0) { p3_position = "Approaching"; } std::basic_string p3_direction = "None"; if (p3_x > 0) { p3_direction = "Right"; } else if (p3_x < 0) { p3_direction = "Left"; } else if (p3_y > 0){ p3_direction = "Middle"; } if (id(target1_angle).state != p1_angle){ id(target1_angle).publish_state(p1_angle); } if (id(target2_angle).state != p2_angle){ id(target2_angle).publish_state(p2_angle); } if (id(target3_angle).state != p3_angle){ id(target3_angle).publish_state(p3_angle); } if (p1_position != id(target1_position).state){ id(target1_position).publish_state(p1_position); } if (p2_position != id(target2_position).state){ id(target2_position).publish_state(p2_position); } if (p3_position != id(target3_position).state){ id(target3_position).publish_state(p3_position); } if (p1_direction != id(target1_direction).state){ id(target1_direction).publish_state(p1_direction); } if (p2_direction != id(target2_direction).state){ id(target2_direction).publish_state(p2_direction); } if (p3_direction != id(target3_direction).state){ id(target3_direction).publish_state(p3_direction); } // public all info if (id(target1_x).state != p1_x){ id(target1_x).publish_state(p1_x); } if (id(target1_y).state != p1_y){ id(target1_y).publish_state(p1_y); } float p1_m_speed_pub = float(p1_speed) / 100.0f; if (id(target1_speed).state != p1_m_speed_pub){ id(target1_speed).publish_state(p1_m_speed_pub); } if (id(target1_resolution).state != p1_distance_resolution){ id(target1_resolution).publish_state(p1_distance_resolution); } if (id(target2_x).state != p2_x){ id(target2_x).publish_state(p2_x); } if (id(target2_y).state != p2_y){ id(target2_y).publish_state(p2_y); } if (id(target2_speed).state != p2_speed){ id(target2_speed).publish_state(p2_speed); } if (id(target2_resolution).state != p2_distance_resolution){ id(target2_resolution).publish_state(p2_distance_resolution); } if (id(target3_x).state != p3_x){ id(target3_x).publish_state(p3_x); } if (id(target3_y).state != p3_y){ id(target3_y).publish_state(p3_y); } if (id(target3_speed).state != p3_speed){ id(target3_speed).publish_state(p3_speed); } if (id(target3_resolution).state != p3_distance_resolution){ id(target3_resolution).publish_state(p3_distance_resolution); } // publish target info if (id(all_target_count).state != all_target_counts){ id(all_target_count).publish_state(all_target_counts); id(any_target_exsits).publish_state(has_target_in_zone_all); }else if (id(any_target_exsits).state != has_target_in_zone_all){ id(any_target_exsits).publish_state(has_target_in_zone_all); } if (id(zone1_target_count).state != target_count_in_zone1){ id(zone1_target_count).publish_state(target_count_in_zone1); id(zone1_target_exsits).publish_state(has_target_in_zone1); }else if (id(zone1_target_exsits).state != has_target_in_zone1){ id(zone1_target_exsits).publish_state(has_target_in_zone1); } if (id(zone2_target_count).state != target_count_in_zone2){ id(zone2_target_count).publish_state(target_count_in_zone2); id(zone2_target_exsits).publish_state(has_target_in_zone2); }else if (id(zone2_target_exsits).state != has_target_in_zone2){ id(zone2_target_exsits).publish_state(has_target_in_zone2); } if (id(zone3_target_count).state != target_count_in_zone3){ id(zone3_target_count).publish_state(target_count_in_zone3); id(zone3_target_exsits).publish_state(has_target_in_zone3); }else if (id(zone3_target_exsits).state != has_target_in_zone3){ id(zone3_target_exsits).publish_state(has_target_in_zone3); } // zout if (id(zone_ex1_target_count).state != target_count_in_zone_ex1){ id(zone_ex1_target_count).publish_state(target_count_in_zone_ex1); } if (id(zone_ex1_target_exsits).state != has_target_in_zone_ex1){ id(zone_ex1_target_exsits).publish_state(has_target_in_zone_ex1); } if (!id(init_zone_publish)){ id(init_zone_publish) = true; }