63static const char *
const TAG =
"ld2420";
70 " Firmware Version : %7s\n"
78 for (uint8_t gate = 0; gate < LD2420_TOTAL_GATES; gate++) {
89 ESP_LOGCONFIG(TAG,
"LD2420 Select:");
92 ESP_LOGW(TAG,
"LD2420 Firmware Version %s and older are only supported in Simple Mode",
ld2420_firmware_ver_);
98 uint8_t *data_bytes = (uint8_t *) data;
99 for (
size_t i = 0; i < size; i++) {
106 std::string version_str = version_string;
107 if (version_str[0] ==
'v') {
108 version_str = version_str.substr(1);
110 version_str.erase(remove(version_str.begin(), version_str.end(),
'.'), version_str.end());
111 int version_integer = stoi(version_str);
112 return version_integer;
116 ESP_LOGCONFIG(TAG,
"Running setup");
118 ESP_LOGE(TAG,
"LD2420 module has failed to respond, check baud rate and serial connections.");
128 std::string fw_str(pfw);
131 listener->on_fw_version(fw_str);
134 for (uint8_t gate = 0; gate < LD2420_TOTAL_GATES; gate++) {
144 ESP_LOGW(TAG,
"LD2420 Frimware Version %s and older are only supported in Simple Mode",
ld2420_firmware_ver_);
154 ESP_LOGCONFIG(TAG,
"LD2420 setup complete.");
160 ESP_LOGCONFIG(TAG,
"No configuration change detected");
163 ESP_LOGCONFIG(TAG,
"Reconfiguring LD2420");
165 ESP_LOGE(TAG,
"LD2420 module has failed to respond, check baud rate and serial connections.");
170 for (uint8_t gate = 0; gate < LD2420_TOTAL_GATES; gate++) {
181 ESP_LOGCONFIG(TAG,
"LD2420 reconfig complete.");
185 ESP_LOGCONFIG(TAG,
"Setting factory defaults");
187 ESP_LOGE(TAG,
"LD2420 module has failed to respond, check baud rate and serial connections.");
197 for (uint8_t gate = 0; gate < LD2420_TOTAL_GATES; gate++) {
210 ESP_LOGCONFIG(TAG,
"LD2420 factory reset complete.");
214 ESP_LOGCONFIG(TAG,
"Restarting LD2420 module");
221 ESP_LOGCONFIG(TAG,
"LD2420 Restarted.");
229 ESP_LOGCONFIG(TAG,
"Reverted config number edits.");
237 static uint8_t buffer[2048];
238 static uint8_t rx_data;
241 this->
readline_(rx_data, buffer,
sizeof(buffer));
247 for (uint8_t gate = 0; gate < LD2420_TOTAL_GATES; ++gate) {
248 this->
radar_data[gate][sample_number] = gate_energy[gate];
257 for (uint8_t gate = 0; gate < LD2420_TOTAL_GATES; ++gate) {
261 for (uint8_t sample_number = 0; sample_number < CALIBRATE_SAMPLES; ++sample_number) {
266 if (this->
radar_data[gate][sample_number] > peak) {
272 this->
gate_avg[gate] = sum / CALIBRATE_SAMPLES;
276 uint32_t calculated_value =
277 (
static_cast<uint32_t
>(this->
gate_peak[gate]) + (move_factor *
static_cast<uint32_t
>(this->
gate_peak[gate])));
278 this->
new_config.
move_thresh[gate] =
static_cast<uint16_t
>(calculated_value <= 65535 ? calculated_value : 65535);
280 (
static_cast<uint32_t
>(this->
gate_peak[gate]) + (still_factor *
static_cast<uint32_t
>(this->
gate_peak[gate])));
281 this->
new_config.
still_thresh[gate] =
static_cast<uint16_t
>(calculated_value <= 65535 ? calculated_value : 65535);
286 for (uint8_t gate = 0; gate < LD2420_TOTAL_GATES; ++gate) {
288 ESP_LOGI(TAG,
"Gate: %2d Avg: %5d Peak: %5d", gate, this->
gate_avg[gate], this->
gate_peak[gate]);
301 for (uint8_t gate = 0; gate < LD2420_TOTAL_GATES; gate++) {
304 for (uint8_t i = 0; i < CALIBRATE_SAMPLES; i++) {
326 buffer[pos++] = rx_data;
332 if (memcmp(&buffer[pos - 4], &CMD_FRAME_FOOTER,
sizeof(CMD_FRAME_FOOTER)) == 0) {
336 }
else if ((buffer[pos - 2] == 0x0D && buffer[pos - 1] == 0x0A) && (
get_mode_() == CMD_SYSTEM_MODE_SIMPLE)) {
339 }
else if ((memcmp(&buffer[pos - 4], &ENERGY_FRAME_FOOTER,
sizeof(ENERGY_FRAME_FOOTER)) == 0) &&
340 (
get_mode_() == CMD_SYSTEM_MODE_ENERGY)) {
354 memcpy(&
range, &buffer[index],
sizeof(
range));
355 index +=
sizeof(
range);
357 for (uint8_t i = 0; i < elements; i++) {
368 const int32_t current_millis =
millis();
388 const uint8_t bufsize = 16;
391 char *endptr{
nullptr};
392 char outbuf[bufsize]{0};
394 if (inbuf[pos - 2] ==
'O' && inbuf[pos - 1] ==
'F' && inbuf[pos] ==
'F') {
396 }
else if (inbuf[pos - 1] ==
'O' && inbuf[pos] ==
'N') {
399 if (inbuf[pos] >=
'0' && inbuf[pos] <=
'9') {
400 if (index < bufsize - 1) {
401 outbuf[index++] = inbuf[pos];
412 outbuf[index] =
'\0';
416 if (
get_mode_() == CMD_SYSTEM_MODE_SIMPLE) {
418 const int32_t current_millis =
millis();
424 for (
auto &listener : this->listeners_)
432 uint8_t reg_element = 0;
433 uint8_t data_element = 0;
434 uint16_t data_pos = 0;
436 ESP_LOGW(TAG,
"LD2420 reply - received command reply frame is corrupt, length exceeds %d bytes.", CMD_MAX_BYTES);
439 ESP_LOGW(TAG,
"LD2420 reply - received command frame is corrupt, length is less than 2 bytes.");
442 memcpy(&this->
cmd_reply_.
error, &buffer[CMD_ERROR_WORD],
sizeof(this->cmd_reply_.error));
449 case (CMD_ENABLE_CONF):
450 ESP_LOGD(TAG,
"LD2420 reply - set config enable: CMD = %2X %s", CMD_ENABLE_CONF, result);
452 case (CMD_DISABLE_CONF):
453 ESP_LOGD(TAG,
"LD2420 reply - set config disable: CMD = %2X %s", CMD_DISABLE_CONF, result);
455 case (CMD_READ_REGISTER):
456 ESP_LOGD(TAG,
"LD2420 reply - read register: CMD = %2X %s", CMD_READ_REGISTER, result);
459 for (uint16_t index = 0; index < (CMD_REG_DATA_REPLY_SIZE *
460 ((buffer[CMD_FRAME_DATA_LENGTH] - 4) / CMD_REG_DATA_REPLY_SIZE));
461 index += CMD_REG_DATA_REPLY_SIZE) {
462 memcpy(&this->
cmd_reply_.
data[reg_element], &buffer[data_pos + index],
sizeof(CMD_REG_DATA_REPLY_SIZE));
467 case (CMD_WRITE_REGISTER):
468 ESP_LOGD(TAG,
"LD2420 reply - write register: CMD = %2X %s", CMD_WRITE_REGISTER, result);
470 case (CMD_WRITE_ABD_PARAM):
471 ESP_LOGD(TAG,
"LD2420 reply - write gate parameter(s): %2X %s", CMD_WRITE_ABD_PARAM, result);
473 case (CMD_READ_ABD_PARAM):
474 ESP_LOGD(TAG,
"LD2420 reply - read gate parameter(s): %2X %s", CMD_READ_ABD_PARAM, result);
475 data_pos = CMD_ABD_DATA_REPLY_START;
476 for (uint16_t index = 0; index < (CMD_ABD_DATA_REPLY_SIZE *
477 ((buffer[CMD_FRAME_DATA_LENGTH] - 4) / CMD_ABD_DATA_REPLY_SIZE));
478 index += CMD_ABD_DATA_REPLY_SIZE) {
479 memcpy(&this->
cmd_reply_.
data[data_element], &buffer[data_pos + index],
480 sizeof(this->cmd_reply_.data[data_element]));
485 case (CMD_WRITE_SYS_PARAM):
486 ESP_LOGD(TAG,
"LD2420 reply - set system parameter(s): %2X %s", CMD_WRITE_SYS_PARAM, result);
488 case (CMD_READ_VERSION):
490 ESP_LOGD(TAG,
"LD2420 reply - module firmware version: %7s %s", this->
ld2420_firmware_ver_, result);
498 uint32_t start_millis =
millis();
500 uint8_t ack_buffer[64];
501 uint8_t cmd_buffer[64];
503 if (frame.
command != CMD_RESTART)
519 for (uint16_t index = 0; index < frame.
data_length; index++) {
520 memcpy(&cmd_buffer[frame.
length], &frame.
data[index],
sizeof(frame.
data[index]));
526 for (uint16_t index = 0; index < frame.
length; index++) {
531 if (frame.
command == CMD_RESTART) {
541 if ((
millis() - start_millis) > 1000) {
543 error = LD2420_ERROR_TIMEOUT;
559 cmd_frame.
header = CMD_FRAME_HEADER;
560 cmd_frame.
command = enable ? CMD_ENABLE_CONF : CMD_DISABLE_CONF;
562 memcpy(&cmd_frame.
data[0], &CMD_PROTOCOL_VER,
sizeof(CMD_PROTOCOL_VER));
565 cmd_frame.
footer = CMD_FRAME_FOOTER;
566 ESP_LOGD(TAG,
"Sending set config %s command: %2X", enable ?
"enable" :
"disable", cmd_frame.
command);
576 cmd_frame.
header = CMD_FRAME_HEADER;
577 cmd_frame.
command = CMD_RESTART;
578 cmd_frame.
footer = CMD_FRAME_FOOTER;
579 ESP_LOGD(TAG,
"Sending restart command: %2X", cmd_frame.
command);
586 cmd_frame.
header = CMD_FRAME_HEADER;
587 cmd_frame.
command = CMD_READ_REGISTER;
588 cmd_frame.
data[1] = reg;
590 cmd_frame.
footer = CMD_FRAME_FOOTER;
591 ESP_LOGD(TAG,
"Sending read register %4X command: %2X", reg, cmd_frame.
command);
598 cmd_frame.
header = CMD_FRAME_HEADER;
599 cmd_frame.
command = CMD_WRITE_REGISTER;
600 memcpy(&cmd_frame.
data[cmd_frame.
data_length], ®,
sizeof(CMD_REG_DATA_REPLY_SIZE));
602 memcpy(&cmd_frame.
data[cmd_frame.
data_length], &value,
sizeof(CMD_REG_DATA_REPLY_SIZE));
604 cmd_frame.
footer = CMD_FRAME_FOOTER;
605 ESP_LOGD(TAG,
"Sending write register %4X command: %2X data = %4X", reg, cmd_frame.
command, value);
615 cmd_frame.
header = CMD_FRAME_HEADER;
616 cmd_frame.
command = CMD_READ_ABD_PARAM;
617 memcpy(&cmd_frame.
data[cmd_frame.
data_length], &CMD_GATE_MOVE_THRESH[gate],
sizeof(CMD_GATE_MOVE_THRESH[gate]));
619 memcpy(&cmd_frame.
data[cmd_frame.
data_length], &CMD_GATE_STILL_THRESH[gate],
sizeof(CMD_GATE_STILL_THRESH[gate]));
621 cmd_frame.
footer = CMD_FRAME_FOOTER;
622 ESP_LOGD(TAG,
"Sending read gate %d high/low theshold command: %2X", gate, cmd_frame.
command);
635 cmd_frame.
header = CMD_FRAME_HEADER;
636 cmd_frame.
command = CMD_READ_ABD_PARAM;
638 sizeof(CMD_MIN_GATE_REG));
641 sizeof(CMD_MAX_GATE_REG));
644 sizeof(CMD_TIMEOUT_REG));
646 cmd_frame.
footer = CMD_FRAME_FOOTER;
647 ESP_LOGD(TAG,
"Sending read gate min max and timeout command: %2X", cmd_frame.
command);
659 uint16_t unknown_parm = 0x0000;
661 cmd_frame.
header = CMD_FRAME_HEADER;
662 cmd_frame.
command = CMD_WRITE_SYS_PARAM;
663 memcpy(&cmd_frame.
data[cmd_frame.
data_length], &CMD_SYSTEM_MODE,
sizeof(CMD_SYSTEM_MODE));
667 memcpy(&cmd_frame.
data[cmd_frame.
data_length], &unknown_parm,
sizeof(unknown_parm));
669 cmd_frame.
footer = CMD_FRAME_FOOTER;
670 ESP_LOGD(TAG,
"Sending write system mode command: %2X", cmd_frame.
command);
678 cmd_frame.
header = CMD_FRAME_HEADER;
679 cmd_frame.
command = CMD_READ_VERSION;
680 cmd_frame.
footer = CMD_FRAME_FOOTER;
682 ESP_LOGD(TAG,
"Sending read firmware version command: %2X", cmd_frame.
command);
695 cmd_frame.
header = CMD_FRAME_HEADER;
696 cmd_frame.
command = CMD_WRITE_ABD_PARAM;
698 sizeof(CMD_MIN_GATE_REG));
700 memcpy(&cmd_frame.
data[cmd_frame.
data_length], &min_gate_distance,
sizeof(min_gate_distance));
701 cmd_frame.
data_length +=
sizeof(min_gate_distance);
703 sizeof(CMD_MAX_GATE_REG));
705 memcpy(&cmd_frame.
data[cmd_frame.
data_length], &max_gate_distance,
sizeof(max_gate_distance));
706 cmd_frame.
data_length +=
sizeof(max_gate_distance);
708 sizeof(CMD_TIMEOUT_REG));
710 memcpy(&cmd_frame.
data[cmd_frame.
data_length], &timeout,
sizeof(timeout));
713 cmd_frame.
footer = CMD_FRAME_FOOTER;
715 ESP_LOGD(TAG,
"Sending write gate min max and timeout command: %2X", cmd_frame.
command);
724 uint16_t move_threshold_gate = CMD_GATE_MOVE_THRESH[gate];
725 uint16_t still_threshold_gate = CMD_GATE_STILL_THRESH[gate];
728 cmd_frame.
header = CMD_FRAME_HEADER;
729 cmd_frame.
command = CMD_WRITE_ABD_PARAM;
730 memcpy(&cmd_frame.
data[cmd_frame.
data_length], &move_threshold_gate,
sizeof(move_threshold_gate));
731 cmd_frame.
data_length +=
sizeof(move_threshold_gate);
732 memcpy(&cmd_frame.
data[cmd_frame.
data_length], &this->new_config.move_thresh[gate],
733 sizeof(this->new_config.move_thresh[gate]));
735 memcpy(&cmd_frame.
data[cmd_frame.
data_length], &still_threshold_gate,
sizeof(still_threshold_gate));
736 cmd_frame.
data_length +=
sizeof(still_threshold_gate);
737 memcpy(&cmd_frame.
data[cmd_frame.
data_length], &this->new_config.still_thresh[gate],
738 sizeof(this->new_config.still_thresh[gate]));
740 cmd_frame.
footer = CMD_FRAME_FOOTER;
741 ESP_LOGD(TAG,
"Sending set gate %4X sensitivity command: %2X", gate, cmd_frame.
command);
759 for (uint8_t gate = 0; gate < LD2420_TOTAL_GATES; gate++) {
BedjetMode mode
BedJet operating mode.
virtual void mark_failed()
Mark this component as failed.
void set_timeout(const std::string &name, uint32_t timeout, std::function< void()> &&f)
Set a timeout function with a unique name.
int get_firmware_int_(const char *version_string)
void dump_config() override
void init_gate_config_numbers()
void readline_(int rx_data, uint8_t *buffer, int len)
void set_system_mode(uint16_t mode)
void handle_ack_data_(uint8_t *buffer, int len)
void revert_config_action()
float gate_still_sensitivity_factor
int32_t last_periodic_millis
void restart_module_action()
std::vector< number::Number * > gate_still_threshold_numbers_
number::Number * gate_select_number_
uint8_t set_config_mode(bool enable)
button::Button * factory_reset_button_
void update_radar_data(uint16_t const *gate_energy, uint8_t sample_number)
float get_setup_priority() const override
void send_module_restart()
void set_distance_(uint16_t distance)
void set_min_max_distances_timeout(uint32_t max_gate_distance, uint32_t min_gate_distance, uint32_t timeout)
uint8_t calc_checksum(void *data, size_t size)
void set_gate_threshold(uint8_t gate)
char ld2420_firmware_ver_[8]
void handle_energy_mode_(uint8_t *buffer, int len)
uint16_t gate_avg[LD2420_TOTAL_GATES]
button::Button * revert_config_button_
void auto_calibrate_sensitivity()
void set_calibration_(bool state)
number::Number * min_gate_distance_number_
button::Button * apply_config_button_
int32_t report_periodic_millis
uint16_t radar_data[LD2420_TOTAL_GATES][CALIBRATE_SAMPLES]
std::vector< LD2420Listener * > listeners_
void get_firmware_version_()
std::vector< number::Number * > gate_move_threshold_numbers_
float gate_move_sensitivity_factor
void set_mode_(uint16_t mode)
uint16_t gate_energy_[LD2420_TOTAL_GATES]
void apply_config_action()
RegConfigT current_config
int get_gate_threshold_(uint8_t gate)
number::Number * gate_still_sensitivity_factor_number_
void handle_cmd_error(uint8_t error)
void factory_reset_action()
int send_cmd_from_array(CmdFrameT cmd_frame)
void get_reg_value_(uint16_t reg)
void refresh_gate_config_numbers()
uint16_t gate_peak[LD2420_TOTAL_GATES]
int32_t last_normal_periodic_millis
void handle_simple_mode_(const uint8_t *inbuf, int len)
number::Number * max_gate_distance_number_
select::Select * operating_selector_
button::Button * restart_module_button_
void set_operating_mode(const std::string &state)
int get_min_max_distances_timeout_()
number::Number * gate_timeout_number_
uint16_t total_sample_number_counter
uint8_t current_operating_mode
uint8_t sample_number_counter
void set_presence_(bool presence)
number::Number * gate_move_sensitivity_factor_number_
void set_reg_value(uint16_t reg, uint16_t value)
void set_cmd_active_(bool active)
void publish_state(float state)
void publish_state(const std::string &state)
void write_byte(uint8_t data)
const float BUS
For communication buses like i2c/spi.
Providing packet encoding functions for exchanging data with a remote host.
void IRAM_ATTR HOT delay_microseconds_safe(uint32_t us)
Delay for the given amount of microseconds, possibly yielding to other processes during the wait.
uint32_t IRAM_ATTR HOT millis()
uint32_t still_thresh[LD2420_TOTAL_GATES]
uint32_t move_thresh[LD2420_TOTAL_GATES]