mirror of
https://github.com/esphome/esphome.git
synced 2026-08-17 10:52:56 +08:00
[ld6002b] Add LD6002B 60GHz presence radar (1/5) (#17819)
Co-authored-by: Jonathan Swoboda <154711427+swoboda1337@users.noreply.github.com>
This commit is contained in:
co-authored by
Jonathan Swoboda
parent
a7740091ec
commit
f8cabc9d1f
@@ -288,6 +288,7 @@ esphome/components/ld2412/* @Rihan9
|
||||
esphome/components/ld2420/* @descipher
|
||||
esphome/components/ld2450/* @hareeshmu
|
||||
esphome/components/ld24xx/* @kbx81
|
||||
esphome/components/ld6002b/* @hepter
|
||||
esphome/components/ledc/* @OttoWinter
|
||||
esphome/components/libretiny/* @kuba2k2
|
||||
esphome/components/libretiny_pwm/* @kuba2k2
|
||||
|
||||
@@ -0,0 +1,73 @@
|
||||
from esphome import pins
|
||||
import esphome.codegen as cg
|
||||
from esphome.components import uart
|
||||
import esphome.config_validation as cv
|
||||
from esphome.const import CONF_ID, CONF_WAKEUP_PIN
|
||||
|
||||
from .const import CONF_AUTO_WAKE, CONF_WAKEUP_PULSE
|
||||
|
||||
CODEOWNERS = ["@hepter"]
|
||||
DEPENDENCIES = ["uart"]
|
||||
MULTI_CONF = True
|
||||
|
||||
ld6002b_ns = cg.esphome_ns.namespace("ld6002b")
|
||||
LD6002BComponent = ld6002b_ns.class_("LD6002BComponent", cg.Component, uart.UARTDevice)
|
||||
|
||||
|
||||
def _validate_wakeup_options(config):
|
||||
"""Reject wake options that would silently do nothing.
|
||||
|
||||
Runs before the schema so the defaults for the keys below have not been
|
||||
filled in yet and an explicit user value is still distinguishable from one.
|
||||
"""
|
||||
if not isinstance(config, dict):
|
||||
return config
|
||||
if CONF_WAKEUP_PIN in config:
|
||||
return config
|
||||
for key in (CONF_AUTO_WAKE, CONF_WAKEUP_PULSE):
|
||||
if key in config:
|
||||
raise cv.Invalid(
|
||||
f"'{key}' requires '{CONF_WAKEUP_PIN}' to be configured", path=[key]
|
||||
)
|
||||
return config
|
||||
|
||||
|
||||
CONFIG_SCHEMA = cv.All(
|
||||
_validate_wakeup_options,
|
||||
cv.Schema(
|
||||
{
|
||||
cv.GenerateID(): cv.declare_id(LD6002BComponent),
|
||||
cv.Optional(CONF_WAKEUP_PIN): pins.gpio_output_pin_schema,
|
||||
cv.Optional(
|
||||
CONF_WAKEUP_PULSE, default="50ms"
|
||||
): cv.positive_time_period_milliseconds,
|
||||
cv.Optional(CONF_AUTO_WAKE, default=True): cv.boolean,
|
||||
}
|
||||
)
|
||||
.extend(uart.UART_DEVICE_SCHEMA)
|
||||
.extend(cv.COMPONENT_SCHEMA),
|
||||
)
|
||||
|
||||
FINAL_VALIDATE_SCHEMA = uart.final_validate_device_schema(
|
||||
"ld6002b",
|
||||
baud_rate=115200,
|
||||
require_tx=True,
|
||||
require_rx=True,
|
||||
data_bits=8,
|
||||
parity="NONE",
|
||||
stop_bits=1,
|
||||
)
|
||||
|
||||
|
||||
async def to_code(config):
|
||||
var = cg.new_Pvariable(config[CONF_ID])
|
||||
await cg.register_component(var, config)
|
||||
await uart.register_uart_device(var, config)
|
||||
|
||||
if wakeup_pin_config := config.get(CONF_WAKEUP_PIN):
|
||||
pin = await cg.gpio_pin_expression(wakeup_pin_config)
|
||||
cg.add(var.set_wakeup_pin(pin))
|
||||
|
||||
cg.add(var.set_wakeup_pulse_ms(config[CONF_WAKEUP_PULSE].total_milliseconds))
|
||||
|
||||
cg.add(var.set_auto_wake(config[CONF_AUTO_WAKE]))
|
||||
@@ -0,0 +1,40 @@
|
||||
import esphome.codegen as cg
|
||||
from esphome.components import binary_sensor
|
||||
import esphome.config_validation as cv
|
||||
from esphome.const import CONF_TARGET, DEVICE_CLASS_OCCUPANCY
|
||||
|
||||
from . import LD6002BComponent
|
||||
from .const import CONF_LD6002B_ID
|
||||
|
||||
DEPENDENCIES = ["ld6002b"]
|
||||
|
||||
MAX_TARGETS = 3
|
||||
|
||||
CONFIG_SCHEMA = cv.Schema(
|
||||
{
|
||||
cv.GenerateID(CONF_LD6002B_ID): cv.use_id(LD6002BComponent),
|
||||
cv.Optional(CONF_TARGET): binary_sensor.binary_sensor_schema(
|
||||
device_class=DEVICE_CLASS_OCCUPANCY,
|
||||
),
|
||||
}
|
||||
).extend(
|
||||
{
|
||||
cv.Optional(f"target_{i + 1}"): binary_sensor.binary_sensor_schema(
|
||||
device_class=DEVICE_CLASS_OCCUPANCY,
|
||||
)
|
||||
for i in range(MAX_TARGETS)
|
||||
}
|
||||
)
|
||||
|
||||
|
||||
async def to_code(config):
|
||||
hub = await cg.get_variable(config[CONF_LD6002B_ID])
|
||||
|
||||
if target_config := config.get(CONF_TARGET):
|
||||
sens = await binary_sensor.new_binary_sensor(target_config)
|
||||
cg.add(hub.set_presence_binary_sensor(sens))
|
||||
|
||||
for i in range(MAX_TARGETS):
|
||||
if target_config := config.get(f"target_{i + 1}"):
|
||||
sens = await binary_sensor.new_binary_sensor(target_config)
|
||||
cg.add(hub.set_target_presence_binary_sensor(i, sens))
|
||||
@@ -0,0 +1,3 @@
|
||||
CONF_AUTO_WAKE = "auto_wake"
|
||||
CONF_LD6002B_ID = "ld6002b_id"
|
||||
CONF_WAKEUP_PULSE = "wakeup_pulse"
|
||||
@@ -0,0 +1,485 @@
|
||||
#include "ld6002b.h"
|
||||
#include "esphome/core/log.h"
|
||||
#include <algorithm>
|
||||
#include <cinttypes>
|
||||
#include <cstring>
|
||||
|
||||
namespace esphome::ld6002b {
|
||||
|
||||
static const char *const TAG = "ld6002b";
|
||||
|
||||
static constexpr uint8_t TF_SOF = 0x01;
|
||||
static constexpr uint32_t SETUP_DELAY_MS = 100;
|
||||
|
||||
// Command/message types
|
||||
static constexpr uint16_t TYPE_CONTROL = 0x0201;
|
||||
|
||||
static constexpr uint16_t TYPE_REPORT_TARGET = 0x0A04;
|
||||
|
||||
// Control command values for TYPE_CONTROL
|
||||
static constexpr uint32_t CMD_POINT_CLOUD_ON = 0x06;
|
||||
static constexpr uint32_t CMD_POINT_CLOUD_OFF = 0x07;
|
||||
static constexpr uint32_t CMD_TARGET_DISPLAY_ON = 0x08;
|
||||
static constexpr uint32_t CMD_TARGET_DISPLAY_OFF = 0x09;
|
||||
|
||||
static constexpr uint16_t TARGET_DATA_LEN = 20; // x,y,z,dop_idx,cluster_id
|
||||
|
||||
#ifdef ESPHOME_LOG_HAS_VERBOSE
|
||||
static const char *control_command_name(uint32_t command) {
|
||||
switch (command) {
|
||||
case CMD_POINT_CLOUD_ON:
|
||||
return "point_cloud_on";
|
||||
case CMD_POINT_CLOUD_OFF:
|
||||
return "point_cloud_off";
|
||||
case CMD_TARGET_DISPLAY_ON:
|
||||
return "target_display_on";
|
||||
case CMD_TARGET_DISPLAY_OFF:
|
||||
return "target_display_off";
|
||||
default:
|
||||
return "unknown";
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
uint16_t LD6002BComponent::read_u16_be(const uint8_t *data) { return (static_cast<uint16_t>(data[0]) << 8) | data[1]; }
|
||||
|
||||
uint32_t LD6002BComponent::read_u32_le(const uint8_t *data) {
|
||||
return static_cast<uint32_t>(data[0]) | (static_cast<uint32_t>(data[1]) << 8) |
|
||||
(static_cast<uint32_t>(data[2]) << 16) | (static_cast<uint32_t>(data[3]) << 24);
|
||||
}
|
||||
|
||||
void LD6002BComponent::write_u32_le(uint8_t *data, uint32_t value) {
|
||||
data[0] = value & 0xFF;
|
||||
data[1] = (value >> 8) & 0xFF;
|
||||
data[2] = (value >> 16) & 0xFF;
|
||||
data[3] = (value >> 24) & 0xFF;
|
||||
}
|
||||
|
||||
void LD6002BComponent::setup() {
|
||||
// One allocation for the component lifetime; the parser reuses it for the header and every payload.
|
||||
RAMAllocator<uint8_t> allocator;
|
||||
this->data_buf_ = allocator.allocate(DEFAULT_MAX_DATA_LEN);
|
||||
if (this->data_buf_ == nullptr) {
|
||||
this->mark_failed(LOG_STR("Failed to allocate frame buffer"));
|
||||
return;
|
||||
}
|
||||
if (this->wakeup_pin_ != nullptr) {
|
||||
this->wakeup_pin_->setup();
|
||||
this->wakeup_pin_->digital_write(true);
|
||||
}
|
||||
|
||||
this->set_timeout(SETUP_DELAY_MS, [this]() {
|
||||
bool want_target_stream = false;
|
||||
#ifdef USE_BINARY_SENSOR
|
||||
want_target_stream = want_target_stream || this->presence_binary_sensor_ != nullptr;
|
||||
if (!want_target_stream) {
|
||||
for (auto *sensor : this->target_presence_) {
|
||||
if (sensor != nullptr) {
|
||||
want_target_stream = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
if (want_target_stream) {
|
||||
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);
|
||||
});
|
||||
}
|
||||
|
||||
void LD6002BComponent::dump_config() {
|
||||
ESP_LOGCONFIG(TAG,
|
||||
"HLK-LD6002B:\n"
|
||||
" Auto wake: %s",
|
||||
this->auto_wake_ ? "true" : "false");
|
||||
if (this->wakeup_pin_ != nullptr) {
|
||||
LOG_PIN(" Wake-up Pin: ", this->wakeup_pin_);
|
||||
ESP_LOGCONFIG(TAG, " Wake Pulse: %ums", this->wakeup_pulse_ms_);
|
||||
}
|
||||
#ifdef USE_BINARY_SENSOR
|
||||
LOG_BINARY_SENSOR(" ", "Presence", this->presence_binary_sensor_);
|
||||
for (uint8_t i = 0; i < MAX_TARGETS; i++) {
|
||||
LOG_BINARY_SENSOR(" ", "Target Presence", this->target_presence_[i]);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
void LD6002BComponent::loop() {
|
||||
while (this->available()) {
|
||||
uint8_t byte = this->read();
|
||||
this->parse_byte_(byte);
|
||||
}
|
||||
this->process_command_queue_();
|
||||
}
|
||||
|
||||
void LD6002BComponent::reset_parser_() {
|
||||
this->parse_state_ = ParseState::SOF;
|
||||
this->header_pos_ = 0;
|
||||
this->header_xor_ = 0;
|
||||
this->data_len_ = 0;
|
||||
this->data_pos_ = 0;
|
||||
this->data_xor_ = 0;
|
||||
this->discard_remaining_ = 0;
|
||||
this->frame_oversize_ = false;
|
||||
}
|
||||
|
||||
void LD6002BComponent::parse_byte_(uint8_t byte) {
|
||||
switch (this->parse_state_) {
|
||||
case ParseState::DISCARD:
|
||||
// discard_remaining_ is unsigned: an unguarded decrement at zero would swallow 4 GB of stream.
|
||||
if (this->discard_remaining_ > 0) {
|
||||
this->discard_remaining_--;
|
||||
}
|
||||
if (this->discard_remaining_ == 0) {
|
||||
this->reset_parser_();
|
||||
}
|
||||
return;
|
||||
case ParseState::SOF:
|
||||
if (byte != TF_SOF)
|
||||
return;
|
||||
this->header_pos_ = 0;
|
||||
this->header_xor_ = 0;
|
||||
this->header_xor_ ^= byte;
|
||||
this->parse_state_ = ParseState::HEADER;
|
||||
return;
|
||||
case ParseState::HEADER:
|
||||
if (this->header_pos_ < 6) {
|
||||
this->data_buf_[this->header_pos_] = byte;
|
||||
this->header_xor_ ^= byte;
|
||||
this->header_pos_++;
|
||||
if (this->header_pos_ == 6) {
|
||||
this->frame_id_ = read_u16_be(this->data_buf_);
|
||||
this->data_len_ = read_u16_be(this->data_buf_ + 2);
|
||||
this->frame_type_ = read_u16_be(this->data_buf_ + 4);
|
||||
// The length is only trustworthy once the header checksum has been verified, so just
|
||||
// remember that the frame is oversized and let the HCK state act on it.
|
||||
this->frame_oversize_ = this->data_len_ > DEFAULT_MAX_DATA_LEN;
|
||||
this->parse_state_ = ParseState::HCK;
|
||||
}
|
||||
}
|
||||
return;
|
||||
case ParseState::HCK: {
|
||||
uint8_t expected = static_cast<uint8_t>(~this->header_xor_);
|
||||
if (byte != expected) {
|
||||
ESP_LOGV(TAG, "Header checksum mismatch");
|
||||
this->reset_parser_();
|
||||
return;
|
||||
}
|
||||
if (this->frame_oversize_) {
|
||||
ESP_LOGW(TAG, "Frame too large: %u", this->data_len_);
|
||||
// The header is verified, so the length can be trusted: skip the payload and its checksum.
|
||||
this->discard_remaining_ = static_cast<uint32_t>(this->data_len_) + 1;
|
||||
this->parse_state_ = ParseState::DISCARD;
|
||||
return;
|
||||
}
|
||||
if (this->data_len_ == 0) {
|
||||
this->handle_frame_(this->frame_type_, nullptr, 0);
|
||||
this->reset_parser_();
|
||||
} else {
|
||||
this->data_pos_ = 0;
|
||||
this->data_xor_ = 0;
|
||||
this->parse_state_ = ParseState::DATA;
|
||||
}
|
||||
return;
|
||||
}
|
||||
case ParseState::DATA:
|
||||
this->data_buf_[this->data_pos_++] = byte;
|
||||
this->data_xor_ ^= byte;
|
||||
if (this->data_pos_ >= this->data_len_) {
|
||||
this->parse_state_ = ParseState::DCK;
|
||||
}
|
||||
return;
|
||||
case ParseState::DCK: {
|
||||
uint8_t expected = static_cast<uint8_t>(~this->data_xor_);
|
||||
if (byte == expected) {
|
||||
this->handle_frame_(this->frame_type_, this->data_buf_, this->data_len_);
|
||||
} else {
|
||||
ESP_LOGV(TAG, "Data checksum mismatch");
|
||||
}
|
||||
this->reset_parser_();
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void LD6002BComponent::handle_frame_(uint16_t type, const uint8_t *data, uint16_t len) {
|
||||
this->last_traffic_ms_ = millis();
|
||||
if (this->stale_ack_count_ > 0 && millis() - this->stale_ack_ms_ > STALE_ACK_MAX_AGE_MS) {
|
||||
this->stale_ack_count_ = 0;
|
||||
}
|
||||
// ACKs carry no id and arrive in send order: debt from earlier attempts is paid before the active command.
|
||||
if (len == 0 && this->stale_ack_count_ > 0 && this->stale_ack_type_ == type) {
|
||||
this->stale_ack_count_--;
|
||||
ESP_LOGV(TAG, "Ignoring ACK for command 0x%04X from an earlier attempt (module frame 0x%04X)", type,
|
||||
this->frame_id_);
|
||||
return;
|
||||
}
|
||||
if (len == 0 && this->command_active_ && this->command_sent_ && type == this->active_command_.type) {
|
||||
ESP_LOGV(TAG, "ACK for command 0x%04X (module frame 0x%04X)", type, this->frame_id_);
|
||||
// This settles one expected reply; the rest stay owed and become the debt for the next command.
|
||||
this->send_generation_++;
|
||||
this->stale_ack_type_ = type;
|
||||
this->stale_ack_count_ = this->acks_expected_ > 0 ? static_cast<uint8_t>(this->acks_expected_ - 1) : 0;
|
||||
this->stale_ack_ms_ = millis();
|
||||
this->command_active_ = false;
|
||||
this->command_sent_ = false;
|
||||
this->last_send_ms_ = 0;
|
||||
this->process_command_queue_();
|
||||
return;
|
||||
}
|
||||
|
||||
switch (type) {
|
||||
case TYPE_REPORT_TARGET:
|
||||
this->handle_target_report_(data, len);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void LD6002BComponent::handle_target_report_(const uint8_t *data, uint16_t len) {
|
||||
if (len < 4)
|
||||
return;
|
||||
|
||||
uint32_t target_num = read_u32_le(data);
|
||||
uint16_t available = (len - 4) / TARGET_DATA_LEN;
|
||||
// Un-narrowed: a report of e.g. 256 targets must not truncate to 0 and read as "absent".
|
||||
const uint32_t reported = std::min<uint32_t>(target_num, available);
|
||||
uint8_t count = static_cast<uint8_t>(std::min<uint32_t>(reported, MAX_TARGETS));
|
||||
|
||||
// The module re-sorts its array by cluster id, so slots key on the id to track the person.
|
||||
std::array<int32_t, MAX_TARGETS> wire_cluster{};
|
||||
std::array<bool, MAX_TARGETS> wire_placed{};
|
||||
std::array<bool, MAX_TARGETS> slot_seen{};
|
||||
for (uint8_t i = 0; i < count; i++) {
|
||||
uint16_t cluster_offset = 4 + (i * TARGET_DATA_LEN) + 16;
|
||||
wire_cluster[i] = static_cast<int32_t>(read_u32_le(data + cluster_offset));
|
||||
}
|
||||
for (uint8_t i = 0; i < count; i++) {
|
||||
for (uint8_t s = 0; s < MAX_TARGETS; s++) {
|
||||
if (this->slot_occupied_[s] && !slot_seen[s] && this->slot_cluster_[s] == wire_cluster[i]) {
|
||||
slot_seen[s] = true;
|
||||
wire_placed[i] = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
for (uint8_t s = 0; s < MAX_TARGETS; s++) {
|
||||
if (!slot_seen[s]) {
|
||||
this->slot_occupied_[s] = false;
|
||||
}
|
||||
}
|
||||
for (uint8_t i = 0; i < count; i++) {
|
||||
if (wire_placed[i]) {
|
||||
continue;
|
||||
}
|
||||
for (uint8_t s = 0; s < MAX_TARGETS; s++) {
|
||||
if (!this->slot_occupied_[s]) {
|
||||
this->slot_occupied_[s] = true;
|
||||
this->slot_cluster_[s] = wire_cluster[i];
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
this->target_presence_any_ = (reported > 0);
|
||||
#ifdef USE_BINARY_SENSOR
|
||||
if (this->presence_binary_sensor_ != nullptr) {
|
||||
this->presence_binary_sensor_->publish_state(this->target_presence_any_);
|
||||
}
|
||||
#endif
|
||||
|
||||
for (uint8_t i = 0; i < MAX_TARGETS; i++) {
|
||||
#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]);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
void LD6002BComponent::queue_command_(uint16_t type, const uint8_t *data, uint8_t len) {
|
||||
if (len > CMD_MAX_DATA_LEN) {
|
||||
ESP_LOGW(TAG, "Command data too large: %u", len);
|
||||
return;
|
||||
}
|
||||
if (this->cmd_count_ >= CMD_QUEUE_SIZE) {
|
||||
ESP_LOGW(TAG, "Command queue full, dropping command 0x%04X", type);
|
||||
return;
|
||||
}
|
||||
|
||||
PendingCommand &cmd = this->cmd_queue_[this->cmd_tail_];
|
||||
cmd.type = type;
|
||||
cmd.len = len;
|
||||
if (len > 0 && data != nullptr) {
|
||||
std::memcpy(cmd.data.data(), data, len);
|
||||
}
|
||||
|
||||
this->cmd_tail_ = (this->cmd_tail_ + 1) % CMD_QUEUE_SIZE;
|
||||
this->cmd_count_++;
|
||||
this->process_command_queue_();
|
||||
}
|
||||
|
||||
void LD6002BComponent::process_command_queue_() {
|
||||
uint32_t now = millis();
|
||||
if (this->command_active_) {
|
||||
// A sleeping module consumes the opening attempt as its wake-up instead of answering it.
|
||||
const uint32_t ack_timeout = this->attempts_sent_ <= 1 ? CMD_FIRST_ACK_TIMEOUT_MS : CMD_ACK_TIMEOUT_MS;
|
||||
if (this->command_sent_ && now - this->last_send_ms_ >= ack_timeout) {
|
||||
const uint32_t active_control_command =
|
||||
(this->active_command_.type == TYPE_CONTROL && this->active_command_.len >= 4)
|
||||
? read_u32_le(this->active_command_.data.data())
|
||||
: 0;
|
||||
if (this->retries_left_ > 0) {
|
||||
#ifdef ESPHOME_LOG_HAS_VERBOSE
|
||||
if (active_control_command != 0) {
|
||||
ESP_LOGV(TAG, "Retrying %s (0x%02" PRIX32 "), %u attempt(s) remaining",
|
||||
control_command_name(active_control_command), active_control_command, this->retries_left_);
|
||||
} else {
|
||||
// Writes without a control subcommand (hold delay, z-range) had no retry trace at all.
|
||||
ESP_LOGV(TAG, "Retrying command 0x%04X, %u attempt(s) remaining", this->active_command_.type,
|
||||
this->retries_left_);
|
||||
}
|
||||
#endif
|
||||
this->command_sent_ = false;
|
||||
this->last_send_ms_ = 0;
|
||||
this->send_command_(this->active_command_.type, this->active_command_.data.data(), this->active_command_.len);
|
||||
this->retries_left_--;
|
||||
} else {
|
||||
if (active_control_command != 0) {
|
||||
ESP_LOGW(TAG, "Command 0x%04X subcommand 0x%02" PRIX32 " timed out", this->active_command_.type,
|
||||
active_control_command);
|
||||
} else {
|
||||
ESP_LOGW(TAG, "Command 0x%04X timed out", this->active_command_.type);
|
||||
}
|
||||
// A reply may still be in flight for the attempt we just gave up on, so carry one over as
|
||||
// debt rather than clearing the ledger, or that late ACK would retire the successor. Only
|
||||
// one: reaching this point means nothing was answered at all, so the older attempts are
|
||||
// speculative, and carrying them would swallow the successor's own replies.
|
||||
const uint16_t owed = (this->stale_ack_type_ == this->active_command_.type ? this->stale_ack_count_ : 0) +
|
||||
(this->acks_expected_ > 0 ? 1 : 0);
|
||||
this->stale_ack_type_ = this->active_command_.type;
|
||||
this->stale_ack_count_ = static_cast<uint8_t>(std::min<uint16_t>(owed, 255));
|
||||
this->stale_ack_ms_ = now;
|
||||
this->send_generation_++;
|
||||
this->command_active_ = false;
|
||||
this->command_sent_ = false;
|
||||
this->last_send_ms_ = 0;
|
||||
}
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
if (this->cmd_count_ == 0)
|
||||
return;
|
||||
|
||||
this->active_command_ = this->cmd_queue_[this->cmd_head_];
|
||||
this->cmd_head_ = (this->cmd_head_ + 1) % CMD_QUEUE_SIZE;
|
||||
this->cmd_count_--;
|
||||
|
||||
this->send_generation_++;
|
||||
this->retries_left_ = CMD_MAX_RETRIES;
|
||||
this->command_active_ = true;
|
||||
this->command_sent_ = false;
|
||||
this->last_send_ms_ = 0;
|
||||
this->attempts_sent_ = 0;
|
||||
this->acks_expected_ = 0;
|
||||
if (this->stale_ack_type_ != this->active_command_.type) {
|
||||
this->stale_ack_count_ = 0;
|
||||
}
|
||||
this->send_command_(this->active_command_.type, this->active_command_.data.data(), this->active_command_.len);
|
||||
}
|
||||
|
||||
void LD6002BComponent::send_command_(uint16_t type, const uint8_t *data, uint8_t len) {
|
||||
this->send_command_internal_(type, data, len, true);
|
||||
}
|
||||
|
||||
void LD6002BComponent::send_command_internal_(uint16_t type, const uint8_t *data, uint8_t len, bool track) {
|
||||
if (len > CMD_MAX_DATA_LEN) {
|
||||
ESP_LOGW(TAG, "Command data too large: %u", len);
|
||||
if (track) {
|
||||
// Release the slot: an unwritten command is never acked and never times out.
|
||||
this->command_active_ = false;
|
||||
this->command_sent_ = false;
|
||||
this->last_send_ms_ = 0;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
// Anonymous timeouts never replace each other; with a pulse already pending the module is waking anyway.
|
||||
if (this->auto_wake_ && this->wakeup_pin_ != nullptr && !this->wake_pulse_pending_) {
|
||||
// Snapshot the payload: the deferred write must not depend on state a completing command changes.
|
||||
if (len > 0 && data != nullptr) {
|
||||
std::memcpy(this->wake_scratch_.data(), data, len);
|
||||
}
|
||||
this->wake_pulse_pending_ = true;
|
||||
this->wakeup_pin_->digital_write(false);
|
||||
const uint8_t generation = this->send_generation_;
|
||||
this->set_timeout(this->wakeup_pulse_ms_, [this, type, len, track, generation]() {
|
||||
this->wakeup_pin_->digital_write(true);
|
||||
this->wake_pulse_pending_ = false;
|
||||
// Anonymous timeouts are never cancelled, so a tracked pulse whose command has since been
|
||||
// retired must not transmit: the frame would land after its successor and be booked to it.
|
||||
if (track && generation != this->send_generation_) {
|
||||
return;
|
||||
}
|
||||
this->write_frame_(type, (len > 0) ? this->wake_scratch_.data() : nullptr, len, track);
|
||||
});
|
||||
return;
|
||||
}
|
||||
|
||||
this->write_frame_(type, data, len, track);
|
||||
}
|
||||
|
||||
void LD6002BComponent::write_frame_(uint16_t type, const uint8_t *data, uint8_t len, bool track) {
|
||||
uint16_t frame_id = this->next_frame_id_++ & 0x7FFF;
|
||||
frame_id |= 0x8000;
|
||||
|
||||
uint8_t header_xor = 0;
|
||||
auto write_header = [&](uint8_t b) {
|
||||
this->write_byte(b);
|
||||
header_xor ^= b;
|
||||
};
|
||||
|
||||
write_header(TF_SOF);
|
||||
write_header((frame_id >> 8) & 0xFF);
|
||||
write_header(frame_id & 0xFF);
|
||||
write_header((len >> 8) & 0xFF);
|
||||
write_header(len & 0xFF);
|
||||
write_header((type >> 8) & 0xFF);
|
||||
write_header(type & 0xFF);
|
||||
|
||||
this->write_byte(static_cast<uint8_t>(~header_xor));
|
||||
|
||||
if (len > 0 && data != nullptr) {
|
||||
uint8_t data_xor = 0;
|
||||
for (uint8_t i = 0; i < len; i++) {
|
||||
this->write_byte(data[i]);
|
||||
data_xor ^= data[i];
|
||||
}
|
||||
this->write_byte(static_cast<uint8_t>(~data_xor));
|
||||
}
|
||||
const uint32_t now = millis();
|
||||
if (track) {
|
||||
// A frame sent to a module that has had time to fall asleep is its wake-up, and goes unanswered.
|
||||
if (this->last_traffic_ms_ != 0 && now - this->last_traffic_ms_ < MODULE_AWAKE_MS) {
|
||||
this->acks_expected_++;
|
||||
}
|
||||
this->last_send_ms_ = now;
|
||||
this->command_sent_ = true;
|
||||
this->attempts_sent_++;
|
||||
}
|
||||
this->last_traffic_ms_ = now;
|
||||
}
|
||||
|
||||
void LD6002BComponent::send_control_command_(uint32_t command) {
|
||||
uint8_t data[4];
|
||||
write_u32_le(data, command);
|
||||
this->queue_command_(TYPE_CONTROL, data, sizeof(data));
|
||||
}
|
||||
|
||||
} // namespace esphome::ld6002b
|
||||
@@ -0,0 +1,133 @@
|
||||
#pragma once
|
||||
|
||||
#include "esphome/core/defines.h"
|
||||
#include "esphome/core/component.h"
|
||||
#include "esphome/core/helpers.h"
|
||||
#include "esphome/core/gpio.h"
|
||||
#include "esphome/components/uart/uart.h"
|
||||
#ifdef USE_BINARY_SENSOR
|
||||
#include "esphome/components/binary_sensor/binary_sensor.h"
|
||||
#endif
|
||||
|
||||
#include <array>
|
||||
|
||||
namespace esphome::ld6002b {
|
||||
|
||||
static constexpr uint8_t MAX_TARGETS = 3;
|
||||
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;
|
||||
|
||||
class LD6002BComponent : public Component, public uart::UARTDevice {
|
||||
public:
|
||||
void setup() override;
|
||||
void loop() override;
|
||||
void dump_config() override;
|
||||
float get_setup_priority() const override { return setup_priority::DATA; }
|
||||
|
||||
void set_wakeup_pin(GPIOPin *pin) { this->wakeup_pin_ = pin; }
|
||||
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_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) {
|
||||
if (target >= MAX_TARGETS)
|
||||
return;
|
||||
this->target_presence_[target] = sensor;
|
||||
}
|
||||
#endif
|
||||
|
||||
protected:
|
||||
enum class ParseState : uint8_t { SOF, HEADER, HCK, DATA, DCK, DISCARD };
|
||||
|
||||
struct PendingCommand {
|
||||
uint16_t type{0};
|
||||
uint8_t len{0};
|
||||
std::array<uint8_t, CMD_MAX_DATA_LEN> data{};
|
||||
};
|
||||
|
||||
void parse_byte_(uint8_t byte);
|
||||
void reset_parser_();
|
||||
void handle_frame_(uint16_t type, const uint8_t *data, uint16_t len);
|
||||
void handle_target_report_(const uint8_t *data, uint16_t len);
|
||||
|
||||
void queue_command_(uint16_t type, const uint8_t *data, uint8_t len);
|
||||
void process_command_queue_();
|
||||
void send_command_(uint16_t type, const uint8_t *data, uint8_t len);
|
||||
void send_command_internal_(uint16_t type, const uint8_t *data, uint8_t len, bool track);
|
||||
void write_frame_(uint16_t type, const uint8_t *data, uint8_t len, bool track);
|
||||
void send_control_command_(uint32_t command);
|
||||
|
||||
static uint16_t read_u16_be(const uint8_t *data);
|
||||
static uint32_t read_u32_le(const uint8_t *data);
|
||||
static void write_u32_le(uint8_t *data, uint32_t value);
|
||||
|
||||
#ifdef USE_BINARY_SENSOR
|
||||
binary_sensor::BinarySensor *presence_binary_sensor_{nullptr};
|
||||
std::array<binary_sensor::BinarySensor *, MAX_TARGETS> target_presence_{};
|
||||
#endif
|
||||
|
||||
GPIOPin *wakeup_pin_{nullptr};
|
||||
uint32_t wakeup_pulse_ms_{50};
|
||||
bool auto_wake_{true};
|
||||
|
||||
ParseState parse_state_{ParseState::SOF};
|
||||
uint8_t header_pos_{0};
|
||||
uint8_t header_xor_{0};
|
||||
uint16_t data_len_{0};
|
||||
uint16_t frame_type_{0};
|
||||
uint16_t frame_id_{0};
|
||||
uint16_t data_pos_{0};
|
||||
uint8_t data_xor_{0};
|
||||
uint32_t discard_remaining_{0};
|
||||
bool frame_oversize_{false};
|
||||
uint8_t *data_buf_{nullptr};
|
||||
uint16_t next_frame_id_{0};
|
||||
|
||||
// Sized for the boot burst: with every platform configured, setup() enqueues
|
||||
// roughly ten GET/config commands back to back before the first ack lands.
|
||||
static constexpr uint8_t CMD_QUEUE_SIZE = 16;
|
||||
static constexpr uint32_t CMD_ACK_TIMEOUT_MS = 300;
|
||||
// A sleeping module consumes the first frame to wake and answers only the one after it.
|
||||
static constexpr uint32_t CMD_FIRST_ACK_TIMEOUT_MS = 600;
|
||||
// How long the module stays awake after any frame, and so still answers the next one.
|
||||
static constexpr uint32_t MODULE_AWAKE_MS = 10000;
|
||||
static constexpr uint8_t CMD_MAX_RETRIES = 3;
|
||||
// A reply cannot trail the frame that earned it for longer than this; the field worst case is ~726ms.
|
||||
static constexpr uint32_t STALE_ACK_MAX_AGE_MS = 1000;
|
||||
|
||||
std::array<PendingCommand, CMD_QUEUE_SIZE> cmd_queue_{};
|
||||
uint8_t cmd_head_{0};
|
||||
uint8_t cmd_tail_{0};
|
||||
uint8_t cmd_count_{0};
|
||||
bool command_active_{false};
|
||||
bool command_sent_{false};
|
||||
PendingCommand active_command_{};
|
||||
// Frame a pending wake pulse will write, snapshotted because active_command_ may move on first.
|
||||
std::array<uint8_t, CMD_MAX_DATA_LEN> wake_scratch_{};
|
||||
bool wake_pulse_pending_{false};
|
||||
uint8_t retries_left_{0};
|
||||
uint32_t last_send_ms_{0};
|
||||
// Last frame seen in either direction; any traffic keeps the module awake.
|
||||
uint32_t last_traffic_ms_{0};
|
||||
// Frames transmitted for the command in flight, including retries; drives the retry budget.
|
||||
uint8_t attempts_sent_{0};
|
||||
// Subset of those the module can actually answer: a frame that woke it is consumed, not replied to.
|
||||
uint8_t acks_expected_{0};
|
||||
// ACKs still owed for superseded attempts; they carry no id, only their arrival order.
|
||||
uint16_t stale_ack_type_{0};
|
||||
uint8_t stale_ack_count_{0};
|
||||
// When that debt was booked, so a debt no reply can still settle expires instead of eating a live ACK.
|
||||
uint32_t stale_ack_ms_{0};
|
||||
// Bumped whenever the active command changes, so a deferred send can tell it was retired.
|
||||
uint8_t send_generation_{0};
|
||||
|
||||
// Which person owns each target_N slot, so a slot survives the module re-sorting its array.
|
||||
std::array<int32_t, MAX_TARGETS> slot_cluster_{};
|
||||
std::array<bool, MAX_TARGETS> slot_occupied_{};
|
||||
|
||||
bool target_presence_any_{false};
|
||||
};
|
||||
|
||||
} // namespace esphome::ld6002b
|
||||
@@ -0,0 +1,11 @@
|
||||
ld6002b:
|
||||
id: ld6002b_radar
|
||||
wakeup_pin: GPIO14
|
||||
|
||||
binary_sensor:
|
||||
- platform: ld6002b
|
||||
ld6002b_id: ld6002b_radar
|
||||
target:
|
||||
name: Presence
|
||||
target_1:
|
||||
name: Target-1 Presence
|
||||
@@ -0,0 +1,3 @@
|
||||
packages:
|
||||
uart_115200: !include ../../test_build_components/common/uart_115200/esp32-idf.yaml
|
||||
ld6002b: !include common.yaml
|
||||
@@ -0,0 +1,3 @@
|
||||
packages:
|
||||
uart_115200: !include ../../test_build_components/common/uart_115200/esp8266-ard.yaml
|
||||
ld6002b: !include common.yaml
|
||||
@@ -0,0 +1,3 @@
|
||||
packages:
|
||||
uart_115200: !include ../../test_build_components/common/uart_115200/rp2040-ard.yaml
|
||||
ld6002b: !include common.yaml
|
||||
Reference in New Issue
Block a user