commit 8bb3072402382ede556e417ff4976f9fe2d422dd Author: rafal Date: Sun Jan 18 23:54:14 2026 +0000 Upload files to "/" diff --git a/mmWaveLD2450_c6.yaml b/mmWaveLD2450_c6.yaml new file mode 100644 index 0000000..301ae55 --- /dev/null +++ b/mmWaveLD2450_c6.yaml @@ -0,0 +1,1380 @@ +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; + }