esphome 2024.12.3__py3-none-any.whl

esphome/components/ld2420/ld2420.cpp

@@ -0,0 +1,778 @@
+#include "ld2420.h"
+#include "esphome/core/helpers.h"
+
+/*
+Configure commands - little endian
+
+No command can exceed 64 bytes, otherwise they would need be to be split up into multiple sends.
+
+All send command frames will have:
+  Header = FD FC FB FA, Bytes 0 - 3, uint32_t 0xFAFBFCFD
+  Length, bytes 4 - 5, uint16_t 0x0002, must be at least 2 for the command byte if no addon data.
+  Command bytes 6 - 7, uint16_t
+  Footer = 04 03 02 01 - uint32_t 0x01020304, Always last 4 Bytes.
+Receive
+  Error bytes 8-9 uint16_t, 0 = success, all other positive values = error
+
+Enable config mode:
+Send:
+  UART Tx: FD FC FB FA 04 00 FF 00 02 00 04 03 02 01
+  Command = FF 00 - uint16_t 0x00FF
+  Protocol version = 02 00, can be 1 or 2 - uint16_t 0x0002
+Reply:
+  UART Rx: FD FC FB FA 06 00 FF 01 00 00 02 00 04 03 02 01
+
+Disable config mode:
+Send:
+  UART Tx: FD FC FB FA 02 00 FE 00 04 03 02 01
+  Command = FE 00 - uint16_t 0x00FE
+Receive:
+  UART Rx: FD FC FB FA 04 00 FE 01 00 00 04 03 02 01
+
+Configure system parameters:
+
+UART Tx: FD FC FB FA 08 00 12 00 00 00 64 00 00 00 04 03 02 01  Set system parms
+Command = 12 00 - uint16_t 0x0012, Param
+There are three documented parameters for modes:
+  00 64 = Basic status mode
+    This mode outputs text as presence "ON" or  "OFF" and "Range XXXX"
+    where XXXX is a decimal value for distance in cm
+  00 04 = Energy output mode
+    This mode outputs detailed signal energy values for each gate and the target distance.
+    The data format consist of the following.
+    Header HH, Length LL, Persence PP, Distance DD, 16 Gate Energies EE, Footer FF
+    HH HH HH HH LL LL PP DD DD EE EE .. 16x   .. FF FF FF FF
+    F4 F3 F2 F1 23 00 00 00 00 00 00 .. .. .. .. F8 F7 F6 F5
+  00 00 = debug output mode
+    This mode outputs detailed values consisting of 20 Dopplers, 16 Ranges for a total 20 * 16 * 4 bytes
+    The data format consist of the following.
+    Header HH, Doppler DD, Range RR, Footer FF
+    HH HH HH HH DD DD DD DD .. 20x   .. RR RR RR RR .. 16x   .. FF FF FF FF
+    AA BF 10 14 00 00 00 00 .. .. .. .. 00 00 00 00 .. .. .. .. FD FC FB FA
+
+Configure gate sensitivity parameters:
+UART Tx: FD FC FB FA 0E 00 07 00 10 00 60 EA 00 00 20 00 60 EA 00 00 04 03 02 01
+Command = 12 00 - uint16_t 0x0007
+Gate 0 high thresh = 10 00 uint16_t 0x0010, Threshold value = 60 EA 00 00 uint32_t 0x0000EA60
+Gate 0 low thresh = 20 00 uint16_t 0x0020, Threshold value = 60 EA 00 00 uint32_t 0x0000EA60
+*/
+
+namespace esphome {
+namespace ld2420 {
+
+static const char *const TAG = "ld2420";
+
+float LD2420Component::get_setup_priority() const { return setup_priority::BUS; }
+
+void LD2420Component::dump_config() {
+  ESP_LOGCONFIG(TAG, "LD2420:");
+  ESP_LOGCONFIG(TAG, "  Firmware Version : %7s", this->ld2420_firmware_ver_);
+  ESP_LOGCONFIG(TAG, "LD2420 Number:");
+#ifdef USE_NUMBER
+  LOG_NUMBER(TAG, "  Gate Timeout:", this->gate_timeout_number_);
+  LOG_NUMBER(TAG, "  Gate Max Distance:", this->max_gate_distance_number_);
+  LOG_NUMBER(TAG, "  Gate Min Distance:", this->min_gate_distance_number_);
+  LOG_NUMBER(TAG, "  Gate Select:", this->gate_select_number_);
+  for (uint8_t gate = 0; gate < LD2420_TOTAL_GATES; gate++) {
+    LOG_NUMBER(TAG, "  Gate Move Threshold:", this->gate_move_threshold_numbers_[gate]);
+    LOG_NUMBER(TAG, "  Gate Still Threshold::", this->gate_still_threshold_numbers_[gate]);
+  }
+#endif
+#ifdef USE_BUTTON
+  LOG_BUTTON(TAG, "  Apply Config:", this->apply_config_button_);
+  LOG_BUTTON(TAG, "  Revert Edits:", this->revert_config_button_);
+  LOG_BUTTON(TAG, "  Factory Reset:", this->factory_reset_button_);
+  LOG_BUTTON(TAG, "  Restart Module:", this->restart_module_button_);
+#endif
+  ESP_LOGCONFIG(TAG, "LD2420 Select:");
+  LOG_SELECT(TAG, "  Operating Mode", this->operating_selector_);
+  if (this->get_firmware_int_(ld2420_firmware_ver_) < CALIBRATE_VERSION_MIN) {
+    ESP_LOGW(TAG, "LD2420 Firmware Version %s and older are only supported in Simple Mode", ld2420_firmware_ver_);
+  }
+}
+
+uint8_t LD2420Component::calc_checksum(void *data, size_t size) {
+  uint8_t checksum = 0;
+  uint8_t *data_bytes = (uint8_t *) data;
+  for (size_t i = 0; i < size; i++) {
+    checksum ^= data_bytes[i];  // XOR operation
+  }
+  return checksum;
+}
+
+int LD2420Component::get_firmware_int_(const char *version_string) {
+  std::string version_str = version_string;
+  if (version_str[0] == 'v') {
+    version_str = version_str.substr(1);
+  }
+  version_str.erase(remove(version_str.begin(), version_str.end(), '.'), version_str.end());
+  int version_integer = stoi(version_str);
+  return version_integer;
+}
+
+void LD2420Component::setup() {
+  ESP_LOGCONFIG(TAG, "Setting up LD2420...");
+  if (this->set_config_mode(true) == LD2420_ERROR_TIMEOUT) {
+    ESP_LOGE(TAG, "LD2420 module has failed to respond, check baud rate and serial connections.");
+    this->mark_failed();
+    return;
+  }
+  this->get_min_max_distances_timeout_();
+#ifdef USE_NUMBER
+  this->init_gate_config_numbers();
+#endif
+  this->get_firmware_version_();
+  const char *pfw = this->ld2420_firmware_ver_;
+  std::string fw_str(pfw);
+
+  for (auto &listener : listeners_) {
+    listener->on_fw_version(fw_str);
+  }
+
+  for (uint8_t gate = 0; gate < LD2420_TOTAL_GATES; gate++) {
+    delay_microseconds_safe(125);
+    this->get_gate_threshold_(gate);
+  }
+
+  memcpy(&this->new_config, &this->current_config, sizeof(this->current_config));
+  if (get_firmware_int_(ld2420_firmware_ver_) < CALIBRATE_VERSION_MIN) {
+    this->set_operating_mode(OP_SIMPLE_MODE_STRING);
+    this->operating_selector_->publish_state(OP_SIMPLE_MODE_STRING);
+    this->set_mode_(CMD_SYSTEM_MODE_SIMPLE);
+    ESP_LOGW(TAG, "LD2420 Frimware Version %s and older are only supported in Simple Mode", ld2420_firmware_ver_);
+  } else {
+    this->set_mode_(CMD_SYSTEM_MODE_ENERGY);
+    this->operating_selector_->publish_state(OP_NORMAL_MODE_STRING);
+  }
+#ifdef USE_NUMBER
+  this->init_gate_config_numbers();
+#endif
+  this->set_system_mode(this->system_mode_);
+  this->set_config_mode(false);
+  ESP_LOGCONFIG(TAG, "LD2420 setup complete.");
+}
+
+void LD2420Component::apply_config_action() {
+  const uint8_t checksum = calc_checksum(&this->new_config, sizeof(this->new_config));
+  if (checksum == calc_checksum(&this->current_config, sizeof(this->current_config))) {
+    ESP_LOGCONFIG(TAG, "No configuration change detected");
+    return;
+  }
+  ESP_LOGCONFIG(TAG, "Reconfiguring LD2420...");
+  if (this->set_config_mode(true) == LD2420_ERROR_TIMEOUT) {
+    ESP_LOGE(TAG, "LD2420 module has failed to respond, check baud rate and serial connections.");
+    this->mark_failed();
+    return;
+  }
+  this->set_min_max_distances_timeout(this->new_config.max_gate, this->new_config.min_gate, this->new_config.timeout);
+  for (uint8_t gate = 0; gate < LD2420_TOTAL_GATES; gate++) {
+    delay_microseconds_safe(125);
+    this->set_gate_threshold(gate);
+  }
+  memcpy(&current_config, &new_config, sizeof(new_config));
+#ifdef USE_NUMBER
+  this->init_gate_config_numbers();
+#endif
+  this->set_system_mode(this->system_mode_);
+  this->set_config_mode(false);  // Disable config mode to save new values in LD2420 nvm
+  this->set_operating_mode(OP_NORMAL_MODE_STRING);
+  ESP_LOGCONFIG(TAG, "LD2420 reconfig complete.");
+}
+
+void LD2420Component::factory_reset_action() {
+  ESP_LOGCONFIG(TAG, "Setting factory defaults...");
+  if (this->set_config_mode(true) == LD2420_ERROR_TIMEOUT) {
+    ESP_LOGE(TAG, "LD2420 module has failed to respond, check baud rate and serial connections.");
+    this->mark_failed();
+    return;
+  }
+  this->set_min_max_distances_timeout(FACTORY_MAX_GATE, FACTORY_MIN_GATE, FACTORY_TIMEOUT);
+#ifdef USE_NUMBER
+  this->gate_timeout_number_->state = FACTORY_TIMEOUT;
+  this->min_gate_distance_number_->state = FACTORY_MIN_GATE;
+  this->max_gate_distance_number_->state = FACTORY_MAX_GATE;
+#endif
+  for (uint8_t gate = 0; gate < LD2420_TOTAL_GATES; gate++) {
+    this->new_config.move_thresh[gate] = FACTORY_MOVE_THRESH[gate];
+    this->new_config.still_thresh[gate] = FACTORY_STILL_THRESH[gate];
+    delay_microseconds_safe(125);
+    this->set_gate_threshold(gate);
+  }
+  memcpy(&this->current_config, &this->new_config, sizeof(this->new_config));
+  this->set_system_mode(this->system_mode_);
+  this->set_config_mode(false);
+#ifdef USE_NUMBER
+  this->init_gate_config_numbers();
+  this->refresh_gate_config_numbers();
+#endif
+  ESP_LOGCONFIG(TAG, "LD2420 factory reset complete.");
+}
+
+void LD2420Component::restart_module_action() {
+  ESP_LOGCONFIG(TAG, "Restarting LD2420 module...");
+  this->send_module_restart();
+  this->set_timeout(250, [this]() {
+    this->set_config_mode(true);
+    this->set_system_mode(system_mode_);
+    this->set_config_mode(false);
+  });
+  ESP_LOGCONFIG(TAG, "LD2420 Restarted.");
+}
+
+void LD2420Component::revert_config_action() {
+  memcpy(&this->new_config, &this->current_config, sizeof(this->current_config));
+#ifdef USE_NUMBER
+  this->init_gate_config_numbers();
+#endif
+  ESP_LOGCONFIG(TAG, "Reverted config number edits.");
+}
+
+void LD2420Component::loop() {
+  // If there is a active send command do not process it here, the send command call will handle it.
+  if (!get_cmd_active_()) {
+    if (!available())
+      return;
+    static uint8_t buffer[2048];
+    static uint8_t rx_data;
+    while (available()) {
+      rx_data = read();
+      this->readline_(rx_data, buffer, sizeof(buffer));
+    }
+  }
+}
+
+void LD2420Component::update_radar_data(uint16_t const *gate_energy, uint8_t sample_number) {
+  for (uint8_t gate = 0; gate < LD2420_TOTAL_GATES; ++gate) {
+    this->radar_data[gate][sample_number] = gate_energy[gate];
+  }
+  this->total_sample_number_counter++;
+}
+
+void LD2420Component::auto_calibrate_sensitivity() {
+  // Calculate average and peak values for each gate
+  const float move_factor = gate_move_sensitivity_factor + 1;
+  const float still_factor = (gate_still_sensitivity_factor / 2) + 1;
+  for (uint8_t gate = 0; gate < LD2420_TOTAL_GATES; ++gate) {
+    uint32_t sum = 0;
+    uint16_t peak = 0;
+
+    for (uint8_t sample_number = 0; sample_number < CALIBRATE_SAMPLES; ++sample_number) {
+      // Calculate average
+      sum += this->radar_data[gate][sample_number];
+
+      // Calculate max value
+      if (this->radar_data[gate][sample_number] > peak) {
+        peak = this->radar_data[gate][sample_number];
+      }
+    }
+
+    // Store average and peak values
+    this->gate_avg[gate] = sum / CALIBRATE_SAMPLES;
+    if (this->gate_peak[gate] < peak)
+      this->gate_peak[gate] = peak;
+
+    uint32_t calculated_value =
+        (static_cast<uint32_t>(this->gate_peak[gate]) + (move_factor * static_cast<uint32_t>(this->gate_peak[gate])));
+    this->new_config.move_thresh[gate] = static_cast<uint16_t>(calculated_value <= 65535 ? calculated_value : 65535);
+    calculated_value =
+        (static_cast<uint32_t>(this->gate_peak[gate]) + (still_factor * static_cast<uint32_t>(this->gate_peak[gate])));
+    this->new_config.still_thresh[gate] = static_cast<uint16_t>(calculated_value <= 65535 ? calculated_value : 65535);
+  }
+}
+
+void LD2420Component::report_gate_data() {
+  for (uint8_t gate = 0; gate < LD2420_TOTAL_GATES; ++gate) {
+    // Output results
+    ESP_LOGI(TAG, "Gate: %2d Avg: %5d Peak: %5d", gate, this->gate_avg[gate], this->gate_peak[gate]);
+  }
+  ESP_LOGI(TAG, "Total samples: %d", this->total_sample_number_counter);
+}
+
+void LD2420Component::set_operating_mode(const std::string &state) {
+  // If unsupported firmware ignore mode select
+  if (get_firmware_int_(ld2420_firmware_ver_) >= CALIBRATE_VERSION_MIN) {
+    this->current_operating_mode = OP_MODE_TO_UINT.at(state);
+    // Entering Auto Calibrate we need to clear the privoiuos data collection
+    this->operating_selector_->publish_state(state);
+    if (current_operating_mode == OP_CALIBRATE_MODE) {
+      this->set_calibration_(true);
+      for (uint8_t gate = 0; gate < LD2420_TOTAL_GATES; gate++) {
+        this->gate_avg[gate] = 0;
+        this->gate_peak[gate] = 0;
+        for (uint8_t i = 0; i < CALIBRATE_SAMPLES; i++) {
+          this->radar_data[gate][i] = 0;
+        }
+        this->total_sample_number_counter = 0;
+      }
+    } else {
+      // Set the current data back so we don't have new data that can be applied in error.
+      if (this->get_calibration_())
+        memcpy(&this->new_config, &this->current_config, sizeof(this->current_config));
+      this->set_calibration_(false);
+    }
+  } else {
+    this->current_operating_mode = OP_SIMPLE_MODE;
+    this->operating_selector_->publish_state(OP_SIMPLE_MODE_STRING);
+  }
+}
+
+void LD2420Component::readline_(int rx_data, uint8_t *buffer, int len) {
+  static int pos = 0;
+
+  if (rx_data >= 0) {
+    if (pos < len - 1) {
+      buffer[pos++] = rx_data;
+      buffer[pos] = 0;
+    } else {
+      pos = 0;
+    }
+    if (pos >= 4) {
+      if (memcmp(&buffer[pos - 4], &CMD_FRAME_FOOTER, sizeof(CMD_FRAME_FOOTER)) == 0) {
+        this->set_cmd_active_(false);  // Set command state to inactive after responce.
+        this->handle_ack_data_(buffer, pos);
+        pos = 0;
+      } else if ((buffer[pos - 2] == 0x0D && buffer[pos - 1] == 0x0A) && (get_mode_() == CMD_SYSTEM_MODE_SIMPLE)) {
+        this->handle_simple_mode_(buffer, pos);
+        pos = 0;
+      } else if ((memcmp(&buffer[pos - 4], &ENERGY_FRAME_FOOTER, sizeof(ENERGY_FRAME_FOOTER)) == 0) &&
+                 (get_mode_() == CMD_SYSTEM_MODE_ENERGY)) {
+        this->handle_energy_mode_(buffer, pos);
+        pos = 0;
+      }
+    }
+  }
+}
+
+void LD2420Component::handle_energy_mode_(uint8_t *buffer, int len) {
+  uint8_t index = 6;  // Start at presence byte position
+  uint16_t range;
+  const uint8_t elements = sizeof(this->gate_energy_) / sizeof(this->gate_energy_[0]);
+  this->set_presence_(buffer[index]);
+  index++;
+  memcpy(&range, &buffer[index], sizeof(range));
+  index += sizeof(range);
+  this->set_distance_(range);
+  for (uint8_t i = 0; i < elements; i++) {  // NOLINT
+    memcpy(&this->gate_energy_[i], &buffer[index], sizeof(this->gate_energy_[0]));
+    index += sizeof(this->gate_energy_[0]);
+  }
+
+  if (this->current_operating_mode == OP_CALIBRATE_MODE) {
+    this->update_radar_data(gate_energy_, sample_number_counter);
+    this->sample_number_counter > CALIBRATE_SAMPLES ? this->sample_number_counter = 0 : this->sample_number_counter++;
+  }
+
+  // Resonable refresh rate for home assistant database size health
+  const int32_t current_millis = millis();
+  if (current_millis - this->last_periodic_millis < REFRESH_RATE_MS)
+    return;
+  this->last_periodic_millis = current_millis;
+  for (auto &listener : this->listeners_) {
+    listener->on_distance(get_distance_());
+    listener->on_presence(get_presence_());
+    listener->on_energy(this->gate_energy_, sizeof(this->gate_energy_) / sizeof(this->gate_energy_[0]));
+  }
+
+  if (this->current_operating_mode == OP_CALIBRATE_MODE) {
+    this->auto_calibrate_sensitivity();
+    if (current_millis - this->report_periodic_millis > REFRESH_RATE_MS * CALIBRATE_REPORT_INTERVAL) {
+      this->report_periodic_millis = current_millis;
+      this->report_gate_data();
+    }
+  }
+}
+
+void LD2420Component::handle_simple_mode_(const uint8_t *inbuf, int len) {
+  const uint8_t bufsize = 16;
+  uint8_t index{0};
+  uint8_t pos{0};
+  char *endptr{nullptr};
+  char outbuf[bufsize]{0};
+  while (true) {
+    if (inbuf[pos - 2] == 'O' && inbuf[pos - 1] == 'F' && inbuf[pos] == 'F') {
+      set_presence_(false);
+    } else if (inbuf[pos - 1] == 'O' && inbuf[pos] == 'N') {
+      set_presence_(true);
+    }
+    if (inbuf[pos] >= '0' && inbuf[pos] <= '9') {
+      if (index < bufsize - 1) {
+        outbuf[index++] = inbuf[pos];
+        pos++;
+      }
+    } else {
+      if (pos < len - 1) {
+        pos++;
+      } else {
+        break;
+      }
+    }
+  }
+  outbuf[index] = '\0';
+  if (index > 1)
+    set_distance_(strtol(outbuf, &endptr, 10));
+
+  if (get_mode_() == CMD_SYSTEM_MODE_SIMPLE) {
+    // Resonable refresh rate for home assistant database size health
+    const int32_t current_millis = millis();
+    if (current_millis - this->last_normal_periodic_millis < REFRESH_RATE_MS)
+      return;
+    this->last_normal_periodic_millis = current_millis;
+    for (auto &listener : this->listeners_)
+      listener->on_distance(get_distance_());
+    for (auto &listener : this->listeners_)
+      listener->on_presence(get_presence_());
+  }
+}
+
+void LD2420Component::handle_ack_data_(uint8_t *buffer, int len) {
+  this->cmd_reply_.command = buffer[CMD_FRAME_COMMAND];
+  this->cmd_reply_.length = buffer[CMD_FRAME_DATA_LENGTH];
+  uint8_t reg_element = 0;
+  uint8_t data_element = 0;
+  uint16_t data_pos = 0;
+  if (this->cmd_reply_.length > CMD_MAX_BYTES) {
+    ESP_LOGW(TAG, "LD2420 reply - received command reply frame is corrupt, length exceeds %d bytes.", CMD_MAX_BYTES);
+    return;
+  } else if (this->cmd_reply_.length < 2) {
+    ESP_LOGW(TAG, "LD2420 reply - received command frame is corrupt, length is less than 2 bytes.");
+    return;
+  }
+  memcpy(&this->cmd_reply_.error, &buffer[CMD_ERROR_WORD], sizeof(this->cmd_reply_.error));
+  const char *result = this->cmd_reply_.error ? "failure" : "success";
+  if (this->cmd_reply_.error > 0) {
+    return;
+  };
+  this->cmd_reply_.ack = true;
+  switch ((uint16_t) this->cmd_reply_.command) {
+    case (CMD_ENABLE_CONF):
+      ESP_LOGD(TAG, "LD2420 reply - set config enable: CMD = %2X %s", CMD_ENABLE_CONF, result);
+      break;
+    case (CMD_DISABLE_CONF):
+      ESP_LOGD(TAG, "LD2420 reply - set config disable: CMD = %2X %s", CMD_DISABLE_CONF, result);
+      break;
+    case (CMD_READ_REGISTER):
+      ESP_LOGD(TAG, "LD2420 reply - read register: CMD = %2X %s", CMD_READ_REGISTER, result);
+      // TODO Read/Write register is not implemented yet, this will get flushed out to a proper header file
+      data_pos = 0x0A;
+      for (uint16_t index = 0; index < (CMD_REG_DATA_REPLY_SIZE *  // NOLINT
+                                        ((buffer[CMD_FRAME_DATA_LENGTH] - 4) / CMD_REG_DATA_REPLY_SIZE));
+           index += CMD_REG_DATA_REPLY_SIZE) {
+        memcpy(&this->cmd_reply_.data[reg_element], &buffer[data_pos + index], sizeof(CMD_REG_DATA_REPLY_SIZE));
+        byteswap(this->cmd_reply_.data[reg_element]);
+        reg_element++;
+      }
+      break;
+    case (CMD_WRITE_REGISTER):
+      ESP_LOGD(TAG, "LD2420 reply - write register: CMD = %2X %s", CMD_WRITE_REGISTER, result);
+      break;
+    case (CMD_WRITE_ABD_PARAM):
+      ESP_LOGD(TAG, "LD2420 reply - write gate parameter(s): %2X %s", CMD_WRITE_ABD_PARAM, result);
+      break;
+    case (CMD_READ_ABD_PARAM):
+      ESP_LOGD(TAG, "LD2420 reply - read gate parameter(s): %2X %s", CMD_READ_ABD_PARAM, result);
+      data_pos = CMD_ABD_DATA_REPLY_START;
+      for (uint16_t index = 0; index < (CMD_ABD_DATA_REPLY_SIZE *  // NOLINT
+                                        ((buffer[CMD_FRAME_DATA_LENGTH] - 4) / CMD_ABD_DATA_REPLY_SIZE));
+           index += CMD_ABD_DATA_REPLY_SIZE) {
+        memcpy(&this->cmd_reply_.data[data_element], &buffer[data_pos + index],
+               sizeof(this->cmd_reply_.data[data_element]));
+        byteswap(this->cmd_reply_.data[data_element]);
+        data_element++;
+      }
+      break;
+    case (CMD_WRITE_SYS_PARAM):
+      ESP_LOGD(TAG, "LD2420 reply - set system parameter(s): %2X %s", CMD_WRITE_SYS_PARAM, result);
+      break;
+    case (CMD_READ_VERSION):
+      memcpy(this->ld2420_firmware_ver_, &buffer[12], buffer[10]);
+      ESP_LOGD(TAG, "LD2420 reply - module firmware version: %7s %s", this->ld2420_firmware_ver_, result);
+      break;
+    default:
+      break;
+  }
+}
+
+int LD2420Component::send_cmd_from_array(CmdFrameT frame) {
+  uint32_t start_millis = millis();
+  uint8_t error = 0;
+  uint8_t ack_buffer[64];
+  uint8_t cmd_buffer[64];
+  this->cmd_reply_.ack = false;
+  if (frame.command != CMD_RESTART)
+    this->set_cmd_active_(true);  // Restart does not reply, thus no ack state required.
+  uint8_t retry = 3;
+  while (retry) {
+    frame.length = 0;
+    uint16_t frame_data_bytes = frame.data_length + 2;  // Always add two bytes for the cmd size
+
+    memcpy(&cmd_buffer[frame.length], &frame.header, sizeof(frame.header));
+    frame.length += sizeof(frame.header);
+
+    memcpy(&cmd_buffer[frame.length], &frame_data_bytes, sizeof(frame.data_length));
+    frame.length += sizeof(frame.data_length);
+
+    memcpy(&cmd_buffer[frame.length], &frame.command, sizeof(frame.command));
+    frame.length += sizeof(frame.command);
+
+    for (uint16_t index = 0; index < frame.data_length; index++) {
+      memcpy(&cmd_buffer[frame.length], &frame.data[index], sizeof(frame.data[index]));
+      frame.length += sizeof(frame.data[index]);
+    }
+
+    memcpy(cmd_buffer + frame.length, &frame.footer, sizeof(frame.footer));
+    frame.length += sizeof(frame.footer);
+    for (uint16_t index = 0; index < frame.length; index++) {
+      this->write_byte(cmd_buffer[index]);
+    }
+
+    error = 0;
+    if (frame.command == CMD_RESTART) {
+      return 0;  // restart does not reply exit now
+    }
+
+    while (!this->cmd_reply_.ack) {
+      while (available()) {
+        this->readline_(read(), ack_buffer, sizeof(ack_buffer));
+      }
+      delay_microseconds_safe(1450);
+      // Wait on an Rx from the LD2420 for up to 3 1 second loops, otherwise it could trigger a WDT.
+      if ((millis() - start_millis) > 1000) {
+        start_millis = millis();
+        error = LD2420_ERROR_TIMEOUT;
+        retry--;
+        break;
+      }
+    }
+    if (this->cmd_reply_.ack)
+      retry = 0;
+    if (this->cmd_reply_.error > 0)
+      handle_cmd_error(error);
+  }
+  return error;
+}
+
+uint8_t LD2420Component::set_config_mode(bool enable) {
+  CmdFrameT cmd_frame;
+  cmd_frame.data_length = 0;
+  cmd_frame.header = CMD_FRAME_HEADER;
+  cmd_frame.command = enable ? CMD_ENABLE_CONF : CMD_DISABLE_CONF;
+  if (enable) {
+    memcpy(&cmd_frame.data[0], &CMD_PROTOCOL_VER, sizeof(CMD_PROTOCOL_VER));
+    cmd_frame.data_length += sizeof(CMD_PROTOCOL_VER);
+  }
+  cmd_frame.footer = CMD_FRAME_FOOTER;
+  ESP_LOGD(TAG, "Sending set config %s command: %2X", enable ? "enable" : "disable", cmd_frame.command);
+  return this->send_cmd_from_array(cmd_frame);
+}
+
+// Sends a restart and set system running mode to normal
+void LD2420Component::send_module_restart() { this->ld2420_restart(); }
+
+void LD2420Component::ld2420_restart() {
+  CmdFrameT cmd_frame;
+  cmd_frame.data_length = 0;
+  cmd_frame.header = CMD_FRAME_HEADER;
+  cmd_frame.command = CMD_RESTART;
+  cmd_frame.footer = CMD_FRAME_FOOTER;
+  ESP_LOGD(TAG, "Sending restart command: %2X", cmd_frame.command);
+  this->send_cmd_from_array(cmd_frame);
+}
+
+void LD2420Component::get_reg_value_(uint16_t reg) {
+  CmdFrameT cmd_frame;
+  cmd_frame.data_length = 0;
+  cmd_frame.header = CMD_FRAME_HEADER;
+  cmd_frame.command = CMD_READ_REGISTER;
+  cmd_frame.data[1] = reg;
+  cmd_frame.data_length += 2;
+  cmd_frame.footer = CMD_FRAME_FOOTER;
+  ESP_LOGD(TAG, "Sending read register %4X command: %2X", reg, cmd_frame.command);
+  this->send_cmd_from_array(cmd_frame);
+}
+
+void LD2420Component::set_reg_value(uint16_t reg, uint16_t value) {
+  CmdFrameT cmd_frame;
+  cmd_frame.data_length = 0;
+  cmd_frame.header = CMD_FRAME_HEADER;
+  cmd_frame.command = CMD_WRITE_REGISTER;
+  memcpy(&cmd_frame.data[cmd_frame.data_length], &reg, sizeof(CMD_REG_DATA_REPLY_SIZE));
+  cmd_frame.data_length += 2;
+  memcpy(&cmd_frame.data[cmd_frame.data_length], &value, sizeof(CMD_REG_DATA_REPLY_SIZE));
+  cmd_frame.data_length += 2;
+  cmd_frame.footer = CMD_FRAME_FOOTER;
+  ESP_LOGD(TAG, "Sending write register %4X command: %2X data = %4X", reg, cmd_frame.command, value);
+  this->send_cmd_from_array(cmd_frame);
+}
+
+void LD2420Component::handle_cmd_error(uint8_t error) { ESP_LOGI(TAG, "Command failed: %s", ERR_MESSAGE[error]); }
+
+int LD2420Component::get_gate_threshold_(uint8_t gate) {
+  uint8_t error;
+  CmdFrameT cmd_frame;
+  cmd_frame.data_length = 0;
+  cmd_frame.header = CMD_FRAME_HEADER;
+  cmd_frame.command = CMD_READ_ABD_PARAM;
+  memcpy(&cmd_frame.data[cmd_frame.data_length], &CMD_GATE_MOVE_THRESH[gate], sizeof(CMD_GATE_MOVE_THRESH[gate]));
+  cmd_frame.data_length += 2;
+  memcpy(&cmd_frame.data[cmd_frame.data_length], &CMD_GATE_STILL_THRESH[gate], sizeof(CMD_GATE_STILL_THRESH[gate]));
+  cmd_frame.data_length += 2;
+  cmd_frame.footer = CMD_FRAME_FOOTER;
+  ESP_LOGD(TAG, "Sending read gate %d high/low theshold command: %2X", gate, cmd_frame.command);
+  error = this->send_cmd_from_array(cmd_frame);
+  if (error == 0) {
+    this->current_config.move_thresh[gate] = cmd_reply_.data[0];
+    this->current_config.still_thresh[gate] = cmd_reply_.data[1];
+  }
+  return error;
+}
+
+int LD2420Component::get_min_max_distances_timeout_() {
+  uint8_t error;
+  CmdFrameT cmd_frame;
+  cmd_frame.data_length = 0;
+  cmd_frame.header = CMD_FRAME_HEADER;
+  cmd_frame.command = CMD_READ_ABD_PARAM;
+  memcpy(&cmd_frame.data[cmd_frame.data_length], &CMD_MIN_GATE_REG,
+         sizeof(CMD_MIN_GATE_REG));  // Register: global min detect gate number
+  cmd_frame.data_length += sizeof(CMD_MIN_GATE_REG);
+  memcpy(&cmd_frame.data[cmd_frame.data_length], &CMD_MAX_GATE_REG,
+         sizeof(CMD_MAX_GATE_REG));  // Register: global max detect gate number
+  cmd_frame.data_length += sizeof(CMD_MAX_GATE_REG);
+  memcpy(&cmd_frame.data[cmd_frame.data_length], &CMD_TIMEOUT_REG,
+         sizeof(CMD_TIMEOUT_REG));  // Register: global delay time
+  cmd_frame.data_length += sizeof(CMD_TIMEOUT_REG);
+  cmd_frame.footer = CMD_FRAME_FOOTER;
+  ESP_LOGD(TAG, "Sending read gate min max and timeout command: %2X", cmd_frame.command);
+  error = this->send_cmd_from_array(cmd_frame);
+  if (error == 0) {
+    this->current_config.min_gate = (uint16_t) cmd_reply_.data[0];
+    this->current_config.max_gate = (uint16_t) cmd_reply_.data[1];
+    this->current_config.timeout = (uint16_t) cmd_reply_.data[2];
+  }
+  return error;
+}
+
+void LD2420Component::set_system_mode(uint16_t mode) {
+  CmdFrameT cmd_frame;
+  uint16_t unknown_parm = 0x0000;
+  cmd_frame.data_length = 0;
+  cmd_frame.header = CMD_FRAME_HEADER;
+  cmd_frame.command = CMD_WRITE_SYS_PARAM;
+  memcpy(&cmd_frame.data[cmd_frame.data_length], &CMD_SYSTEM_MODE, sizeof(CMD_SYSTEM_MODE));
+  cmd_frame.data_length += sizeof(CMD_SYSTEM_MODE);
+  memcpy(&cmd_frame.data[cmd_frame.data_length], &mode, sizeof(mode));
+  cmd_frame.data_length += sizeof(mode);
+  memcpy(&cmd_frame.data[cmd_frame.data_length], &unknown_parm, sizeof(unknown_parm));
+  cmd_frame.data_length += sizeof(unknown_parm);
+  cmd_frame.footer = CMD_FRAME_FOOTER;
+  ESP_LOGD(TAG, "Sending write system mode command: %2X", cmd_frame.command);
+  if (this->send_cmd_from_array(cmd_frame) == 0)
+    set_mode_(mode);
+}
+
+void LD2420Component::get_firmware_version_() {
+  CmdFrameT cmd_frame;
+  cmd_frame.data_length = 0;
+  cmd_frame.header = CMD_FRAME_HEADER;
+  cmd_frame.command = CMD_READ_VERSION;
+  cmd_frame.footer = CMD_FRAME_FOOTER;
+
+  ESP_LOGD(TAG, "Sending read firmware version command: %2X", cmd_frame.command);
+  this->send_cmd_from_array(cmd_frame);
+}
+
+void LD2420Component::set_min_max_distances_timeout(uint32_t max_gate_distance, uint32_t min_gate_distance,  // NOLINT
+                                                    uint32_t timeout) {
+  // Header H, Length L, Register R, Value V, Footer F
+  //                        |Min Gate         |Max Gate         |Timeout          |
+  // HH HH HH HH LL LL CC CC RR RR VV VV VV VV RR RR VV VV VV VV RR RR VV VV VV VV FF FF FF FF
+  // FD FC FB FA 14 00 07 00 00 00 01 00 00 00 01 00 09 00 00 00 04 00 0A 00 00 00 04 03 02 01 e.g.
+
+  CmdFrameT cmd_frame;
+  cmd_frame.data_length = 0;
+  cmd_frame.header = CMD_FRAME_HEADER;
+  cmd_frame.command = CMD_WRITE_ABD_PARAM;
+  memcpy(&cmd_frame.data[cmd_frame.data_length], &CMD_MIN_GATE_REG,
+         sizeof(CMD_MIN_GATE_REG));  // Register: global min detect gate number
+  cmd_frame.data_length += sizeof(CMD_MIN_GATE_REG);
+  memcpy(&cmd_frame.data[cmd_frame.data_length], &min_gate_distance, sizeof(min_gate_distance));
+  cmd_frame.data_length += sizeof(min_gate_distance);
+  memcpy(&cmd_frame.data[cmd_frame.data_length], &CMD_MAX_GATE_REG,
+         sizeof(CMD_MAX_GATE_REG));  // Register: global max detect gate number
+  cmd_frame.data_length += sizeof(CMD_MAX_GATE_REG);
+  memcpy(&cmd_frame.data[cmd_frame.data_length], &max_gate_distance, sizeof(max_gate_distance));
+  cmd_frame.data_length += sizeof(max_gate_distance);
+  memcpy(&cmd_frame.data[cmd_frame.data_length], &CMD_TIMEOUT_REG,
+         sizeof(CMD_TIMEOUT_REG));  // Register: global delay time
+  cmd_frame.data_length += sizeof(CMD_TIMEOUT_REG);
+  memcpy(&cmd_frame.data[cmd_frame.data_length], &timeout, sizeof(timeout));
+  ;
+  cmd_frame.data_length += sizeof(timeout);
+  cmd_frame.footer = CMD_FRAME_FOOTER;
+
+  ESP_LOGD(TAG, "Sending write gate min max and timeout command: %2X", cmd_frame.command);
+  this->send_cmd_from_array(cmd_frame);
+}
+
+void LD2420Component::set_gate_threshold(uint8_t gate) {
+  // Header H, Length L, Command C, Register R, Value V, Footer F
+  // HH HH HH HH LL LL CC CC RR RR VV VV VV VV RR RR VV VV VV VV FF FF FF FF
+  // FD FC FB FA 14 00 07 00 10 00 00 FF 00 00 00 01 00 0F 00 00 04 03 02 01
+
+  uint16_t move_threshold_gate = CMD_GATE_MOVE_THRESH[gate];
+  uint16_t still_threshold_gate = CMD_GATE_STILL_THRESH[gate];
+  CmdFrameT cmd_frame;
+  cmd_frame.data_length = 0;
+  cmd_frame.header = CMD_FRAME_HEADER;
+  cmd_frame.command = CMD_WRITE_ABD_PARAM;
+  memcpy(&cmd_frame.data[cmd_frame.data_length], &move_threshold_gate, sizeof(move_threshold_gate));
+  cmd_frame.data_length += sizeof(move_threshold_gate);
+  memcpy(&cmd_frame.data[cmd_frame.data_length], &this->new_config.move_thresh[gate],
+         sizeof(this->new_config.move_thresh[gate]));
+  cmd_frame.data_length += sizeof(this->new_config.move_thresh[gate]);
+  memcpy(&cmd_frame.data[cmd_frame.data_length], &still_threshold_gate, sizeof(still_threshold_gate));
+  cmd_frame.data_length += sizeof(still_threshold_gate);
+  memcpy(&cmd_frame.data[cmd_frame.data_length], &this->new_config.still_thresh[gate],
+         sizeof(this->new_config.still_thresh[gate]));
+  cmd_frame.data_length += sizeof(this->new_config.still_thresh[gate]);
+  cmd_frame.footer = CMD_FRAME_FOOTER;
+  ESP_LOGD(TAG, "Sending set gate %4X sensitivity command: %2X", gate, cmd_frame.command);
+  this->send_cmd_from_array(cmd_frame);
+}
+
+#ifdef USE_NUMBER
+void LD2420Component::init_gate_config_numbers() {
+  if (this->gate_timeout_number_ != nullptr)
+    this->gate_timeout_number_->publish_state(static_cast<uint16_t>(this->current_config.timeout));
+  if (this->gate_select_number_ != nullptr)
+    this->gate_select_number_->publish_state(0);
+  if (this->min_gate_distance_number_ != nullptr)
+    this->min_gate_distance_number_->publish_state(static_cast<uint16_t>(this->current_config.min_gate));
+  if (this->max_gate_distance_number_ != nullptr)
+    this->max_gate_distance_number_->publish_state(static_cast<uint16_t>(this->current_config.max_gate));
+  if (this->gate_move_sensitivity_factor_number_ != nullptr)
+    this->gate_move_sensitivity_factor_number_->publish_state(this->gate_move_sensitivity_factor);
+  if (this->gate_still_sensitivity_factor_number_ != nullptr)
+    this->gate_still_sensitivity_factor_number_->publish_state(this->gate_still_sensitivity_factor);
+  for (uint8_t gate = 0; gate < LD2420_TOTAL_GATES; gate++) {
+    if (this->gate_still_threshold_numbers_[gate] != nullptr) {
+      this->gate_still_threshold_numbers_[gate]->publish_state(
+          static_cast<uint16_t>(this->current_config.still_thresh[gate]));
+    }
+    if (this->gate_move_threshold_numbers_[gate] != nullptr) {
+      this->gate_move_threshold_numbers_[gate]->publish_state(
+          static_cast<uint16_t>(this->current_config.move_thresh[gate]));
+    }
+  }
+}
+
+void LD2420Component::refresh_gate_config_numbers() {
+  this->gate_timeout_number_->publish_state(this->new_config.timeout);
+  this->min_gate_distance_number_->publish_state(this->new_config.min_gate);
+  this->max_gate_distance_number_->publish_state(this->new_config.max_gate);
+}
+
+#endif
+
+}  // namespace ld2420
+}  // namespace esphome