mmWaveLD2450/mmWaveLD2450_c6.yaml
2026-01-18 23:54:14 +00:00

1381 lines
38 KiB
YAML
Raw Permalink Blame History

This file contains ambiguous Unicode characters

This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.

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<char> p1_position = "Static";
if (p1_speed > 0) {
p1_position = "Moving away";
} else if (p1_speed < 0) {
p1_position = "Approaching";
}
std::basic_string<char> 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<char> p2_position = "Static";
if (p2_speed > 0) {
p2_position = "Moving away";
} else if (p2_speed < 0) {
p2_position = "Approaching";
}
std::basic_string<char> 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<char> p3_position = "Static";
if (p3_speed > 0) {
p3_position = "Moving away";
} else if (p3_speed < 0) {
p3_position = "Approaching";
}
std::basic_string<char> 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;
}