From 53b1b3a2532d86ee36ac58fb211f7ab24464a558 Mon Sep 17 00:00:00 2001 From: Mustafa KURU Date: Thu, 6 Aug 2026 22:36:32 +0300 Subject: [PATCH] [ld6002b] Add target sensors (2/5) (#17820) Co-authored-by: Jonathan Swoboda <154711427+swoboda1337@users.noreply.github.com> --- esphome/components/ld6002b/binary_sensor.py | 4 +- esphome/components/ld6002b/const.py | 5 + esphome/components/ld6002b/ld6002b.cpp | 109 +++++++++++++++++++- esphome/components/ld6002b/ld6002b.h | 58 +++++++++++ esphome/components/ld6002b/sensor.py | 105 +++++++++++++++++++ tests/components/ld6002b/common.yaml | 39 +++++++ 6 files changed, 315 insertions(+), 5 deletions(-) create mode 100644 esphome/components/ld6002b/sensor.py diff --git a/esphome/components/ld6002b/binary_sensor.py b/esphome/components/ld6002b/binary_sensor.py index 2e38d35c66..319ace6f5d 100644 --- a/esphome/components/ld6002b/binary_sensor.py +++ b/esphome/components/ld6002b/binary_sensor.py @@ -4,12 +4,10 @@ import esphome.config_validation as cv from esphome.const import CONF_TARGET, DEVICE_CLASS_OCCUPANCY from . import LD6002BComponent -from .const import CONF_LD6002B_ID +from .const import CONF_LD6002B_ID, MAX_TARGETS DEPENDENCIES = ["ld6002b"] -MAX_TARGETS = 3 - CONFIG_SCHEMA = cv.Schema( { cv.GenerateID(CONF_LD6002B_ID): cv.use_id(LD6002BComponent), diff --git a/esphome/components/ld6002b/const.py b/esphome/components/ld6002b/const.py index e7606e2fae..4419a92d23 100644 --- a/esphome/components/ld6002b/const.py +++ b/esphome/components/ld6002b/const.py @@ -1,3 +1,8 @@ CONF_AUTO_WAKE = "auto_wake" +CONF_CLUSTER_ID = "cluster_id" +CONF_DOPPLER_INDEX = "doppler_index" CONF_LD6002B_ID = "ld6002b_id" CONF_WAKEUP_PULSE = "wakeup_pulse" +CONF_Z = "z" + +MAX_TARGETS = 3 diff --git a/esphome/components/ld6002b/ld6002b.cpp b/esphome/components/ld6002b/ld6002b.cpp index f18b944e12..2a09e98c25 100644 --- a/esphome/components/ld6002b/ld6002b.cpp +++ b/esphome/components/ld6002b/ld6002b.cpp @@ -2,6 +2,7 @@ #include "esphome/core/log.h" #include #include +#include #include namespace esphome::ld6002b { @@ -48,6 +49,20 @@ uint32_t LD6002BComponent::read_u32_le(const uint8_t *data) { (static_cast(data[2]) << 16) | (static_cast(data[3]) << 24); } +int32_t LD6002BComponent::read_int32_le(const uint8_t *data) { + uint32_t raw = read_u32_le(data); + int32_t value; + std::memcpy(&value, &raw, sizeof(value)); + return value; +} + +float LD6002BComponent::read_f32_le(const uint8_t *data) { + uint32_t raw = read_u32_le(data); + float value; + std::memcpy(&value, &raw, sizeof(value)); + return value; +} + void LD6002BComponent::write_u32_le(uint8_t *data, uint32_t value) { data[0] = value & 0xFF; data[1] = (value >> 8) & 0xFF; @@ -70,6 +85,18 @@ void LD6002BComponent::setup() { this->set_timeout(SETUP_DELAY_MS, [this]() { bool want_target_stream = false; +#ifdef USE_SENSOR + want_target_stream = want_target_stream || this->target_count_sensor_ != nullptr; + if (!want_target_stream) { + for (const auto &target : this->targets_) { + if (target.x != nullptr || target.y != nullptr || target.z != nullptr || target.dop_idx != nullptr || + target.cluster_id != nullptr) { + want_target_stream = true; + break; + } + } + } +#endif #ifdef USE_BINARY_SENSOR want_target_stream = want_target_stream || this->presence_binary_sensor_ != nullptr; if (!want_target_stream) { @@ -85,7 +112,6 @@ void LD6002BComponent::setup() { this->send_control_command_(CMD_TARGET_DISPLAY_ON); } - // Point-cloud streaming is introduced in a later part; make sure it is off. this->send_control_command_(CMD_POINT_CLOUD_OFF); }); } @@ -99,6 +125,16 @@ void LD6002BComponent::dump_config() { LOG_PIN(" Wake-up Pin: ", this->wakeup_pin_); ESP_LOGCONFIG(TAG, " Wake Pulse: %ums", this->wakeup_pulse_ms_); } +#ifdef USE_SENSOR + LOG_SENSOR(" ", "Target Count", this->target_count_sensor_); + for (auto &target : this->targets_) { + LOG_SENSOR(" ", "Target X", target.x); + LOG_SENSOR(" ", "Target Y", target.y); + LOG_SENSOR(" ", "Target Z", target.z); + LOG_SENSOR(" ", "Target Doppler Index", target.dop_idx); + LOG_SENSOR(" ", "Target Cluster ID", target.cluster_id); + } +#endif #ifdef USE_BINARY_SENSOR LOG_BINARY_SENSOR(" ", "Presence", this->presence_binary_sensor_); for (uint8_t i = 0; i < MAX_TARGETS; i++) { @@ -254,6 +290,7 @@ void LD6002BComponent::handle_target_report_(const uint8_t *data, uint16_t len) std::array wire_cluster{}; std::array wire_placed{}; std::array slot_seen{}; + std::array slot_wire{}; for (uint8_t i = 0; i < count; i++) { uint16_t cluster_offset = 4 + (i * TARGET_DATA_LEN) + 16; wire_cluster[i] = static_cast(read_u32_le(data + cluster_offset)); @@ -263,6 +300,7 @@ void LD6002BComponent::handle_target_report_(const uint8_t *data, uint16_t len) if (this->slot_occupied_[s] && !slot_seen[s] && this->slot_cluster_[s] == wire_cluster[i]) { slot_seen[s] = true; wire_placed[i] = true; + slot_wire[s] = i; break; } } @@ -280,11 +318,21 @@ void LD6002BComponent::handle_target_report_(const uint8_t *data, uint16_t len) if (!this->slot_occupied_[s]) { this->slot_occupied_[s] = true; this->slot_cluster_[s] = wire_cluster[i]; + slot_wire[s] = i; break; } } } +#ifdef USE_SENSOR + if (this->target_count_sensor_ != nullptr) { + if (reported != this->last_target_count_) { + this->target_count_sensor_->publish_state(reported); + this->last_target_count_ = reported; + } + } +#endif + this->target_presence_any_ = (reported > 0); #ifdef USE_BINARY_SENSOR if (this->presence_binary_sensor_ != nullptr) { @@ -293,11 +341,68 @@ void LD6002BComponent::handle_target_report_(const uint8_t *data, uint16_t len) #endif for (uint8_t i = 0; i < MAX_TARGETS; i++) { + bool has_target = this->slot_occupied_[i]; + if (has_target) { +#ifdef USE_SENSOR + uint16_t offset = 4 + (slot_wire[i] * TARGET_DATA_LEN); + float x = read_f32_le(data + offset + 0); + float y = read_f32_le(data + offset + 4); + float z = read_f32_le(data + offset + 8); + int32_t dop_idx = read_int32_le(data + offset + 12); + int32_t cluster_id = this->slot_cluster_[i]; + TargetSensors &target = this->targets_[i]; + if (target.x != nullptr) { + target.x->publish_state(x); + } + if (target.y != nullptr) { + target.y->publish_state(y); + } + if (target.z != nullptr) { + target.z->publish_state(z); + } + if (target.dop_idx != nullptr) { + target.dop_idx->publish_state(static_cast(dop_idx)); + } + if (target.cluster_id != nullptr) { + if (!this->last_cluster_id_valid_[i] || cluster_id != this->last_cluster_id_[i]) { + target.cluster_id->publish_state(static_cast(cluster_id)); + this->last_cluster_id_[i] = cluster_id; + this->last_cluster_id_valid_[i] = true; + } + } +#endif + } else { +#ifdef USE_SENSOR + TargetSensors &target = this->targets_[i]; + if (this->last_target_presence_[i]) { + if (target.x != nullptr) { + target.x->publish_state(NAN); + } + if (target.y != nullptr) { + target.y->publish_state(NAN); + } + if (target.z != nullptr) { + target.z->publish_state(NAN); + } + if (target.dop_idx != nullptr) { + target.dop_idx->publish_state(NAN); + } + if (target.cluster_id != nullptr) { + target.cluster_id->publish_state(NAN); + } + // The slot is free: the next person's id is new even when it repeats this one. + this->last_cluster_id_valid_[i] = false; + } +#endif + } #ifdef USE_BINARY_SENSOR if (this->target_presence_[i] != nullptr) { // publish_state() already skips unchanged states, no manual de-dup needed. - this->target_presence_[i]->publish_state(this->slot_occupied_[i]); + this->target_presence_[i]->publish_state(has_target); } +#endif +#ifdef USE_SENSOR + this->last_target_presence_[i] = has_target; #endif } } diff --git a/esphome/components/ld6002b/ld6002b.h b/esphome/components/ld6002b/ld6002b.h index 2a68507e41..8bbfb9f6e4 100644 --- a/esphome/components/ld6002b/ld6002b.h +++ b/esphome/components/ld6002b/ld6002b.h @@ -5,6 +5,9 @@ #include "esphome/core/helpers.h" #include "esphome/core/gpio.h" #include "esphome/components/uart/uart.h" +#ifdef USE_SENSOR +#include "esphome/components/sensor/sensor.h" +#endif #ifdef USE_BINARY_SENSOR #include "esphome/components/binary_sensor/binary_sensor.h" #endif @@ -18,6 +21,17 @@ static constexpr size_t DEFAULT_MAX_DATA_LEN = 1024; // Largest protocol payload is TYPE_SET_AREA: int32 area id + 6 floats = 28 bytes. static constexpr size_t CMD_MAX_DATA_LEN = 28; +#ifdef USE_SENSOR +struct TargetSensors { + sensor::Sensor *x{nullptr}; + sensor::Sensor *y{nullptr}; + sensor::Sensor *z{nullptr}; + sensor::Sensor *dop_idx{nullptr}; + sensor::Sensor *cluster_id{nullptr}; +}; + +#endif + class LD6002BComponent : public Component, public uart::UARTDevice { public: void setup() override; @@ -29,6 +43,36 @@ class LD6002BComponent : public Component, public uart::UARTDevice { void set_wakeup_pulse_ms(uint32_t ms) { this->wakeup_pulse_ms_ = ms; } void set_auto_wake(bool enable) { this->auto_wake_ = enable; } +#ifdef USE_SENSOR + void set_target_count_sensor(sensor::Sensor *sensor) { this->target_count_sensor_ = sensor; } + + void set_target_x_sensor(uint8_t target, sensor::Sensor *sensor) { + if (target >= MAX_TARGETS) + return; + this->targets_[target].x = sensor; + } + void set_target_y_sensor(uint8_t target, sensor::Sensor *sensor) { + if (target >= MAX_TARGETS) + return; + this->targets_[target].y = sensor; + } + void set_target_z_sensor(uint8_t target, sensor::Sensor *sensor) { + if (target >= MAX_TARGETS) + return; + this->targets_[target].z = sensor; + } + void set_target_dop_idx_sensor(uint8_t target, sensor::Sensor *sensor) { + if (target >= MAX_TARGETS) + return; + this->targets_[target].dop_idx = sensor; + } + void set_target_cluster_id_sensor(uint8_t target, sensor::Sensor *sensor) { + if (target >= MAX_TARGETS) + return; + this->targets_[target].cluster_id = sensor; + } +#endif + #ifdef USE_BINARY_SENSOR void set_presence_binary_sensor(binary_sensor::BinarySensor *sensor) { this->presence_binary_sensor_ = sensor; } void set_target_presence_binary_sensor(uint8_t target, binary_sensor::BinarySensor *sensor) { @@ -61,8 +105,14 @@ class LD6002BComponent : public Component, public uart::UARTDevice { static uint16_t read_u16_be(const uint8_t *data); static uint32_t read_u32_le(const uint8_t *data); + static int32_t read_int32_le(const uint8_t *data); + static float read_f32_le(const uint8_t *data); static void write_u32_le(uint8_t *data, uint32_t value); +#ifdef USE_SENSOR + std::array targets_{}; + sensor::Sensor *target_count_sensor_{nullptr}; +#endif #ifdef USE_BINARY_SENSOR binary_sensor::BinarySensor *presence_binary_sensor_{nullptr}; std::array target_presence_{}; @@ -128,6 +178,14 @@ class LD6002BComponent : public Component, public uart::UARTDevice { std::array slot_occupied_{}; bool target_presence_any_{false}; + +#ifdef USE_SENSOR + std::array last_target_presence_{}; // one-shot NAN clear for target sensors + // A cluster id names a person, so like the counts it is published on change, not per frame. + std::array last_cluster_id_{}; + std::array last_cluster_id_valid_{}; + uint32_t last_target_count_{0xFFFFFFFF}; +#endif }; } // namespace esphome::ld6002b diff --git a/esphome/components/ld6002b/sensor.py b/esphome/components/ld6002b/sensor.py new file mode 100644 index 0000000000..3aa9b0f98a --- /dev/null +++ b/esphome/components/ld6002b/sensor.py @@ -0,0 +1,105 @@ +import esphome.codegen as cg +from esphome.components import sensor +from esphome.components.const import CONF_TARGET_COUNT +import esphome.config_validation as cv +from esphome.const import ( + CONF_X, + CONF_Y, + DEVICE_CLASS_DISTANCE, + STATE_CLASS_MEASUREMENT, + UNIT_METER, +) + +from . import LD6002BComponent +from .const import ( + CONF_CLUSTER_ID, + CONF_DOPPLER_INDEX, + CONF_LD6002B_ID, + CONF_Z, + MAX_TARGETS, +) + +DEPENDENCIES = ["ld6002b"] + +# The ld2450 defaults for a streamed value: hold the last reading for a second so a +# dropped frame does not read as absence, then rate-limit what reaches the frontend. +_VALUE_SENSOR_FILTERS = [ + { + "timeout": { + "timeout": cv.TimePeriod(milliseconds=1000), + "value": "last", + } + }, + {"throttle_with_priority": cv.TimePeriod(milliseconds=1000)}, +] + +TARGET_SCHEMA = cv.Schema( + { + cv.Optional(CONF_X): sensor.sensor_schema( + unit_of_measurement=UNIT_METER, + accuracy_decimals=2, + device_class=DEVICE_CLASS_DISTANCE, + filters=_VALUE_SENSOR_FILTERS, + state_class=STATE_CLASS_MEASUREMENT, + ), + cv.Optional(CONF_Y): sensor.sensor_schema( + unit_of_measurement=UNIT_METER, + accuracy_decimals=2, + device_class=DEVICE_CLASS_DISTANCE, + filters=_VALUE_SENSOR_FILTERS, + state_class=STATE_CLASS_MEASUREMENT, + ), + cv.Optional(CONF_Z): sensor.sensor_schema( + unit_of_measurement=UNIT_METER, + accuracy_decimals=2, + device_class=DEVICE_CLASS_DISTANCE, + filters=_VALUE_SENSOR_FILTERS, + state_class=STATE_CLASS_MEASUREMENT, + ), + cv.Optional(CONF_DOPPLER_INDEX): sensor.sensor_schema( + accuracy_decimals=0, + filters=_VALUE_SENSOR_FILTERS, + state_class=STATE_CLASS_MEASUREMENT, + ), + cv.Optional(CONF_CLUSTER_ID): sensor.sensor_schema( + accuracy_decimals=0, + ), + } +) + + +CONFIG_SCHEMA = cv.Schema( + { + cv.GenerateID(CONF_LD6002B_ID): cv.use_id(LD6002BComponent), + cv.Optional(CONF_TARGET_COUNT): sensor.sensor_schema( + accuracy_decimals=0, + state_class=STATE_CLASS_MEASUREMENT, + ), + } +).extend({cv.Optional(f"target_{i + 1}"): TARGET_SCHEMA for i in range(MAX_TARGETS)}) + + +async def to_code(config): + hub = await cg.get_variable(config[CONF_LD6002B_ID]) + + if target_count_config := config.get(CONF_TARGET_COUNT): + sens = await sensor.new_sensor(target_count_config) + cg.add(hub.set_target_count_sensor(sens)) + + for i in range(MAX_TARGETS): + if target_config := config.get(f"target_{i + 1}"): + if x_config := target_config.get(CONF_X): + sens = await sensor.new_sensor(x_config) + cg.add(hub.set_target_x_sensor(i, sens)) + if y_config := target_config.get(CONF_Y): + sens = await sensor.new_sensor(y_config) + cg.add(hub.set_target_y_sensor(i, sens)) + if z_config := target_config.get(CONF_Z): + sens = await sensor.new_sensor(z_config) + cg.add(hub.set_target_z_sensor(i, sens)) + if doppler_index_config := target_config.get(CONF_DOPPLER_INDEX): + sens = await sensor.new_sensor(doppler_index_config) + cg.add(hub.set_target_dop_idx_sensor(i, sens)) + if cluster_id_config := target_config.get(CONF_CLUSTER_ID): + sens = await sensor.new_sensor(cluster_id_config) + cg.add(hub.set_target_cluster_id_sensor(i, sens)) diff --git a/tests/components/ld6002b/common.yaml b/tests/components/ld6002b/common.yaml index da08122608..15ab06c394 100644 --- a/tests/components/ld6002b/common.yaml +++ b/tests/components/ld6002b/common.yaml @@ -2,6 +2,45 @@ ld6002b: id: ld6002b_radar wakeup_pin: GPIO14 +sensor: + - platform: ld6002b + ld6002b_id: ld6002b_radar + target_count: + name: Target Count + target_1: + x: + name: Target-1 X + y: + name: Target-1 Y + z: + name: Target-1 Z + doppler_index: + name: Target-1 Dop + cluster_id: + name: Target-1 Cluster + target_2: + x: + name: Target-2 X + y: + name: Target-2 Y + z: + name: Target-2 Z + doppler_index: + name: Target-2 Dop + cluster_id: + name: Target-2 Cluster + target_3: + x: + name: Target-3 X + y: + name: Target-3 Y + z: + name: Target-3 Z + doppler_index: + name: Target-3 Dop + cluster_id: + name: Target-3 Cluster + binary_sensor: - platform: ld6002b ld6002b_id: ld6002b_radar