ld2420.cpp

Go to the documentation of this file.

#include "ld2420.h"
#include "esphome/core/application.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, Presence 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";
 
// Local const's
static const uint16_t REFRESH_RATE_MS = 1000;
 
// Command sets
static const uint16_t CMD_DISABLE_CONF = 0x00FE;
static const uint16_t CMD_ENABLE_CONF = 0x00FF;
static const uint16_t CMD_PARM_HIGH_TRESH = 0x0012;
static const uint16_t CMD_PARM_LOW_TRESH = 0x0021;
static const uint16_t CMD_PROTOCOL_VER = 0x0002;
static const uint16_t CMD_READ_ABD_PARAM = 0x0008;
static const uint16_t CMD_READ_REG_ADDR = 0x0020;
static const uint16_t CMD_READ_REGISTER = 0x0002;
static const uint16_t CMD_READ_SERIAL_NUM = 0x0011;
static const uint16_t CMD_READ_SYS_PARAM = 0x0013;
static const uint16_t CMD_READ_VERSION = 0x0000;
static const uint16_t CMD_RESTART = 0x0068;
static const uint16_t CMD_SYSTEM_MODE = 0x0000;
static const uint16_t CMD_SYSTEM_MODE_GR = 0x0003;
static const uint16_t CMD_SYSTEM_MODE_MTT = 0x0001;
static const uint16_t CMD_SYSTEM_MODE_SIMPLE = 0x0064;
static const uint16_t CMD_SYSTEM_MODE_DEBUG = 0x0000;
static const uint16_t CMD_SYSTEM_MODE_ENERGY = 0x0004;
static const uint16_t CMD_SYSTEM_MODE_VS = 0x0002;
static const uint16_t CMD_WRITE_ABD_PARAM = 0x0007;
static const uint16_t CMD_WRITE_REGISTER = 0x0001;
static const uint16_t CMD_WRITE_SYS_PARAM = 0x0012;
 
static const uint8_t CMD_ABD_DATA_REPLY_SIZE = 0x04;
static const uint8_t CMD_ABD_DATA_REPLY_START = 0x0A;
static const uint8_t CMD_MAX_BYTES = 0x64;
static const uint8_t CMD_REG_DATA_REPLY_SIZE = 0x02;
 
static const uint8_t LD2420_ERROR_NONE = 0x00;
static const uint8_t LD2420_ERROR_TIMEOUT = 0x02;
static const uint8_t LD2420_ERROR_UNKNOWN = 0x01;
 
// Register address values
static const uint16_t CMD_MIN_GATE_REG = 0x0000;
static const uint16_t CMD_MAX_GATE_REG = 0x0001;
static const uint16_t CMD_TIMEOUT_REG = 0x0004;
static const uint16_t CMD_GATE_MOVE_THRESH[TOTAL_GATES] = {0x0010, 0x0011, 0x0012, 0x0013, 0x0014, 0x0015,
                                                           0x0016, 0x0017, 0x0018, 0x0019, 0x001A, 0x001B,
                                                           0x001C, 0x001D, 0x001E, 0x001F};
static const uint16_t CMD_GATE_STILL_THRESH[TOTAL_GATES] = {0x0020, 0x0021, 0x0022, 0x0023, 0x0024, 0x0025,
                                                            0x0026, 0x0027, 0x0028, 0x0029, 0x002A, 0x002B,
                                                            0x002C, 0x002D, 0x002E, 0x002F};
static const uint32_t FACTORY_MOVE_THRESH[TOTAL_GATES] = {60000, 30000, 400, 250, 250, 250, 250, 250,
                                                          250,   250,   250, 250, 250, 250, 250, 250};
static const uint32_t FACTORY_STILL_THRESH[TOTAL_GATES] = {40000, 20000, 200, 200, 200, 200, 200, 150,
                                                           150,   100,   100, 100, 100, 100, 100, 100};
static const uint16_t FACTORY_TIMEOUT = 120;
static const uint16_t FACTORY_MIN_GATE = 1;
static const uint16_t FACTORY_MAX_GATE = 12;
 
// COMMAND_BYTE Header & Footer
static const uint32_t CMD_FRAME_FOOTER = 0x01020304;
static const uint32_t CMD_FRAME_HEADER = 0xFAFBFCFD;
static const uint32_t DEBUG_FRAME_FOOTER = 0xFAFBFCFD;
static const uint32_t DEBUG_FRAME_HEADER = 0x1410BFAA;
static const uint32_t ENERGY_FRAME_FOOTER = 0xF5F6F7F8;
static const uint32_t ENERGY_FRAME_HEADER = 0xF1F2F3F4;
static const int CALIBRATE_VERSION_MIN = 154;
static const uint8_t CMD_FRAME_COMMAND = 6;
static const uint8_t CMD_FRAME_DATA_LENGTH = 4;
static const uint8_t CMD_FRAME_STATUS = 7;
static const uint8_t CMD_ERROR_WORD = 8;
static const uint8_t ENERGY_SENSOR_START = 9;
static const uint8_t CALIBRATE_REPORT_INTERVAL = 4;
static const std::string OP_NORMAL_MODE_STRING = "Normal";
static const std::string OP_SIMPLE_MODE_STRING = "Simple";
 
// Memory-efficient lookup tables
struct StringToUint8 {
  const char *str;
  const uint8_t value;
};
 
static constexpr StringToUint8 OP_MODE_BY_STR[] = {
    {"Normal", OP_NORMAL_MODE},
    {"Calibrate", OP_CALIBRATE_MODE},
    {"Simple", OP_SIMPLE_MODE},
};
 
static constexpr const char *ERR_MESSAGE[] = {
    "None",
    "Unknown",
    "Timeout",
};
 
// Helper function for lookups
template<size_t N> uint8_t find_uint8(const StringToUint8 (&arr)[N], const std::string &str) {
  for (const auto &entry : arr) {
    if (str == entry.str) {
      return entry.value;
    }
  }
  return 0xFF;  // Not found
}
 
static uint8_t 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;
}
 
static int 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;
}
 
float LD2420Component::get_setup_priority() const { return setup_priority::BUS; }
 
void LD2420Component::dump_config() {
  ESP_LOGCONFIG(TAG,
                "LD2420:\n"
                "  Firmware version: %7s",
                this->firmware_ver_);
#ifdef USE_NUMBER
  ESP_LOGCONFIG(TAG, "Number:");
  LOG_NUMBER("  ", "Gate Timeout:", this->gate_timeout_number_);
  LOG_NUMBER("  ", "Gate Max Distance:", this->max_gate_distance_number_);
  LOG_NUMBER("  ", "Gate Min Distance:", this->min_gate_distance_number_);
  LOG_NUMBER("  ", "Gate Select:", this->gate_select_number_);
  for (uint8_t gate = 0; gate < TOTAL_GATES; gate++) {
    LOG_NUMBER("  ", "Gate Move Threshold:", this->gate_move_threshold_numbers_[gate]);
    LOG_NUMBER("  ", "Gate Still Threshold::", this->gate_still_threshold_numbers_[gate]);
  }
#endif
#ifdef USE_BUTTON
  LOG_BUTTON("  ", "Apply Config:", this->apply_config_button_);
  LOG_BUTTON("  ", "Revert Edits:", this->revert_config_button_);
  LOG_BUTTON("  ", "Factory Reset:", this->factory_reset_button_);
  LOG_BUTTON("  ", "Restart Module:", this->restart_module_button_);
#endif
  ESP_LOGCONFIG(TAG, "Select:");
  LOG_SELECT("  ", "Operating Mode", this->operating_selector_);
  if (ld2420::get_firmware_int(this->firmware_ver_) < CALIBRATE_VERSION_MIN) {
    ESP_LOGW(TAG, "Firmware version %s and older supports Simple Mode only", this->firmware_ver_);
  }
}
 
void LD2420Component::setup() {
  if (this->set_config_mode(true) == LD2420_ERROR_TIMEOUT) {
    ESP_LOGE(TAG, ESP_LOG_MSG_COMM_FAIL);
    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->firmware_ver_;
  std::string fw_str(pfw);
 
  for (auto &listener : this->listeners_) {
    listener->on_fw_version(fw_str);
  }
 
  for (uint8_t gate = 0; gate < TOTAL_GATES; gate++) {
    delay_microseconds_safe(125);
    this->get_gate_threshold_(gate);
  }
 
  memcpy(&this->new_config, &this->current_config, sizeof(this->current_config));
  if (ld2420::get_firmware_int(this->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, "Firmware version %s and older supports Simple Mode only", this->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);
}
 
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_LOGD(TAG, "No configuration change detected");
    return;
  }
  ESP_LOGD(TAG, "Reconfiguring");
  if (this->set_config_mode(true) == LD2420_ERROR_TIMEOUT) {
    ESP_LOGE(TAG, ESP_LOG_MSG_COMM_FAIL);
    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 < 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);
}
 
void LD2420Component::factory_reset_action() {
  ESP_LOGD(TAG, "Setting factory defaults");
  if (this->set_config_mode(true) == LD2420_ERROR_TIMEOUT) {
    ESP_LOGE(TAG, ESP_LOG_MSG_COMM_FAIL);
    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 < 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
}
 
void LD2420Component::restart_module_action() {
  ESP_LOGD(TAG, "Restarting");
  this->send_module_restart();
  this->set_timeout(250, [this]() {
    this->set_config_mode(true);
    this->set_system_mode(this->system_mode_);
    this->set_config_mode(false);
  });
}
 
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_LOGD(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.
  while (!this->cmd_active_ && this->available()) {
    this->readline_(this->read(), this->buffer_data_, MAX_LINE_LENGTH);
  }
}
 
void LD2420Component::update_radar_data(uint16_t const *gate_energy, uint8_t sample_number) {
  for (uint8_t gate = 0; gate < 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 < 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 < 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 (ld2420::get_firmware_int(firmware_ver_) >= CALIBRATE_VERSION_MIN) {
    this->current_operating_mode = find_uint8(OP_MODE_BY_STR, 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 < 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) {
  if (rx_data < 0) {
    return;  // No data available
  }
  if (this->buffer_pos_ < len - 1) {
    buffer[this->buffer_pos_++] = rx_data;
    buffer[this->buffer_pos_] = 0;
  } else {
    // We should never get here, but just in case...
    ESP_LOGW(TAG, "Max command length exceeded; ignoring");
    this->buffer_pos_ = 0;
  }
  if (this->buffer_pos_ < 4) {
    return;  // Not enough data to process yet
  }
  if (memcmp(&buffer[this->buffer_pos_ - 4], &CMD_FRAME_FOOTER, sizeof(CMD_FRAME_FOOTER)) == 0) {
    this->cmd_active_ = false;  // Set command state to inactive after response
    this->handle_ack_data_(buffer, this->buffer_pos_);
    this->buffer_pos_ = 0;
  } else if ((buffer[this->buffer_pos_ - 2] == 0x0D && buffer[this->buffer_pos_ - 1] == 0x0A) &&
             (this->get_mode_() == CMD_SYSTEM_MODE_SIMPLE)) {
    this->handle_simple_mode_(buffer, this->buffer_pos_);
    this->buffer_pos_ = 0;
  } else if ((memcmp(&buffer[this->buffer_pos_ - 4], &ENERGY_FRAME_FOOTER, sizeof(ENERGY_FRAME_FOOTER)) == 0) &&
             (this->get_mode_() == CMD_SYSTEM_MODE_ENERGY)) {
    this->handle_energy_mode_(buffer, this->buffer_pos_);
    this->buffer_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 = App.get_loop_component_start_time();
  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(this->get_distance_());
    listener->on_presence(this->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') {
      this->set_presence_(false);
    } else if (inbuf[pos - 1] == 'O' && inbuf[pos] == 'N') {
      this->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) {
    this->set_distance_(strtol(outbuf, &endptr, 10));
  }
 
  if (this->get_mode_() == CMD_SYSTEM_MODE_SIMPLE) {
    // Resonable refresh rate for home assistant database size health
    const int32_t current_millis = App.get_loop_component_start_time();
    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(this->get_distance_());
    for (auto &listener : this->listeners_)
      listener->on_presence(this->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, "Reply frame too long");
    return;
  } else if (this->cmd_reply_.length < 2) {
    ESP_LOGW(TAG, "Command frame too short");
    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_LOGV(TAG, "Set config enable: CMD = %2X %s", CMD_ENABLE_CONF, result);
      break;
    case (CMD_DISABLE_CONF):
      ESP_LOGV(TAG, "Set config disable: CMD = %2X %s", CMD_DISABLE_CONF, result);
      break;
    case (CMD_READ_REGISTER):
      ESP_LOGV(TAG, "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_LOGV(TAG, "Write register: CMD = %2X %s", CMD_WRITE_REGISTER, result);
      break;
    case (CMD_WRITE_ABD_PARAM):
      ESP_LOGV(TAG, "Write gate parameter(s): %2X %s", CMD_WRITE_ABD_PARAM, result);
      break;
    case (CMD_READ_ABD_PARAM):
      ESP_LOGV(TAG, "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_LOGV(TAG, "Set system parameter(s): %2X %s", CMD_WRITE_SYS_PARAM, result);
      break;
    case (CMD_READ_VERSION):
      memcpy(this->firmware_ver_, &buffer[12], buffer[10]);
      ESP_LOGV(TAG, "Firmware version: %7s %s", this->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[MAX_LINE_LENGTH];
  uint8_t cmd_buffer[MAX_LINE_LENGTH];
  this->cmd_reply_.ack = false;
  if (frame.command != CMD_RESTART) {
    this->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);
    this->write_array(cmd_buffer, frame.length);
 
    error = 0;
    if (frame.command == CMD_RESTART) {
      return 0;  // restart does not reply exit now
    }
 
    while (!this->cmd_reply_.ack) {
      while (this->available()) {
        this->readline_(this->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) {
      this->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_LOGV(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_LOGV(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_LOGV(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_LOGV(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_LOGE(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_LOGV(TAG, "Sending read gate %d high/low threshold 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_LOGV(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_LOGV(TAG, "Sending write system mode command: %2X", cmd_frame.command);
  if (this->send_cmd_from_array(cmd_frame) == 0) {
    this->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_LOGV(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_LOGV(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_LOGV(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 < 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