feat: FbStateMachine 状态转换 + 变量操作实现

eqpalg/utility/fb_state_machine.cpp

@@ -0,0 +1,226 @@
+// eqpalg/utility/fb_state_machine.cpp
+#include <eqpalg/utility/fb_state_machine.h>
+#include <cmath>
+#include <cfloat>
+#include <string>
+
+// ========== 配置 ==========
+
+void FbStateMachine::configure(bool keepMode, TimeDur timeout) {
+  keep_mode_ = keepMode;
+  timeout_ = timeout;
+  state_ = FbState::Idle;
+}
+
+// ========== 每周期状态更新 ==========
+
+FbUpdateResult FbStateMachine::update(bool actTriggered, TimePoint now,
+                                       std::map<std::string, double>& mm_vars,
+                                       size_t tag_count) {
+  FbUpdateResult result;
+  result.funVarsNeedReset = false;
+
+  switch (state_) {
+
+  case FbState::Idle:
+    if (actTriggered) {
+      // → Started：快照变量
+      snapshotActionStart(now, mm_vars, tag_count);
+      state_ = FbState::Started;
+      start_time_ = now;
+      result.state = FbState::Started;
+    } else {
+      result.state = FbState::Idle;
+      result.funVarsNeedReset = true;  // 空闲时重置 fun_vars
+    }
+    break;
+
+  case FbState::Started:
+    // 更新累积变量（首周期也需要更新）
+    updateActionVars(now, mm_vars, tag_count);
+
+    if (!actTriggered && keep_mode_) {
+      state_ = FbState::Idle;
+      result.state = FbState::NotHold;
+      result.funVarsNeedReset = true;
+    } else if (isAccumulatorNearOverflow(mm_vars, tag_count)) {
+      state_ = FbState::Idle;
+      result.state = FbState::Timeout;
+      result.funVarsNeedReset = true;
+    } else {
+      state_ = FbState::InProgress;
+      result.state = FbState::Started;  // 返回 Started，调用方跳过反馈检查
+    }
+    break;
+
+  case FbState::InProgress:
+    // 先更新累积变量
+    updateActionVars(now, mm_vars, tag_count);
+
+    if (!actTriggered && keep_mode_) {
+      state_ = FbState::Idle;
+      result.state = FbState::NotHold;
+      result.funVarsNeedReset = true;
+    } else if (isAccumulatorNearOverflow(mm_vars, tag_count)) {
+      clearActionAccumulators(mm_vars, tag_count);
+      state_ = FbState::Idle;
+      result.state = FbState::Timeout;
+      result.funVarsNeedReset = true;
+    } else if (timeout_ != TimeDur(-32768) &&
+               (now - start_time_) > timeout_) {
+      clearActionAccumulators(mm_vars, tag_count);
+      state_ = FbState::Idle;
+      result.state = FbState::Timeout;
+      result.funVarsNeedReset = true;
+    } else {
+      result.state = FbState::InProgress;
+    }
+    break;
+
+  // 终端状态 → 下周期自动回 Idle
+  default:
+    state_ = FbState::Idle;
+    result.state = FbState::Idle;
+    result.funVarsNeedReset = true;
+    break;
+  }
+
+  return result;
+}
+
+// ========== 反馈条件检查 ==========
+
+bool FbStateMachine::checkFeedback(bool fbCondition, TimePoint now,
+                                    std::map<std::string, double>& mm_vars) {
+  if (state_ != FbState::InProgress) {
+    return false;
+  }
+
+  if (fbCondition) {
+    recordActionEnd(now, mm_vars);
+    state_ = FbState::Idle;  // 下周期回到 Idle
+    return true;
+  }
+  return false;
+}
+
+// ========== 查询 ==========
+
+bool FbStateMachine::isActive() const {
+  return state_ == FbState::Started || state_ == FbState::InProgress;
+}
+
+// ========== 强制重置 ==========
+
+void FbStateMachine::forceReset() {
+  state_ = FbState::Idle;
+  start_time_ = TimePoint{};
+}
+
+// ========== 内部变量操作 ==========
+
+void FbStateMachine::snapshotActionStart(
+    TimePoint now,
+    std::map<std::string, double>& mm_vars,
+    size_t tag_count) {
+
+  mm_vars["stime"] = std::chrono::duration_cast<std::chrono::milliseconds>(
+                         now.time_since_epoch())
+                         .count();
+  mm_vars["time"] = 0;
+
+  for (size_t i = 0; i < tag_count; i++) {
+    std::string idx = std::to_string(i + 1);
+    double tag_val = mm_vars["tag" + idx];
+    double p_val = mm_vars["p" + idx];
+
+    // s[n] = tag[n] 在动作开始时刻的快照值
+    mm_vars["s" + idx] = tag_val;
+
+    // 极值初始化为当前值
+    mm_vars["mx_tag" + idx] = tag_val;
+    mm_vars["mi_tag" + idx] = tag_val;
+
+    // 累积器初始化为 0
+    mm_vars["mv2_tag" + idx] = 0;
+    mm_vars["mv2_p" + idx] = 0;
+
+    // 上升沿/下降沿初值
+    mm_vars["up_tag" + idx] = (p_val == 0 && tag_val == 1) ? 1.0 : 0.0;
+    mm_vars["dw_tag" + idx] = (p_val == 1 && tag_val == 0) ? 1.0 : 0.0;
+
+    // 若当前已为 1，初始计数为 1
+    if (tag_val == 1) {
+      mm_vars["mv2_tag" + idx] = 1;
+    }
+    if (p_val == 1) {
+      mm_vars["mv2_p" + idx] = 1;
+    }
+  }
+}
+
+void FbStateMachine::updateActionVars(
+    TimePoint now,
+    std::map<std::string, double>& mm_vars,
+    size_t tag_count) {
+
+  mm_vars["time"] = mm_vars["now"] - mm_vars["stime"];
+
+  for (size_t i = 0; i < tag_count; i++) {
+    std::string idx = std::to_string(i + 1);
+    double tag_val = mm_vars["tag" + idx];
+    double p_val = mm_vars["p" + idx];
+
+    // 更新极值
+    mm_vars["mx_tag" + idx] = std::max(tag_val, mm_vars["mx_tag" + idx]);
+    mm_vars["mi_tag" + idx] = std::min(tag_val, mm_vars["mi_tag" + idx]);
+
+    // 累积 1 的计数
+    if (tag_val == 1) {
+      mm_vars["mv2_tag" + idx] += 1;
+    }
+    if (p_val == 1) {
+      mm_vars["mv2_p" + idx] += 1;
+    }
+
+    // 上升沿/下降沿计数
+    mm_vars["up_tag" + idx] += (p_val == 0 && tag_val == 1) ? 1.0 : 0.0;
+    mm_vars["dw_tag" + idx] += (p_val == 1 && tag_val == 0) ? 1.0 : 0.0;
+  }
+}
+
+void FbStateMachine::recordActionEnd(
+    TimePoint now,
+    std::map<std::string, double>& mm_vars) {
+
+  mm_vars["etime"] = mm_vars["now"];
+}
+
+void FbStateMachine::clearActionAccumulators(
+    std::map<std::string, double>& mm_vars,
+    size_t tag_count) {
+
+  for (size_t i = 0; i < tag_count; i++) {
+    std::string idx = std::to_string(i + 1);
+    mm_vars["mv2_tag" + idx] = 0;
+    mm_vars["mv2_p" + idx] = 0;
+    mm_vars["up_tag" + idx] = 0;
+    mm_vars["dw_tag" + idx] = 0;
+  }
+}
+
+bool FbStateMachine::isAccumulatorNearOverflow(
+    const std::map<std::string, double>& mm_vars,
+    size_t tag_count) const {
+
+  for (size_t i = 0; i < tag_count; i++) {
+    std::string key = "mv2_tag" + std::to_string(i + 1);
+    auto it = mm_vars.find(key);
+    if (it != mm_vars.end()) {
+      if (std::abs(it->second - DBL_MAX) < 2.0) {
+        return true;
+      }
+    }
+  }
+  return false;
+}