tormatic_cover.cpp

Go to the documentation of this file.

#include <cinttypes>
#include <vector>
 
#include "tormatic_cover.h"
 
using namespace std;
 
namespace esphome::tormatic {
 
static const char *const TAG = "tormatic.cover";
 
// Time to poll the UART when flushing after desync. At 9600 baud, a full
// 12-byte message takes ~12.5ms, so 15ms guarantees all bytes have arrived.
static constexpr uint32_t DRAIN_TIMEOUT_MS = 15;
 
using namespace esphome::cover;
 
void Tormatic::setup() {
  auto restore = this->restore_state_();
  if (restore.has_value()) {
    restore->apply(this);
    return;
  }
 
  // Assume gate is closed without preexisting state.
  this->position = 0.0f;
}
 
cover::CoverTraits Tormatic::get_traits() {
  auto traits = CoverTraits();
  traits.set_supports_stop(true);
  traits.set_supports_position(true);
  traits.set_is_assumed_state(false);
  return traits;
}
 
void Tormatic::dump_config() {
  LOG_COVER("", "Tormatic Cover", this);
  ESP_LOGCONFIG(TAG,
                "  Open Duration: %.1fs\n"
                "  Close Duration: %.1fs",
                this->open_duration_ / 1e3f, this->close_duration_ / 1e3f);
 
  auto restore = this->restore_state_();
  if (restore.has_value()) {
    ESP_LOGCONFIG(TAG, "  Saved position %d%%", (int) (restore->position * 100.f));
  }
}
 
void Tormatic::update() { this->request_gate_status_(); }
 
void Tormatic::loop() {
  auto o_status = this->read_gate_status_();
  if (o_status) {
    auto status = o_status.value();
 
    this->recalibrate_duration_(status);
    this->handle_gate_status_(status);
  }
 
  this->recompute_position_();
  this->stop_at_target_();
}
 
void Tormatic::control(const cover::CoverCall &call) {
  if (call.get_stop()) {
    this->send_gate_command_(PAUSED);
    return;
  }
 
  auto pos_val = call.get_position();
  if (pos_val.has_value()) {
    auto pos = *pos_val;
    this->control_position_(pos);
    return;
  }
}
 
// Wrap the Cover's publish_state with a rate limiter. Publishes if the last
// publish was longer than ratelimit milliseconds ago. 0 to disable.
void Tormatic::publish_state(bool save, uint32_t ratelimit) {
  auto now = millis();
  if ((now - this->last_publish_time_) < ratelimit) {
    return;
  }
  this->last_publish_time_ = now;
 
  Cover::publish_state(save);
};
 
// Recalibrate the gate's estimated open or close duration based on the
// actual time the operation took.
void Tormatic::recalibrate_duration_(GateStatus s) {
  if (this->current_status_ == s) {
    return;
  }
 
  auto now = millis();
  auto old = this->current_status_;
 
  // Gate paused halfway through opening or closing, invalidate the start time
  // of the current operation. Close/open durations can only be accurately
  // calibrated on full open or close cycle due to motor acceleration.
  if (s == PAUSED) {
    ESP_LOGD(TAG, "Gate paused, clearing direction start time");
    this->direction_start_time_ = 0;
    return;
  }
 
  // Record the start time of a state transition if the gate was in the fully
  // open or closed position before the command.
  if ((old == CLOSED && s == OPENING) || (old == OPENED && s == CLOSING)) {
    ESP_LOGD(TAG, "Gate started moving from fully open or closed state");
    this->direction_start_time_ = now;
    return;
  }
 
  // The gate was resumed from a paused state, don't attempt recalibration.
  if (this->direction_start_time_ == 0) {
    return;
  }
 
  if (s == OPENED) {
    this->open_duration_ = now - this->direction_start_time_;
    ESP_LOGI(TAG, "Recalibrated the gate's open duration to %" PRIu32 "ms", this->open_duration_);
  }
  if (s == CLOSED) {
    this->close_duration_ = now - this->direction_start_time_;
    ESP_LOGI(TAG, "Recalibrated the gate's close duration to %" PRIu32 "ms", this->close_duration_);
  }
 
  this->direction_start_time_ = 0;
}
 
// Set the Cover's internal state based on a status message
// received from the unit.
void Tormatic::handle_gate_status_(GateStatus s) {
  if (this->current_status_ == s) {
    return;
  }
 
  ESP_LOGI(TAG, "Status changed from %s to %s", gate_status_to_str(this->current_status_), gate_status_to_str(s));
 
  switch (s) {
    case OPENED:
      // The Novoferm 423 doesn't respond to the first 'Close' command after
      // being opened completely. Sending a pause command after opening fixes
      // that.
      this->send_gate_command_(PAUSED);
 
      this->position = COVER_OPEN;
      break;
    case CLOSED:
      this->position = COVER_CLOSED;
      break;
    default:
      break;
  }
 
  this->current_status_ = s;
  this->current_operation = gate_status_to_cover_operation(s);
 
  this->publish_state(true);
 
  // This timestamp is used to generate position deltas on every loop() while
  // the gate is moving. Bump it on each state transition so the first tick
  // doesn't generate a huge delta.
  this->last_recompute_time_ = millis();
}
 
// Recompute the gate's position and publish the results while
// the gate is moving. No-op when the gate is idle.
void Tormatic::recompute_position_() {
  if (this->current_operation == COVER_OPERATION_IDLE) {
    return;
  }
 
  const uint32_t now = millis();
  uint32_t diff = now - this->last_recompute_time_;
 
  auto direction = +1.0f;
  uint32_t duration = this->open_duration_;
  if (this->current_operation == COVER_OPERATION_CLOSING) {
    direction = -1.0f;
    duration = this->close_duration_;
  }
 
  if (duration == 0)
    return;
 
  auto delta = direction * diff / duration;
 
  this->position = clamp(this->position + delta, COVER_CLOSED, COVER_OPEN);
 
  this->last_recompute_time_ = now;
 
  this->publish_state(true, 250);
}
 
// Start moving the gate in the direction of the target position.
void Tormatic::control_position_(float target) {
  if (target == this->position) {
    return;
  }
 
  if (target == COVER_OPEN) {
    ESP_LOGI(TAG, "Fully opening gate");
    this->send_gate_command_(OPENED);
    return;
  }
  if (target == COVER_CLOSED) {
    ESP_LOGI(TAG, "Fully closing gate");
    this->send_gate_command_(CLOSED);
    return;
  }
 
  // Don't set target position when fully opening or closing the gate, the gate
  // stops automatically when it reaches the configured open/closed positions.
  this->target_position_ = target;
 
  if (target > this->position) {
    ESP_LOGI(TAG, "Opening gate towards %.1f", target);
    this->send_gate_command_(OPENED);
    return;
  }
 
  if (target < this->position) {
    ESP_LOGI(TAG, "Closing gate towards %.1f", target);
    this->send_gate_command_(CLOSED);
    return;
  }
}
 
// Stop the gate if it is moving at or beyond its target position. Target
// position is only set when the gate is requested to move to a halfway
// position.
void Tormatic::stop_at_target_() {
  if (this->current_operation == COVER_OPERATION_IDLE) {
    return;
  }
  if (!this->target_position_) {
    return;
  }
  auto target = this->target_position_.value();
 
  if (this->current_operation == COVER_OPERATION_OPENING && this->position < target) {
    return;
  }
  if (this->current_operation == COVER_OPERATION_CLOSING && this->position > target) {
    return;
  }
 
  this->send_gate_command_(PAUSED);
  this->target_position_.reset();
}
 
// Read a GateStatus from the unit. The unit only sends messages in response to
// status requests or commands, so a message needs to be sent first.
optional<GateStatus> Tormatic::read_gate_status_() {
  if (!this->pending_hdr_) {
    if (this->available() < sizeof(MessageHeader)) {
      return {};
    }
 
    this->pending_hdr_ = this->read_data_<MessageHeader>();
    if (!this->pending_hdr_) {
      return {};
    }
 
    // Sanity check: valid messages have small payloads (3-4 bytes). A large
    // or impossible payload_size means the stream is out of sync (corrupted
    // byte, dropped data, etc.). Flush the buffer so we can resync on the
    // next request/response cycle.
    if (this->pending_hdr_->payload_size() > sizeof(CommandRequestReply)) {
      ESP_LOGW(TAG, "Unexpected payload size %" PRIu32 ", flushing rx buffer", this->pending_hdr_->payload_size());
      this->pending_hdr_.reset();
      this->drain_rx_();
      return {};
    }
  }
 
  auto hdr = this->pending_hdr_.value();
 
  // Wait for all payload bytes to arrive before processing.
  if (this->available() < hdr.payload_size()) {
    return {};
  }
 
  this->pending_hdr_.reset();
 
  switch (hdr.type) {
    case STATUS: {
      if (hdr.payload_size() != sizeof(StatusReply)) {
        ESP_LOGE(TAG, "Header specifies payload size %" PRIu32 " but size of StatusReply is %zu", hdr.payload_size(),
                 sizeof(StatusReply));
        this->drain_rx_(hdr.payload_size());
        return {};
      }
 
      auto o_status = this->read_data_<StatusReply>();
      if (!o_status) {
        return {};
      }
 
      return o_status->state;
    }
 
    case COMMAND:
      // Commands initiated by control() are simply echoed back by the unit, but
      // don't guarantee that the unit's internal state has been transitioned,
      // nor that the motor started moving. A subsequent status request may
      // still return the previous state. Discard these messages, don't use them
      // to drive the Cover state machine.
      break;
 
    default:
      // Unknown message type, drain the remaining amount of bytes specified in
      // the header.
      ESP_LOGE(TAG, "Reading remaining %" PRIu32 " payload bytes of unknown type 0x%x", hdr.payload_size(), hdr.type);
      break;
  }
 
  // Drain any unhandled payload bytes described by the message header, if any.
  this->drain_rx_(hdr.payload_size());
 
  return {};
}
 
// Send a message to the unit requesting the gate's status.
void Tormatic::request_gate_status_() {
  ESP_LOGV(TAG, "Requesting gate status");
  StatusRequest req(GATE);
  this->send_message_(STATUS, req);
}
 
// Send a message to the unit issuing a command.
void Tormatic::send_gate_command_(GateStatus s) {
  ESP_LOGI(TAG, "Sending gate command %s", gate_status_to_str(s));
  CommandRequestReply req(s);
  this->send_message_(COMMAND, req);
}
 
template<typename T> void Tormatic::send_message_(MessageType t, T req) {
  MessageHeader hdr(t, ++this->seq_tx_, sizeof(req));
 
  auto out = serialize(hdr);
  auto reqv = serialize(req);
  out.insert(out.end(), reqv.begin(), reqv.end());
 
  this->write_array(out);
}
 
template<typename T> optional<T> Tormatic::read_data_() {
  T obj;
  uint32_t start = millis();
 
  auto ok = this->read_array((uint8_t *) &obj, sizeof(obj));
  if (!ok) {
    // Couldn't read object successfully, timeout?
    return {};
  }
  obj.byteswap();
 
  ESP_LOGV(TAG, "Read %s in %" PRIu32 " ms", obj.print().c_str(), millis() - start);
  return obj;
}
 
// Drain bytes from the uart rx buffer. When n > 0, drain exactly n bytes
// (caller must ensure they are available). When n == 0, poll for 15ms to
// guarantee a full packet time at 9600 baud has elapsed, consuming any
// bytes still in transit.
void Tormatic::drain_rx_(uint16_t n) {
  uint8_t data;
  if (n > 0) {
    for (uint16_t i = 0; i < n; i++) {
      if (!this->read_byte(&data)) {
        return;
      }
    }
  } else {
    uint32_t start = millis();
    while (millis() - start < DRAIN_TIMEOUT_MS) {
      if (this->available()) {
        this->read_byte(&data);
      }
    }
  }
}
 
}  // namespace esphome::tormatic