Initial standalone track core

2026-05-15 16:02:44 +08:00
commit 84598cad20
15 changed files with 1733 additions and 0 deletions
@@ -0,0 +1,104 @@
+#include "track_core/model.hpp"
+
+#include <cmath>
+#include <cstdio>
+
+namespace track_core {
+namespace {
+template <class... Ts>
+struct overloads : Ts... {
+    using Ts::operator()...;
+};
+} // namespace
+
+std::string Color::hex() const {
+    char buffer[8]{};
+    static_cast<void>(std::snprintf(buffer, sizeof(buffer), "#%02X%02X%02X", r, g, b));
+    return buffer;
+}
+
+expected<unit, TrackError> TrackPidConfig::validate(bool allow_empty) const {
+    const auto speed_suppression_ok =
+        !speed_suppression.has_value() ||
+        (std::isfinite(speed_suppression->ratio_min) &&
+         std::isfinite(speed_suppression->sigma_m) &&
+         speed_suppression->ratio_min > 0.0F &&
+         speed_suppression->ratio_min < 1.0F &&
+         speed_suppression->sigma_m > 0.0F);
+    if (!speed_suppression_ok) {
+        return unexpected<TrackError>{TrackError::invalid_arg};
+    }
+    if (schemas.empty()) {
+        if (allow_empty) {
+            return {};
+        }
+        return unexpected<TrackError>{TrackError::invalid_arg};
+    }
+    for (const auto &schema : schemas) {
+        const auto ok = std::visit(overloads{
+                                       [](const TrackPidSegment &pid) {
+                                           return pid.duration_s > 0 &&
+                                                  std::isfinite(pid.min_speed_m_s) &&
+                                                  std::isfinite(pid.max_speed_m_s) &&
+                                                  std::isfinite(pid.kp) &&
+                                                  std::isfinite(pid.ki) &&
+                                                  std::isfinite(pid.kd) &&
+                                                  std::isfinite(pid.slew_rate_limit) &&
+                                                  (!pid.fine_tune.has_value() ||
+                                                   (std::isfinite(pid.fine_tune->gain_scale) &&
+                                                    pid.fine_tune->gain_scale >= 0.0F)) &&
+                                                  pid.min_speed_m_s >= 0.0F &&
+                                                  pid.max_speed_m_s >= 0.0F &&
+                                                  pid.min_speed_m_s <= pid.max_speed_m_s &&
+                                                  pid.kp >= 0.0F &&
+                                                  pid.ki >= 0.0F &&
+                                                  pid.kd >= 0.0F &&
+                                                  pid.slew_rate_limit >= 0.0F;
+                                       },
+                                       [](const TrackConstantSegment &constant) {
+                                           return constant.duration_s > 0 &&
+                                                  std::isfinite(constant.speed_m_s) &&
+                                                  constant.speed_m_s >= 0.0F;
+                                       }},
+                                   schema.segment);
+        if (!ok) {
+            return unexpected<TrackError>{TrackError::invalid_arg};
+        }
+    }
+    return {};
+}
+
+expected<unit, TrackError> TrackPidSetTuning::validate() const {
+    const auto fine_tune_ok =
+        !fine_tune.has_value() ||
+        (std::isfinite(fine_tune->gain_scale) && fine_tune->gain_scale >= 0.0F);
+    if (!std::isfinite(min_speed_m_s) ||
+        !std::isfinite(max_speed_m_s) ||
+        !std::isfinite(kp) ||
+        !std::isfinite(ki) ||
+        !std::isfinite(kd) ||
+        !std::isfinite(slew_rate_limit) ||
+        !fine_tune_ok ||
+        min_speed_m_s < 0.0F ||
+        max_speed_m_s < 0.0F ||
+        min_speed_m_s > max_speed_m_s ||
+        kp < 0.0F ||
+        ki < 0.0F ||
+        kd < 0.0F ||
+        slew_rate_limit < 0.0F) {
+        return unexpected<TrackError>{TrackError::invalid_arg};
+    }
+    return {};
+}
+
+void TrackPidSetTuning::apply_to(TrackPidSegment &segment) const {
+    segment.min_speed_m_s = min_speed_m_s;
+    segment.max_speed_m_s = max_speed_m_s;
+    segment.kp = kp;
+    segment.ki = ki;
+    segment.kd = kd;
+    segment.slew_rate_limit = slew_rate_limit;
+    segment.fine_tune = fine_tune;
+}
+
+} // namespace track_core