Merge pull request #74 from Spitfire-Cowboy/rowan/upstream-motion-test-coverage

test: add firmware motion math coverage

firmware/README.md

@@ -17,6 +17,16 @@ python3 ./fetch_repos.py
 idf.py build
 ```
 
+### Host-side tests
+
+The motion coordinate 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/motion.cpp

@@ -4,7 +4,7 @@
  * SPDX-License-Identifier: MIT
  */
 #include "motion.h"
-#include <cmath>
+#include "motion_math.h"
 
 using namespace uitk;
 using namespace stackchan::motion;
@@ -77,27 +77,15 @@ void Motion::stop()
 
 void Motion::lookAtNormalized(float x, float y, int speed)
 {
-    int yaw_angle =
-        uitk::map_range(x, -1.0f, 1.0f, (float)_yaw_servo->getAngleLimit().x, (float)_yaw_servo->getAngleLimit().y);
-    int pitch_angle =
-        uitk::map_range(y, -1.0f, 1.0f, (float)_pitch_servo->getAngleLimit().x, (float)_pitch_servo->getAngleLimit().y);
-    moveWithSpeed(yaw_angle, pitch_angle, speed);
+    auto angles = calculateNormalizedLookAngles(
+        x, y, _yaw_servo->getAngleLimit().x, _yaw_servo->getAngleLimit().y, _pitch_servo->getAngleLimit().x, _pitch_servo->getAngleLimit().y);
+    moveWithSpeed(angles.yaw, angles.pitch, speed);
 }
 
 void Motion::lookAtPoint(float x, float y, float z, int speed)
 {
-    // Yaw: 绕 Z 轴旋转。使用 atan2(y, x)
-    float yaw_rad = std::atan2(y, x);
-
-    // Pitch: 俯仰。使用 atan2(z, sqrt(x*x + y*y))
-    float ground_dist = std::sqrt(x * x + y * y);
-    float pitch_rad   = std::atan2(z, ground_dist);
-
-    // 将弧度转换为你的舵机单位 (-1280~1280 等)
-    int yaw_angle   = static_cast<int>(to_degrees(yaw_rad) * 10);
-    int pitch_angle = static_cast<int>(to_degrees(pitch_rad) * 10);
-
-    moveWithSpeed(yaw_angle, pitch_angle, speed);
+    auto angles = calculatePointLookAngles(x, y, z);
+    moveWithSpeed(angles.yaw, angles.pitch, speed);
 }
 
 bool Motion::isMoving()

firmware/main/stackchan/motion/motion.h

@@ -163,13 +163,6 @@ private:
     std::unique_ptr<Servo> _yaw_servo;
     std::unique_ptr<Servo> _pitch_servo;
     bool _is_modify_locked = false;
-
-    static constexpr float RAD_TO_DEG = 180.0f / M_PI;
-
-    inline float to_degrees(float radians)
-    {
-        return radians * RAD_TO_DEG;
-    }
 };
 
 }  // namespace stackchan::motion

firmware/main/stackchan/motion/motion_math.cpp

@@ -0,0 +1,62 @@
+/*
+ * SPDX-FileCopyrightText: 2026 M5Stack Technology CO LTD
+ *
+ * SPDX-License-Identifier: MIT
+ */
+#include "motion_math.h"
+#include <algorithm>
+#include <cmath>
+
+namespace stackchan::motion {
+
+namespace {
+
+constexpr float kRadiansToDegrees = 180.0f / static_cast<float>(M_PI);
+
+float clampNormalized(float value)
+{
+    return std::clamp(value, -1.0f, 1.0f);
+}
+
+float mapRange(float value, float inMin, float inMax, float outMin, float outMax)
+{
+    if (inMax == inMin) {
+        return outMin;
+    }
+    float ratio = (value - inMin) / (inMax - inMin);
+    return outMin + ratio * (outMax - outMin);
+}
+
+int radiansToTenthsOfDegrees(float radians)
+{
+    return static_cast<int>(radians * kRadiansToDegrees * 10.0f);
+}
+
+}  // namespace
+
+int mapNormalizedValueToAngle(float value, int minimum, int maximum)
+{
+    return static_cast<int>(mapRange(clampNormalized(value), -1.0f, 1.0f, static_cast<float>(minimum), static_cast<float>(maximum)));
+}
+
+MotionAngles calculateNormalizedLookAngles(float x, float y, int yawMin, int yawMax, int pitchMin, int pitchMax)
+{
+    return {
+        .yaw = mapNormalizedValueToAngle(x, yawMin, yawMax),
+        .pitch = mapNormalizedValueToAngle(y, pitchMin, pitchMax),
+    };
+}
+
+MotionAngles calculatePointLookAngles(float x, float y, float z)
+{
+    float yawRadians = std::atan2(y, x);
+    float groundDistance = std::sqrt(x * x + y * y);
+    float pitchRadians = std::atan2(z, groundDistance);
+
+    return {
+        .yaw = radiansToTenthsOfDegrees(yawRadians),
+        .pitch = radiansToTenthsOfDegrees(pitchRadians),
+    };
+}
+
+}  // namespace stackchan::motion

firmware/main/stackchan/motion/motion_math.h

@@ -0,0 +1,19 @@
+/*
+ * SPDX-FileCopyrightText: 2026 M5Stack Technology CO LTD
+ *
+ * SPDX-License-Identifier: MIT
+ */
+#pragma once
+
+namespace stackchan::motion {
+
+struct MotionAngles {
+    int yaw;
+    int pitch;
+};
+
+int mapNormalizedValueToAngle(float value, int minimum, int maximum);
+MotionAngles calculateNormalizedLookAngles(float x, float y, int yawMin, int yawMax, int pitchMin, int pitchMax);
+MotionAngles calculatePointLookAngles(float x, float y, float z);
+
+}  // 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(motion_math_test
+    motion_math_test.cpp
+    ../main/stackchan/motion/motion_math.cpp
+)
+
+target_include_directories(motion_math_test PRIVATE
+    ../main
+)
+
+target_compile_features(motion_math_test PRIVATE cxx_std_17)
+
+add_test(NAME motion_math_test COMMAND $<TARGET_FILE:motion_math_test>)

firmware/tests/motion_math_test.cpp

@@ -0,0 +1,69 @@
+/*
+ * SPDX-FileCopyrightText: 2026 M5Stack Technology CO LTD
+ *
+ * SPDX-License-Identifier: MIT
+ */
+#include <cmath>
+#include <cstdlib>
+#include <iostream>
+#include <stackchan/motion/motion_math.h>
+
+namespace {
+
+using stackchan::motion::MotionAngles;
+using stackchan::motion::calculateNormalizedLookAngles;
+using stackchan::motion::calculatePointLookAngles;
+using stackchan::motion::mapNormalizedValueToAngle;
+
+void expectEqual(int actual, int expected, const char* label)
+{
+    if (actual != expected) {
+        std::cerr << label << ": expected " << expected << ", got " << actual << '\n';
+        std::exit(1);
+    }
+}
+
+void expectAngles(MotionAngles actual, MotionAngles expected, const char* label)
+{
+    expectEqual(actual.yaw, expected.yaw, label);
+    expectEqual(actual.pitch, expected.pitch, label);
+}
+
+void testNormalizedMapping()
+{
+    expectEqual(mapNormalizedValueToAngle(-1.0f, -1280, 1280), -1280, "normalized min");
+    expectEqual(mapNormalizedValueToAngle(0.0f, -1280, 1280), 0, "normalized center");
+    expectEqual(mapNormalizedValueToAngle(1.0f, -1280, 1280), 1280, "normalized max");
+    expectEqual(mapNormalizedValueToAngle(2.0f, -1280, 1280), 1280, "normalized clamp high");
+    expectEqual(mapNormalizedValueToAngle(-2.0f, 30, 870), 30, "normalized clamp low");
+}
+
+void testNormalizedLookAngles()
+{
+    expectAngles(
+        calculateNormalizedLookAngles(0.5f, -0.5f, -1280, 1280, 30, 870),
+        MotionAngles{640, 240},
+        "normalized look"
+    );
+}
+
+void testPointLookAngles()
+{
+    expectAngles(calculatePointLookAngles(1.0f, 0.0f, 0.0f), MotionAngles{0, 0}, "point forward");
+    expectAngles(calculatePointLookAngles(0.0f, 1.0f, 0.0f), MotionAngles{900, 0}, "point left");
+    expectAngles(calculatePointLookAngles(0.0f, -1.0f, 0.0f), MotionAngles{-900, 0}, "point right");
+
+    MotionAngles diagonal = calculatePointLookAngles(1.0f, 1.0f, 1.0f);
+    expectEqual(diagonal.yaw, 450, "point diagonal yaw");
+    expectEqual(diagonal.pitch, 352, "point diagonal pitch");
+}
+
+}  // namespace
+
+int main()
+{
+    testNormalizedMapping();
+    testNormalizedLookAngles();
+    testPointLookAngles();
+    return 0;
+}