From 0243584307fd8e707a55b62ee867fe5ee2c8cb96 Mon Sep 17 00:00:00 2001 From: John Malone Date: Tue, 19 May 2026 06:39:07 -0500 Subject: [PATCH] test: add firmware motion math coverage --- firmware/README.md | 10 +++ firmware/main/stackchan/motion/motion.cpp | 24 ++----- firmware/main/stackchan/motion/motion.h | 7 -- .../main/stackchan/motion/motion_math.cpp | 62 +++++++++++++++++ firmware/main/stackchan/motion/motion_math.h | 19 +++++ firmware/tests/CMakeLists.txt | 18 +++++ firmware/tests/motion_math_test.cpp | 69 +++++++++++++++++++ 7 files changed, 184 insertions(+), 25 deletions(-) create mode 100644 firmware/main/stackchan/motion/motion_math.cpp create mode 100644 firmware/main/stackchan/motion/motion_math.h create mode 100644 firmware/tests/CMakeLists.txt create mode 100644 firmware/tests/motion_math_test.cpp diff --git a/firmware/README.md b/firmware/README.md index ffc20f2f..20a73bf6 100644 --- a/firmware/README.md +++ b/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 diff --git a/firmware/main/stackchan/motion/motion.cpp b/firmware/main/stackchan/motion/motion.cpp index 8d74832d..21e54683 100644 --- a/firmware/main/stackchan/motion/motion.cpp +++ b/firmware/main/stackchan/motion/motion.cpp @@ -4,7 +4,7 @@ * SPDX-License-Identifier: MIT */ #include "motion.h" -#include +#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(to_degrees(yaw_rad) * 10); - int pitch_angle = static_cast(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() diff --git a/firmware/main/stackchan/motion/motion.h b/firmware/main/stackchan/motion/motion.h index 030863b0..871d69f5 100644 --- a/firmware/main/stackchan/motion/motion.h +++ b/firmware/main/stackchan/motion/motion.h @@ -163,13 +163,6 @@ class Motion { std::unique_ptr _yaw_servo; std::unique_ptr _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 diff --git a/firmware/main/stackchan/motion/motion_math.cpp b/firmware/main/stackchan/motion/motion_math.cpp new file mode 100644 index 00000000..f3843713 --- /dev/null +++ b/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 +#include + +namespace stackchan::motion { + +namespace { + +constexpr float kRadiansToDegrees = 180.0f / static_cast(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(radians * kRadiansToDegrees * 10.0f); +} + +} // namespace + +int mapNormalizedValueToAngle(float value, int minimum, int maximum) +{ + return static_cast(mapRange(clampNormalized(value), -1.0f, 1.0f, static_cast(minimum), static_cast(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 diff --git a/firmware/main/stackchan/motion/motion_math.h b/firmware/main/stackchan/motion/motion_math.h new file mode 100644 index 00000000..9a0642e7 --- /dev/null +++ b/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 diff --git a/firmware/tests/CMakeLists.txt b/firmware/tests/CMakeLists.txt new file mode 100644 index 00000000..df2ecb53 --- /dev/null +++ b/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 $) diff --git a/firmware/tests/motion_math_test.cpp b/firmware/tests/motion_math_test.cpp new file mode 100644 index 00000000..0aa12107 --- /dev/null +++ b/firmware/tests/motion_math_test.cpp @@ -0,0 +1,69 @@ +/* + * SPDX-FileCopyrightText: 2026 M5Stack Technology CO LTD + * + * SPDX-License-Identifier: MIT + */ +#include +#include +#include +#include + +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; +}