Test the queue room check for a running sensor

esphome/components/dfrobot_sen0395/automation.h

@@ -1,5 +1,7 @@
 #pragma once
 
+#include <algorithm>
+
 #include "esphome/core/automation.h"
 #include "esphome/core/helpers.h"
 
@@ -27,9 +29,12 @@ class DfrobotSen0395SettingsAction final : public Action<Ts...>, public Parented
   TEMPLATABLE_VALUE(float, det_max4)
 
   void play(const Ts &...x) override {
-    this->parent_->enqueue(make_unique<PowerCommand>(0));
+    // Worst case: stop, factory reset, six settings, save, start
+    if (!this->parent_->has_queue_room(10))
+      return;
+    this->parent_->enqueue(CommandType::COMMAND_TYPE_POWER, false);
     if (this->factory_reset_.has_value() && this->factory_reset_.value(x...) == true) {
-      this->parent_->enqueue(make_unique<FactoryResetCommand>());
+      this->parent_->enqueue(CommandType::COMMAND_TYPE_FACTORY_RESET);
     }
     if (this->det_min1_.has_value() && this->det_max1_.has_value()) {
       // Each segment is read once, so the guard and the command see the same values.
@@ -37,10 +42,10 @@ class DfrobotSen0395SettingsAction final : public Action<Ts...>, public Parented
       if (min1 >= 0) {
         const float max1 = this->det_max1_.value(x...);
         if (max1 >= 0) {
-          this->parent_->enqueue(make_unique<DetRangeCfgCommand>(
-              min1, max1, this->det_min2_.value_or(x..., -1), this->det_max2_.value_or(x..., -1),
-              this->det_min3_.value_or(x..., -1), this->det_max3_.value_or(x..., -1),
-              this->det_min4_.value_or(x..., -1), this->det_max4_.value_or(x..., -1)));
+          this->parent_->enqueue_det_range_cfg(min1, max1, this->det_min2_.value_or(x..., -1),
+                                               this->det_max2_.value_or(x..., -1), this->det_min3_.value_or(x..., -1),
+                                               this->det_max3_.value_or(x..., -1), this->det_min4_.value_or(x..., -1),
+                                               this->det_max4_.value_or(x..., -1));
         }
       }
     }
@@ -48,38 +53,35 @@ class DfrobotSen0395SettingsAction final : public Action<Ts...>, public Parented
       float detect = this->delay_after_detect_.value(x...);
       float disappear = this->delay_after_disappear_.value(x...);
       if (detect >= 0 && disappear >= 0) {
-        this->parent_->enqueue(make_unique<SetLatencyCommand>(detect, disappear));
+        this->parent_->enqueue_set_latency(detect, disappear);
       }
     }
     if (this->start_after_power_on_.has_value()) {
       int8_t val = this->start_after_power_on_.value(x...);
       if (val >= 0) {
-        this->parent_->enqueue(make_unique<SensorCfgStartCommand>(val));
+        this->parent_->enqueue(CommandType::COMMAND_TYPE_SENSOR_CFG_START, val);
       }
     }
     if (this->turn_on_led_.has_value()) {
       int8_t val = this->turn_on_led_.value(x...);
       if (val >= 0) {
-        this->parent_->enqueue(make_unique<LedModeCommand>(val));
+        this->parent_->enqueue(CommandType::COMMAND_TYPE_LED_MODE, val);
       }
     }
     if (this->presence_via_uart_.has_value()) {
       int8_t val = this->presence_via_uart_.value(x...);
       if (val >= 0) {
-        this->parent_->enqueue(make_unique<UartOutputCommand>(val));
+        this->parent_->enqueue(CommandType::COMMAND_TYPE_UART_OUTPUT, val);
       }
     }
     if (this->sensitivity_.has_value()) {
       int8_t val = this->sensitivity_.value(x...);
       if (val >= 0) {
-        if (val > 9) {
-          val = 9;
-        }
-        this->parent_->enqueue(make_unique<SensitivityCommand>(val));
+        this->parent_->enqueue(CommandType::COMMAND_TYPE_SENSITIVITY, std::min<int8_t>(val, 9));
       }
     }
-    this->parent_->enqueue(make_unique<SaveCfgCommand>());
-    this->parent_->enqueue(make_unique<PowerCommand>(1));
+    this->parent_->enqueue(CommandType::COMMAND_TYPE_SAVE_CFG);
+    this->parent_->enqueue(CommandType::COMMAND_TYPE_POWER, true);
   }
 };
 

esphome/components/dfrobot_sen0395/commands.cpp

@@ -1,332 +0,0 @@
-#include "commands.h"
-
-#include <cmath>
-
-#include "esphome/core/log.h"
-
-#include "dfrobot_sen0395.h"
-
-namespace esphome::dfrobot_sen0395 {
-
-ESPHOME_LOG_TAG(TAG, "dfrobot_sen0395.commands");
-
-uint8_t Command::execute(DfrobotSen0395Component *parent) {
-  this->parent_ = parent;
-  if (this->cmd_sent_) {
-    if (this->parent_->read_message_()) {
-      std::string message(this->parent_->read_buffer_);
-      if (message.rfind("is not recognized as a CLI command") != std::string::npos) {
-        ESP_LOGD(TAG, "Command not recognized properly by sensor");
-        if (this->retries_left_ > 0) {
-          this->retries_left_ -= 1;
-          this->cmd_sent_ = false;
-          ESP_LOGD(TAG, "Retrying");
-          return 0;
-        } else {
-          this->parent_->find_prompt_();
-          return 1;  // Command done
-        }
-      }
-      uint8_t rc = on_message(message);
-      if (rc == 2) {
-        if (this->retries_left_ > 0) {
-          this->retries_left_ -= 1;
-          this->cmd_sent_ = false;
-          ESP_LOGD(TAG, "Retrying");
-          return 0;
-        } else {
-          this->parent_->find_prompt_();
-          return 1;  // Command done
-        }
-      } else if (rc == 0) {
-        return 0;
-      } else {
-        this->parent_->find_prompt_();
-        return 1;
-      }
-    }
-    if (millis() - this->parent_->ts_last_cmd_sent_ > this->timeout_ms_) {
-      ESP_LOGD(TAG, "Command timeout");
-      if (this->retries_left_ > 0) {
-        this->retries_left_ -= 1;
-        this->cmd_sent_ = false;
-        ESP_LOGD(TAG, "Retrying");
-      } else {
-        return 1;  // Command done
-      }
-    }
-  } else if (this->parent_->send_cmd_(this->cmd_.c_str(), this->cmd_duration_ms_)) {
-    this->cmd_sent_ = true;
-  }
-  return 0;  // Command not done yet
-}
-
-uint8_t ReadStateCommand::execute(DfrobotSen0395Component *parent) {
-  this->parent_ = parent;
-  if (this->parent_->read_message_()) {
-    std::string message(this->parent_->read_buffer_);
-    if (message.rfind("$JYBSS,0, , , *") != std::string::npos) {
-      this->parent_->set_detected_(false);
-      this->parent_->set_active(true);
-      return 1;  // Command done
-    } else if (message.rfind("$JYBSS,1, , , *") != std::string::npos) {
-      this->parent_->set_detected_(true);
-      this->parent_->set_active(true);
-      return 1;  // Command done
-    }
-  }
-  if (millis() - this->parent_->ts_last_cmd_sent_ > this->timeout_ms_) {
-    return 1;  // Command done, timeout
-  }
-  return 0;  // Command not done yet.
-}
-
-uint8_t ReadStateCommand::on_message(std::string &message) { return 1; }
-
-uint8_t PowerCommand::on_message(std::string &message) {
-  if (message == "sensor stopped already") {
-    this->parent_->set_active(false);
-    ESP_LOGI(TAG, "Stopped sensor (already stopped)");
-    return 1;  // Command done
-  } else if (message == "sensor started already") {
-    this->parent_->set_active(true);
-    ESP_LOGI(TAG, "Started sensor (already started)");
-    return 1;  // Command done
-  } else if (message == "new parameter isn't save, can't startSensor") {
-    this->parent_->set_active(false);
-    ESP_LOGE(TAG, "Can't start sensor! (Use SaveCfgCommand to save config first)");
-    return 1;  // Command done
-  } else if (message == "Done") {
-    this->parent_->set_active(this->power_on_);
-    if (this->power_on_) {
-      ESP_LOGI(TAG, "Started sensor");
-    } else {
-      ESP_LOGI(TAG, "Stopped sensor");
-    }
-    return 1;  // Command done
-  }
-  return 0;  // Command not done yet.
-}
-
-DetRangeCfgCommand::DetRangeCfgCommand(float min1, float max1, float min2, float max2, float min3, float max3,
-                                       float min4, float max4) {
-  // TODO: Print warning when values are rounded
-  if (min1 < 0 || max1 < 0) {
-    this->min1_ = min1 = 0;
-    this->max1_ = max1 = 0;
-    this->min2_ = min2 = this->max2_ = max2 = this->min3_ = min3 = this->max3_ = max3 = this->min4_ = min4 =
-        this->max4_ = max4 = -1;
-
-    ESP_LOGW(TAG, "DetRangeCfgCommand invalid input parameters. Using range config 0 0.");
-
-    this->cmd_ = "detRangeCfg -1 0 0";
-  } else if (min2 < 0 || max2 < 0) {
-    this->min1_ = min1 = roundf(min1 / 0.15f) * 0.15f;
-    this->max1_ = max1 = roundf(max1 / 0.15f) * 0.15f;
-    this->min2_ = min2 = this->max2_ = max2 = this->min3_ = min3 = this->max3_ = max3 = this->min4_ = min4 =
-        this->max4_ = max4 = -1;
-
-    char buf[72];  // max 72: "detRangeCfg -1 "(15) + 8 * (float(5) + space(1)) + null
-    snprintf(buf, sizeof(buf), "detRangeCfg -1 %.0f %.0f", min1 / 0.15f, max1 / 0.15f);
-    this->cmd_ = buf;
-  } else if (min3 < 0 || max3 < 0) {
-    this->min1_ = min1 = roundf(min1 / 0.15f) * 0.15f;
-    this->max1_ = max1 = roundf(max1 / 0.15f) * 0.15f;
-    this->min2_ = min2 = roundf(min2 / 0.15f) * 0.15f;
-    this->max2_ = max2 = roundf(max2 / 0.15f) * 0.15f;
-    this->min3_ = min3 = this->max3_ = max3 = this->min4_ = min4 = this->max4_ = max4 = -1;
-
-    char buf[72];  // max 72: "detRangeCfg -1 "(15) + 8 * (float(5) + space(1)) + null
-    snprintf(buf, sizeof(buf), "detRangeCfg -1 %.0f %.0f %.0f %.0f", min1 / 0.15f, max1 / 0.15f, min2 / 0.15f,
-             max2 / 0.15f);
-    this->cmd_ = buf;
-  } else if (min4 < 0 || max4 < 0) {
-    this->min1_ = min1 = roundf(min1 / 0.15f) * 0.15f;
-    this->max1_ = max1 = roundf(max1 / 0.15f) * 0.15f;
-    this->min2_ = min2 = roundf(min2 / 0.15f) * 0.15f;
-    this->max2_ = max2 = roundf(max2 / 0.15f) * 0.15f;
-    this->min3_ = min3 = roundf(min3 / 0.15f) * 0.15f;
-    this->max3_ = max3 = roundf(max3 / 0.15f) * 0.15f;
-    this->min4_ = min4 = this->max4_ = max4 = -1;
-
-    char buf[72];  // max 72: "detRangeCfg -1 "(15) + 8 * (float(5) + space(1)) + null
-    snprintf(buf, sizeof(buf), "detRangeCfg -1 %.0f %.0f %.0f %.0f %.0f %.0f", min1 / 0.15f, max1 / 0.15f, min2 / 0.15f,
-             max2 / 0.15f, min3 / 0.15f, max3 / 0.15f);
-    this->cmd_ = buf;
-  } else {
-    this->min1_ = min1 = roundf(min1 / 0.15f) * 0.15f;
-    this->max1_ = max1 = roundf(max1 / 0.15f) * 0.15f;
-    this->min2_ = min2 = roundf(min2 / 0.15f) * 0.15f;
-    this->max2_ = max2 = roundf(max2 / 0.15f) * 0.15f;
-    this->min3_ = min3 = roundf(min3 / 0.15f) * 0.15f;
-    this->max3_ = max3 = roundf(max3 / 0.15f) * 0.15f;
-    this->min4_ = min4 = roundf(min4 / 0.15f) * 0.15f;
-    this->max4_ = max4 = roundf(max4 / 0.15f) * 0.15f;
-
-    char buf[72];  // max 72: "detRangeCfg -1 "(15) + 8 * (float(5) + space(1)) + null
-    snprintf(buf, sizeof(buf), "detRangeCfg -1 %.0f %.0f %.0f %.0f %.0f %.0f %.0f %.0f", min1 / 0.15f, max1 / 0.15f,
-             min2 / 0.15f, max2 / 0.15f, min3 / 0.15f, max3 / 0.15f, min4 / 0.15f, max4 / 0.15f);
-    this->cmd_ = buf;
-  }
-
-  this->min1_ = min1;
-  this->max1_ = max1;
-  this->min2_ = min2;
-  this->max2_ = max2;
-  this->min3_ = min3;
-  this->max3_ = max3;
-  this->min4_ = min4;
-  this->max4_ = max4;
-};
-
-uint8_t DetRangeCfgCommand::on_message(std::string &message) {
-  if (message == "sensor is not stopped") {
-    ESP_LOGE(TAG, "Cannot configure range config. Sensor is not stopped!");
-    return 1;  // Command done
-  } else if (message == "Done") {
-    ESP_LOGI(TAG,
-             "Updated detection area config:\n"
-             "  Detection area 1 from %.02fm to %.02fm.",
-             this->min1_, this->max1_);
-    if (this->min2_ >= 0 && this->max2_ >= 0) {
-      ESP_LOGI(TAG, "  Detection area 2 from %.02fm to %.02fm.", this->min2_, this->max2_);
-    }
-    if (this->min3_ >= 0 && this->max3_ >= 0) {
-      ESP_LOGI(TAG, "  Detection area 3 from %.02fm to %.02fm.", this->min3_, this->max3_);
-    }
-    if (this->min4_ >= 0 && this->max4_ >= 0) {
-      ESP_LOGI(TAG, "  Detection area 4 from %.02fm to %.02fm.", this->min4_, this->max4_);
-    }
-    ESP_LOGD(TAG, "  Used command: %s", this->cmd_.c_str());
-    return 1;  // Command done
-  }
-  return 0;  // Command not done yet.
-}
-
-SetLatencyCommand::SetLatencyCommand(float delay_after_detection, float delay_after_disappear) {
-  delay_after_detection = std::round(delay_after_detection / 0.025f) * 0.025f;
-  delay_after_disappear = std::round(delay_after_disappear / 0.025f) * 0.025f;
-  this->delay_after_detection_ = clamp(delay_after_detection, 0.0f, 1638.375f);
-  this->delay_after_disappear_ = clamp(delay_after_disappear, 0.0f, 1638.375f);
-  // max 32: "setLatency "(11) + float(8) + " "(1) + float(8) + null, rounded to 32
-  char buf[32];
-  snprintf(buf, sizeof(buf), "setLatency %.03f %.03f", this->delay_after_detection_, this->delay_after_disappear_);
-  this->cmd_ = buf;
-};
-
-uint8_t SetLatencyCommand::on_message(std::string &message) {
-  if (message == "sensor is not stopped") {
-    ESP_LOGE(TAG, "Cannot configure output latency. Sensor is not stopped!");
-    return 1;  // Command done
-  } else if (message == "Done") {
-    ESP_LOGI(TAG,
-             "Updated output latency config:\n"
-             "  Signal that someone was detected is delayed by %.03f s.\n"
-             "  Signal that nobody is detected anymore is delayed by %.03f s.",
-             this->delay_after_detection_, this->delay_after_disappear_);
-    ESP_LOGD(TAG, "  Used command: %s", this->cmd_.c_str());
-    return 1;  // Command done
-  }
-  return 0;  // Command not done yet
-}
-
-uint8_t SensorCfgStartCommand::on_message(std::string &message) {
-  if (message == "sensor is not stopped") {
-    ESP_LOGE(TAG, "Cannot configure sensor startup behavior. Sensor is not stopped!");
-    return 1;  // Command done
-  } else if (message == "Done") {
-    ESP_LOGI(TAG, "Updated sensor startup behavior:");
-    if (startup_mode_) {
-      this->parent_->set_start_after_boot(true);
-      ESP_LOGI(TAG, "Sensor will start automatically after power-on.");
-    } else {
-      this->parent_->set_start_after_boot(false);
-      ESP_LOGI(TAG, "Sensor needs to be started manually after power-on.");
-    }
-    ESP_LOGD(TAG, "Used command: %s", this->cmd_.c_str());
-    return 1;  // Command done
-  }
-  return 0;  // Command not done yet
-}
-
-uint8_t FactoryResetCommand::on_message(std::string &message) {
-  if (message == "sensor is not stopped") {
-    ESP_LOGE(TAG, "Cannot factory reset. Sensor is not stopped!");
-    return 1;  // Command done
-  } else if (message == "Done") {
-    ESP_LOGI(TAG, "Sensor factory reset done.");
-    return 1;  // Command done
-  }
-  return 0;  // Command not done yet
-}
-
-uint8_t ResetSystemCommand::on_message(std::string &message) {
-  if (message == "leapMMW:/>") {
-    ESP_LOGI(TAG, "Restarted sensor.");
-    return 1;  // Command done
-  }
-  return 0;  // Command not done yet
-}
-
-uint8_t SaveCfgCommand::on_message(std::string &message) {
-  if (message == "no parameter has changed") {
-    ESP_LOGI(TAG, "Not saving config (no parameter changed).");
-    return 1;  // Command done
-  } else if (message == "Done") {
-    ESP_LOGI(TAG, "Saved config. Saving a lot may damage the sensor.");
-    return 1;  // Command done
-  }
-  return 0;  // Command not done yet
-}
-
-uint8_t LedModeCommand::on_message(std::string &message) {
-  if (message == "sensor is not stopped") {
-    ESP_LOGE(TAG, "Cannot set led mode. Sensor is not stopped!");
-    return 1;  // Command done
-  } else if (message == "Done") {
-    ESP_LOGI(TAG, "Set led mode done.");
-    if (this->active_) {
-      this->parent_->set_led_active(true);
-      ESP_LOGI(TAG, "Sensor LED will blink.");
-    } else {
-      this->parent_->set_led_active(false);
-      ESP_LOGI(TAG, "Turned off LED.");
-    }
-    ESP_LOGD(TAG, "Used command: %s", this->cmd_.c_str());
-    return 1;  // Command done
-  }
-  return 0;  // Command not done yet
-}
-
-uint8_t UartOutputCommand::on_message(std::string &message) {
-  if (message == "sensor is not stopped") {
-    ESP_LOGE(TAG, "Cannot set uart output mode. Sensor is not stopped!");
-    return 1;  // Command done
-  } else if (message == "Done") {
-    ESP_LOGI(TAG, "Set uart mode done.");
-    if (this->active_) {
-      this->parent_->set_uart_presence_active(true);
-      ESP_LOGI(TAG, "Presence information is sent via UART and GPIO.");
-    } else {
-      this->parent_->set_uart_presence_active(false);
-      ESP_LOGI(TAG, "Presence information is only sent via GPIO.");
-    }
-    ESP_LOGD(TAG, "Used command: %s", this->cmd_.c_str());
-    return 1;  // Command done
-  }
-  return 0;  // Command not done yet
-}
-
-uint8_t SensitivityCommand::on_message(std::string &message) {
-  if (message == "sensor is not stopped") {
-    ESP_LOGE(TAG, "Cannot set sensitivity. Sensor is not stopped!");
-    return 1;  // Command done
-  } else if (message == "Done") {
-    ESP_LOGI(TAG, "Set sensitivity done. Set to value %d.", this->sensitivity_);
-    ESP_LOGD(TAG, "Used command: %s", this->cmd_.c_str());
-    return 1;  // Command done
-  }
-  return 0;  // Command not done yet
-}
-
-}  // namespace esphome::dfrobot_sen0395

esphome/components/dfrobot_sen0395/commands.h

@@ -1,152 +0,0 @@
-#pragma once
-
-#include <cstdint>
-#include <string>
-
-#include "esphome/core/helpers.h"
-
-namespace esphome::dfrobot_sen0395 {
-
-class DfrobotSen0395Component;
-
-// Use command queue and time stamps to avoid blocking.
-// When component has run time, check if minimum time (1s) between
-// commands has passed. After that run a command from the queue.
-class Command {
- public:
-  virtual ~Command() = default;
-  virtual uint8_t execute(DfrobotSen0395Component *parent);
-  virtual uint8_t on_message(std::string &message) = 0;
-
- protected:
-  DfrobotSen0395Component *parent_{nullptr};
-  std::string cmd_;
-  bool cmd_sent_{false};
-  int8_t retries_left_{2};
-  uint32_t cmd_duration_ms_{1000};
-  uint32_t timeout_ms_{1500};
-};
-
-class ReadStateCommand : public Command {
- public:
-  ReadStateCommand() { timeout_ms_ = 500; }
-  uint8_t execute(DfrobotSen0395Component *parent) override;
-  uint8_t on_message(std::string &message) override;
-};
-
-class PowerCommand : public Command {
- public:
-  PowerCommand(bool power_on) : power_on_(power_on) {
-    if (power_on) {
-      cmd_ = "sensorStart";
-    } else {
-      cmd_ = "sensorStop";
-    }
-  };
-  uint8_t on_message(std::string &message) override;
-
- protected:
-  bool power_on_;
-};
-
-class DetRangeCfgCommand : public Command {
- public:
-  DetRangeCfgCommand(float min1, float max1, float min2, float max2, float min3, float max3, float min4, float max4);
-  uint8_t on_message(std::string &message) override;
-
- protected:
-  float min1_, max1_, min2_, max2_, min3_, max3_, min4_, max4_;
-  // TODO: Set min max values in component, so they can be published as sensor.
-};
-
-class SetLatencyCommand : public Command {
- public:
-  SetLatencyCommand(float delay_after_detection, float delay_after_disappear);
-  uint8_t on_message(std::string &message) override;
-
- protected:
-  float delay_after_detection_;
-  float delay_after_disappear_;
-};
-
-class SensorCfgStartCommand : public Command {
- public:
-  SensorCfgStartCommand(bool startup_mode) : startup_mode_(startup_mode) {
-    char tmp_cmd[20];  // "sensorCfgStart " (15) + "0/1" (1) + null = 17
-    buf_append_printf(tmp_cmd, sizeof(tmp_cmd), 0, "sensorCfgStart %d", startup_mode);
-    cmd_ = std::string(tmp_cmd);
-  }
-  uint8_t on_message(std::string &message) override;
-
- protected:
-  bool startup_mode_;
-};
-
-class FactoryResetCommand : public Command {
- public:
-  FactoryResetCommand() { cmd_ = "factoryReset 0x45670123 0xCDEF89AB 0x956128C6 0xDF54AC89"; };
-  uint8_t on_message(std::string &message) override;
-};
-
-class ResetSystemCommand : public Command {
- public:
-  ResetSystemCommand() { cmd_ = "resetSystem"; }
-  uint8_t on_message(std::string &message) override;
-};
-
-class SaveCfgCommand : public Command {
- public:
-  SaveCfgCommand() {
-    cmd_ = "saveCfg 0x45670123 0xCDEF89AB 0x956128C6 0xDF54AC89";
-    cmd_duration_ms_ = 3000;
-    timeout_ms_ = 3500;
-  }
-  uint8_t on_message(std::string &message) override;
-};
-
-class LedModeCommand : public Command {
- public:
-  LedModeCommand(bool active) : active_(active) {
-    if (active) {
-      cmd_ = "setLedMode 1 0";
-    } else {
-      cmd_ = "setLedMode 1 1";
-    }
-  };
-  uint8_t on_message(std::string &message) override;
-
- protected:
-  bool active_;
-};
-
-class UartOutputCommand : public Command {
- public:
-  UartOutputCommand(bool active) : active_(active) {
-    if (active) {
-      cmd_ = "setUartOutput 1 1";
-    } else {
-      cmd_ = "setUartOutput 1 0";
-    }
-  };
-  uint8_t on_message(std::string &message) override;
-
- protected:
-  bool active_;
-};
-
-class SensitivityCommand : public Command {
- public:
-  SensitivityCommand(uint8_t sensitivity) : sensitivity_(sensitivity) {
-    if (sensitivity > 9)
-      sensitivity_ = sensitivity = 9;
-    char tmp_cmd[20];  // "setSensitivity " (15) + "0-9" (1) + null = 17
-    buf_append_printf(tmp_cmd, sizeof(tmp_cmd), 0, "setSensitivity %d", sensitivity);
-    cmd_ = std::string(tmp_cmd);
-  };
-  uint8_t on_message(std::string &message) override;
-
- protected:
-  uint8_t sensitivity_;
-};
-
-}  // namespace esphome::dfrobot_sen0395

esphome/components/dfrobot_sen0395/dfrobot_sen0395.cpp

@@ -1,13 +1,27 @@
 #include "dfrobot_sen0395.h"
 
+#include <cmath>
+#include <cstring>
+
+#include "esphome/core/application.h"
 #include "esphome/core/helpers.h"
 #include "esphome/core/log.h"
+#include "esphome/core/progmem.h"
 
 namespace esphome::dfrobot_sen0395 {
 
 ESPHOME_LOG_TAG(TAG, "dfrobot_sen0395");
-const char ASCII_CR = 0x0D;
-const char ASCII_LF = 0x0A;
+static constexpr char ASCII_CR = 0x0D;
+static constexpr char ASCII_LF = 0x0A;
+static constexpr float DET_RANGE_STEP_M = 0.15f;
+static constexpr float LATENCY_STEP_S = 0.025f;
+
+// What each CommandType cannot do while the sensor runs; empty for commands that do not need it stopped
+PROGMEM_STRING_TABLE(BusyActions, "", "configure range config", "configure output latency",
+                     "configure sensor startup behavior", "factory reset", "", "", "set led mode",
+                     "set uart output mode", "set sensitivity");
+static_assert(BusyActions::COUNT == static_cast<size_t>(CommandType::COMMAND_TYPE_SENSITIVITY) + 1,
+              "BusyActions needs one entry per CommandType");
 
 void DfrobotSen0395Component::dump_config() {
   ESP_LOGCONFIG(TAG, "Dfrobot Mmwave Radar:");
@@ -23,23 +37,270 @@ void DfrobotSen0395Component::dump_config() {
 }
 
 void DfrobotSen0395Component::loop() {
-  if (cmd_queue_.is_empty()) {
-    // Command queue empty. Read sensor state.
-    cmd_queue_.enqueue(make_unique<ReadStateCommand>());
+  if (this->cmd_queue_.empty()) {
+    this->read_state_();
+    return;
   }
-
-  // Commands are non-blocking and need to be called repeatedly.
-  if (cmd_queue_.process(this)) {
-    // Dequeue if command is done
-    cmd_queue_.dequeue();
+  // Commands are non-blocking and need to be called repeatedly
+  if (this->run_command_(this->cmd_queue_.front(), App.get_loop_component_start_time())) {
+    this->cmd_queue_.pop();
+    this->cmd_sent_ = false;
+    this->retries_left_ = 2;
   }
 }
 
-int8_t DfrobotSen0395Component::enqueue(std::unique_ptr<Command> cmd) {
-  return cmd_queue_.enqueue(std::move(cmd));  // Transfer ownership using std::move
+bool DfrobotSen0395Component::enqueue(CommandType type, uint8_t arg) {
+  if (!this->cmd_queue_.push({type, arg})) {
+    ESP_LOGE(TAG, "Command queue is full");
+    return false;
+  }
+  return true;
 }
 
-uint8_t DfrobotSen0395Component::read_message_() {
+bool DfrobotSen0395Component::has_queue_room(size_t count) const {
+  if (COMMAND_QUEUE_SIZE - this->cmd_queue_.size() >= count)
+    return true;
+  ESP_LOGE(TAG, "Command queue is full");
+  return false;
+}
+
+void DfrobotSen0395Component::enqueue_det_range_cfg(float min1, float max1, float min2, float max2, float min3,
+                                                    float max3, float min4, float max4) {
+  // TODO: Print warning when values are rounded
+  const float values[8] = {min1, max1, min2, max2, min3, max3, min4, max4};
+  int16_t steps[8];
+  bool in_use = true;
+  for (size_t i = 0; i < 8; i += 2) {
+    in_use = in_use && values[i] >= 0 && values[i + 1] >= 0;
+    for (size_t j = i; j < i + 2; j++) {
+      steps[j] = in_use ? static_cast<int16_t>(clamp(roundf(values[j] / DET_RANGE_STEP_M), 0.0f, 32767.0f)) : -1;
+    }
+  }
+  if (steps[0] < 0) {
+    ESP_LOGW(TAG, "Invalid detection range, using 0 0");
+    steps[0] = steps[1] = 0;
+  }
+  if (this->enqueue(CommandType::COMMAND_TYPE_DET_RANGE_CFG))
+    memcpy(this->det_range_steps_, steps, sizeof(steps));
+}
+
+void DfrobotSen0395Component::enqueue_set_latency(float delay_after_detection, float delay_after_disappear) {
+  if (!this->enqueue(CommandType::COMMAND_TYPE_SET_LATENCY))
+    return;
+  // 1638.375 s is 65535 steps
+  this->latency_steps_[0] =
+      static_cast<uint16_t>(clamp(std::round(delay_after_detection / LATENCY_STEP_S), 0.0f, 65535.0f));
+  this->latency_steps_[1] =
+      static_cast<uint16_t>(clamp(std::round(delay_after_disappear / LATENCY_STEP_S), 0.0f, 65535.0f));
+}
+
+void DfrobotSen0395Component::enqueue_saved_setting(CommandType type, bool value) {
+  const bool was_active = this->active_;
+  if (!this->has_queue_room(was_active ? 4 : 2))
+    return;
+  if (was_active)
+    this->enqueue(CommandType::COMMAND_TYPE_POWER, false);
+  this->enqueue(type, value);
+  this->enqueue(CommandType::COMMAND_TYPE_SAVE_CFG);
+  if (was_active)
+    this->enqueue(CommandType::COMMAND_TYPE_POWER, true);
+}
+
+bool DfrobotSen0395Component::run_command_(const Command &cmd, uint32_t now) {
+  // The interval between two commands must be larger than the command duration
+  const uint32_t duration = cmd.type == CommandType::COMMAND_TYPE_SAVE_CFG ? 3000 : 1000;
+  if (!this->cmd_sent_) {
+    if (now - this->ts_last_cmd_sent_ > duration) {
+      char buf[64];  // "detRangeCfg -1" + 8 * " 32767" + null
+      this->format_command(cmd, buf, sizeof(buf));
+      ESP_LOGD(TAG, "Sending command: %s", buf);
+      this->write_str(buf);
+      this->ts_last_cmd_sent_ = now;
+      this->cmd_sent_ = true;
+    }
+    return false;  // Command not done yet
+  }
+
+  if (this->read_message_()) {
+    if (ESPHOME_strstr_P(this->read_buffer_, ESPHOME_PSTR("is not recognized as a CLI command")) == nullptr) {
+      if (!this->handle_reply_(cmd))
+        return false;         // Command not done yet
+      this->read_message_();  // Skip the prompt
+      return true;
+    }
+    ESP_LOGD(TAG, "Command not recognized properly by sensor");
+  } else if (now - this->ts_last_cmd_sent_ <= duration + 500) {
+    return false;  // Command not done yet
+  } else {
+    ESP_LOGD(TAG, "Command timeout");
+  }
+
+  if (this->retries_left_ == 0) {
+    this->read_message_();  // Skip the prompt
+    return true;            // Give up
+  }
+  this->retries_left_--;
+  this->cmd_sent_ = false;
+  ESP_LOGD(TAG, "Retrying");
+  return false;
+}
+
+void DfrobotSen0395Component::read_state_() {
+  if (!this->read_message_())
+    return;
+  if (ESPHOME_strstr_P(this->read_buffer_, ESPHOME_PSTR("$JYBSS,0, , , *")) != nullptr) {
+    this->set_detected_(false);
+    this->set_active(true);
+  } else if (ESPHOME_strstr_P(this->read_buffer_, ESPHOME_PSTR("$JYBSS,1, , , *")) != nullptr) {
+    this->set_detected_(true);
+    this->set_active(true);
+  }
+}
+
+static size_t append_cfg_key(char *buf, size_t size, size_t pos) {
+  return buf_append_str(buf, size, pos, " 0x45670123 0xCDEF89AB 0x956128C6 0xDF54AC89");
+}
+
+void DfrobotSen0395Component::format_command(const Command &cmd, char *buf, size_t size) const {
+  buf[0] = '\0';
+  switch (cmd.type) {
+    case CommandType::COMMAND_TYPE_POWER:
+      if (cmd.arg) {
+        buf_append_str(buf, size, 0, "sensorStart");
+      } else {
+        buf_append_str(buf, size, 0, "sensorStop");
+      }
+      break;
+    case CommandType::COMMAND_TYPE_DET_RANGE_CFG: {
+      size_t pos = buf_append_str(buf, size, 0, "detRangeCfg -1");
+      for (size_t i = 0; i < 8 && this->det_range_steps_[i] >= 0; i += 2) {
+        pos = buf_append_printf(buf, size, pos, " %d %d", this->det_range_steps_[i], this->det_range_steps_[i + 1]);
+      }
+      break;
+    }
+    case CommandType::COMMAND_TYPE_SET_LATENCY:
+      buf_append_printf(buf, size, 0, "setLatency %.03f %.03f", this->latency_steps_[0] * LATENCY_STEP_S,
+                        this->latency_steps_[1] * LATENCY_STEP_S);
+      break;
+    case CommandType::COMMAND_TYPE_SENSOR_CFG_START:
+      buf_append_printf(buf, size, 0, "sensorCfgStart %d", cmd.arg);
+      break;
+    case CommandType::COMMAND_TYPE_FACTORY_RESET:
+      append_cfg_key(buf, size, buf_append_str(buf, size, 0, "factoryReset"));
+      break;
+    case CommandType::COMMAND_TYPE_RESET_SYSTEM:
+      buf_append_str(buf, size, 0, "resetSystem");
+      break;
+    case CommandType::COMMAND_TYPE_SAVE_CFG:
+      append_cfg_key(buf, size, buf_append_str(buf, size, 0, "saveCfg"));
+      break;
+    case CommandType::COMMAND_TYPE_LED_MODE:
+      buf_append_printf(buf, size, 0, "setLedMode 1 %d", cmd.arg ? 0 : 1);
+      break;
+    case CommandType::COMMAND_TYPE_UART_OUTPUT:
+      buf_append_printf(buf, size, 0, "setUartOutput 1 %d", cmd.arg ? 1 : 0);
+      break;
+    case CommandType::COMMAND_TYPE_SENSITIVITY:
+      buf_append_printf(buf, size, 0, "setSensitivity %d", cmd.arg);
+      break;
+  }
+}
+
+bool DfrobotSen0395Component::handle_reply_(const Command &cmd) {
+  const char *message = this->read_buffer_;
+  auto is = [message](ESPHOME_PGM_P text) { return ESPHOME_strcmp_P(message, text) == 0; };
+
+  // Commands with their own replies
+  switch (cmd.type) {
+    case CommandType::COMMAND_TYPE_POWER:
+      if (is(ESPHOME_PSTR("sensor stopped already"))) {
+        this->set_active(false);
+        ESP_LOGI(TAG, "Stopped sensor (already stopped)");
+      } else if (is(ESPHOME_PSTR("sensor started already"))) {
+        this->set_active(true);
+        ESP_LOGI(TAG, "Started sensor (already started)");
+      } else if (is(ESPHOME_PSTR("new parameter isn't save, can't startSensor"))) {
+        this->set_active(false);
+        ESP_LOGE(TAG, "Can't start sensor! (Use SaveCfgCommand to save config first)");
+      } else if (is(ESPHOME_PSTR("Done"))) {
+        this->set_active(cmd.arg);
+        ESP_LOGI(TAG, "%s", cmd.arg ? LOG_STR_LITERAL("Started sensor") : LOG_STR_LITERAL("Stopped sensor"));
+      } else {
+        return false;
+      }
+      return true;
+    case CommandType::COMMAND_TYPE_RESET_SYSTEM:
+      if (!is(ESPHOME_PSTR("leapMMW:/>")))
+        return false;
+      ESP_LOGI(TAG, "Restarted sensor.");
+      return true;
+    case CommandType::COMMAND_TYPE_SAVE_CFG:
+      if (is(ESPHOME_PSTR("no parameter has changed"))) {
+        ESP_LOGI(TAG, "Not saving config (no parameter changed).");
+      } else if (is(ESPHOME_PSTR("Done"))) {
+        ESP_LOGI(TAG, "Saved config. Saving a lot may damage the sensor.");
+      } else {
+        return false;
+      }
+      return true;
+    default:
+      break;
+  }
+
+  // Configuration commands, which need the sensor stopped
+  if (is(ESPHOME_PSTR("sensor is not stopped"))) {
+    ESP_LOGE(TAG, "Cannot %s. Sensor is not stopped!",
+             LOG_STR_ARG(BusyActions::get_log_str(static_cast<uint8_t>(cmd.type), 0)));
+    return true;
+  }
+  if (!is(ESPHOME_PSTR("Done")))
+    return false;
+
+  switch (cmd.type) {
+    case CommandType::COMMAND_TYPE_DET_RANGE_CFG:
+      ESP_LOGI(TAG, "Updated detection area config:");
+      for (size_t i = 0; i < 8 && this->det_range_steps_[i] >= 0; i += 2) {
+        ESP_LOGI(TAG, "  Detection area %u from %.02fm to %.02fm.", static_cast<unsigned>(i / 2 + 1),
+                 this->det_range_steps_[i] * DET_RANGE_STEP_M, this->det_range_steps_[i + 1] * DET_RANGE_STEP_M);
+      }
+      break;
+    case CommandType::COMMAND_TYPE_SET_LATENCY:
+      ESP_LOGI(TAG,
+               "Updated output latency config:\n"
+               "  Signal that someone was detected is delayed by %.03f s.\n"
+               "  Signal that nobody is detected anymore is delayed by %.03f s.",
+               this->latency_steps_[0] * LATENCY_STEP_S, this->latency_steps_[1] * LATENCY_STEP_S);
+      break;
+    case CommandType::COMMAND_TYPE_SENSOR_CFG_START:
+      this->set_start_after_boot(cmd.arg);
+      ESP_LOGI(TAG, "Updated sensor startup behavior: %s",
+               cmd.arg ? LOG_STR_LITERAL("sensor will start automatically after power-on.")
+                       : LOG_STR_LITERAL("sensor needs to be started manually after power-on."));
+      break;
+    case CommandType::COMMAND_TYPE_FACTORY_RESET:
+      ESP_LOGI(TAG, "Sensor factory reset done.");
+      break;
+    case CommandType::COMMAND_TYPE_LED_MODE:
+      this->set_led_active(cmd.arg);
+      ESP_LOGI(TAG, "Set led mode done. %s",
+               cmd.arg ? LOG_STR_LITERAL("Sensor LED will blink.") : LOG_STR_LITERAL("Turned off LED."));
+      break;
+    case CommandType::COMMAND_TYPE_UART_OUTPUT:
+      this->set_uart_presence_active(cmd.arg);
+      ESP_LOGI(TAG, "Set uart mode done. %s",
+               cmd.arg ? LOG_STR_LITERAL("Presence information is sent via UART and GPIO.")
+                       : LOG_STR_LITERAL("Presence information is only sent via GPIO."));
+      break;
+    case CommandType::COMMAND_TYPE_SENSITIVITY:
+      ESP_LOGI(TAG, "Set sensitivity done. Set to value %d.", cmd.arg);
+      break;
+    default:
+      break;
+  }
+  return true;
+}
+
+bool DfrobotSen0395Component::read_message_() {
   while (this->available()) {
     uint8_t byte;
     this->read_byte(&byte);
@@ -62,33 +323,11 @@ uint8_t DfrobotSen0395Component::read_message_() {
       this->read_buffer_[this->read_pos_] = 0;
       this->read_pos_ = 0;
       ESP_LOGV(TAG, "Message: %s", this->read_buffer_);
-      return 1;  // Full message in buffer
-    } else {
-      this->read_pos_++;
+      return true;  // Full message in buffer
     }
+    this->read_pos_++;
   }
-  return 0;  // No full message yet
-}
-
-uint8_t DfrobotSen0395Component::find_prompt_() {
-  if (this->read_message_()) {
-    std::string message(this->read_buffer_);
-    if (message.rfind("leapMMW:/>") != std::string::npos) {
-      return 1;  // Prompt found
-    }
-  }
-  return 0;  // Not found yet
-}
-
-uint8_t DfrobotSen0395Component::send_cmd_(const char *cmd, uint32_t duration) {
-  // The interval between two commands must be larger than the specified duration (in ms).
-  if (millis() - ts_last_cmd_sent_ > duration) {
-    this->write_str(cmd);
-    ts_last_cmd_sent_ = millis();
-    return 1;  // Command sent
-  }
-  // Could not send command yet as command duration did not fully pass yet.
-  return 0;
+  return false;  // No full message yet
 }
 
 void DfrobotSen0395Component::set_detected_(bool detected) {
@@ -99,44 +338,4 @@ void DfrobotSen0395Component::set_detected_(bool detected) {
 #endif
 }
 
-int8_t CircularCommandQueue::enqueue(std::unique_ptr<Command> cmd) {
-  if (this->is_full()) {
-    ESP_LOGE(TAG, "Command queue is full");
-    return -1;
-  } else if (this->is_empty()) {
-    front_++;
-  }
-  rear_ = (rear_ + 1) % COMMAND_QUEUE_SIZE;
-  commands_[rear_] = std::move(cmd);  // Transfer ownership using std::move
-  return 1;
-}
-
-std::unique_ptr<Command> CircularCommandQueue::dequeue() {
-  if (this->is_empty())
-    return nullptr;
-  std::unique_ptr<Command> dequeued_cmd = std::move(commands_[front_]);
-  if (front_ == rear_) {
-    front_ = -1;
-    rear_ = -1;
-  } else {
-    front_ = (front_ + 1) % COMMAND_QUEUE_SIZE;
-  }
-
-  return dequeued_cmd;
-}
-
-bool CircularCommandQueue::is_empty() { return front_ == -1; }
-
-bool CircularCommandQueue::is_full() { return (rear_ + 1) % COMMAND_QUEUE_SIZE == front_; }
-
-// Run execute method of first in line command.
-// Execute is non-blocking and has to be called until it returns 1.
-uint8_t CircularCommandQueue::process(DfrobotSen0395Component *parent) {
-  if (!is_empty()) {
-    return commands_[front_]->execute(parent);
-  } else {
-    return 1;
-  }
-}
-
 }  // namespace esphome::dfrobot_sen0395

esphome/components/dfrobot_sen0395/dfrobot_sen0395.h

@@ -3,6 +3,7 @@
 #include "esphome/components/uart/uart.h"
 #include "esphome/core/automation.h"
 #include "esphome/core/component.h"
+#include "esphome/core/helpers.h"
 
 #ifdef USE_BINARY_SENSOR
 #include "esphome/components/binary_sensor/binary_sensor.h"
@@ -11,29 +12,30 @@
 #include "esphome/components/switch/switch.h"
 #endif
 
-#include "commands.h"
-
 namespace esphome::dfrobot_sen0395 {
 
-const uint8_t MMWAVE_READ_BUFFER_LENGTH = 255;
+static constexpr uint8_t MMWAVE_READ_BUFFER_LENGTH = 255;
+static constexpr uint8_t COMMAND_QUEUE_SIZE = 20;
 
-// forward declaration due to circular dependency
-class DfrobotSen0395Component;
+// Keep in sync with the BusyActions table in dfrobot_sen0395.cpp
+enum class CommandType : uint8_t {
+  COMMAND_TYPE_POWER,
+  COMMAND_TYPE_DET_RANGE_CFG,
+  COMMAND_TYPE_SET_LATENCY,
+  COMMAND_TYPE_SENSOR_CFG_START,
+  COMMAND_TYPE_FACTORY_RESET,
+  COMMAND_TYPE_RESET_SYSTEM,
+  COMMAND_TYPE_SAVE_CFG,
+  COMMAND_TYPE_LED_MODE,
+  COMMAND_TYPE_UART_OUTPUT,
+  COMMAND_TYPE_SENSITIVITY,
+};
 
-static const uint8_t COMMAND_QUEUE_SIZE = 20;
-
-class CircularCommandQueue {
- public:
-  int8_t enqueue(std::unique_ptr<Command> cmd);
-  std::unique_ptr<Command> dequeue();
-  bool is_empty();
-  bool is_full();
-  uint8_t process(DfrobotSen0395Component *parent);
-
- protected:
-  int front_{-1};
-  int rear_{-1};
-  std::unique_ptr<Command> commands_[COMMAND_QUEUE_SIZE];
+/// A queued command. arg is the on/off flag, startup mode or sensitivity; the detection range and
+/// latency values are held by the component.
+struct Command {
+  CommandType type;
+  uint8_t arg;
 };
 
 class DfrobotSen0395Component final : public uart::UARTDevice, public Component {
@@ -93,9 +95,21 @@ class DfrobotSen0395Component final : public uart::UARTDevice, public Component
   }
 #endif
 
-  int8_t enqueue(std::unique_ptr<Command> cmd);
+  // Commands run one at a time from a queue, at least 1 s apart, so loop() never blocks
+  bool enqueue(CommandType type, uint8_t arg = 0);
+  /// True when count more commands fit; logs when they do not, so a stop/start sequence is never queued in part
+  bool has_queue_room(size_t count) const;
+  /// Queue a detection range config; segments are used up to the first one with a negative bound
+  void enqueue_det_range_cfg(float min1, float max1, float min2, float max2, float min3, float max3, float min4,
+                             float max4);
+  /// Queue an output latency config, both delays in seconds
+  void enqueue_set_latency(float delay_after_detection, float delay_after_disappear);
+  /// Queue a setting change, stopping the sensor around it and saving it when the sensor is running
+  void enqueue_saved_setting(CommandType type, bool value);
   /// Queue a sensor reset
-  void reset_system() { this->enqueue(make_unique<ResetSystemCommand>()); }
+  void reset_system() { this->enqueue(CommandType::COMMAND_TYPE_RESET_SYSTEM); }
+  /// Write the text sent to the sensor for cmd into buf
+  void format_command(const Command &cmd, char *buf, size_t size) const;
 
  protected:
 #ifdef USE_BINARY_SENSOR
@@ -109,17 +123,22 @@ class DfrobotSen0395Component final : public uart::UARTDevice, public Component
   bool start_after_boot_{false};
   char read_buffer_[MMWAVE_READ_BUFFER_LENGTH];
   size_t read_pos_{0};
-  CircularCommandQueue cmd_queue_;
+  bool cmd_sent_{false};
+  uint8_t retries_left_{2};
+  StaticRingBuffer<Command, COMMAND_QUEUE_SIZE> cmd_queue_;
   uint32_t ts_last_cmd_sent_{0};
+  // Range and latency of the queued commands, in sensor steps (0.15 m, 0.025 s); a later command overwrites them
+  int16_t det_range_steps_[8]{};
+  uint16_t latency_steps_[2]{};
 
-  uint8_t read_message_();
-  uint8_t find_prompt_();
-  uint8_t send_cmd_(const char *cmd, uint32_t duration);
+  bool read_message_();
+  void read_state_();
+  /// Run the command at the front of the queue; true when it is done
+  bool run_command_(const Command &cmd, uint32_t now);
+  /// Handle a reply to a sent command; true when the command is done
+  bool handle_reply_(const Command &cmd);
 
   void set_detected_(bool detected);
-
-  friend class Command;
-  friend class ReadStateCommand;
 };
 
 }  // namespace esphome::dfrobot_sen0395

esphome/components/dfrobot_sen0395/switch/dfrobot_sen0395_switch.cpp

@@ -2,45 +2,18 @@
 
 namespace esphome::dfrobot_sen0395 {
 
-void Sen0395PowerSwitch::write_state(bool state) { this->parent_->enqueue(make_unique<PowerCommand>(state)); }
+void Sen0395PowerSwitch::write_state(bool state) { this->parent_->enqueue(CommandType::COMMAND_TYPE_POWER, state); }
 
 void Sen0395LedSwitch::write_state(bool state) {
-  bool was_active = false;
-  if (this->parent_->is_active()) {
-    was_active = true;
-    this->parent_->enqueue(make_unique<PowerCommand>(false));
-  }
-  this->parent_->enqueue(make_unique<LedModeCommand>(state));
-  this->parent_->enqueue(make_unique<SaveCfgCommand>());
-  if (was_active) {
-    this->parent_->enqueue(make_unique<PowerCommand>(true));
-  }
+  this->parent_->enqueue_saved_setting(CommandType::COMMAND_TYPE_LED_MODE, state);
 }
 
 void Sen0395UartPresenceSwitch::write_state(bool state) {
-  bool was_active = false;
-  if (this->parent_->is_active()) {
-    was_active = true;
-    this->parent_->enqueue(make_unique<PowerCommand>(false));
-  }
-  this->parent_->enqueue(make_unique<UartOutputCommand>(state));
-  this->parent_->enqueue(make_unique<SaveCfgCommand>());
-  if (was_active) {
-    this->parent_->enqueue(make_unique<PowerCommand>(true));
-  }
+  this->parent_->enqueue_saved_setting(CommandType::COMMAND_TYPE_UART_OUTPUT, state);
 }
 
 void Sen0395StartAfterBootSwitch::write_state(bool state) {
-  bool was_active = false;
-  if (this->parent_->is_active()) {
-    was_active = true;
-    this->parent_->enqueue(make_unique<PowerCommand>(false));
-  }
-  this->parent_->enqueue(make_unique<SensorCfgStartCommand>(state));
-  this->parent_->enqueue(make_unique<SaveCfgCommand>());
-  if (was_active) {
-    this->parent_->enqueue(make_unique<PowerCommand>(true));
-  }
+  this->parent_->enqueue_saved_setting(CommandType::COMMAND_TYPE_SENSOR_CFG_START, state);
 }
 
 }  // namespace esphome::dfrobot_sen0395

tests/components/dfrobot_sen0395/format_command.cpp

@@ -0,0 +1,65 @@
+#include <gtest/gtest.h>
+#include <string>
+#include "esphome/components/dfrobot_sen0395/dfrobot_sen0395.h"
+
+namespace esphome::dfrobot_sen0395::testing {
+
+// The text each command sends, as the per-command classes sent it before the queue held plain values.
+class FormatCommandTest : public ::testing::Test {
+ protected:
+  std::string format_(CommandType type, uint8_t arg = 0) {
+    char buf[64];
+    this->radar_.format_command({type, arg}, buf, sizeof(buf));
+    return buf;
+  }
+
+  DfrobotSen0395Component radar_;
+};
+
+TEST_F(FormatCommandTest, FixedCommands) {
+  EXPECT_EQ(this->format_(CommandType::COMMAND_TYPE_POWER, true), "sensorStart");
+  EXPECT_EQ(this->format_(CommandType::COMMAND_TYPE_POWER, false), "sensorStop");
+  EXPECT_EQ(this->format_(CommandType::COMMAND_TYPE_FACTORY_RESET),
+            "factoryReset 0x45670123 0xCDEF89AB 0x956128C6 0xDF54AC89");
+  EXPECT_EQ(this->format_(CommandType::COMMAND_TYPE_SAVE_CFG), "saveCfg 0x45670123 0xCDEF89AB 0x956128C6 0xDF54AC89");
+  EXPECT_EQ(this->format_(CommandType::COMMAND_TYPE_RESET_SYSTEM), "resetSystem");
+  EXPECT_EQ(this->format_(CommandType::COMMAND_TYPE_LED_MODE, true), "setLedMode 1 0");
+  EXPECT_EQ(this->format_(CommandType::COMMAND_TYPE_LED_MODE, false), "setLedMode 1 1");
+  EXPECT_EQ(this->format_(CommandType::COMMAND_TYPE_UART_OUTPUT, true), "setUartOutput 1 1");
+  EXPECT_EQ(this->format_(CommandType::COMMAND_TYPE_UART_OUTPUT, false), "setUartOutput 1 0");
+  EXPECT_EQ(this->format_(CommandType::COMMAND_TYPE_SENSOR_CFG_START, true), "sensorCfgStart 1");
+  EXPECT_EQ(this->format_(CommandType::COMMAND_TYPE_SENSOR_CFG_START, false), "sensorCfgStart 0");
+  EXPECT_EQ(this->format_(CommandType::COMMAND_TYPE_SENSITIVITY, 7), "setSensitivity 7");
+}
+
+TEST_F(FormatCommandTest, DetectionRangeSegments) {
+  this->radar_.enqueue_det_range_cfg(0.3f, 1.2f, -1, -1, -1, -1, -1, -1);
+  EXPECT_EQ(this->format_(CommandType::COMMAND_TYPE_DET_RANGE_CFG), "detRangeCfg -1 2 8");
+  this->radar_.enqueue_det_range_cfg(0.0f, 1.5f, 2.0f, 3.0f, 4.0f, 5.0f, 6.0f, 9.0f);
+  EXPECT_EQ(this->format_(CommandType::COMMAND_TYPE_DET_RANGE_CFG), "detRangeCfg -1 0 10 13 20 27 33 40 60");
+  // Segments stop at the first one with a negative bound
+  this->radar_.enqueue_det_range_cfg(0.0f, 1.5f, 2.0f, -1, 4.0f, 5.0f, 6.0f, 9.0f);
+  EXPECT_EQ(this->format_(CommandType::COMMAND_TYPE_DET_RANGE_CFG), "detRangeCfg -1 0 10");
+}
+
+TEST_F(FormatCommandTest, DetectionRangeRoundsHalfStepsUp) {
+  // 0.075 m is half a 0.15 m step
+  this->radar_.enqueue_det_range_cfg(0.0f, 0.075f, -1, -1, -1, -1, -1, -1);
+  EXPECT_EQ(this->format_(CommandType::COMMAND_TYPE_DET_RANGE_CFG), "detRangeCfg -1 0 1");
+}
+
+TEST_F(FormatCommandTest, DetectionRangeNegativeFirstSegmentFallsBack) {
+  this->radar_.enqueue_det_range_cfg(-1, 3.0f, -1, -1, -1, -1, -1, -1);
+  EXPECT_EQ(this->format_(CommandType::COMMAND_TYPE_DET_RANGE_CFG), "detRangeCfg -1 0 0");
+}
+
+TEST_F(FormatCommandTest, Latency) {
+  this->radar_.enqueue_set_latency(0.0f, 2.5f);
+  EXPECT_EQ(this->format_(CommandType::COMMAND_TYPE_SET_LATENCY), "setLatency 0.000 2.500");
+  this->radar_.enqueue_set_latency(0.0125f, 0.0374f);
+  EXPECT_EQ(this->format_(CommandType::COMMAND_TYPE_SET_LATENCY), "setLatency 0.025 0.025");
+  this->radar_.enqueue_set_latency(1638.375f, 5000.0f);
+  EXPECT_EQ(this->format_(CommandType::COMMAND_TYPE_SET_LATENCY), "setLatency 1638.375 1638.375");
+}
+
+}  // namespace esphome::dfrobot_sen0395::testing

tests/components/dfrobot_sen0395/settings_action.cpp

@@ -29,7 +29,7 @@ float segment_value(size_t index, float x, int y = 0) {
 }  // namespace
 
 // The component is final and its command queue protected, so these tests check the segment
-// reads and their arguments; the enqueued DetRangeCfgCommand itself is not inspected.
+// reads and their arguments; the queued detection range command itself is not inspected.
 class SettingsActionTest : public ::testing::Test {
  protected:
   void SetUp() override { call_record() = CallRecord{}; }
@@ -142,4 +142,33 @@ TEST_F(SettingsActionTest, FloatTriggerUnpairedSegmentIsNotEvaluated) {
   EXPECT_EQ(call_record().segment_calls[0], 0);
 }
 
+TEST_F(SettingsActionTest, QueuesNothingWithoutRoomForWholeSequence) {
+  for (int i = 0; i < 11; i++)
+    ASSERT_TRUE(this->radar_.enqueue(CommandType::COMMAND_TYPE_SAVE_CFG));
+  DfrobotSen0395SettingsAction<> action;
+  action.set_parent(&this->radar_);
+  action.play();
+  EXPECT_TRUE(this->radar_.has_queue_room(9));
+  this->radar_.enqueue_saved_setting(CommandType::COMMAND_TYPE_LED_MODE, true);
+  EXPECT_TRUE(this->radar_.has_queue_room(7));
+  EXPECT_FALSE(this->radar_.has_queue_room(8));
+}
+
+// A running sensor needs stop, change, save and start queued together
+TEST_F(SettingsActionTest, RunningSensorQueuesNothingWithThreeFreeSlots) {
+  this->radar_.set_active(true);
+  for (int i = 0; i < COMMAND_QUEUE_SIZE - 3; i++)
+    ASSERT_TRUE(this->radar_.enqueue(CommandType::COMMAND_TYPE_SAVE_CFG));
+  this->radar_.enqueue_saved_setting(CommandType::COMMAND_TYPE_LED_MODE, true);
+  EXPECT_TRUE(this->radar_.has_queue_room(3));
+}
+
+TEST_F(SettingsActionTest, RunningSensorQueuesAllFourWithFourFreeSlots) {
+  this->radar_.set_active(true);
+  for (int i = 0; i < COMMAND_QUEUE_SIZE - 4; i++)
+    ASSERT_TRUE(this->radar_.enqueue(CommandType::COMMAND_TYPE_SAVE_CFG));
+  this->radar_.enqueue_saved_setting(CommandType::COMMAND_TYPE_LED_MODE, true);
+  EXPECT_FALSE(this->radar_.has_queue_room(1));
+}
+
 }  // namespace esphome::dfrobot_sen0395::testing