test: add firmware servo speed coverage

firmware/README.md

@@ -17,6 +17,16 @@ python3 ./fetch_repos.py
 idf.py build
 ```
 
+### Host-side tests
+
+Servo motion profiling helpers can be tested without ESP-IDF hardware:
+
+```bash
+cmake -S tests -B build-host-tests
+cmake --build build-host-tests
+ctest --test-dir build-host-tests --output-on-failure
+```
+
 ### Flash
 
 ```bash

firmware/main/stackchan/motion/servo.cpp

@@ -4,6 +4,7 @@
  * SPDX-License-Identifier: MIT
  */
 #include "servo.h"
+#include "servo_math.h"
 #include <hal/hal.h>
 
 using namespace uitk;
@@ -118,37 +119,14 @@ void Servo::update_angle_anim_target(int angle)
 
 uitk::SpringOptions_t Servo::map_speed_to_spring_options(int speed)
 {
-    speed = uitk::clamp(speed, 0, 1000);
-
-    // 1. 计算 Stiffness (刚度)
-    // 使用二次方映射: k = k_min + (speed/1000)^2 * k_range
-    // 当 speed=500 时，k 约为 10 + 0.25 * 640 = 170
-    float k_min           = 10.0f;
-    float k_max           = 650.0f;
-    float normalizedSpeed = speed / 1000.0f;
-    float stiffness       = k_min + (normalizedSpeed * normalizedSpeed) * (k_max - k_min);
-
-    // 2. 计算 Damping (阻尼)
-    // 为了保持临界阻尼(无过冲，最快稳定)，公式为 d = 2 * sqrt(m * k)
-    // 如果想要带一点点弹性感(bounce)，可以将系数从 2.0 降到 1.5~1.8
-    float mass    = 1.0f;
-    float damping = 2.0f * sqrtf(mass * stiffness);
-
-    // 3. 构造选项
+    auto profile = calculateServoSpringProfile(speed);
+
     uitk::SpringOptions_t options = _default_spring_options;
-    options.stiffness             = stiffness;
-    options.damping               = damping;
-    options.mass                  = mass;
-
-    // 4. 动态调整静止阈值
-    // 高速时阈值大一点可以防止由于离散计算导致的微小抖动
-    if (speed > 800) {
-        options.restDelta = 0.5f;
-        options.restSpeed = 0.5f;
-    } else {
-        options.restDelta = 0.1f;
-        options.restSpeed = 0.1f;
-    }
+    options.stiffness = profile.stiffness;
+    options.damping = profile.damping;
+    options.mass = profile.mass;
+    options.restDelta = profile.restDelta;
+    options.restSpeed = profile.restSpeed;
 
     return options;
 }

firmware/main/stackchan/motion/servo_math.cpp

@@ -0,0 +1,45 @@
+/*
+ * SPDX-FileCopyrightText: 2026 M5Stack Technology CO LTD
+ *
+ * SPDX-License-Identifier: MIT
+ */
+#include "servo_math.h"
+#include <algorithm>
+#include <cmath>
+
+namespace stackchan::motion {
+
+int clampServoSpeed(int speed)
+{
+    return std::clamp(speed, 0, 1000);
+}
+
+ServoSpringProfile calculateServoSpringProfile(int speed)
+{
+    speed = clampServoSpeed(speed);
+
+    float kMin = 10.0f;
+    float kMax = 650.0f;
+    float normalizedSpeed = speed / 1000.0f;
+    float stiffness = kMin + (normalizedSpeed * normalizedSpeed) * (kMax - kMin);
+
+    float mass = 1.0f;
+    float damping = 2.0f * std::sqrt(mass * stiffness);
+
+    ServoSpringProfile profile{
+        .stiffness = stiffness,
+        .damping = damping,
+        .mass = mass,
+        .restSpeed = 0.1f,
+        .restDelta = 0.1f,
+    };
+
+    if (speed > 800) {
+        profile.restDelta = 0.5f;
+        profile.restSpeed = 0.5f;
+    }
+
+    return profile;
+}
+
+}  // namespace stackchan::motion

firmware/main/stackchan/motion/servo_math.h

@@ -0,0 +1,21 @@
+/*
+ * SPDX-FileCopyrightText: 2026 M5Stack Technology CO LTD
+ *
+ * SPDX-License-Identifier: MIT
+ */
+#pragma once
+
+namespace stackchan::motion {
+
+struct ServoSpringProfile {
+    float stiffness;
+    float damping;
+    float mass;
+    float restSpeed;
+    float restDelta;
+};
+
+int clampServoSpeed(int speed);
+ServoSpringProfile calculateServoSpringProfile(int speed);
+
+}  // namespace stackchan::motion

firmware/tests/CMakeLists.txt

@@ -0,0 +1,18 @@
+cmake_minimum_required(VERSION 3.16)
+
+project(stackchan_firmware_tests LANGUAGES CXX)
+
+enable_testing()
+
+add_executable(servo_math_test
+    servo_math_test.cpp
+    ../main/stackchan/motion/servo_math.cpp
+)
+
+target_include_directories(servo_math_test PRIVATE
+    ../main
+)
+
+target_compile_features(servo_math_test PRIVATE cxx_std_17)
+
+add_test(NAME servo_math_test COMMAND $<TARGET_FILE:servo_math_test>)

firmware/tests/servo_math_test.cpp

@@ -0,0 +1,76 @@
+/*
+ * SPDX-FileCopyrightText: 2026 M5Stack Technology CO LTD
+ *
+ * SPDX-License-Identifier: MIT
+ */
+#include <cmath>
+#include <cstdlib>
+#include <iostream>
+#include <stackchan/motion/servo_math.h>
+
+namespace {
+
+using stackchan::motion::ServoSpringProfile;
+using stackchan::motion::calculateServoSpringProfile;
+using stackchan::motion::clampServoSpeed;
+
+void expectEqual(int actual, int expected, const char* label)
+{
+    if (actual != expected) {
+        std::cerr << label << ": expected " << expected << ", got " << actual << '\n';
+        std::exit(1);
+    }
+}
+
+void expectNear(float actual, float expected, float tolerance, const char* label)
+{
+    if (std::fabs(actual - expected) > tolerance) {
+        std::cerr << label << ": expected " << expected << ", got " << actual << '\n';
+        std::exit(1);
+    }
+}
+
+void testSpeedClamp()
+{
+    expectEqual(clampServoSpeed(-10), 0, "speed clamp low");
+    expectEqual(clampServoSpeed(500), 500, "speed clamp mid");
+    expectEqual(clampServoSpeed(1200), 1000, "speed clamp high");
+}
+
+void testProfileAtDefaultMidSpeed()
+{
+    ServoSpringProfile profile = calculateServoSpringProfile(500);
+    expectNear(profile.stiffness, 170.0f, 0.01f, "mid stiffness");
+    expectNear(profile.damping, 26.0768f, 0.01f, "mid damping");
+    expectNear(profile.mass, 1.0f, 0.0f, "mid mass");
+    expectNear(profile.restSpeed, 0.1f, 0.0f, "mid rest speed");
+    expectNear(profile.restDelta, 0.1f, 0.0f, "mid rest delta");
+}
+
+void testProfileAtHighSpeedUsesRelaxedRestThresholds()
+{
+    ServoSpringProfile profile = calculateServoSpringProfile(900);
+    expectNear(profile.stiffness, 528.4f, 0.01f, "high stiffness");
+    expectNear(profile.damping, 45.974f, 0.02f, "high damping");
+    expectNear(profile.restSpeed, 0.5f, 0.0f, "high rest speed");
+    expectNear(profile.restDelta, 0.5f, 0.0f, "high rest delta");
+}
+
+void testProfileClampMatchesEndpoints()
+{
+    ServoSpringProfile low = calculateServoSpringProfile(-1);
+    ServoSpringProfile high = calculateServoSpringProfile(1001);
+    expectNear(low.stiffness, 10.0f, 0.0f, "low stiffness");
+    expectNear(high.stiffness, 650.0f, 0.0f, "high stiffness");
+}
+
+}  // namespace
+
+int main()
+{
+    testSpeedClamp();
+    testProfileAtDefaultMidSpeed();
+    testProfileAtHighSpeedUsesRelaxedRestThresholds();
+    testProfileClampMatchesEndpoints();
+    return 0;
+}