63 static const char *
const TAG =
"ld2420";
68 ESP_LOGCONFIG(TAG,
"LD2420:");
70 ESP_LOGCONFIG(TAG,
"LD2420 Number:");
76 for (uint8_t gate = 0; gate < LD2420_TOTAL_GATES; gate++) {
87 ESP_LOGCONFIG(TAG,
"LD2420 Select:");
90 ESP_LOGW(TAG,
"LD2420 Firmware Version %s and older are only supported in Simple Mode",
ld2420_firmware_ver_);
96 uint8_t *data_bytes = (uint8_t *) data;
97 for (
size_t i = 0; i < size; i++) {
98 checksum ^= data_bytes[i];
104 std::string version_str = version_string;
105 if (version_str[0] ==
'v') {
106 version_str = version_str.substr(1);
108 version_str.erase(
remove(version_str.begin(), version_str.end(),
'.'), version_str.end());
109 int version_integer = stoi(version_str);
110 return version_integer;
114 ESP_LOGCONFIG(TAG,
"Setting up LD2420...");
116 ESP_LOGE(TAG,
"LD2420 module has failed to respond, check baud rate and serial connections.");
126 std::string fw_str(pfw);
129 listener->on_fw_version(fw_str);
132 for (uint8_t gate = 0; gate < LD2420_TOTAL_GATES; gate++) {
142 ESP_LOGW(TAG,
"LD2420 Frimware Version %s and older are only supported in Simple Mode",
ld2420_firmware_ver_);
152 ESP_LOGCONFIG(TAG,
"LD2420 setup complete.");
158 ESP_LOGCONFIG(TAG,
"No configuration change detected");
161 ESP_LOGCONFIG(TAG,
"Reconfiguring LD2420...");
163 ESP_LOGE(TAG,
"LD2420 module has failed to respond, check baud rate and serial connections.");
168 for (uint8_t gate = 0; gate < LD2420_TOTAL_GATES; gate++) {
179 ESP_LOGCONFIG(TAG,
"LD2420 reconfig complete.");
183 ESP_LOGCONFIG(TAG,
"Setiing factory defaults...");
185 ESP_LOGE(TAG,
"LD2420 module has failed to respond, check baud rate and serial connections.");
195 for (uint8_t gate = 0; gate < LD2420_TOTAL_GATES; gate++) {
208 ESP_LOGCONFIG(TAG,
"LD2420 factory reset complete.");
212 ESP_LOGCONFIG(TAG,
"Restarting LD2420 module...");
219 ESP_LOGCONFIG(TAG,
"LD2420 Restarted.");
227 ESP_LOGCONFIG(TAG,
"Reverted config number edits.");
235 static uint8_t buffer[2048];
236 static uint8_t rx_data;
239 this->
readline_(rx_data, buffer,
sizeof(buffer));
245 for (uint8_t gate = 0; gate < LD2420_TOTAL_GATES; ++gate) {
246 this->
radar_data[gate][sample_number] = gate_energy[gate];
255 for (uint8_t gate = 0; gate < LD2420_TOTAL_GATES; ++gate) {
259 for (uint8_t sample_number = 0; sample_number < CALIBRATE_SAMPLES; ++sample_number) {
264 if (this->
radar_data[gate][sample_number] > peak) {
270 this->
gate_avg[gate] = sum / CALIBRATE_SAMPLES;
274 uint32_t calculated_value =
275 (
static_cast<uint32_t
>(this->
gate_peak[gate]) + (move_factor * static_cast<uint32_t>(this->
gate_peak[gate])));
276 this->
new_config.
move_thresh[gate] =
static_cast<uint16_t
>(calculated_value <= 65535 ? calculated_value : 65535);
278 (
static_cast<uint32_t
>(this->
gate_peak[gate]) + (still_factor * static_cast<uint32_t>(this->
gate_peak[gate])));
279 this->
new_config.
still_thresh[gate] =
static_cast<uint16_t
>(calculated_value <= 65535 ? calculated_value : 65535);
284 for (uint8_t gate = 0; gate < LD2420_TOTAL_GATES; ++gate) {
286 ESP_LOGI(TAG,
"Gate: %2d Avg: %5d Peak: %5d", gate, this->
gate_avg[gate], this->
gate_peak[gate]);
299 for (uint8_t gate = 0; gate < LD2420_TOTAL_GATES; gate++) {
302 for (uint8_t i = 0; i < CALIBRATE_SAMPLES; i++) {
324 buffer[pos++] = rx_data;
330 if (memcmp(&buffer[pos - 4], &CMD_FRAME_FOOTER,
sizeof(CMD_FRAME_FOOTER)) == 0) {
334 }
else if ((buffer[pos - 2] == 0x0D && buffer[pos - 1] == 0x0A) && (
get_mode_() == CMD_SYSTEM_MODE_SIMPLE)) {
337 }
else if ((memcmp(&buffer[pos - 4], &ENERGY_FRAME_FOOTER,
sizeof(ENERGY_FRAME_FOOTER)) == 0) &&
338 (
get_mode_() == CMD_SYSTEM_MODE_ENERGY)) {
352 memcpy(&range, &buffer[index],
sizeof(range));
353 index +=
sizeof(range);
355 for (uint8_t i = 0; i < elements; i++) {
366 const int32_t current_millis =
millis();
386 const uint8_t bufsize = 16;
389 char *endptr{
nullptr};
390 char outbuf[bufsize]{0};
392 if (inbuf[pos - 2] ==
'O' && inbuf[pos - 1] ==
'F' && inbuf[pos] ==
'F') {
394 }
else if (inbuf[pos - 1] ==
'O' && inbuf[pos] ==
'N') {
397 if (inbuf[pos] >=
'0' && inbuf[pos] <=
'9') {
398 if (index < bufsize - 1) {
399 outbuf[index++] = inbuf[pos];
410 outbuf[index] =
'\0';
414 if (
get_mode_() == CMD_SYSTEM_MODE_SIMPLE) {
416 const int32_t current_millis =
millis();
422 for (
auto &listener : this->listeners_)
430 uint8_t reg_element = 0;
431 uint8_t data_element = 0;
432 uint16_t data_pos = 0;
434 ESP_LOGW(TAG,
"LD2420 reply - received command reply frame is corrupt, length exceeds %d bytes.", CMD_MAX_BYTES);
437 ESP_LOGW(TAG,
"LD2420 reply - received command frame is corrupt, length is less than 2 bytes.");
440 memcpy(&this->
cmd_reply_.
error, &buffer[CMD_ERROR_WORD],
sizeof(this->cmd_reply_.error));
447 case (CMD_ENABLE_CONF):
448 ESP_LOGD(TAG,
"LD2420 reply - set config enable: CMD = %2X %s", CMD_ENABLE_CONF, result);
450 case (CMD_DISABLE_CONF):
451 ESP_LOGD(TAG,
"LD2420 reply - set config disable: CMD = %2X %s", CMD_DISABLE_CONF, result);
453 case (CMD_READ_REGISTER):
454 ESP_LOGD(TAG,
"LD2420 reply - read register: CMD = %2X %s", CMD_READ_REGISTER, result);
457 for (uint16_t index = 0; index < (CMD_REG_DATA_REPLY_SIZE *
458 ((buffer[CMD_FRAME_DATA_LENGTH] - 4) / CMD_REG_DATA_REPLY_SIZE));
459 index += CMD_REG_DATA_REPLY_SIZE) {
460 memcpy(&this->
cmd_reply_.
data[reg_element], &buffer[data_pos + index],
sizeof(CMD_REG_DATA_REPLY_SIZE));
465 case (CMD_WRITE_REGISTER):
466 ESP_LOGD(TAG,
"LD2420 reply - write register: CMD = %2X %s", CMD_WRITE_REGISTER, result);
468 case (CMD_WRITE_ABD_PARAM):
469 ESP_LOGD(TAG,
"LD2420 reply - write gate parameter(s): %2X %s", CMD_WRITE_ABD_PARAM, result);
471 case (CMD_READ_ABD_PARAM):
472 ESP_LOGD(TAG,
"LD2420 reply - read gate parameter(s): %2X %s", CMD_READ_ABD_PARAM, result);
473 data_pos = CMD_ABD_DATA_REPLY_START;
474 for (uint16_t index = 0; index < (CMD_ABD_DATA_REPLY_SIZE *
475 ((buffer[CMD_FRAME_DATA_LENGTH] - 4) / CMD_ABD_DATA_REPLY_SIZE));
476 index += CMD_ABD_DATA_REPLY_SIZE) {
477 memcpy(&this->
cmd_reply_.
data[data_element], &buffer[data_pos + index],
478 sizeof(this->cmd_reply_.data[data_element]));
483 case (CMD_WRITE_SYS_PARAM):
484 ESP_LOGD(TAG,
"LD2420 reply - set system parameter(s): %2X %s", CMD_WRITE_SYS_PARAM, result);
486 case (CMD_READ_VERSION):
488 ESP_LOGD(TAG,
"LD2420 reply - module firmware version: %7s %s", this->
ld2420_firmware_ver_, result);
496 uint32_t start_millis =
millis();
498 uint8_t ack_buffer[64];
499 uint8_t cmd_buffer[64];
501 if (frame.
command != CMD_RESTART)
517 for (uint16_t index = 0; index < frame.
data_length; index++) {
518 memcpy(&cmd_buffer[frame.
length], &frame.
data[index],
sizeof(frame.
data[index]));
524 for (uint16_t index = 0; index < frame.
length; index++) {
529 if (frame.
command == CMD_RESTART) {
539 if ((
millis() - start_millis) > 1000) {
541 error = LD2420_ERROR_TIMEOUT;
557 cmd_frame.
header = CMD_FRAME_HEADER;
558 cmd_frame.
command = enable ? CMD_ENABLE_CONF : CMD_DISABLE_CONF;
560 memcpy(&cmd_frame.
data[0], &CMD_PROTOCOL_VER,
sizeof(CMD_PROTOCOL_VER));
563 cmd_frame.
footer = CMD_FRAME_FOOTER;
564 ESP_LOGD(TAG,
"Sending set config %s command: %2X", enable ?
"enable" :
"disable", cmd_frame.
command);
574 cmd_frame.
header = CMD_FRAME_HEADER;
575 cmd_frame.
command = CMD_RESTART;
576 cmd_frame.
footer = CMD_FRAME_FOOTER;
577 ESP_LOGD(TAG,
"Sending restart command: %2X", cmd_frame.
command);
584 cmd_frame.
header = CMD_FRAME_HEADER;
585 cmd_frame.
command = CMD_READ_REGISTER;
586 cmd_frame.
data[1] = reg;
588 cmd_frame.
footer = CMD_FRAME_FOOTER;
589 ESP_LOGD(TAG,
"Sending read register %4X command: %2X", reg, cmd_frame.
command);
596 cmd_frame.
header = CMD_FRAME_HEADER;
597 cmd_frame.
command = CMD_WRITE_REGISTER;
598 memcpy(&cmd_frame.
data[cmd_frame.
data_length], ®,
sizeof(CMD_REG_DATA_REPLY_SIZE));
600 memcpy(&cmd_frame.
data[cmd_frame.
data_length], &value,
sizeof(CMD_REG_DATA_REPLY_SIZE));
602 cmd_frame.
footer = CMD_FRAME_FOOTER;
603 ESP_LOGD(TAG,
"Sending write register %4X command: %2X data = %4X", reg, cmd_frame.
command, value);
613 cmd_frame.
header = CMD_FRAME_HEADER;
614 cmd_frame.
command = CMD_READ_ABD_PARAM;
615 memcpy(&cmd_frame.
data[cmd_frame.
data_length], &CMD_GATE_MOVE_THRESH[gate],
sizeof(CMD_GATE_MOVE_THRESH[gate]));
617 memcpy(&cmd_frame.
data[cmd_frame.
data_length], &CMD_GATE_STILL_THRESH[gate],
sizeof(CMD_GATE_STILL_THRESH[gate]));
619 cmd_frame.
footer = CMD_FRAME_FOOTER;
620 ESP_LOGD(TAG,
"Sending read gate %d high/low theshold command: %2X", gate, cmd_frame.
command);
633 cmd_frame.
header = CMD_FRAME_HEADER;
634 cmd_frame.
command = CMD_READ_ABD_PARAM;
636 sizeof(CMD_MIN_GATE_REG));
639 sizeof(CMD_MAX_GATE_REG));
642 sizeof(CMD_TIMEOUT_REG));
644 cmd_frame.
footer = CMD_FRAME_FOOTER;
645 ESP_LOGD(TAG,
"Sending read gate min max and timeout command: %2X", cmd_frame.
command);
657 uint16_t unknown_parm = 0x0000;
659 cmd_frame.
header = CMD_FRAME_HEADER;
660 cmd_frame.
command = CMD_WRITE_SYS_PARAM;
661 memcpy(&cmd_frame.
data[cmd_frame.
data_length], &CMD_SYSTEM_MODE,
sizeof(CMD_SYSTEM_MODE));
665 memcpy(&cmd_frame.
data[cmd_frame.
data_length], &unknown_parm,
sizeof(unknown_parm));
667 cmd_frame.
footer = CMD_FRAME_FOOTER;
668 ESP_LOGD(TAG,
"Sending write system mode command: %2X", cmd_frame.
command);
676 cmd_frame.
header = CMD_FRAME_HEADER;
677 cmd_frame.
command = CMD_READ_VERSION;
678 cmd_frame.
footer = CMD_FRAME_FOOTER;
680 ESP_LOGD(TAG,
"Sending read firmware version command: %2X", cmd_frame.
command);
693 cmd_frame.
header = CMD_FRAME_HEADER;
694 cmd_frame.
command = CMD_WRITE_ABD_PARAM;
696 sizeof(CMD_MIN_GATE_REG));
698 memcpy(&cmd_frame.
data[cmd_frame.
data_length], &min_gate_distance,
sizeof(min_gate_distance));
699 cmd_frame.
data_length +=
sizeof(min_gate_distance);
701 sizeof(CMD_MAX_GATE_REG));
703 memcpy(&cmd_frame.
data[cmd_frame.
data_length], &max_gate_distance,
sizeof(max_gate_distance));
704 cmd_frame.
data_length +=
sizeof(max_gate_distance);
706 sizeof(CMD_TIMEOUT_REG));
708 memcpy(&cmd_frame.
data[cmd_frame.
data_length], &timeout,
sizeof(timeout));
711 cmd_frame.
footer = CMD_FRAME_FOOTER;
713 ESP_LOGD(TAG,
"Sending write gate min max and timeout command: %2X", cmd_frame.
command);
722 uint16_t move_threshold_gate = CMD_GATE_MOVE_THRESH[gate];
723 uint16_t still_threshold_gate = CMD_GATE_STILL_THRESH[gate];
726 cmd_frame.
header = CMD_FRAME_HEADER;
727 cmd_frame.
command = CMD_WRITE_ABD_PARAM;
728 memcpy(&cmd_frame.
data[cmd_frame.
data_length], &move_threshold_gate,
sizeof(move_threshold_gate));
729 cmd_frame.
data_length +=
sizeof(move_threshold_gate);
730 memcpy(&cmd_frame.
data[cmd_frame.
data_length], &this->new_config.move_thresh[gate],
731 sizeof(this->new_config.move_thresh[gate]));
733 memcpy(&cmd_frame.
data[cmd_frame.
data_length], &still_threshold_gate,
sizeof(still_threshold_gate));
734 cmd_frame.
data_length +=
sizeof(still_threshold_gate);
735 memcpy(&cmd_frame.
data[cmd_frame.
data_length], &this->new_config.still_thresh[gate],
736 sizeof(this->new_config.still_thresh[gate]));
738 cmd_frame.
footer = CMD_FRAME_FOOTER;
739 ESP_LOGD(TAG,
"Sending set gate %4X sensitivity command: %2X", gate, cmd_frame.
command);
757 for (uint8_t gate = 0; gate < LD2420_TOTAL_GATES; gate++) {
uint16_t total_sample_number_counter
std::vector< number::Number * > gate_still_threshold_numbers_
select::Select * operating_selector_
void factory_reset_action()
int32_t last_periodic_millis
void get_reg_value_(uint16_t reg)
void set_system_mode(uint16_t mode)
void write_byte(uint8_t data)
uint8_t current_operating_mode
uint8_t calc_checksum(void *data, size_t size)
uint16_t gate_avg[LD2420_TOTAL_GATES]
void set_calibration_(bool state)
void revert_config_action()
void handle_simple_mode_(const uint8_t *inbuf, int len)
void dump_config() override
button::Button * apply_config_button_
int32_t report_periodic_millis
uint16_t gate_energy_[LD2420_TOTAL_GATES]
void set_timeout(const std::string &name, uint32_t timeout, std::function< void()> &&f)
Set a timeout function with a unique name.
int get_min_max_distances_timeout_()
void publish_state(float state)
int32_t last_normal_periodic_millis
int send_cmd_from_array(CmdFrameT cmd_frame)
void set_mode_(uint16_t mode)
uint32_t still_thresh[LD2420_TOTAL_GATES]
void set_distance_(uint16_t distance)
void handle_energy_mode_(uint8_t *buffer, int len)
number::Number * gate_select_number_
void delay_microseconds_safe(uint32_t us)
Delay for the given amount of microseconds, possibly yielding to other processes during the wait...
uint16_t radar_data[LD2420_TOTAL_GATES][CALIBRATE_SAMPLES]
number::Number * max_gate_distance_number_
void set_gate_threshold(uint8_t gate)
uint32_t IRAM_ATTR HOT millis()
number::Number * gate_move_sensitivity_factor_number_
void auto_calibrate_sensitivity()
void init_gate_config_numbers()
void set_presence_(bool presence)
void set_operating_mode(const std::string &state)
RegConfigT current_config
float gate_move_sensitivity_factor
button::Button * restart_module_button_
const float BUS
For communication buses like i2c/spi.
int get_firmware_int_(const char *version_string)
void readline_(int rx_data, uint8_t *buffer, int len)
void set_reg_value(uint16_t reg, uint16_t value)
void update_radar_data(uint16_t const *gate_energy, uint8_t sample_number)
void get_firmware_version_()
BedjetMode mode
BedJet operating mode.
void set_cmd_active_(bool active)
button::Button * revert_config_button_
number::Number * min_gate_distance_number_
char ld2420_firmware_ver_[8]
std::vector< number::Number * > gate_move_threshold_numbers_
number::Number * gate_still_sensitivity_factor_number_
float get_setup_priority() const override
void set_min_max_distances_timeout(uint32_t max_gate_distance, uint32_t min_gate_distance, uint32_t timeout)
void restart_module_action()
constexpr14 T byteswap(T n)
void refresh_gate_config_numbers()
void apply_config_action()
virtual void mark_failed()
Mark this component as failed.
void publish_state(const std::string &state)
void handle_ack_data_(uint8_t *buffer, int len)
float gate_still_sensitivity_factor
uint32_t move_thresh[LD2420_TOTAL_GATES]
Implementation of SPI Controller mode.
void handle_cmd_error(uint8_t error)
std::vector< LD2420Listener * > listeners_
uint8_t sample_number_counter
uint8_t set_config_mode(bool enable)
int get_gate_threshold_(uint8_t gate)
button::Button * factory_reset_button_
uint16_t gate_peak[LD2420_TOTAL_GATES]
void send_module_restart()
number::Number * gate_timeout_number_