feat:add s scurve

.config

@@ -256,7 +256,7 @@ CONFIG_RT_USING_PWM=y
 # CONFIG_RT_USING_SDIO is not set
 CONFIG_RT_USING_SPI=y
 # CONFIG_RT_USING_SOFT_SPI is not set
-# CONFIG_RT_USING_QSPI is not set
+CONFIG_RT_USING_QSPI=y
 # CONFIG_RT_USING_SPI_MSD is not set
 # CONFIG_RT_USING_SFUD is not set
 # CONFIG_RT_USING_ENC28J60 is not set
@@ -1531,7 +1531,7 @@ CONFIG_BSP_UART3_TX_BUFSIZE=512
 CONFIG_BSP_USING_PWM=y
 CONFIG_BSP_USING_PWM1=y
 CONFIG_BSP_USING_PWM1_CH1=y
-# CONFIG_BSP_USING_QSPI is not set
+CONFIG_BSP_USING_QSPI=y
 CONFIG_BSP_USING_ADC=y
 CONFIG_BSP_USING_ADC1=y
 # CONFIG_BSP_USING_SPI is not set

CMakeLists.txt

@@ -29,12 +29,14 @@ PROJECT(rtthread C CXX ASM)
 INCLUDE_DIRECTORIES(
 	.
 	applications
+	applications/common
 	applications/controller
 	applications/erpc
 	applications/erpc/service
 	applications/erpc/client
 	applications/erpc/common
 	applications/erpc/proto/generated
+	applications/planner
 	applications/servo_manager
 	board
 	board/CubeMX_Config/Inc
@@ -103,25 +105,29 @@ ADD_DEFINITIONS(
 
 # Library source files
 SET(RT_APP_ERPC_SOURCES
+	applications/erpc/service/src/servo_service_impl.cpp
+	applications/erpc/proto/generated/servo_service_server.cpp
 	applications/erpc/common/src/erpc_error_handler.cpp
-	applications/erpc/proto/generated/face_servo_interface.cpp
+	applications/erpc/proto/generated/servo_service_interface.cpp
-	applications/erpc/service/src/head_service_impl.cpp
-	applications/erpc/proto/generated/face_servo_server.cpp
 	applications/erpc/service/src/erpc_server_main.cpp
 )
 
 SET(RT_APPLICATIONS_SOURCES
-	applications/main.c
+	applications/main.cpp
 )
 
 SET(RT_CHERRYUSB_SOURCES
+	rt-thread/components/drivers/usb/cherryusb/core/usbd_core.c
 	rt-thread/components/drivers/usb/cherryusb/platform/rtthread/usb_msh.c
-	rt-thread/components/drivers/usb/cherryusb/port/dwc2/usb_dc_dwc2.c
 	rt-thread/components/drivers/usb/cherryusb/osal/usb_osal_rtthread.c
-	rt-thread/components/drivers/usb/cherryusb/port/dwc2/usb_glue_st.c
 	rt-thread/components/drivers/usb/cherryusb/class/cdc/usbd_cdc_acm.c
 	rt-thread/components/drivers/usb/cherryusb/platform/rtthread/usb_check.c
-	rt-thread/components/drivers/usb/cherryusb/core/usbd_core.c
+	rt-thread/components/drivers/usb/cherryusb/port/dwc2/usb_glue_st.c
+	rt-thread/components/drivers/usb/cherryusb/port/dwc2/usb_dc_dwc2.c
+)
+
+SET(RT_COMMON_SOURCES
+	applications/common/curve/src/s_curve.cpp
 )
 
 SET(RT_COMPILER_SOURCES
@@ -136,11 +142,13 @@ SET(RT_COMPILER_SOURCES
 
 SET(RT_CONTROLLER_SOURCES
 	applications/controller/src/pid_controller.cpp
+	applications/planner/s_curve_planner/src/s_curve_position_planner.cpp
+	applications/planner/s_curve_planner/src/s_curve_velocity_planner.cpp
 )
 
 SET(RT_CPP_SOURCES
-	rt-thread/components/libc/cplusplus/cxx_crt.cpp
 	rt-thread/components/libc/cplusplus/cxx_crt_init.c
+	rt-thread/components/libc/cplusplus/cxx_crt.cpp
 )
 
 SET(RT_DEVICEDRIVERS_SOURCES
@@ -158,6 +166,7 @@ SET(RT_DEVICEDRIVERS_SOURCES
 	rt-thread/components/drivers/misc/rt_drv_pwm.c
 	rt-thread/components/drivers/pin/dev_pin.c
 	rt-thread/components/drivers/serial/dev_serial.c
+	rt-thread/components/drivers/spi/dev_qspi_core.c
 	rt-thread/components/drivers/spi/dev_spi.c
 	rt-thread/components/drivers/spi/dev_spi_core.c
 )
@@ -171,6 +180,7 @@ SET(RT_DRIVERS_SOURCES
 	libraries/HAL_Drivers/drivers/drv_adc.c
 	libraries/HAL_Drivers/drivers/drv_gpio.c
 	libraries/HAL_Drivers/drivers/drv_pwm.c
+	libraries/HAL_Drivers/drivers/drv_qspi.c
 	libraries/HAL_Drivers/drivers/drv_spi.c
 	libraries/HAL_Drivers/drivers/drv_tim.c
 	libraries/HAL_Drivers/drivers/drv_usart.c
@@ -178,29 +188,29 @@ SET(RT_DRIVERS_SOURCES
 )
 
 SET(RT_ERPC_SOURCES
-	packages/erpc-1.14.0/erpc_c/infra/erpc_framed_transport.cpp
-	packages/erpc-1.14.0/erpc_c/infra/erpc_message_buffer.cpp
-	packages/erpc-1.14.0/erpc_c/infra/erpc_crc16.cpp
-	packages/erpc-1.14.0/erpc_c/setup/erpc_setup_mbf_dynamic.cpp
 	packages/erpc-1.14.0/erpc_c/infra/erpc_message_loggers.cpp
-	packages/erpc-1.14.0/erpc_c/setup/erpc_server_setup.cpp
+	packages/erpc-1.14.0/erpc_c/setup/erpc_setup_mbf_dynamic.cpp
-	packages/erpc-1.14.0/erpc_c/infra/erpc_transport_arbitrator.cpp
 	packages/erpc-1.14.0/erpc_c/port/erpc_threading_pthreads.cpp
-	packages/erpc-1.14.0/erpc_c/setup/erpc_setup_mbf_static.cpp
-	packages/erpc-1.14.0/erpc_c/infra/erpc_simple_server.cpp
-	packages/erpc-1.14.0/erpc_c/port/erpc_port_rtt.cpp
 	packages/erpc-1.14.0/erpc_c/infra/erpc_server.cpp
+	packages/erpc-1.14.0/erpc_c/port/erpc_port_rtt.cpp
+	packages/erpc-1.14.0/erpc_c/setup/erpc_server_setup.cpp
+	packages/erpc-1.14.0/erpc_c/infra/erpc_simple_server.cpp
+	packages/erpc-1.14.0/erpc_c/infra/erpc_framed_transport.cpp
+	packages/erpc-1.14.0/erpc_c/setup/erpc_setup_mbf_static.cpp
 	packages/erpc-1.14.0/erpc_c/transports/erpc_usb_cdc_transport.cpp
-	packages/erpc-1.14.0/erpc_c/infra/erpc_basic_codec.cpp
-	packages/erpc-1.14.0/erpc_c/infra/erpc_pre_post_action.cpp
-	packages/erpc-1.14.0/erpc_c/transports/erpc_inter_thread_buffer_transport.cpp
 	packages/erpc-1.14.0/erpc_c/setup/erpc_setup_usb_cdc.cpp
+	packages/erpc-1.14.0/erpc_c/infra/erpc_transport_arbitrator.cpp
+	packages/erpc-1.14.0/erpc_c/transports/erpc_inter_thread_buffer_transport.cpp
+	packages/erpc-1.14.0/erpc_c/infra/erpc_crc16.cpp
+	packages/erpc-1.14.0/erpc_c/infra/erpc_basic_codec.cpp
+	packages/erpc-1.14.0/erpc_c/infra/erpc_message_buffer.cpp
+	packages/erpc-1.14.0/erpc_c/infra/erpc_pre_post_action.cpp
 )
 
 SET(RT_FINSH_SOURCES
+	rt-thread/components/finsh/cmd.c
 	rt-thread/components/finsh/shell.c
 	rt-thread/components/finsh/msh_parse.c
-	rt-thread/components/finsh/cmd.c
 	rt-thread/components/finsh/msh.c
 )
 
@@ -223,15 +233,15 @@ SET(RT_KERNEL_SOURCES
 )
 
 SET(RT_KLIBC_SOURCES
+	rt-thread/src/klibc/rt_vsscanf.c
+	rt-thread/src/klibc/kstring.c
 	rt-thread/src/klibc/kerrno.c
 	rt-thread/src/klibc/kstdio.c
-	rt-thread/src/klibc/kstring.c
-	rt-thread/src/klibc/rt_vsscanf.c
 )
 
 SET(RT_KTIME_SOURCES
-	rt-thread/components/drivers/ktime/src/hrtimer.c
 	rt-thread/components/drivers/ktime/src/boottime.c
+	rt-thread/components/drivers/ktime/src/hrtimer.c
 	rt-thread/components/drivers/ktime/src/cputimer.c
 )
 
@@ -245,35 +255,35 @@ SET(RT_LIBCPU_SOURCES
 )
 
 SET(RT_LIBRARIES_SOURCES
-	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_crc_ex.c
-	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_comp.c
-	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_lptim.c
 	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_gpio.c
-	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_dma.c
-	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_pwr_ex.c
-	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_adc_ex.c
-	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_uart.c
-	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_tim_ex.c
 	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_pwr.c
+	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_cryp.c
+	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_cryp_ex.c
+	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_adc.c
 	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_rcc.c
 	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_sram.c
-	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_cryp_ex.c
+	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_adc_ex.c
 	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_qspi.c
-	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_adc.c
-	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_uart_ex.c
-	libraries/STM32H7xx_HAL/CMSIS/Device/ST/STM32H7xx/Source/Templates/system_stm32h7xx.c
-	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal.c
-	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_usart.c
-	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_tim.c
-	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_cec.c
 	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_rcc_ex.c
-	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_cortex.c
+	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_tim_ex.c
-	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_mdma.c
+	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_usart.c
-	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_crc.c
 	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_spi.c
+	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_uart.c
 	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_dma_ex.c
-	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_cryp.c
+	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_lptim.c
+	libraries/STM32H7xx_HAL/CMSIS/Device/ST/STM32H7xx/Source/Templates/system_stm32h7xx.c
+	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_crc_ex.c
+	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_tim.c
+	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_dma.c
 	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_rng.c
+	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_uart_ex.c
+	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_comp.c
+	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal.c
+	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_mdma.c
+	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_cec.c
+	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_pwr_ex.c
+	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_cortex.c
+	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_crc.c
 )
 
 SET(RT_POSIX_SOURCES
@@ -290,8 +300,10 @@ SET(RT_POSIX_SOURCES
 	rt-thread/components/libc/posix/tls/emutls.c
 )
 
-SET(RT_SERVO_MANAGER_SOURCES
+SET(RT_SERVO_CONTROL_SOURCES
+	applications/servo_manager/src/servo_control.cpp
 	applications/servo_manager/src/servo_manager.cpp
+	applications/servo_manager/src/servo_driver.cpp
 )
 
 SET(RT_UTC_UTEST_SOURCES
@@ -319,6 +331,7 @@ SET(RT_COMPILER_LIBS
 # Libraries
 ADD_LIBRARY(rtt_app_erpc OBJECT ${RT_APP_ERPC_SOURCES})
 ADD_LIBRARY(rtt_CherryUSB OBJECT ${RT_CHERRYUSB_SOURCES})
+ADD_LIBRARY(rtt_common OBJECT ${RT_COMMON_SOURCES})
 ADD_LIBRARY(rtt_Compiler OBJECT ${RT_COMPILER_SOURCES})
 ADD_LIBRARY(rtt_controller OBJECT ${RT_CONTROLLER_SOURCES})
 ADD_LIBRARY(rtt_CPP OBJECT ${RT_CPP_SOURCES})
@@ -332,7 +345,7 @@ ADD_LIBRARY(rtt_ktime OBJECT ${RT_KTIME_SOURCES})
 ADD_LIBRARY(rtt_libcpu OBJECT ${RT_LIBCPU_SOURCES})
 ADD_LIBRARY(rtt_Libraries OBJECT ${RT_LIBRARIES_SOURCES})
 ADD_LIBRARY(rtt_POSIX OBJECT ${RT_POSIX_SOURCES})
-ADD_LIBRARY(rtt_servo_manager OBJECT ${RT_SERVO_MANAGER_SOURCES})
+ADD_LIBRARY(rtt_servo_control OBJECT ${RT_SERVO_CONTROL_SOURCES})
 
 # Interface libraries
 ADD_LIBRARY(rtt_utc_UTest INTERFACE)
@@ -349,6 +362,7 @@ ADD_EXECUTABLE(${CMAKE_PROJECT_NAME}.elf ${RT_APPLICATIONS_SOURCES})
 TARGET_LINK_LIBRARIES(${CMAKE_PROJECT_NAME}.elf
 	rtt_app_erpc
 	rtt_CherryUSB
+	rtt_common
 	rtt_Compiler
 	rtt_controller
 	rtt_CPP
@@ -362,7 +376,7 @@ TARGET_LINK_LIBRARIES(${CMAKE_PROJECT_NAME}.elf
 	rtt_libcpu
 	rtt_Libraries
 	rtt_POSIX
-	rtt_servo_manager
+	rtt_servo_control
 	rtt_utc_UTest
 	rtt_utestcases
 )

applications/SConscript

@@ -3,6 +3,7 @@ import os
 
 cwd = GetCurrentDir()
 src = Glob('*.c')
+src += Glob('*.cpp')
 CPPPATH = [cwd]
 
 group = DefineGroup('Applications', src, depend = [''], CPPPATH = CPPPATH)

applications/common/SConscript

@@ -0,0 +1,10 @@
+from building import *
+import os
+
+cwd = GetCurrentDir()
+src = Glob('curve/src/*.cpp')
+CPPPATH = [cwd]
+
+group = DefineGroup('common', src, depend = [''], CPPPATH = CPPPATH)
+
+Return('group')

applications/common/curve/include/s_curve.h

@@ -0,0 +1,167 @@
+//
+// Created by lgv on 2026/3/2.
+//
+/**
+ * @file s_curve.h
+ * @brief 严格点到点位置 S 曲线轨迹生成器
+ *
+ * 实现标准 7 段式位置 S 曲线速度剖面：
+ * 1. 加加速度上升段（加速度逐渐增大）
+ * 2. 匀加速段
+ * 3. 加加速度下降段（加速度逐渐降到 0）
+ * 4. 匀速巡航段
+ * 5. 加加速度下降段（减速度逐渐增大）
+ * 6. 匀减速段
+ * 7. 加加速度上升段（减速度逐渐降到 0）
+ *
+ * 本类只负责严格点到点 profile 规划与采样，不负责实时目标跟踪。
+ */
+
+#pragma once
+
+#include <algorithm>
+#include <cmath>
+#include <vector>
+
+
+namespace cmvr
+{
+
+/**
+ * @brief 单次点到点运动的 S 曲线剖面参数
+ */
+struct SCurveProfile
+{
+  // 7 个阶段的持续时间
+  double t1;  // 加加速度上升时间（加速阶段）
+  double t2;  // 匀加速时间
+  double t3;  // 加加速度下降时间（加速阶段结束）
+  double t4;  // 匀速时间（巡航）
+  double t5;  // 加加速度下降时间（减速阶段）
+  double t6;  // 匀减速时间
+  double t7;  // 加加速度上升时间（减速阶段结束）
+
+  double total_time;
+
+  // 运动约束
+  double j_max;   // 最大加加速度 (rad/s³)
+  double a_max;   // 最大加速度 (rad/s²)
+  double v_max;   // 最大速度 (rad/s)
+
+  // 运动参数
+  double distance;      // 运动总距离
+  double direction;     // +1 或 -1
+  double v_cruise;      // 实际达到的巡航速度
+  double a_limit;       // 实际达到的加速度上限
+
+  // 初始条件
+  double p0;  // 初始位置
+  double v0;  // 初始速度
+  double a0;  // 初始加速度
+  double vf;  // 终止速度
+
+  SCurveProfile()
+    : t1(0), t2(0), t3(0), t4(0), t5(0), t6(0), t7(0)
+    , total_time(0)
+    , j_max(50.0), a_max(10.0), v_max(3.0)
+    , distance(0), direction(1.0), v_cruise(0), a_limit(0)
+    , p0(0), v0(0), a0(0), vf(0)
+  {}
+};
+
+/**
+ * @brief 严格点到点位置 S 曲线规划器
+ */
+class SCurve
+{
+public:
+  /**
+   * @brief 构造函数（带运动约束）
+   * @param max_velocity 最大速度 (rad/s)
+   * @param max_acceleration 最大加速度 (rad/s²)
+   * @param max_jerk 最大加加速度 (rad/s³)
+   */
+  SCurve(double max_velocity = 3.0,
+                  double max_acceleration = 10.0,
+                  double max_jerk = 50.0);
+
+  /**
+   * @brief 设置运动约束
+   */
+  void setConstraints(double max_velocity, double max_acceleration, double max_jerk);
+
+  /**
+   * @brief 获取当前运动约束
+   */
+  void getConstraints(double& max_velocity, double& max_acceleration, double& max_jerk) const;
+
+  /**
+   * @brief 计算点到点运动的完整 S 曲线剖面
+   * @param start_position 起始位置
+   * @param end_position 终止位置
+   * @param start_velocity 起始速度（默认 0）
+   * @param end_velocity 终止速度（默认 0）
+   * @return 计算得到的剖面参数
+   */
+  SCurveProfile calculateProfile(double start_position, double end_position,
+                                  double start_velocity = 0.0, double end_velocity = 0.0);
+
+  /**
+   * @brief 给定剖面下，获取时刻 t 的位置
+   * @param profile S 曲线剖面
+   * @param t 从起点开始的时间
+   * @return 时刻 t 的位置
+   */
+  double getPositionAtTime(const SCurveProfile& profile, double t) const;
+
+  /**
+   * @brief 给定剖面下，获取时刻 t 的速度
+   * @param profile S 曲线剖面
+   * @param t 从起点开始的时间
+   * @return 时刻 t 的速度
+   */
+  double getVelocityAtTime(const SCurveProfile& profile, double t) const;
+
+  /**
+   * @brief 给定剖面下，获取时刻 t 的加速度
+   * @param profile S 曲线剖面
+   * @param t 从起点开始的时间
+   * @return 时刻 t 的加速度
+   */
+  double getAccelerationAtTime(const SCurveProfile& profile, double t) const;
+
+  /**
+   * @brief 给定剖面下，获取时刻 t 的加加速度
+   * @param profile S 曲线剖面
+   * @param t 从起点开始的时间
+   * @return 时刻 t 的加加速度
+   */
+  double getJerkAtTime(const SCurveProfile& profile, double t) const;
+
+  /**
+   * @brief 按固定时间间隔生成轨迹点
+   * @param profile S 曲线剖面
+   * @param dt 轨迹点时间间隔
+   * @param positions 输出：每个时间点的位置
+   * @param velocities 输出：每个时间点的速度
+   * @param accelerations 输出：每个时间点的加速度
+   */
+  void generateTrajectory(const SCurveProfile& profile, double dt,
+                          std::vector<double>& positions,
+                          std::vector<double>& velocities,
+                          std::vector<double>& accelerations) const;
+
+private:
+  double max_velocity_;
+  double max_acceleration_;
+  double max_jerk_;
+
+  static constexpr double EPSILON = 1e-9;
+  static constexpr double VELOCITY_THRESHOLD = 1e-6;
+
+  double computeSegmentJerk(const SCurveProfile& profile, double t) const;
+  void calculateShortProfile(SCurveProfile& profile) const;
+  void calculateLongProfile(SCurveProfile& profile) const;
+};
+
+}  // namespace cmvr

applications/common/curve/src/s_curve.cpp

@@ -0,0 +1,542 @@
+//
+// Created by lgv on 2026/3/2.
+//
+
+
+/**
+ * @file s_curve_generator.cpp
+ * @brief S 曲线轨迹生成器实现
+ */
+
+#include "common/curve/include/s_curve.h"
+#include <cmath>
+#include <algorithm>
+#include <stdexcept>
+
+namespace cmvr
+{
+SCurve::SCurve(double max_velocity, double max_acceleration, double max_jerk)
+  : max_velocity_(std::abs(max_velocity))
+  , max_acceleration_(std::abs(max_acceleration))
+  , max_jerk_(std::abs(max_jerk))
+{}
+
+void SCurve::setConstraints(double max_velocity, double max_acceleration, double max_jerk)
+{
+  max_velocity_ = std::abs(max_velocity);
+  max_acceleration_ = std::abs(max_acceleration);
+  max_jerk_ = std::abs(max_jerk);
+}
+
+void SCurve::getConstraints(double& max_velocity, double& max_acceleration, double& max_jerk) const
+{
+  max_velocity = max_velocity_;
+  max_acceleration = max_acceleration_;
+  max_jerk = max_jerk_;
+}
+
+SCurveProfile SCurve::calculateProfile(double start_position, double end_position,
+                                                 double start_velocity, double end_velocity)
+{
+  SCurveProfile profile;
+  profile.j_max = max_jerk_;
+  profile.a_max = max_acceleration_;
+  profile.v_max = max_velocity_;
+  profile.p0 = start_position;
+  profile.v0 = start_velocity;
+  profile.a0 = 0.0;  // Assume starting from zero acceleration
+  profile.vf = end_velocity;
+
+  double displacement = end_position - start_position;
+  profile.distance = std::abs(displacement);
+  profile.direction = (displacement >= 0) ? 1.0 : -1.0;
+
+  if (profile.distance < EPSILON) {
+    // 无需运动
+    profile.total_time = 0.0;
+    return profile;
+  }
+
+  const double start_speed = profile.direction * start_velocity;
+  const double end_speed = profile.direction * end_velocity;
+  if (start_speed < -VELOCITY_THRESHOLD || end_speed < -VELOCITY_THRESHOLD) {
+    profile.total_time = 0.0;
+    return profile;
+  }
+
+  if (std::abs(start_speed) > VELOCITY_THRESHOLD ||
+      std::abs(end_speed) > VELOCITY_THRESHOLD) {
+    auto calc_transition = [&](double v_from, double v_to,
+                               double& t_j, double& t_a,
+                               double& duration, double& distance) {
+      t_j = 0.0;
+      t_a = 0.0;
+      duration = 0.0;
+      distance = 0.0;
+
+      const double dv = std::abs(v_to - v_from);
+      if (dv <= 1e-12 || profile.a_max <= 1e-12 || profile.j_max <= 1e-12) {
+        return;
+      }
+
+      const double dv_switch = profile.a_max * profile.a_max / profile.j_max;
+      if (dv < dv_switch) {
+        t_j = std::sqrt(dv / profile.j_max);
+        t_a = 0.0;
+      } else {
+        t_j = profile.a_max / profile.j_max;
+        t_a = dv / profile.a_max - t_j;
+      }
+
+      duration = 2.0 * t_j + t_a;
+      distance = 0.5 * (v_from + v_to) * duration;
+    };
+
+    const double v_cap = std::max({profile.v_max, start_speed, end_speed});
+    const double v_peak_min = std::max(start_speed, end_speed);
+
+    double acc_tj = 0.0, acc_ta = 0.0, acc_duration = 0.0, acc_distance = 0.0;
+    double dec_tj = 0.0, dec_ta = 0.0, dec_duration = 0.0, dec_distance = 0.0;
+    calc_transition(start_speed, v_peak_min,
+                    acc_tj, acc_ta, acc_duration, acc_distance);
+    calc_transition(v_peak_min, end_speed,
+                    dec_tj, dec_ta, dec_duration, dec_distance);
+    const double d_min = acc_distance + dec_distance;
+    if (profile.distance + EPSILON < d_min) {
+      profile.total_time = 0.0;
+      return profile;
+    }
+
+    calc_transition(start_speed, v_cap,
+                    acc_tj, acc_ta, acc_duration, acc_distance);
+    calc_transition(v_cap, end_speed,
+                    dec_tj, dec_ta, dec_duration, dec_distance);
+    const double d_cap = acc_distance + dec_distance;
+
+    double v_peak = v_cap;
+    double t4 = 0.0;
+
+    if (profile.distance > d_cap + EPSILON) {
+      t4 = (profile.distance - d_cap) / std::max(v_peak, EPSILON);
+    } else {
+      double lo = v_peak_min;
+      double hi = v_cap;
+      for (int iter = 0; iter < 80; ++iter) {
+        const double mid = 0.5 * (lo + hi);
+        double mid_acc_tj = 0.0, mid_acc_ta = 0.0, mid_acc_duration = 0.0, mid_acc_distance = 0.0;
+        double mid_dec_tj = 0.0, mid_dec_ta = 0.0, mid_dec_duration = 0.0, mid_dec_distance = 0.0;
+        calc_transition(start_speed, mid,
+                        mid_acc_tj, mid_acc_ta, mid_acc_duration, mid_acc_distance);
+        calc_transition(mid, end_speed,
+                        mid_dec_tj, mid_dec_ta, mid_dec_duration, mid_dec_distance);
+        const double d_mid = mid_acc_distance + mid_dec_distance;
+        if (d_mid < profile.distance) {
+          lo = mid;
+        } else {
+          hi = mid;
+        }
+      }
+      v_peak = hi;
+      calc_transition(start_speed, v_peak,
+                      acc_tj, acc_ta, acc_duration, acc_distance);
+      calc_transition(v_peak, end_speed,
+                      dec_tj, dec_ta, dec_duration, dec_distance);
+    }
+
+    profile.t1 = acc_tj;
+    profile.t2 = acc_ta;
+    profile.t3 = acc_tj;
+    profile.t4 = t4;
+    profile.t5 = dec_tj;
+    profile.t6 = dec_ta;
+    profile.t7 = dec_tj;
+    profile.v_cruise = v_peak;
+    profile.a_limit = std::max(profile.j_max * profile.t1, profile.j_max * profile.t5);
+    profile.total_time = profile.t1 + profile.t2 + profile.t3 + profile.t4 +
+                         profile.t5 + profile.t6 + profile.t7;
+    return profile;
+  }
+
+  const double j = profile.j_max;
+  const double a = profile.a_max;
+  const double v = profile.v_max;
+  const double t_j_a = a / j;
+
+  if (v < a * t_j_a) {
+    const double t_j_v = std::sqrt(v / j);
+    const double s_min = 2.0 * j * t_j_v * t_j_v * t_j_v;
+    if (profile.distance > s_min + EPSILON) {
+      calculateLongProfile(profile);
+    } else {
+      calculateShortProfile(profile);
+    }
+  } else {
+    const double t_a_v = v / a - t_j_a;
+    const double s_half_v =
+      a * (t_j_a * t_j_a + 1.5 * t_j_a * t_a_v + 0.5 * t_a_v * t_a_v);
+    const double s_min = 2.0 * s_half_v;
+    if (profile.distance > s_min + EPSILON) {
+      calculateLongProfile(profile);
+    } else {
+      calculateShortProfile(profile);
+    }
+  }
+
+  return profile;
+}
+
+void SCurve::calculateShortProfile(SCurveProfile& profile) const
+{
+  // 短距离：无巡航段（t4 = 0），与 moveL_SCurveLocal 中的 SCurveProfile1D 保持一致。
+  const double j = profile.j_max;
+  const double a = profile.a_max;
+  const double v = profile.v_max;
+  const double d = profile.distance;
+  const double t_j_a = a / j;
+
+  if (v < a * t_j_a) {
+    // 达不到 a_max：极短距离时只有 4 个 jerk 段。
+    const double t1 = std::cbrt(d / (2.0 * j));
+    profile.t1 = t1;
+    profile.t2 = 0.0;
+    profile.t3 = t1;
+    profile.t4 = 0.0;
+    profile.t5 = t1;
+    profile.t6 = 0.0;
+    profile.t7 = t1;
+    profile.v_cruise = j * t1 * t1;
+    profile.a_limit = j * t1;
+  } else {
+    // 能达到 a_max，但达不到 v_max：求无巡航的匀加速持续时间。
+    const double D = t_j_a * t_j_a + 4.0 * (d / a);
+    double t2 = (-3.0 * t_j_a + std::sqrt(std::max(0.0, D))) * 0.5;
+
+    if (t2 < 0.0) {
+      const double t1 = std::cbrt(d / (2.0 * j));
+      profile.t1 = t1;
+      profile.t2 = 0.0;
+      profile.t3 = t1;
+      profile.t4 = 0.0;
+      profile.t5 = t1;
+      profile.t6 = 0.0;
+      profile.t7 = t1;
+      profile.v_cruise = j * t1 * t1;
+      profile.a_limit = j * t1;
+    } else {
+      profile.t1 = t_j_a;
+      profile.t2 = t2;
+      profile.t3 = t_j_a;
+      profile.t4 = 0.0;
+      profile.t5 = t_j_a;
+      profile.t6 = t2;
+      profile.t7 = t_j_a;
+      profile.v_cruise = a * (t2 + t_j_a);
+      profile.a_limit = a;
+    }
+  }
+
+  profile.total_time = profile.t1 + profile.t2 + profile.t3 + profile.t4 +
+                       profile.t5 + profile.t6 + profile.t7;
+}
+
+void SCurve::calculateLongProfile(SCurveProfile& profile) const
+{
+  // 含巡航段的完整 7 段式 S 曲线；与 SCurveProfile1D 的 full-profile 公式一致。
+  const double j = profile.j_max;
+  const double a = profile.a_max;
+  const double v = profile.v_max;
+  const double d = profile.distance;
+  const double t_j_a = a / j;
+
+  if (v < a * t_j_a) {
+    // 达不到 a_max，但可以达到 v_max 并进入巡航。
+    const double t_j_v = std::sqrt(v / j);
+    const double s_min = 2.0 * j * t_j_v * t_j_v * t_j_v;
+    profile.t1 = t_j_v;
+    profile.t2 = 0.0;
+    profile.t3 = t_j_v;
+    profile.t4 = std::max(0.0, (d - s_min) / v);
+    profile.t5 = t_j_v;
+    profile.t6 = 0.0;
+    profile.t7 = t_j_v;
+    profile.v_cruise = v;
+    profile.a_limit = j * t_j_v;
+  } else {
+    // 能达到 a_max 和 v_max。
+    const double t_a = v / a - t_j_a;
+    const double s_half =
+      a * (t_j_a * t_j_a + 1.5 * t_j_a * t_a + 0.5 * t_a * t_a);
+    const double s_min = 2.0 * s_half;
+
+    profile.t1 = t_j_a;
+    profile.t2 = t_a;
+    profile.t3 = t_j_a;
+    profile.t4 = std::max(0.0, (d - s_min) / v);
+    profile.t5 = t_j_a;
+    profile.t6 = t_a;
+    profile.t7 = t_j_a;
+    profile.v_cruise = v;
+    profile.a_limit = a;
+  }
+
+  profile.total_time = profile.t1 + profile.t2 + profile.t3 + profile.t4 +
+                       profile.t5 + profile.t6 + profile.t7;
+}
+
+double SCurve::computeSegmentJerk(const SCurveProfile& profile, double t) const
+{
+  double j = profile.j_max * profile.direction;
+
+  // 判断所处阶段
+  double t_end1 = profile.t1;
+  double t_end2 = t_end1 + profile.t2;
+  double t_end3 = t_end2 + profile.t3;
+  double t_end4 = t_end3 + profile.t4;
+  double t_end5 = t_end4 + profile.t5;
+  double t_end6 = t_end5 + profile.t6;
+  // double t_end7 = t_end6 + profile.t7;  // = total_time
+
+  if (t < t_end1) {
+    return j;  // 段 1：正加加速度（加速）
+  } else if (t < t_end2) {
+    return 0.0;  // 段 2：加加速度为 0（匀加速）
+  } else if (t < t_end3) {
+    return -j;  // 段 3：负加加速度（减小加速度）
+  } else if (t < t_end4) {
+    return 0.0;  // 段 4：加加速度为 0（巡航）
+  } else if (t < t_end5) {
+    return -j;  // 段 5：负加加速度（开始减速）
+  } else if (t < t_end6) {
+    return 0.0;  // 段 6：加加速度为 0（匀减速）
+  } else {
+    return j;  // 段 7：正加加速度（减速结束）
+  }
+}
+
+double SCurve::getPositionAtTime(const SCurveProfile& profile, double t) const
+{
+  if (t <= 0) return profile.p0;
+  if (t >= profile.total_time) return profile.p0 + profile.distance * profile.direction;
+
+  double j = profile.j_max * profile.direction;
+  double p = profile.p0;
+  double v = profile.v0;
+  double a = profile.a0;
+
+  // 时间边界
+  double t_end1 = profile.t1;
+  double t_end2 = t_end1 + profile.t2;
+  double t_end3 = t_end2 + profile.t3;
+  double t_end4 = t_end3 + profile.t4;
+  double t_end5 = t_end4 + profile.t5;
+  double t_end6 = t_end5 + profile.t6;
+
+  // 依次处理各阶段
+  auto processSegment = [&](double dt, double jerk) {
+    p += v * dt + 0.5 * a * dt * dt + (1.0/6.0) * jerk * dt * dt * dt;
+    v += a * dt + 0.5 * jerk * dt * dt;
+    a += jerk * dt;
+  };
+
+  // 段 1
+  if (t <= t_end1) {
+    processSegment(t, j);
+    return p;
+  }
+  processSegment(profile.t1, j);
+
+  // 段 2
+  if (t <= t_end2) {
+    processSegment(t - t_end1, 0.0);
+    return p;
+  }
+  processSegment(profile.t2, 0.0);
+
+  // 段 3
+  if (t <= t_end3) {
+    processSegment(t - t_end2, -j);
+    return p;
+  }
+  processSegment(profile.t3, -j);
+
+  // 段 4（巡航）
+  if (t <= t_end4) {
+    processSegment(t - t_end3, 0.0);
+    return p;
+  }
+  processSegment(profile.t4, 0.0);
+
+  // 段 5
+  if (t <= t_end5) {
+    processSegment(t - t_end4, -j);
+    return p;
+  }
+  processSegment(profile.t5, -j);
+
+  // 段 6
+  if (t <= t_end6) {
+    processSegment(t - t_end5, 0.0);
+    return p;
+  }
+  processSegment(profile.t6, 0.0);
+
+  // 段 7
+  processSegment(t - t_end6, j);
+  return p;
+}
+
+double SCurve::getVelocityAtTime(const SCurveProfile& profile, double t) const
+{
+  if (t <= 0) return profile.v0;
+  if (t >= profile.total_time) return profile.vf;
+
+  double j = profile.j_max * profile.direction;
+  double v = profile.v0;
+  double a = profile.a0;
+
+  // 时间边界
+  double t_end1 = profile.t1;
+  double t_end2 = t_end1 + profile.t2;
+  double t_end3 = t_end2 + profile.t3;
+  double t_end4 = t_end3 + profile.t4;
+  double t_end5 = t_end4 + profile.t5;
+  double t_end6 = t_end5 + profile.t6;
+
+  auto processSegment = [&](double dt, double jerk) {
+    v += a * dt + 0.5 * jerk * dt * dt;
+    a += jerk * dt;
+  };
+
+  if (t <= t_end1) {
+    v += a * t + 0.5 * j * t * t;
+    return v;
+  }
+  processSegment(profile.t1, j);
+
+  if (t <= t_end2) {
+    v += a * (t - t_end1);
+    return v;
+  }
+  processSegment(profile.t2, 0.0);
+
+  if (t <= t_end3) {
+    double dt = t - t_end2;
+    v += a * dt + 0.5 * (-j) * dt * dt;
+    return v;
+  }
+  processSegment(profile.t3, -j);
+
+  if (t <= t_end4) {
+    v += a * (t - t_end3);
+    return v;
+  }
+  processSegment(profile.t4, 0.0);
+
+  if (t <= t_end5) {
+    double dt = t - t_end4;
+    v += a * dt + 0.5 * (-j) * dt * dt;
+    return v;
+  }
+  processSegment(profile.t5, -j);
+
+  if (t <= t_end6) {
+    v += a * (t - t_end5);
+    return v;
+  }
+  processSegment(profile.t6, 0.0);
+
+  double dt = t - t_end6;
+  v += a * dt + 0.5 * j * dt * dt;
+  return v;
+}
+
+double SCurve::getAccelerationAtTime(const SCurveProfile& profile, double t) const
+{
+  if (t <= 0 || t >= profile.total_time) return 0.0;
+
+  double j = profile.j_max * profile.direction;
+  double a = profile.a0;
+
+  // 时间边界
+  double t_end1 = profile.t1;
+  double t_end2 = t_end1 + profile.t2;
+  double t_end3 = t_end2 + profile.t3;
+  double t_end4 = t_end3 + profile.t4;
+  double t_end5 = t_end4 + profile.t5;
+  double t_end6 = t_end5 + profile.t6;
+
+  if (t <= t_end1) {
+    return a + j * t;
+  }
+  a += j * profile.t1;
+
+  if (t <= t_end2) {
+    return a;
+  }
+
+  if (t <= t_end3) {
+    return a + (-j) * (t - t_end2);
+  }
+  a += (-j) * profile.t3;
+
+  if (t <= t_end4) {
+    return a;  // Should be ~0
+  }
+
+  if (t <= t_end5) {
+    return a + (-j) * (t - t_end4);
+  }
+  a += (-j) * profile.t5;
+
+  if (t <= t_end6) {
+    return a;
+  }
+
+  return a + j * (t - t_end6);
+}
+
+double SCurve::getJerkAtTime(const SCurveProfile& profile, double t) const
+{
+  if (t <= 0.0 || t >= profile.total_time) {
+    return 0.0;
+  }
+  return computeSegmentJerk(profile, t);
+}
+
+void SCurve::generateTrajectory(const SCurveProfile& profile, double dt,
+                                          std::vector<double>& positions,
+                                          std::vector<double>& velocities,
+                                          std::vector<double>& accelerations) const
+{
+  positions.clear();
+  velocities.clear();
+  accelerations.clear();
+
+  if (profile.total_time <= 0 || dt <= 0) {
+    positions.push_back(profile.p0);
+    velocities.push_back(profile.v0);
+    accelerations.push_back(profile.a0);
+    return;
+  }
+
+  int num_points = static_cast<int>(std::ceil(profile.total_time / dt)) + 1;
+  positions.reserve(num_points);
+  velocities.reserve(num_points);
+  accelerations.reserve(num_points);
+
+  for (double t = 0; t <= profile.total_time; t += dt) {
+    positions.push_back(getPositionAtTime(profile, t));
+    velocities.push_back(getVelocityAtTime(profile, t));
+    accelerations.push_back(getAccelerationAtTime(profile, t));
+  }
+
+  // 确保包含最终点
+  if (positions.empty() ||
+      std::abs(positions.back() - (profile.p0 + profile.distance * profile.direction)) > EPSILON) {
+    positions.push_back(profile.p0 + profile.distance * profile.direction);
+    velocities.push_back(profile.vf);
+    accelerations.push_back(0.0);
+  }
+}
+
+}

applications/common/type/common_type.h

@@ -0,0 +1,9 @@
+//
+// Created by Administrator on 2026/4/22.
+//
+
+#ifndef RTTHREAD_COMMON_TYPE_H
+#define RTTHREAD_COMMON_TYPE_H
+#define PI (3.14159265358979323846)
+
+#endif //RTTHREAD_COMMON_TYPE_H

applications/common/type/servo_types.h

@@ -0,0 +1,13 @@
+//
+// Created by Administrator on 2026/4/22.
+//
+
+#ifndef RTTHREAD_SERVO_TYPES_H
+#define RTTHREAD_SERVO_TYPES_H
+enum class MotionMode
+{
+    Immediate,   // 直接到达
+    SCurve       // S 曲线
+};
+
+#endif //RTTHREAD_SERVO_TYPES_H

applications/erpc/SConscript

@@ -22,13 +22,13 @@ if GetDepend(['ERPC_MODE_CLIENT']):
 gen_dir = p('proto', 'generated')
 
 # interface 建议始终编译（两端都可能需要）
-src += [p('proto', 'generated', 'face_servo_interface.cpp')]
+src += [p('proto', 'generated', 'servo_service_interface.cpp')]
 src += [p('common', 'src', 'erpc_error_handler.cpp')]
 if GetDepend(['ERPC_MODE_SERVER']):
-    src += [p('proto', 'generated', 'face_servo_server.cpp')]
+    src += [p('proto', 'generated', 'servo_service_server.cpp')]
 
 if GetDepend(['ERPC_MODE_CLIENT']):
-    src += [p('proto', 'generated', 'face_servo_client.cpp')]
+    src += [p('proto', 'generated', 'servo_service_client.cpp')]
 
 CPPPATH = [
     cwd,

applications/erpc/proto/face_servo.erpc

@@ -1,30 +0,0 @@
-// face_servo.erpc
-// 生成命令  .\erpcgen.exe -o .\generated\ -g c .\face_servo.erpc
-//  .\erpcgen.exe -g py .\face_servo.erpc
-
-program face_servo
-
-// 舵机ID字符串最大长度
-const int32 kServoIdMaxLen = 32
-
-// 最多支持的舵机数量
-const int32 kMaxServoCount = 48
-
-// 按 ID 设置角度的命令
-struct ServoCmd {
-    string id @max_length(kServoIdMaxLen)
-    float  angle_deg
-}
-
-interface FaceServo
-{
-    // 接口1：一次性设置所有舵机角度（可变长度）
-    // angles_deg 的顺序由固件映射表顺序定义
-    setAllAngles(list<float> angles_deg @max_length(kMaxServoCount)) -> int32
-
-    // 接口2：按字符串ID批量设置角度（可变长度）
-    setAngles(list<ServoCmd> cmds @max_length(kMaxServoCount)) -> int32
-
-    // 单个：按字符串ID设置角度
-    setAngle(string id @max_length(kServoIdMaxLen), float angle_deg) -> int32
-}

applications/erpc/proto/generated/c_face_servo_client.cpp

@@ -1,69 +0,0 @@
-/*
- * Generated by erpcgen 1.14.0 on Wed Jan 14 16:06:40 2026.
- *
- * AUTOGENERATED - DO NOT EDIT
- */
-
-
-#include "c_face_servo_client.h"
-#include "face_servo_client.hpp"
-#include "erpc_manually_constructed.hpp"
-
-using namespace erpc;
-using namespace std;
-using namespace erpcShim;
-
-
-#if ERPC_ALLOCATION_POLICY == ERPC_ALLOCATION_POLICY_DYNAMIC
-FaceServo_client *s_FaceServo_client = nullptr;
-#else
-ERPC_MANUALLY_CONSTRUCTED_STATIC(FaceServo_client, s_FaceServo_client);
-#endif
-
-int32_t setAllAngles(const list_float_1_t * angles_deg)
-{
-    int32_t result;
-    result = s_FaceServo_client->setAllAngles(angles_deg);
-
-    return result;
-}
-
-int32_t setAngles(const list_ServoCmd_1_t * cmds)
-{
-    int32_t result;
-    result = s_FaceServo_client->setAngles(cmds);
-
-    return result;
-}
-
-int32_t setAngle(const char * id, float angle_deg)
-{
-    int32_t result;
-    result = s_FaceServo_client->setAngle(id, angle_deg);
-
-    return result;
-}
-
-void initFaceServo_client(erpc_client_t client)
-{
-#if ERPC_ALLOCATION_POLICY == ERPC_ALLOCATION_POLICY_DYNAMIC
-    erpc_assert(s_FaceServo_client == nullptr);
-    s_FaceServo_client = new FaceServo_client(reinterpret_cast<ClientManager *>(client));
-#else
-    erpc_assert(!s_FaceServo_client.isUsed());
-    s_FaceServo_client.construct(reinterpret_cast<ClientManager *>(client));
-#endif
-}
-
-void deinitFaceServo_client(void)
-{
-#if ERPC_ALLOCATION_POLICY == ERPC_ALLOCATION_POLICY_DYNAMIC
-    if (s_FaceServo_client != nullptr)
-    {
-        delete s_FaceServo_client;
-        s_FaceServo_client = nullptr;
-    }
-#else
-    s_FaceServo_client.destroy();
-#endif
-}

applications/erpc/proto/generated/c_face_servo_client.h

@@ -1,51 +0,0 @@
-/*
- * Generated by erpcgen 1.14.0 on Wed Jan 14 16:06:40 2026.
- *
- * AUTOGENERATED - DO NOT EDIT
- */
-
-
-#if !defined(_c_face_servo_client_h_)
-#define _c_face_servo_client_h_
-
-#include "face_servo_common.h"
-#include "erpc_client_manager.h"
-
-#if defined(__cplusplus)
-extern "C"
-{
-#endif
-
-#if !defined(ERPC_FUNCTIONS_DEFINITIONS)
-#define ERPC_FUNCTIONS_DEFINITIONS
-
-
-/*! @brief FaceServo identifiers */
-enum _FaceServo_ids
-{
-    kFaceServo_service_id = 1,
-    kFaceServo_setAllAngles_id = 1,
-    kFaceServo_setAngles_id = 2,
-    kFaceServo_setAngle_id = 3,
-};
-
-//! @name FaceServo
-//@{
-int32_t setAllAngles(const list_float_1_t * angles_deg);
-
-int32_t setAngles(const list_ServoCmd_1_t * cmds);
-
-int32_t setAngle(const char * id, float angle_deg);
-//@}
-
-#endif // ERPC_FUNCTIONS_DEFINITIONS
-
-void initFaceServo_client(erpc_client_t client);
-
-void deinitFaceServo_client(void);
-
-#if defined(__cplusplus)
-}
-#endif
-
-#endif // _c_face_servo_client_h_

applications/erpc/proto/generated/c_face_servo_server.cpp

@@ -1,89 +0,0 @@
-/*
- * Generated by erpcgen 1.14.0 on Wed Jan 14 16:06:40 2026.
- *
- * AUTOGENERATED - DO NOT EDIT
- */
-
-
-#include <new>
-#include "c_face_servo_server.h"
-#include "face_servo_server.hpp"
-#include "erpc_manually_constructed.hpp"
-
-using namespace erpc;
-using namespace std;
-using namespace erpcShim;
-
-
-class FaceServo_server: public FaceServo_interface
-{
-    public:
-        virtual ~FaceServo_server() {};
-
-
-        int32_t setAllAngles(const list_float_1_t * angles_deg)
-        {
-            int32_t result;
-            result = ::setAllAngles(angles_deg);
-
-            return result;
-        }
-
-        int32_t setAngles(const list_ServoCmd_1_t * cmds)
-        {
-            int32_t result;
-            result = ::setAngles(cmds);
-
-            return result;
-        }
-
-        int32_t setAngle(const char * id, float angle_deg)
-        {
-            int32_t result;
-            result = ::setAngle(id, angle_deg);
-
-            return result;
-        }
-};
-
-ERPC_MANUALLY_CONSTRUCTED_STATIC(FaceServo_service, s_FaceServo_service);
-ERPC_MANUALLY_CONSTRUCTED_STATIC(FaceServo_server, s_FaceServo_server);
-
-erpc_service_t create_FaceServo_service(void)
-{
-    erpc_service_t service;
-
-#if ERPC_ALLOCATION_POLICY == ERPC_ALLOCATION_POLICY_DYNAMIC
-    service = new (nothrow) FaceServo_service(new (nothrow)FaceServo_server());
-#else
-    if (s_FaceServo_service.isUsed())
-    {
-        service = NULL;
-    }
-    else
-    {
-        s_FaceServo_server.construct();
-        s_FaceServo_service.construct(s_FaceServo_server.get());
-        service = s_FaceServo_service.get();
-    }
-#endif
-
-    return service;
-}
-
-void destroy_FaceServo_service(erpc_service_t service)
-{
-#if ERPC_ALLOCATION_POLICY == ERPC_ALLOCATION_POLICY_DYNAMIC
-    if (service)
-    {
-        delete (FaceServo_server *)(((FaceServo_service *)service)->getHandler());
-        delete (FaceServo_service *)service;
-    }
-#else
-    (void)service;
-    erpc_assert(service == s_FaceServo_service.get());
-    s_FaceServo_service.destroy();
-    s_FaceServo_server.destroy();
-#endif
-}
-

applications/erpc/proto/generated/c_face_servo_server.h

@@ -1,56 +0,0 @@
-/*
- * Generated by erpcgen 1.14.0 on Wed Jan 14 16:06:40 2026.
- *
- * AUTOGENERATED - DO NOT EDIT
- */
-
-
-#if !defined(_c_face_servo_server_h_)
-#define _c_face_servo_server_h_
-
-#include "face_servo_common.h"
-
-#if defined(__cplusplus)
-extern "C"
-{
-#endif
-
-typedef void * erpc_service_t;
-
-#if !defined(ERPC_FUNCTIONS_DEFINITIONS)
-#define ERPC_FUNCTIONS_DEFINITIONS
-
-
-/*! @brief FaceServo identifiers */
-enum _FaceServo_ids
-{
-    kFaceServo_service_id = 1,
-    kFaceServo_setAllAngles_id = 1,
-    kFaceServo_setAngles_id = 2,
-    kFaceServo_setAngle_id = 3,
-};
-
-//! @name FaceServo
-//@{
-int32_t setAllAngles(const list_float_1_t * angles_deg);
-
-int32_t setAngles(const list_ServoCmd_1_t * cmds);
-
-int32_t setAngle(const char * id, float angle_deg);
-//@}
-
-
-#endif // ERPC_FUNCTIONS_DEFINITIONS
-
-/*! @brief Return FaceServo_service service object. */
-erpc_service_t create_FaceServo_service(void);
-
-/*! @brief Destroy FaceServo_service service object. */
-void destroy_FaceServo_service(erpc_service_t service);
-
-
-#if defined(__cplusplus)
-}
-#endif
-
-#endif // _c_face_servo_server_h_

applications/erpc/proto/generated/c_servo_service_client.cpp

@@ -0,0 +1,85 @@
+/*
+ * Generated by erpcgen 1.14.0 on Wed Apr 22 16:48:46 2026.
+ *
+ * AUTOGENERATED - DO NOT EDIT
+ */
+
+
+#include "c_servo_service_client.h"
+#include "servo_service_client.hpp"
+#include "erpc_manually_constructed.hpp"
+
+using namespace erpc;
+using namespace std;
+using namespace erpcShim;
+
+
+#if ERPC_ALLOCATION_POLICY == ERPC_ALLOCATION_POLICY_DYNAMIC
+servo_service_client *s_servo_service_client = nullptr;
+#else
+ERPC_MANUALLY_CONSTRUCTED_STATIC(servo_service_client, s_servo_service_client);
+#endif
+
+bool setConstraints(const char * id, float max_velocity_rad, float max_acceleration_rad, float max_jerk_rad)
+{
+    bool result;
+    result = s_servo_service_client->setConstraints(id, max_velocity_rad, max_acceleration_rad, max_jerk_rad);
+
+    return result;
+}
+
+bool setMode(RpcMotionMode mode)
+{
+    bool result;
+    result = s_servo_service_client->setMode(mode);
+
+    return result;
+}
+
+bool setUpdatePeriodMs(uint32_t ms)
+{
+    bool result;
+    result = s_servo_service_client->setUpdatePeriodMs(ms);
+
+    return result;
+}
+
+int32_t move(const list_ServoCmd_1_t * cmds)
+{
+    int32_t result;
+    result = s_servo_service_client->move(cmds);
+
+    return result;
+}
+
+int32_t moveJ(const list_float_1_t * angles_rad)
+{
+    int32_t result;
+    result = s_servo_service_client->moveJ(angles_rad);
+
+    return result;
+}
+
+void initservo_service_client(erpc_client_t client)
+{
+#if ERPC_ALLOCATION_POLICY == ERPC_ALLOCATION_POLICY_DYNAMIC
+    erpc_assert(s_servo_service_client == nullptr);
+    s_servo_service_client = new servo_service_client(reinterpret_cast<ClientManager *>(client));
+#else
+    erpc_assert(!s_servo_service_client.isUsed());
+    s_servo_service_client.construct(reinterpret_cast<ClientManager *>(client));
+#endif
+}
+
+void deinitservo_service_client(void)
+{
+#if ERPC_ALLOCATION_POLICY == ERPC_ALLOCATION_POLICY_DYNAMIC
+    if (s_servo_service_client != nullptr)
+    {
+        delete s_servo_service_client;
+        s_servo_service_client = nullptr;
+    }
+#else
+    s_servo_service_client.destroy();
+#endif
+}

applications/erpc/proto/generated/c_servo_service_client.h

@@ -0,0 +1,57 @@
+/*
+ * Generated by erpcgen 1.14.0 on Wed Apr 22 16:48:46 2026.
+ *
+ * AUTOGENERATED - DO NOT EDIT
+ */
+
+
+#if !defined(_c_servo_service_client_h_)
+#define _c_servo_service_client_h_
+
+#include "servo_service_common.h"
+#include "erpc_client_manager.h"
+
+#if defined(__cplusplus)
+extern "C"
+{
+#endif
+
+#if !defined(ERPC_FUNCTIONS_DEFINITIONS)
+#define ERPC_FUNCTIONS_DEFINITIONS
+
+
+/*! @brief servo_service identifiers */
+enum _servo_service_ids
+{
+    kservo_service_service_id = 1,
+    kservo_service_setConstraints_id = 1,
+    kservo_service_setMode_id = 2,
+    kservo_service_setUpdatePeriodMs_id = 3,
+    kservo_service_move_id = 4,
+    kservo_service_moveJ_id = 5,
+};
+
+//! @name servo_service
+//@{
+bool setConstraints(const char * id, float max_velocity_rad, float max_acceleration_rad, float max_jerk_rad);
+
+bool setMode(RpcMotionMode mode);
+
+bool setUpdatePeriodMs(uint32_t ms);
+
+int32_t move(const list_ServoCmd_1_t * cmds);
+
+int32_t moveJ(const list_float_1_t * angles_rad);
+//@}
+
+#endif // ERPC_FUNCTIONS_DEFINITIONS
+
+void initservo_service_client(erpc_client_t client);
+
+void deinitservo_service_client(void);
+
+#if defined(__cplusplus)
+}
+#endif
+
+#endif // _c_servo_service_client_h_

applications/erpc/proto/generated/c_servo_service_server.cpp

@@ -0,0 +1,105 @@
+/*
+ * Generated by erpcgen 1.14.0 on Wed Apr 22 16:48:46 2026.
+ *
+ * AUTOGENERATED - DO NOT EDIT
+ */
+
+
+#include <new>
+#include "c_servo_service_server.h"
+#include "servo_service_server.hpp"
+#include "erpc_manually_constructed.hpp"
+
+using namespace erpc;
+using namespace std;
+using namespace erpcShim;
+
+
+class servo_service_server: public servo_service_interface
+{
+    public:
+        virtual ~servo_service_server() {};
+
+
+        bool setConstraints(const char * id, float max_velocity_rad, float max_acceleration_rad, float max_jerk_rad)
+        {
+            bool result;
+            result = ::setConstraints(id, max_velocity_rad, max_acceleration_rad, max_jerk_rad);
+
+            return result;
+        }
+
+        bool setMode(RpcMotionMode mode)
+        {
+            bool result;
+            result = ::setMode(mode);
+
+            return result;
+        }
+
+        bool setUpdatePeriodMs(uint32_t ms)
+        {
+            bool result;
+            result = ::setUpdatePeriodMs(ms);
+
+            return result;
+        }
+
+        int32_t move(const list_ServoCmd_1_t * cmds)
+        {
+            int32_t result;
+            result = ::move(cmds);
+
+            return result;
+        }
+
+        int32_t moveJ(const list_float_1_t * angles_rad)
+        {
+            int32_t result;
+            result = ::moveJ(angles_rad);
+
+            return result;
+        }
+};
+
+ERPC_MANUALLY_CONSTRUCTED_STATIC(servo_service_service, s_servo_service_service);
+ERPC_MANUALLY_CONSTRUCTED_STATIC(servo_service_server, s_servo_service_server);
+
+erpc_service_t create_servo_service_service(void)
+{
+    erpc_service_t service;
+
+#if ERPC_ALLOCATION_POLICY == ERPC_ALLOCATION_POLICY_DYNAMIC
+    service = new (nothrow) servo_service_service(new (nothrow)servo_service_server());
+#else
+    if (s_servo_service_service.isUsed())
+    {
+        service = NULL;
+    }
+    else
+    {
+        s_servo_service_server.construct();
+        s_servo_service_service.construct(s_servo_service_server.get());
+        service = s_servo_service_service.get();
+    }
+#endif
+
+    return service;
+}
+
+void destroy_servo_service_service(erpc_service_t service)
+{
+#if ERPC_ALLOCATION_POLICY == ERPC_ALLOCATION_POLICY_DYNAMIC
+    if (service)
+    {
+        delete (servo_service_server *)(((servo_service_service *)service)->getHandler());
+        delete (servo_service_service *)service;
+    }
+#else
+    (void)service;
+    erpc_assert(service == s_servo_service_service.get());
+    s_servo_service_service.destroy();
+    s_servo_service_server.destroy();
+#endif
+}
+

applications/erpc/proto/generated/c_servo_service_server.h

@@ -0,0 +1,62 @@
+/*
+ * Generated by erpcgen 1.14.0 on Wed Apr 22 16:48:46 2026.
+ *
+ * AUTOGENERATED - DO NOT EDIT
+ */
+
+
+#if !defined(_c_servo_service_server_h_)
+#define _c_servo_service_server_h_
+
+#include "servo_service_common.h"
+
+#if defined(__cplusplus)
+extern "C"
+{
+#endif
+
+typedef void * erpc_service_t;
+
+#if !defined(ERPC_FUNCTIONS_DEFINITIONS)
+#define ERPC_FUNCTIONS_DEFINITIONS
+
+
+/*! @brief servo_service identifiers */
+enum _servo_service_ids
+{
+    kservo_service_service_id = 1,
+    kservo_service_setConstraints_id = 1,
+    kservo_service_setMode_id = 2,
+    kservo_service_setUpdatePeriodMs_id = 3,
+    kservo_service_move_id = 4,
+    kservo_service_moveJ_id = 5,
+};
+
+//! @name servo_service
+//@{
+bool setConstraints(const char * id, float max_velocity_rad, float max_acceleration_rad, float max_jerk_rad);
+
+bool setMode(RpcMotionMode mode);
+
+bool setUpdatePeriodMs(uint32_t ms);
+
+int32_t move(const list_ServoCmd_1_t * cmds);
+
+int32_t moveJ(const list_float_1_t * angles_rad);
+//@}
+
+
+#endif // ERPC_FUNCTIONS_DEFINITIONS
+
+/*! @brief Return servo_service_service service object. */
+erpc_service_t create_servo_service_service(void);
+
+/*! @brief Destroy servo_service_service service object. */
+void destroy_servo_service_service(erpc_service_t service);
+
+
+#if defined(__cplusplus)
+}
+#endif
+
+#endif // _c_servo_service_server_h_

applications/erpc/proto/generated/face_servo_client.hpp

@@ -1,38 +0,0 @@
-/*
- * Generated by erpcgen 1.14.0 on Wed Jan 14 16:06:40 2026.
- *
- * AUTOGENERATED - DO NOT EDIT
- */
-
-
-#if !defined(_face_servo_client_hpp_)
-#define _face_servo_client_hpp_
-
-#include "face_servo_interface.hpp"
-
-#include "erpc_client_manager.h"
-
-namespace erpcShim
-{
-
-class FaceServo_client: public FaceServo_interface
-{
-    public:
-        FaceServo_client(erpc::ClientManager *manager);
-
-        virtual ~FaceServo_client();
-
-        virtual int32_t setAllAngles(const list_float_1_t * angles_deg);
-
-        virtual int32_t setAngles(const list_ServoCmd_1_t * cmds);
-
-        virtual int32_t setAngle(const char * id, float angle_deg);
-
-    protected:
-        erpc::ClientManager *m_clientManager;
-};
-
-} // erpcShim
-
-
-#endif // _face_servo_client_hpp_

applications/erpc/proto/generated/face_servo_interface.hpp

@@ -1,38 +0,0 @@
-/*
- * Generated by erpcgen 1.14.0 on Wed Jan 14 16:06:40 2026.
- *
- * AUTOGENERATED - DO NOT EDIT
- */
-
-
-#if !defined(_face_servo_interface_hpp_)
-#define _face_servo_interface_hpp_
-
-#include "face_servo_common.hpp"
-
-namespace erpcShim
-{
-
-
-// Abstract base class for FaceServo
-class FaceServo_interface
-{
-    public:
-        static const uint8_t m_serviceId = 1;
-        static const uint8_t m_setAllAnglesId = 1;
-        static const uint8_t m_setAnglesId = 2;
-        static const uint8_t m_setAngleId = 3;
-
-        virtual ~FaceServo_interface(void);
-
-        virtual int32_t setAllAngles(const list_float_1_t * angles_deg) = 0;
-
-        virtual int32_t setAngles(const list_ServoCmd_1_t * cmds) = 0;
-
-        virtual int32_t setAngle(const char * id, float angle_deg) = 0;
-private:
-};
-} // erpcShim
-
-
-#endif // _face_servo_interface_hpp_

applications/erpc/proto/generated/face_servo_server.hpp

@@ -1,55 +0,0 @@
-/*
- * Generated by erpcgen 1.14.0 on Wed Jan 14 16:06:40 2026.
- *
- * AUTOGENERATED - DO NOT EDIT
- */
-
-
-#if !defined(_face_servo_server_hpp_)
-#define _face_servo_server_hpp_
-
-#include "face_servo_interface.hpp"
-
-#include "erpc_server.hpp"
-#include "erpc_codec.hpp"
-
-#if 11400 != ERPC_VERSION_NUMBER
-#error "The generated shim code version is different to the rest of eRPC code."
-#endif
-
-
-namespace erpcShim
-{
-
-/*!
- * @brief Service subclass for FaceServo.
- */
-class FaceServo_service : public erpc::Service
-{
-public:
-    FaceServo_service(FaceServo_interface *_FaceServo_interface);
-
-    virtual ~FaceServo_service();
-
-    /*! @brief return service interface handler. */
-    FaceServo_interface* getHandler(void);
-
-    /*! @brief Call the correct server shim based on method unique ID. */
-    virtual erpc_status_t handleInvocation(uint32_t methodId, uint32_t sequence, erpc::Codec * codec, erpc::MessageBufferFactory *messageFactory, erpc::Transport * transport);
-
-private:
-    FaceServo_interface *m_handler;
-    /*! @brief Server shim for setAllAngles of FaceServo interface. */
-    erpc_status_t setAllAngles_shim(erpc::Codec * codec, erpc::MessageBufferFactory *messageFactory, erpc::Transport * transport, uint32_t sequence);
-
-    /*! @brief Server shim for setAngles of FaceServo interface. */
-    erpc_status_t setAngles_shim(erpc::Codec * codec, erpc::MessageBufferFactory *messageFactory, erpc::Transport * transport, uint32_t sequence);
-
-    /*! @brief Server shim for setAngle of FaceServo interface. */
-    erpc_status_t setAngle_shim(erpc::Codec * codec, erpc::MessageBufferFactory *messageFactory, erpc::Transport * transport, uint32_t sequence);
-};
-
-} // erpcShim
-
-
-#endif // _face_servo_server_hpp_

applications/erpc/proto/generated/servo_service_client.cpp

@@ -1,5 +1,5 @@
 /*
- * Generated by erpcgen 1.14.0 on Wed Jan 14 16:06:40 2026.
+ * Generated by erpcgen 1.14.0 on Wed Apr 22 16:48:46 2026.
  *
  * AUTOGENERATED - DO NOT EDIT
  */
@@ -9,7 +9,7 @@
 #include "erpc_port.h"
 #endif
 #include "erpc_codec.hpp"
-#include "face_servo_client.hpp"
+#include "servo_service_client.hpp"
 #include "erpc_manually_constructed.hpp"
 
 #if 11400 != ERPC_VERSION_NUMBER
@@ -20,45 +20,15 @@ using namespace erpc;
 using namespace std;
 using namespace erpcShim;
 
-
-// Constant variable definitions
-#if defined(__MINGW32__)
-__declspec( selectany )
-#else
-#pragma weak kServoIdMaxLen
-#endif
-extern const int32_t kServoIdMaxLen = 32;
-#if defined(__MINGW32__)
-__declspec( selectany )
-#else
-#pragma weak kMaxServoCount
-#endif
-extern const int32_t kMaxServoCount = 48;
-
-//! @brief Function to write struct list_float_1_t
-static void write_list_float_1_t_struct(erpc::Codec * codec, const list_float_1_t * data);
-
 //! @brief Function to write struct ServoCmd
 static void write_ServoCmd_struct(erpc::Codec * codec, const ServoCmd * data);
 
 //! @brief Function to write struct list_ServoCmd_1_t
 static void write_list_ServoCmd_1_t_struct(erpc::Codec * codec, const list_ServoCmd_1_t * data);
 
+//! @brief Function to write struct list_float_1_t
+static void write_list_float_1_t_struct(erpc::Codec * codec, const list_float_1_t * data);
 
-// Write struct list_float_1_t function implementation
-static void write_list_float_1_t_struct(erpc::Codec * codec, const list_float_1_t * data)
-{
-    if(NULL == data)
-    {
-        return;
-    }
-
-    codec->startWriteList(data->elementsCount);
-    for (uint32_t listCount = 0U; listCount < data->elementsCount; ++listCount)
-    {
-        codec->write(data->elements[listCount]);
-    }
-}
 
 // Write struct ServoCmd function implementation
 static void write_ServoCmd_struct(erpc::Codec * codec, const ServoCmd * data)
@@ -74,7 +44,7 @@ static void write_ServoCmd_struct(erpc::Codec * codec, const ServoCmd * data)
         codec->writeString(id_len, (const char*)data->id);
     }
 
-    codec->write(data->angle_deg);
+    codec->write(data->angle_rad);
 }
 
 // Write struct list_ServoCmd_1_t function implementation
@@ -92,24 +62,39 @@ static void write_list_ServoCmd_1_t_struct(erpc::Codec * codec, const list_Servo
     }
 }
 
+// Write struct list_float_1_t function implementation
+static void write_list_float_1_t_struct(erpc::Codec * codec, const list_float_1_t * data)
+{
+    if(NULL == data)
+    {
+        return;
+    }
+
+    codec->startWriteList(data->elementsCount);
+    for (uint32_t listCount = 0U; listCount < data->elementsCount; ++listCount)
+    {
+        codec->write(data->elements[listCount]);
+    }
+}
 
 
 
-FaceServo_client::FaceServo_client(ClientManager *manager)
+
+servo_service_client::servo_service_client(ClientManager *manager)
 :m_clientManager(manager)
 {
 }
 
-FaceServo_client::~FaceServo_client()
+servo_service_client::~servo_service_client()
 {
 }
 
-// FaceServo interface setAllAngles function client shim.
+// servo_service interface setConstraints function client shim.
-int32_t FaceServo_client::setAllAngles(const list_float_1_t * angles_deg)
+bool servo_service_client::setConstraints(const char * id, float max_velocity_rad, float max_acceleration_rad, float max_jerk_rad)
 {
     erpc_status_t err = kErpcStatus_Success;
 
-    int32_t result;
+    bool result;
 
 #if ERPC_PRE_POST_ACTION
     pre_post_action_cb preCB = m_clientManager->getPreCB();
@@ -131,9 +116,19 @@ int32_t FaceServo_client::setAllAngles(const list_float_1_t * angles_deg)
     }
     else
     {
-        codec->startWriteMessage(message_type_t::kInvocationMessage, m_serviceId, m_setAllAnglesId, request.getSequence());
+        codec->startWriteMessage(message_type_t::kInvocationMessage, m_serviceId, m_setConstraintsId, request.getSequence());
 
-        write_list_float_1_t_struct(codec, angles_deg);
+        {
+            uint32_t id_len = strlen((const char*)id);
+
+            codec->writeString(id_len, (const char*)id);
+        }
+
+        codec->write(max_velocity_rad);
+
+        codec->write(max_acceleration_rad);
+
+        codec->write(max_jerk_rad);
 
         // Send message to server
         // Codec status is checked inside this function.
@@ -148,7 +143,7 @@ int32_t FaceServo_client::setAllAngles(const list_float_1_t * angles_deg)
     m_clientManager->releaseRequest(request);
 
     // Invoke error handler callback function
-    m_clientManager->callErrorHandler(err, m_setAllAnglesId);
+    m_clientManager->callErrorHandler(err, m_setConstraintsId);
 
 #if ERPC_PRE_POST_ACTION
     pre_post_action_cb postCB = m_clientManager->getPostCB();
@@ -161,14 +156,140 @@ int32_t FaceServo_client::setAllAngles(const list_float_1_t * angles_deg)
 
     if (err != kErpcStatus_Success)
     {
-        result = -1;
+        result = false;
     }
 
     return result;
 }
 
-// FaceServo interface setAngles function client shim.
+// servo_service interface setMode function client shim.
-int32_t FaceServo_client::setAngles(const list_ServoCmd_1_t * cmds)
+bool servo_service_client::setMode(RpcMotionMode mode)
+{
+    erpc_status_t err = kErpcStatus_Success;
+
+    bool result;
+
+#if ERPC_PRE_POST_ACTION
+    pre_post_action_cb preCB = m_clientManager->getPreCB();
+    if (preCB)
+    {
+        preCB();
+    }
+#endif
+
+    // Get a new request.
+    RequestContext request = m_clientManager->createRequest(false);
+
+    // Encode the request.
+    Codec * codec = request.getCodec();
+
+    if (codec == NULL)
+    {
+        err = kErpcStatus_MemoryError;
+    }
+    else
+    {
+        codec->startWriteMessage(message_type_t::kInvocationMessage, m_serviceId, m_setModeId, request.getSequence());
+
+        codec->write(static_cast<int32_t>(mode));
+
+        // Send message to server
+        // Codec status is checked inside this function.
+        m_clientManager->performRequest(request);
+
+        codec->read(result);
+
+        err = codec->getStatus();
+    }
+
+    // Dispose of the request.
+    m_clientManager->releaseRequest(request);
+
+    // Invoke error handler callback function
+    m_clientManager->callErrorHandler(err, m_setModeId);
+
+#if ERPC_PRE_POST_ACTION
+    pre_post_action_cb postCB = m_clientManager->getPostCB();
+    if (postCB)
+    {
+        postCB();
+    }
+#endif
+
+
+    if (err != kErpcStatus_Success)
+    {
+        result = false;
+    }
+
+    return result;
+}
+
+// servo_service interface setUpdatePeriodMs function client shim.
+bool servo_service_client::setUpdatePeriodMs(uint32_t ms)
+{
+    erpc_status_t err = kErpcStatus_Success;
+
+    bool result;
+
+#if ERPC_PRE_POST_ACTION
+    pre_post_action_cb preCB = m_clientManager->getPreCB();
+    if (preCB)
+    {
+        preCB();
+    }
+#endif
+
+    // Get a new request.
+    RequestContext request = m_clientManager->createRequest(false);
+
+    // Encode the request.
+    Codec * codec = request.getCodec();
+
+    if (codec == NULL)
+    {
+        err = kErpcStatus_MemoryError;
+    }
+    else
+    {
+        codec->startWriteMessage(message_type_t::kInvocationMessage, m_serviceId, m_setUpdatePeriodMsId, request.getSequence());
+
+        codec->write(ms);
+
+        // Send message to server
+        // Codec status is checked inside this function.
+        m_clientManager->performRequest(request);
+
+        codec->read(result);
+
+        err = codec->getStatus();
+    }
+
+    // Dispose of the request.
+    m_clientManager->releaseRequest(request);
+
+    // Invoke error handler callback function
+    m_clientManager->callErrorHandler(err, m_setUpdatePeriodMsId);
+
+#if ERPC_PRE_POST_ACTION
+    pre_post_action_cb postCB = m_clientManager->getPostCB();
+    if (postCB)
+    {
+        postCB();
+    }
+#endif
+
+
+    if (err != kErpcStatus_Success)
+    {
+        result = false;
+    }
+
+    return result;
+}
+
+// servo_service interface move function client shim.
+int32_t servo_service_client::move(const list_ServoCmd_1_t * cmds)
 {
     erpc_status_t err = kErpcStatus_Success;
 
@@ -194,7 +315,7 @@ int32_t FaceServo_client::setAngles(const list_ServoCmd_1_t * cmds)
     }
     else
     {
-        codec->startWriteMessage(message_type_t::kInvocationMessage, m_serviceId, m_setAnglesId, request.getSequence());
+        codec->startWriteMessage(message_type_t::kInvocationMessage, m_serviceId, m_moveId, request.getSequence());
 
         write_list_ServoCmd_1_t_struct(codec, cmds);
 
@@ -211,7 +332,7 @@ int32_t FaceServo_client::setAngles(const list_ServoCmd_1_t * cmds)
     m_clientManager->releaseRequest(request);
 
     // Invoke error handler callback function
-    m_clientManager->callErrorHandler(err, m_setAnglesId);
+    m_clientManager->callErrorHandler(err, m_moveId);
 
 #if ERPC_PRE_POST_ACTION
     pre_post_action_cb postCB = m_clientManager->getPostCB();
@@ -230,8 +351,8 @@ int32_t FaceServo_client::setAngles(const list_ServoCmd_1_t * cmds)
     return result;
 }
 
-// FaceServo interface setAngle function client shim.
+// servo_service interface moveJ function client shim.
-int32_t FaceServo_client::setAngle(const char * id, float angle_deg)
+int32_t servo_service_client::moveJ(const list_float_1_t * angles_rad)
 {
     erpc_status_t err = kErpcStatus_Success;
 
@@ -257,18 +378,9 @@ int32_t FaceServo_client::setAngle(const char * id, float angle_deg)
     }
     else
     {
-        codec->startWriteMessage(message_type_t::kInvocationMessage, m_serviceId, m_setAngleId, request.getSequence());
+        codec->startWriteMessage(message_type_t::kInvocationMessage, m_serviceId, m_moveJId, request.getSequence());
 
-        {
+        write_list_float_1_t_struct(codec, angles_rad);
-            uint32_t id_len = strlen((const char*)id);
-
-            erpc_assert(kServoIdMaxLen >= 0);
-            erpc_assert(id_len <= static_cast<uint32_t>(kServoIdMaxLen));
-
-            codec->writeString(id_len, (const char*)id);
-        }
-
-        codec->write(angle_deg);
 
         // Send message to server
         // Codec status is checked inside this function.
@@ -283,7 +395,7 @@ int32_t FaceServo_client::setAngle(const char * id, float angle_deg)
     m_clientManager->releaseRequest(request);
 
     // Invoke error handler callback function
-    m_clientManager->callErrorHandler(err, m_setAngleId);
+    m_clientManager->callErrorHandler(err, m_moveJId);
 
 #if ERPC_PRE_POST_ACTION
     pre_post_action_cb postCB = m_clientManager->getPostCB();

applications/erpc/proto/generated/servo_service_client.hpp

@@ -0,0 +1,42 @@
+/*
+ * Generated by erpcgen 1.14.0 on Wed Apr 22 16:48:46 2026.
+ *
+ * AUTOGENERATED - DO NOT EDIT
+ */
+
+
+#if !defined(_servo_service_client_hpp_)
+#define _servo_service_client_hpp_
+
+#include "servo_service_interface.hpp"
+
+#include "erpc_client_manager.h"
+
+namespace erpcShim
+{
+
+class servo_service_client: public servo_service_interface
+{
+    public:
+        servo_service_client(erpc::ClientManager *manager);
+
+        virtual ~servo_service_client();
+
+        virtual bool setConstraints(const char * id, float max_velocity_rad, float max_acceleration_rad, float max_jerk_rad);
+
+        virtual bool setMode(RpcMotionMode mode);
+
+        virtual bool setUpdatePeriodMs(uint32_t ms);
+
+        virtual int32_t move(const list_ServoCmd_1_t * cmds);
+
+        virtual int32_t moveJ(const list_float_1_t * angles_rad);
+
+    protected:
+        erpc::ClientManager *m_clientManager;
+};
+
+} // erpcShim
+
+
+#endif // _servo_service_client_hpp_

applications/erpc/proto/generated/servo_service_common.h

@@ -1,12 +1,12 @@
 /*
- * Generated by erpcgen 1.14.0 on Wed Jan 14 16:06:40 2026.
+ * Generated by erpcgen 1.14.0 on Wed Apr 22 16:48:46 2026.
  *
  * AUTOGENERATED - DO NOT EDIT
  */
 
 
-#if !defined(_face_servo_common_h_)
+#if !defined(_servo_service_common_h_)
-#define _face_servo_common_h_
+#define _servo_service_common_h_
 
 
 #if defined(__cplusplus)
@@ -24,8 +24,15 @@ extern "C"
 #endif
 
 
-#if !defined(ERPC_TYPE_DEFINITIONS_FACE_SERVO)
+#if !defined(ERPC_TYPE_DEFINITIONS_SERVO_SERVICE)
-#define ERPC_TYPE_DEFINITIONS_FACE_SERVO
+#define ERPC_TYPE_DEFINITIONS_SERVO_SERVICE
+
+// Enumerators data types declarations
+typedef enum RpcMotionMode
+{
+    RpcMotionModeImmediate = 0,
+    RpcMotionModeSCurve = 1
+} RpcMotionMode;
 
 // Aliases data types declarations
 typedef struct list_float_1_t list_float_1_t;
@@ -42,7 +49,7 @@ struct list_float_1_t
 struct ServoCmd
 {
     char * id;
-    float angle_deg;
+    float angle_rad;
 };
 
 struct list_ServoCmd_1_t
@@ -52,15 +59,10 @@ struct list_ServoCmd_1_t
 };
 
 
-// Constant variable declarations
+#endif // ERPC_TYPE_DEFINITIONS_SERVO_SERVICE
-extern const int32_t kServoIdMaxLen;
-
-extern const int32_t kMaxServoCount;
-
-#endif // ERPC_TYPE_DEFINITIONS_FACE_SERVO
 
 #if defined(__cplusplus)
 }
 #endif
 
-#endif // _face_servo_common_h_
+#endif // _servo_service_common_h_

applications/erpc/proto/generated/servo_service_common.hpp

@@ -1,12 +1,12 @@
 /*
- * Generated by erpcgen 1.14.0 on Wed Jan 14 16:06:40 2026.
+ * Generated by erpcgen 1.14.0 on Wed Apr 22 16:48:46 2026.
  *
  * AUTOGENERATED - DO NOT EDIT
  */
 
 
-#if !defined(_face_servo_common_hpp_)
+#if !defined(_servo_service_common_hpp_)
-#define _face_servo_common_hpp_
+#define _servo_service_common_hpp_
 
 
 #include <cstddef>
@@ -19,8 +19,15 @@
 #endif
 
 
-#if !defined(ERPC_TYPE_DEFINITIONS_FACE_SERVO)
+#if !defined(ERPC_TYPE_DEFINITIONS_SERVO_SERVICE)
-#define ERPC_TYPE_DEFINITIONS_FACE_SERVO
+#define ERPC_TYPE_DEFINITIONS_SERVO_SERVICE
+
+// Enumerators data types declarations
+typedef enum RpcMotionMode
+{
+    RpcMotionModeImmediate = 0,
+    RpcMotionModeSCurve = 1
+} RpcMotionMode;
 
 // Aliases data types declarations
 typedef struct list_float_1_t list_float_1_t;
@@ -37,7 +44,7 @@ struct list_float_1_t
 struct ServoCmd
 {
     char * id;
-    float angle_deg;
+    float angle_rad;
 };
 
 struct list_ServoCmd_1_t
@@ -47,12 +54,7 @@ struct list_ServoCmd_1_t
 };
 
 
-// Constant variable declarations
+#endif // ERPC_TYPE_DEFINITIONS_SERVO_SERVICE
-extern const int32_t kServoIdMaxLen;
-
-extern const int32_t kMaxServoCount;
-
-#endif // ERPC_TYPE_DEFINITIONS_FACE_SERVO
 
 
-#endif // _face_servo_common_hpp_
+#endif // _servo_service_common_hpp_

applications/erpc/proto/generated/servo_service_interface.cpp

@@ -1,11 +1,11 @@
 /*
- * Generated by erpcgen 1.14.0 on Wed Jan 14 16:06:40 2026.
+ * Generated by erpcgen 1.14.0 on Wed Apr 22 16:48:46 2026.
  *
  * AUTOGENERATED - DO NOT EDIT
  */
 
 
-#include "face_servo_interface.hpp"
+#include "servo_service_interface.hpp"
 
 #if 11400 != ERPC_VERSION_NUMBER
 #error "The generated shim code version is different to the rest of eRPC code."
@@ -15,6 +15,6 @@
 using namespace std;
 using namespace erpcShim;
 
-FaceServo_interface::~FaceServo_interface(void)
+servo_service_interface::~servo_service_interface(void)
 {
 }

applications/erpc/proto/generated/servo_service_interface.hpp

@@ -0,0 +1,44 @@
+/*
+ * Generated by erpcgen 1.14.0 on Wed Apr 22 16:48:46 2026.
+ *
+ * AUTOGENERATED - DO NOT EDIT
+ */
+
+
+#if !defined(_servo_service_interface_hpp_)
+#define _servo_service_interface_hpp_
+
+#include "servo_service_common.hpp"
+
+namespace erpcShim
+{
+
+
+// Abstract base class for servo_service
+class servo_service_interface
+{
+    public:
+        static const uint8_t m_serviceId = 1;
+        static const uint8_t m_setConstraintsId = 1;
+        static const uint8_t m_setModeId = 2;
+        static const uint8_t m_setUpdatePeriodMsId = 3;
+        static const uint8_t m_moveId = 4;
+        static const uint8_t m_moveJId = 5;
+
+        virtual ~servo_service_interface(void);
+
+        virtual bool setConstraints(const char * id, float max_velocity_rad, float max_acceleration_rad, float max_jerk_rad) = 0;
+
+        virtual bool setMode(RpcMotionMode mode) = 0;
+
+        virtual bool setUpdatePeriodMs(uint32_t ms) = 0;
+
+        virtual int32_t move(const list_ServoCmd_1_t * cmds) = 0;
+
+        virtual int32_t moveJ(const list_float_1_t * angles_rad) = 0;
+private:
+};
+} // erpcShim
+
+
+#endif // _servo_service_interface_hpp_

applications/erpc/proto/generated/servo_service_server.cpp

@@ -1,11 +1,11 @@
 /*
- * Generated by erpcgen 1.14.0 on Wed Jan 14 16:06:40 2026.
+ * Generated by erpcgen 1.14.0 on Wed Apr 22 16:48:46 2026.
  *
  * AUTOGENERATED - DO NOT EDIT
  */
 
 
-#include "face_servo_server.hpp"
+#include "servo_service_server.hpp"
 #if ERPC_ALLOCATION_POLICY == ERPC_ALLOCATION_POLICY_DYNAMIC
 #include <new>
 #include "erpc_port.h"
@@ -25,50 +25,15 @@ extern bool nestingDetection;
 #endif
 
 
-
-// Constant variable definitions
-#if defined(__MINGW32__)
-__declspec( selectany )
-#else
-#pragma weak kServoIdMaxLen
-#endif
-extern const int32_t kServoIdMaxLen = 32;
-#if defined(__MINGW32__)
-__declspec( selectany )
-#else
-#pragma weak kMaxServoCount
-#endif
-extern const int32_t kMaxServoCount = 48;
-
-//! @brief Function to read struct list_float_1_t
-static void read_list_float_1_t_struct(erpc::Codec * codec, list_float_1_t * data);
-
 //! @brief Function to read struct ServoCmd
 static void read_ServoCmd_struct(erpc::Codec * codec, ServoCmd * data);
 
 //! @brief Function to read struct list_ServoCmd_1_t
 static void read_list_ServoCmd_1_t_struct(erpc::Codec * codec, list_ServoCmd_1_t * data);
 
+//! @brief Function to read struct list_float_1_t
+static void read_list_float_1_t_struct(erpc::Codec * codec, list_float_1_t * data);
 
-// Read struct list_float_1_t function implementation
-static void read_list_float_1_t_struct(erpc::Codec * codec, list_float_1_t * data)
-{
-    if(NULL == data)
-    {
-        return;
-    }
-
-    codec->startReadList(data->elementsCount);
-    data->elements = (float *) erpc_malloc(data->elementsCount * sizeof(float));
-    if ((data->elements == NULL) && (data->elementsCount > 0))
-    {
-        codec->updateStatus(kErpcStatus_MemoryError);
-    }
-    for (uint32_t listCount = 0U; listCount < data->elementsCount; ++listCount)
-    {
-        codec->read(data->elements[listCount]);
-    }
-}
 
 // Read struct ServoCmd function implementation
 static void read_ServoCmd_struct(erpc::Codec * codec, ServoCmd * data)
@@ -94,7 +59,7 @@ static void read_ServoCmd_struct(erpc::Codec * codec, ServoCmd * data)
         }
     }
 
-    codec->read(data->angle_deg);
+    codec->read(data->angle_rad);
 }
 
 // Read struct list_ServoCmd_1_t function implementation
@@ -117,9 +82,26 @@ static void read_list_ServoCmd_1_t_struct(erpc::Codec * codec, list_ServoCmd_1_t
     }
 }
 
+// Read struct list_float_1_t function implementation
+static void read_list_float_1_t_struct(erpc::Codec * codec, list_float_1_t * data)
+{
+    if(NULL == data)
+    {
+        return;
+    }
+
+    codec->startReadList(data->elementsCount);
+    data->elements = (float *) erpc_malloc(data->elementsCount * sizeof(float));
+    if ((data->elements == NULL) && (data->elementsCount > 0))
+    {
+        codec->updateStatus(kErpcStatus_MemoryError);
+    }
+    for (uint32_t listCount = 0U; listCount < data->elementsCount; ++listCount)
+    {
+        codec->read(data->elements[listCount]);
+    }
+}
 
-//! @brief Function to free space allocated inside struct list_float_1_t
-static void free_list_float_1_t_struct(list_float_1_t * data);
 
 //! @brief Function to free space allocated inside struct ServoCmd
 static void free_ServoCmd_struct(ServoCmd * data);
@@ -127,12 +109,9 @@ static void free_ServoCmd_struct(ServoCmd * data);
 //! @brief Function to free space allocated inside struct list_ServoCmd_1_t
 static void free_list_ServoCmd_1_t_struct(list_ServoCmd_1_t * data);
 
+//! @brief Function to free space allocated inside struct list_float_1_t
+static void free_list_float_1_t_struct(list_float_1_t * data);
 
-// Free space allocated inside struct list_float_1_t function implementation
-static void free_list_float_1_t_struct(list_float_1_t * data)
-{
-    erpc_free(data->elements);
-}
 
 // Free space allocated inside struct ServoCmd function implementation
 static void free_ServoCmd_struct(ServoCmd * data)
@@ -151,45 +130,63 @@ static void free_list_ServoCmd_1_t_struct(list_ServoCmd_1_t * data)
     erpc_free(data->elements);
 }
 
+// Free space allocated inside struct list_float_1_t function implementation
+static void free_list_float_1_t_struct(list_float_1_t * data)
+{
+    erpc_free(data->elements);
+}
 
 
-FaceServo_service::FaceServo_service(FaceServo_interface *_FaceServo_interface)
+
-    : erpc::Service(FaceServo_interface::m_serviceId)
+servo_service_service::servo_service_service(servo_service_interface *_servo_service_interface)
-    , m_handler(_FaceServo_interface)
+    : erpc::Service(servo_service_interface::m_serviceId)
+    , m_handler(_servo_service_interface)
 {
 }
 
-FaceServo_service::~FaceServo_service()
+servo_service_service::~servo_service_service()
 {
 }
 
 // return service interface handler.
-FaceServo_interface* FaceServo_service::getHandler(void)
+servo_service_interface* servo_service_service::getHandler(void)
 {
     return m_handler;
 }
 
 // Call the correct server shim based on method unique ID.
-erpc_status_t FaceServo_service::handleInvocation(uint32_t methodId, uint32_t sequence, Codec * codec, MessageBufferFactory *messageFactory, Transport * transport)
+erpc_status_t servo_service_service::handleInvocation(uint32_t methodId, uint32_t sequence, Codec * codec, MessageBufferFactory *messageFactory, Transport * transport)
 {
     erpc_status_t erpcStatus;
     switch (methodId)
     {
-        case FaceServo_interface::m_setAllAnglesId:
+        case servo_service_interface::m_setConstraintsId:
         {
-            erpcStatus = setAllAngles_shim(codec, messageFactory, transport, sequence);
+            erpcStatus = setConstraints_shim(codec, messageFactory, transport, sequence);
             break;
         }
 
-        case FaceServo_interface::m_setAnglesId:
+        case servo_service_interface::m_setModeId:
         {
-            erpcStatus = setAngles_shim(codec, messageFactory, transport, sequence);
+            erpcStatus = setMode_shim(codec, messageFactory, transport, sequence);
             break;
         }
 
-        case FaceServo_interface::m_setAngleId:
+        case servo_service_interface::m_setUpdatePeriodMsId:
         {
-            erpcStatus = setAngle_shim(codec, messageFactory, transport, sequence);
+            erpcStatus = setUpdatePeriodMs_shim(codec, messageFactory, transport, sequence);
+            break;
+        }
+
+        case servo_service_interface::m_moveId:
+        {
+            erpcStatus = move_shim(codec, messageFactory, transport, sequence);
+            break;
+        }
+
+        case servo_service_interface::m_moveJId:
+        {
+            erpcStatus = moveJ_shim(codec, messageFactory, transport, sequence);
             break;
         }
 
@@ -203,22 +200,40 @@ erpc_status_t FaceServo_service::handleInvocation(uint32_t methodId, uint32_t se
     return erpcStatus;
 }
 
-// Server shim for setAllAngles of FaceServo interface.
+// Server shim for setConstraints of servo_service interface.
-erpc_status_t FaceServo_service::setAllAngles_shim(Codec * codec, MessageBufferFactory *messageFactory, Transport * transport, uint32_t sequence)
+erpc_status_t servo_service_service::setConstraints_shim(Codec * codec, MessageBufferFactory *messageFactory, Transport * transport, uint32_t sequence)
 {
     erpc_status_t err = kErpcStatus_Success;
 
-    list_float_1_t *angles_deg = NULL;
+    char * id = NULL;
-    angles_deg = (list_float_1_t *) erpc_malloc(sizeof(list_float_1_t));
+    float max_velocity_rad;
-    if (angles_deg == NULL)
+    float max_acceleration_rad;
-    {
+    float max_jerk_rad;
-        codec->updateStatus(kErpcStatus_MemoryError);
+    bool result;
-    }
-    int32_t result;
 
     // startReadMessage() was already called before this shim was invoked.
 
-    read_list_float_1_t_struct(codec, angles_deg);
+    {
+        uint32_t id_len;
+        char * id_local;
+        codec->readString(id_len, &id_local);
+        id = (char*) erpc_malloc((id_len + 1) * sizeof(char));
+        if ((id == NULL) || (id_local == NULL))
+        {
+            codec->updateStatus(kErpcStatus_MemoryError);
+        }
+        else
+        {
+            memcpy(id, id_local, id_len);
+            (id)[id_len] = 0;
+        }
+    }
+
+    codec->read(max_velocity_rad);
+
+    codec->read(max_acceleration_rad);
+
+    codec->read(max_jerk_rad);
 
     err = codec->getStatus();
     if (err == kErpcStatus_Success)
@@ -227,7 +242,7 @@ erpc_status_t FaceServo_service::setAllAngles_shim(Codec * codec, MessageBufferF
 #if ERPC_NESTED_CALLS_DETECTION
         nestingDetection = true;
 #endif
-        result = m_handler->setAllAngles(angles_deg);
+        result = m_handler->setConstraints(id, max_velocity_rad, max_acceleration_rad, max_jerk_rad);
 #if ERPC_NESTED_CALLS_DETECTION
         nestingDetection = false;
 #endif
@@ -242,24 +257,110 @@ erpc_status_t FaceServo_service::setAllAngles_shim(Codec * codec, MessageBufferF
         codec->reset(transport->reserveHeaderSize());
 
         // Build response message.
-        codec->startWriteMessage(message_type_t::kReplyMessage, FaceServo_interface::m_serviceId, FaceServo_interface::m_setAllAnglesId, sequence);
+        codec->startWriteMessage(message_type_t::kReplyMessage, servo_service_interface::m_serviceId, servo_service_interface::m_setConstraintsId, sequence);
 
         codec->write(result);
 
         err = codec->getStatus();
     }
 
-    if (angles_deg)
+    erpc_free(id);
-    {
-        free_list_float_1_t_struct(angles_deg);
-    }
-    erpc_free(angles_deg);
 
     return err;
 }
 
-// Server shim for setAngles of FaceServo interface.
+// Server shim for setMode of servo_service interface.
-erpc_status_t FaceServo_service::setAngles_shim(Codec * codec, MessageBufferFactory *messageFactory, Transport * transport, uint32_t sequence)
+erpc_status_t servo_service_service::setMode_shim(Codec * codec, MessageBufferFactory *messageFactory, Transport * transport, uint32_t sequence)
+{
+    erpc_status_t err = kErpcStatus_Success;
+
+    RpcMotionMode mode;
+    int32_t _tmp_local_i32;
+    bool result;
+
+    // startReadMessage() was already called before this shim was invoked.
+
+    codec->read(_tmp_local_i32);
+    mode = static_cast<RpcMotionMode>(_tmp_local_i32);
+
+    err = codec->getStatus();
+    if (err == kErpcStatus_Success)
+    {
+        // Invoke the actual served function.
+#if ERPC_NESTED_CALLS_DETECTION
+        nestingDetection = true;
+#endif
+        result = m_handler->setMode(mode);
+#if ERPC_NESTED_CALLS_DETECTION
+        nestingDetection = false;
+#endif
+
+        // preparing MessageBuffer for serializing data
+        err = messageFactory->prepareServerBufferForSend(codec->getBufferRef(), transport->reserveHeaderSize());
+    }
+
+    if (err == kErpcStatus_Success)
+    {
+        // preparing codec for serializing data
+        codec->reset(transport->reserveHeaderSize());
+
+        // Build response message.
+        codec->startWriteMessage(message_type_t::kReplyMessage, servo_service_interface::m_serviceId, servo_service_interface::m_setModeId, sequence);
+
+        codec->write(result);
+
+        err = codec->getStatus();
+    }
+
+    return err;
+}
+
+// Server shim for setUpdatePeriodMs of servo_service interface.
+erpc_status_t servo_service_service::setUpdatePeriodMs_shim(Codec * codec, MessageBufferFactory *messageFactory, Transport * transport, uint32_t sequence)
+{
+    erpc_status_t err = kErpcStatus_Success;
+
+    uint32_t ms;
+    bool result;
+
+    // startReadMessage() was already called before this shim was invoked.
+
+    codec->read(ms);
+
+    err = codec->getStatus();
+    if (err == kErpcStatus_Success)
+    {
+        // Invoke the actual served function.
+#if ERPC_NESTED_CALLS_DETECTION
+        nestingDetection = true;
+#endif
+        result = m_handler->setUpdatePeriodMs(ms);
+#if ERPC_NESTED_CALLS_DETECTION
+        nestingDetection = false;
+#endif
+
+        // preparing MessageBuffer for serializing data
+        err = messageFactory->prepareServerBufferForSend(codec->getBufferRef(), transport->reserveHeaderSize());
+    }
+
+    if (err == kErpcStatus_Success)
+    {
+        // preparing codec for serializing data
+        codec->reset(transport->reserveHeaderSize());
+
+        // Build response message.
+        codec->startWriteMessage(message_type_t::kReplyMessage, servo_service_interface::m_serviceId, servo_service_interface::m_setUpdatePeriodMsId, sequence);
+
+        codec->write(result);
+
+        err = codec->getStatus();
+    }
+
+    return err;
+}
+
+// Server shim for move of servo_service interface.
+erpc_status_t servo_service_service::move_shim(Codec * codec, MessageBufferFactory *messageFactory, Transport * transport, uint32_t sequence)
 {
     erpc_status_t err = kErpcStatus_Success;
 
@@ -282,7 +383,7 @@ erpc_status_t FaceServo_service::setAngles_shim(Codec * codec, MessageBufferFact
 #if ERPC_NESTED_CALLS_DETECTION
         nestingDetection = true;
 #endif
-        result = m_handler->setAngles(cmds);
+        result = m_handler->move(cmds);
 #if ERPC_NESTED_CALLS_DETECTION
         nestingDetection = false;
 #endif
@@ -297,7 +398,7 @@ erpc_status_t FaceServo_service::setAngles_shim(Codec * codec, MessageBufferFact
         codec->reset(transport->reserveHeaderSize());
 
         // Build response message.
-        codec->startWriteMessage(message_type_t::kReplyMessage, FaceServo_interface::m_serviceId, FaceServo_interface::m_setAnglesId, sequence);
+        codec->startWriteMessage(message_type_t::kReplyMessage, servo_service_interface::m_serviceId, servo_service_interface::m_moveId, sequence);
 
         codec->write(result);
 
@@ -313,41 +414,22 @@ erpc_status_t FaceServo_service::setAngles_shim(Codec * codec, MessageBufferFact
     return err;
 }
 
-// Server shim for setAngle of FaceServo interface.
+// Server shim for moveJ of servo_service interface.
-erpc_status_t FaceServo_service::setAngle_shim(Codec * codec, MessageBufferFactory *messageFactory, Transport * transport, uint32_t sequence)
+erpc_status_t servo_service_service::moveJ_shim(Codec * codec, MessageBufferFactory *messageFactory, Transport * transport, uint32_t sequence)
 {
     erpc_status_t err = kErpcStatus_Success;
 
-    char * id = NULL;
+    list_float_1_t *angles_rad = NULL;
-    float angle_deg;
+    angles_rad = (list_float_1_t *) erpc_malloc(sizeof(list_float_1_t));
+    if (angles_rad == NULL)
+    {
+        codec->updateStatus(kErpcStatus_MemoryError);
+    }
     int32_t result;
 
     // startReadMessage() was already called before this shim was invoked.
 
-    {
+    read_list_float_1_t_struct(codec, angles_rad);
-        uint32_t id_len;
-        char * id_local;
-        codec->readString(id_len, &id_local);
-        if ((kServoIdMaxLen >= 0) && (id_len <= static_cast<uint32_t>(kServoIdMaxLen)))
-        {
-            id = (char*) erpc_malloc((kServoIdMaxLen + 1) * sizeof(char));
-            if ((id == NULL) || (id_local == NULL))
-            {
-                codec->updateStatus(kErpcStatus_MemoryError);
-            }
-            else
-            {
-                memcpy(id, id_local, id_len);
-                (id)[id_len] = 0;
-            }
-        }
-        else
-        {
-            codec->updateStatus(kErpcStatus_InvalidArgument);
-        }
-    }
-
-    codec->read(angle_deg);
 
     err = codec->getStatus();
     if (err == kErpcStatus_Success)
@@ -356,7 +438,7 @@ erpc_status_t FaceServo_service::setAngle_shim(Codec * codec, MessageBufferFacto
 #if ERPC_NESTED_CALLS_DETECTION
         nestingDetection = true;
 #endif
-        result = m_handler->setAngle(id, angle_deg);
+        result = m_handler->moveJ(angles_rad);
 #if ERPC_NESTED_CALLS_DETECTION
         nestingDetection = false;
 #endif
@@ -371,14 +453,18 @@ erpc_status_t FaceServo_service::setAngle_shim(Codec * codec, MessageBufferFacto
         codec->reset(transport->reserveHeaderSize());
 
         // Build response message.
-        codec->startWriteMessage(message_type_t::kReplyMessage, FaceServo_interface::m_serviceId, FaceServo_interface::m_setAngleId, sequence);
+        codec->startWriteMessage(message_type_t::kReplyMessage, servo_service_interface::m_serviceId, servo_service_interface::m_moveJId, sequence);
 
         codec->write(result);
 
         err = codec->getStatus();
     }
 
-    erpc_free(id);
+    if (angles_rad)
+    {
+        free_list_float_1_t_struct(angles_rad);
+    }
+    erpc_free(angles_rad);
 
     return err;
 }

applications/erpc/proto/generated/servo_service_server.hpp

@@ -0,0 +1,61 @@
+/*
+ * Generated by erpcgen 1.14.0 on Wed Apr 22 16:48:46 2026.
+ *
+ * AUTOGENERATED - DO NOT EDIT
+ */
+
+
+#if !defined(_servo_service_server_hpp_)
+#define _servo_service_server_hpp_
+
+#include "servo_service_interface.hpp"
+
+#include "erpc_server.hpp"
+#include "erpc_codec.hpp"
+
+#if 11400 != ERPC_VERSION_NUMBER
+#error "The generated shim code version is different to the rest of eRPC code."
+#endif
+
+
+namespace erpcShim
+{
+
+/*!
+ * @brief Service subclass for servo_service.
+ */
+class servo_service_service : public erpc::Service
+{
+public:
+    servo_service_service(servo_service_interface *_servo_service_interface);
+
+    virtual ~servo_service_service();
+
+    /*! @brief return service interface handler. */
+    servo_service_interface* getHandler(void);
+
+    /*! @brief Call the correct server shim based on method unique ID. */
+    virtual erpc_status_t handleInvocation(uint32_t methodId, uint32_t sequence, erpc::Codec * codec, erpc::MessageBufferFactory *messageFactory, erpc::Transport * transport);
+
+private:
+    servo_service_interface *m_handler;
+    /*! @brief Server shim for setConstraints of servo_service interface. */
+    erpc_status_t setConstraints_shim(erpc::Codec * codec, erpc::MessageBufferFactory *messageFactory, erpc::Transport * transport, uint32_t sequence);
+
+    /*! @brief Server shim for setMode of servo_service interface. */
+    erpc_status_t setMode_shim(erpc::Codec * codec, erpc::MessageBufferFactory *messageFactory, erpc::Transport * transport, uint32_t sequence);
+
+    /*! @brief Server shim for setUpdatePeriodMs of servo_service interface. */
+    erpc_status_t setUpdatePeriodMs_shim(erpc::Codec * codec, erpc::MessageBufferFactory *messageFactory, erpc::Transport * transport, uint32_t sequence);
+
+    /*! @brief Server shim for move of servo_service interface. */
+    erpc_status_t move_shim(erpc::Codec * codec, erpc::MessageBufferFactory *messageFactory, erpc::Transport * transport, uint32_t sequence);
+
+    /*! @brief Server shim for moveJ of servo_service interface. */
+    erpc_status_t moveJ_shim(erpc::Codec * codec, erpc::MessageBufferFactory *messageFactory, erpc::Transport * transport, uint32_t sequence);
+};
+
+} // erpcShim
+
+
+#endif // _servo_service_server_hpp_

applications/erpc/proto/servo_service.erpc

@@ -0,0 +1,30 @@
+// servo_service.erpc
+// Generator examples:
+//   .\erpcgen.exe -o .\generated\ -g c .\servo_service.erpc
+//   .\erpcgen.exe -g py .\servo_service.erpc
+
+program servo_service
+
+enum RpcMotionMode {
+    RpcMotionModeImmediate = 0,
+    RpcMotionModeSCurve = 1
+}
+struct ServoCmd {
+    string id
+    float angle_rad
+}
+interface servo_service
+{
+    setConstraints(string id,
+                   float max_velocity_rad,
+                   float max_acceleration_rad,
+                   float max_jerk_rad) -> bool
+
+    setMode(RpcMotionMode mode)->bool
+    setUpdatePeriodMs(uint32 ms) -> bool
+    move(list<ServoCmd> cmds) -> int32
+
+    moveJ(list<float> angles_rad) -> int32
+
+
+}

applications/erpc/service/include/head_service_impl.h

@@ -1,35 +0,0 @@
-//
-// Created by Administrator on 2025/12/24.
-//
-
-#ifndef RTTHREAD_HEAD_SERVICE_IMPL_H
-#define RTTHREAD_HEAD_SERVICE_IMPL_H
-
-
-#include <stdint.h>
-#include "erpc/proto/generated/face_servo_server.hpp"
-#include "servo_manager/include/servo_manager.h"
-
-extern "C" {
-#include <rtthread.h>
-}
-
-class FaceServoServiceImpl : public erpcShim::FaceServo_interface
-{
-public:
-    FaceServoServiceImpl();
-    virtual ~FaceServoServiceImpl() = default;
-
-
-    int32_t setAllAngles(const list_float_1_t* angles_deg) override;
-    int32_t setAngle(const char* id, float angle_deg) override;
-    int32_t setAngles(const list_ServoCmd_1_t* cmds) override;
-private:
-    ServoManager _servos;
-    rt_err_t     _init_err;
-    static const std::vector<ServoManager::ServoConfig> kServoCfg;
-
-};
-
-
-#endif //RTTHREAD_HEAD_SERVICE_IMPL_H

applications/erpc/service/include/servo_service_impl.h

@@ -0,0 +1,42 @@
+//
+// Created by Administrator on 2025/12/24.
+//
+
+#ifndef RTTHREAD_HEAD_SERVICE_IMPL_H
+#define RTTHREAD_HEAD_SERVICE_IMPL_H
+
+#include <stdint.h>
+#include <vector>
+
+#include "erpc/proto/generated/servo_service_server.hpp"
+#include "servo_manager/include/servo_manager.h"
+
+extern "C" {
+#include <rtthread.h>
+}
+
+class ServoServiceImpl : public erpcShim::servo_service_interface
+{
+public:
+    ServoServiceImpl();
+    ~ServoServiceImpl() override;
+
+    bool setConstraints(const char* id,
+                        float max_velocity_rad,
+                        float max_acceleration_rad,
+                        float max_jerk_rad) override;
+    bool setMode(RpcMotionMode mode) override;
+    bool setUpdatePeriodMs(uint32_t ms) override;
+    int32_t move(const list_ServoCmd_1_t* cmds) override;
+    int32_t moveJ(const list_float_1_t* angles_rad) override;
+
+private:
+    ServoManager manager_;
+    rt_err_t init_err_;
+    MotionMode mode_;
+    rt_mutex_t mutex_;
+
+    static const std::vector<ServoDriver::Config> kServoCfg;
+};
+
+#endif // RTTHREAD_HEAD_SERVICE_IMPL_H

applications/erpc/service/src/erpc_server_main.cpp

@@ -13,10 +13,9 @@
 #include "erpc_usb_cdc_transport.hpp"
 
 
-#include "erpc/proto/generated/face_servo_server.hpp"
+#include "erpc/proto/generated/servo_service_server.hpp"
-#include "erpc/proto/generated/face_servo_interface.hpp"
+#include "erpc/proto/generated/servo_service_interface.hpp"
-
+#include "erpc/service/include/servo_service_impl.h"
-#include "erpc/service/include/head_service_impl.h"
 #include "erpc/common/include/erpc_error_handler.h"
 
 
@@ -68,8 +67,8 @@ static void erpc_server_entry(void *parameter)
     }
 
     // 4) register service
-    static FaceServoServiceImpl impl;          // 必须继承 erpcShim::demo_interface
+    static ServoServiceImpl impl;          // 必须继承 erpcShim::demo_interface
-    static erpcShim::FaceServo_service service(&impl);   // 生成的 service wrapper
+    static erpcShim::servo_service_service service(&impl);   // 生成的 service wrapper
 
     erpc_add_service_to_server(server, &service);
 

applications/erpc/service/src/head_service_impl.cpp

@@ -1,129 +0,0 @@
-//
-// Created by Administrator on 2025/12/24.
-//
-
-#include "erpc/service/include/head_service_impl.h"
-#include <rtthread.h>
-
-
-const std::vector<ServoManager::ServoConfig> FaceServoServiceImpl::kServoCfg = {
-    {"eye_l_up", "pwm1", 1, 20000000, 500000, 2500000, -90.f, 90.f, 0.f},
-    // {"eye_r_up", "pwm1", 2, 20000000, 500000, 2500000, 0.f, 180.f, 90.f},
-    // ...
-};
-
-FaceServoServiceImpl::FaceServoServiceImpl()
-: _servos(kServoCfg), _init_err(RT_EOK)
-{
-    _init_err = _servos.init(true, true); // enable=true, go_home=true
-    if (_init_err != RT_EOK)
-    {
-        rt_kprintf("[FaceServo][E] init FAILED, err=%d\n", (int)_init_err);
-    }
-}
-
-int32_t FaceServoServiceImpl::setAllAngles(const list_float_1_t* angles_deg)
-{
-    // rt_kprintf("-");
-    if (_init_err != RT_EOK)
-    {
-        rt_kprintf("[FaceServo][E] setAllAngles: service not ready, init_err=%d\n", (int)_init_err);
-        return _init_err;
-    }
-
-    if (!angles_deg)
-    {
-        rt_kprintf("[FaceServo][E] setAllAngles: angles_deg is null\n");
-        return -RT_EINVAL;
-    }
-
-    if (!angles_deg->elements || angles_deg->elementsCount == 0)
-    {
-        rt_kprintf("[FaceServo][E] setAllAngles: empty list (elements=%p, count=%d)\n",
-                   angles_deg->elements, (int)angles_deg->elementsCount);
-        return -RT_EINVAL;
-    }
-
-    const size_t need = _servos.count();
-    const size_t got  = (size_t)angles_deg->elementsCount;
-
-    // 实际使用数量：谁小用谁
-    const size_t n = (got < need) ? got : need;
-    rt_err_t err = _servos.setAllAngles(angles_deg->elements, n);
-    if (err != RT_EOK)
-    {
-        rt_kprintf("[FaceServo][E] setAllAngles: ServoManager err=%d\n", (int)err);
-        return err;
-    }
-
-    return RT_EOK;
-}
-
-int32_t FaceServoServiceImpl::setAngle(const char* id, float angle_deg)
-{
-    if (_init_err != RT_EOK)
-    {
-        rt_kprintf("[FaceServo][E] setAngle: service not ready, init_err=%d\n", (int)_init_err);
-        return _init_err;
-    }
-
-    if (!id)
-    {
-        rt_kprintf("[FaceServo][E] setAngle: id is null\n");
-        return -RT_EINVAL;
-    }
-
-    rt_err_t err = _servos.setAngle(id, angle_deg);
-    if (err != RT_EOK)
-    {
-        rt_kprintf("[FaceServo][E] setAngle: id=%s angle=%.2f err=%d\n",
-                   id, (double)angle_deg, (int)err);
-        return err;
-    }
-
-    return RT_EOK;
-}
-
-int32_t FaceServoServiceImpl::setAngles(const list_ServoCmd_1_t* cmds)
-{
-    if (_init_err != RT_EOK)
-    {
-        rt_kprintf("[FaceServo][E] setAngles: service not ready, init_err=%d\n", (int)_init_err);
-        return _init_err;
-    }
-
-    if (!cmds)
-    {
-        rt_kprintf("[FaceServo][E] setAngles: cmds is null\n");
-        return -RT_EINVAL;
-    }
-
-    if (!cmds->elements || cmds->elementsCount == 0)
-    {
-        rt_kprintf("[FaceServo][E] setAngles: empty list (elements=%p, count=%d)\n",
-                   cmds->elements, (int)cmds->elementsCount);
-        return -RT_EINVAL;
-    }
-
-    for (uint32_t i = 0; i < cmds->elementsCount; ++i)
-    {
-        const char* id = cmds->elements[i].id;
-        const float angle = cmds->elements[i].angle_deg;
-
-        if (!id)
-        {
-            rt_kprintf("[FaceServo][E] setAngles: cmds[%d].id is null\n", (int)i);
-            return -RT_EINVAL;
-        }
-
-        rt_err_t err = _servos.setAngle(id, angle);
-        if (err != RT_EOK)
-        {
-            rt_kprintf("[FaceServo][E] setAngles: fail i=%d id=%s angle=%.2f err=%d\n",
-                       (int)i, id, (double)angle, (int)err);
-            return err;
-        }
-    }
-
-    return RT_EOK;
-}

applications/erpc/service/src/servo_service_impl.cpp

@@ -0,0 +1,277 @@
+//
+// Created by Administrator on 2025/12/24.
+//
+
+#include "erpc/service/include/servo_service_impl.h"
+#include <string>
+#include "common/type/common_type.h"
+
+const std::vector<ServoDriver::Config> ServoServiceImpl::kServoCfg = {
+    {"eye_l_up", "pwm1", 1, 20000000, 500000, 2500000, -0.5f * PI, 0.5f * PI, 0.0f},
+    // {"eye_r_up", "pwm1", 2, 20000000, 500000, 2500000, 0.0f, kPi, 0.5f * kPi},
+    // ...
+};
+
+ServoServiceImpl::ServoServiceImpl()
+    : manager_(kServoCfg),
+      init_err_(RT_EOK),
+      mode_(MotionMode::Immediate),
+      mutex_(RT_NULL)
+{
+    mutex_ = rt_mutex_create("sv_svc", RT_IPC_FLAG_PRIO);
+    if (!mutex_) {
+        init_err_ = -RT_ENOMEM;
+        rt_kprintf("[ServoService][E] mutex create FAILED, err=%d\n", (int)init_err_);
+        return;
+    }
+
+    init_err_ = manager_.init(true, true);
+    if (init_err_ != RT_EOK) {
+        rt_kprintf("[ServoService][E] init FAILED, err=%d\n", (int)init_err_);
+        return;
+    }
+
+    manager_.setUpdatePeriodMs(1);
+
+    for (const auto& cfg : kServoCfg) {
+        ServoControl* servo = manager_.get(cfg.servo_id);
+        if (!servo) {
+            init_err_ = -RT_ENOSYS;
+            rt_kprintf("[ServoService][E] missing servo control, id=%s\n", cfg.servo_id.c_str());
+            return;
+        }
+
+        servo->setConstraints(
+            3.14,
+            31.4,
+            31.4);
+    }
+
+    init_err_ = manager_.start();
+    if (init_err_ != RT_EOK) {
+        rt_kprintf("[ServoService][E] manager start FAILED, err=%d\n", (int)init_err_);
+    }
+}
+
+ServoServiceImpl::~ServoServiceImpl()
+{
+    if (mutex_) {
+        rt_mutex_delete(mutex_);
+        mutex_ = RT_NULL;
+    }
+}
+
+bool ServoServiceImpl::setConstraints(const char* id,
+                                      float max_velocity_rad,
+                                      float max_acceleration_rad,
+                                      float max_jerk_rad)
+{
+    if (init_err_ != RT_EOK) {
+        rt_kprintf("[ServoService][E] setConstraints: service not ready, init_err=%d\n",
+                   (int)init_err_);
+        return false;
+    }
+
+    if (!id) {
+        rt_kprintf("[ServoService][E] setConstraints: id is null\n");
+        return false;
+    }
+
+    if (max_velocity_rad <= 0.0f ||
+        max_acceleration_rad <= 0.0f ||
+        max_jerk_rad <= 0.0f) {
+        rt_kprintf("[ServoService][E] setConstraints: invalid constraints, id=%s v=%.4f a=%.4f j=%.4f\n",
+                   id,
+                   (double)max_velocity_rad,
+                   (double)max_acceleration_rad,
+                   (double)max_jerk_rad);
+        return false;
+    }
+
+    ServoControl* servo = manager_.get(std::string(id));
+    if (!servo) {
+        rt_kprintf("[ServoService][E] setConstraints: servo not found, id=%s\n", id);
+        return false;
+    }
+
+    servo->setConstraints(max_velocity_rad, max_acceleration_rad, max_jerk_rad);
+    rt_kprintf("[ServoService] setConstraints: id=%s v=%.4f a=%.4f j=%.4f\n",
+               id,
+               (double)max_velocity_rad,
+               (double)max_acceleration_rad,
+               (double)max_jerk_rad);
+    return true;
+}
+
+bool ServoServiceImpl::setMode(RpcMotionMode mode)
+{
+    if (init_err_ != RT_EOK) {
+        rt_kprintf("[ServoService][E] setMode: service not ready, init_err=%d\n",
+                   (int)init_err_);
+        return false;
+    }
+
+    if (!mutex_) {
+        rt_kprintf("[ServoService][E] setMode: mutex is null\n");
+        return false;
+    }
+
+    MotionMode motion_mode;
+    switch (mode) {
+    case RpcMotionModeImmediate:
+        motion_mode = MotionMode::Immediate;
+        break;
+    case RpcMotionModeSCurve:
+        motion_mode = MotionMode::SCurve;
+        break;
+    default:
+        rt_kprintf("[ServoService][E] setMode: invalid mode=%d\n", (int)mode);
+        return false;
+    }
+
+    rt_mutex_take(mutex_, RT_WAITING_FOREVER);
+    mode_ = motion_mode;
+    rt_mutex_release(mutex_);
+
+    rt_kprintf("[ServoService] setMode: mode=%d\n", (int)mode);
+    return true;
+}
+
+bool ServoServiceImpl::setUpdatePeriodMs(uint32_t ms)
+{
+    if (init_err_ != RT_EOK) {
+        rt_kprintf("[ServoService][E] setUpdatePeriodMs: service not ready, init_err=%d\n",
+                   (int)init_err_);
+        return false;
+    }
+
+    manager_.setUpdatePeriodMs(ms);
+    manager_.wake();
+
+    rt_kprintf("[ServoService] setUpdatePeriodMs: ms=%u\n", (unsigned)ms);
+    return true;
+}
+
+int32_t ServoServiceImpl::move(const list_ServoCmd_1_t* cmds)
+{
+    if (init_err_ != RT_EOK) {
+        rt_kprintf("[ServoService][E] move: service not ready, init_err=%d\n",
+                   (int)init_err_);
+        return init_err_;
+    }
+
+    if (!cmds) {
+        rt_kprintf("[ServoService][E] move: cmds is null\n");
+        return -RT_EINVAL;
+    }
+
+    if (!cmds->elements || cmds->elementsCount == 0) {
+        rt_kprintf("[ServoService][E] move: empty list (elements=%p, count=%d)\n",
+                   cmds->elements,
+                   (int)cmds->elementsCount);
+        return -RT_EINVAL;
+    }
+
+    if (!mutex_) {
+        rt_kprintf("[ServoService][E] move: mutex is null\n");
+        return -RT_ENOMEM;
+    }
+
+    rt_mutex_take(mutex_, RT_WAITING_FOREVER);
+    const MotionMode motion_mode = mode_;
+    rt_mutex_release(mutex_);
+
+    for (uint32_t i = 0; i < cmds->elementsCount; ++i) {
+        const char* id = cmds->elements[i].id;
+        if (!id) {
+            rt_kprintf("[ServoService][E] move: cmds[%d].id is null\n", (int)i);
+            return -RT_EINVAL;
+        }
+
+        ServoControl* servo = manager_.get(std::string(id));
+        if (!servo) {
+            rt_kprintf("[ServoService][E] move: servo not found, id=%s\n", id);
+            return -RT_ENOSYS;
+        }
+
+        const rt_err_t err = servo->move(cmds->elements[i].angle_rad, motion_mode);
+        if (err != RT_EOK) {
+            rt_kprintf("[ServoService][E] move: move failed, id=%s angle=%.4f mode=%d err=%d\n",
+                       id,
+                       (double)cmds->elements[i].angle_rad,
+                       (int)motion_mode,
+                       (int)err);
+            return err;
+        }
+    }
+
+    if (motion_mode == MotionMode::SCurve) {
+        manager_.wake();
+    }
+
+    return RT_EOK;
+}
+
+int32_t ServoServiceImpl::moveJ(const list_float_1_t* angles_rad)
+{
+    if (init_err_ != RT_EOK) {
+        rt_kprintf("[ServoService][E] moveJ: service not ready, init_err=%d\n",
+                   (int)init_err_);
+        return init_err_;
+    }
+
+    if (!angles_rad) {
+        rt_kprintf("[ServoService][E] moveJ: angles_rad is null\n");
+        return -RT_EINVAL;
+    }
+
+    if (!angles_rad->elements || angles_rad->elementsCount == 0) {
+        rt_kprintf("[ServoService][E] moveJ: empty list (elements=%p, count=%d)\n",
+                   angles_rad->elements,
+                   (int)angles_rad->elementsCount);
+        return -RT_EINVAL;
+    }
+
+    const size_t need = kServoCfg.size();
+    const size_t got = (size_t)angles_rad->elementsCount;
+    if (got != need) {
+        rt_kprintf("[ServoService][E] moveJ: invalid angles count, need=%d got=%d\n",
+                   (int)need,
+                   (int)got);
+        return -RT_EINVAL;
+    }
+
+    if (!mutex_) {
+        rt_kprintf("[ServoService][E] moveJ: mutex is null\n");
+        return -RT_ENOMEM;
+    }
+
+    rt_mutex_take(mutex_, RT_WAITING_FOREVER);
+    const MotionMode motion_mode = mode_;
+    rt_mutex_release(mutex_);
+
+    for (size_t i = 0; i < need; ++i) {
+        ServoControl* servo = manager_.get(kServoCfg[i].servo_id);
+        if (!servo) {
+            rt_kprintf("[ServoService][E] moveJ: servo not found, id=%s\n",
+                       kServoCfg[i].servo_id.c_str());
+            return -RT_ENOSYS;
+        }
+
+        const rt_err_t err = servo->move(angles_rad->elements[i], motion_mode);
+        if (err != RT_EOK) {
+            rt_kprintf("[ServoService][E] moveJ: move failed, id=%s angle=%.4f mode=%d err=%d\n",
+                       kServoCfg[i].servo_id.c_str(),
+                       (double)angles_rad->elements[i],
+                       (int)motion_mode,
+                       (int)err);
+            return err;
+        }
+    }
+
+    if (motion_mode == MotionMode::SCurve) {
+        manager_.wake();
+    }
+
+    return RT_EOK;
+}

applications/main.c

@@ -1,40 +0,0 @@
-/*
- * Copyright (c) 2006-2018, RT-Thread Development Team
- *
- * SPDX-License-Identifier: Apache-2.0
- *
- * Change Logs:
- * Date           Author       Notes
- * 2019-03-05     whj4674672   first version
- */
-
-#include <rtthread.h>
-#include <rtdevice.h>
-#include <board.h>
-
-/* defined the LED0 pin: PB1 */
-#define LED0_PIN    GET_PIN(C, 13)
-
-
-int main(void)
-{
-    int count = 1;
-    /* set LED0 pin mode to output */
-    rt_pin_mode(LED0_PIN, PIN_MODE_OUTPUT);
-
-
-    while (count++)
-    {
-        rt_pin_write(LED0_PIN, PIN_HIGH);
-        rt_thread_mdelay(100);
-        rt_pin_write(LED0_PIN, PIN_LOW);
-        rt_thread_mdelay(100);
-        // rt_kprintf("Hello RT-Thread!\n");
-    }
-
-    return RT_EOK;
-}
-
-
-
-

applications/main.cpp

@@ -0,0 +1,119 @@
+/*
+ * Copyright (c) 2006-2018, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2019-03-05     whj4674672   first version
+ */
+
+#include <rtdevice.h>
+#include <rtthread.h>
+#include <board.h>
+
+#include "planner/s_curve_planner/include/s_curve_position_planner.h"
+
+/* defined the LED0 pin: PC13 */
+#define LED0_PIN    GET_PIN(C, 13)
+
+namespace
+{
+
+constexpr rt_int32_t kWarmupIterations = 10;
+constexpr rt_int32_t kBenchmarkIterations = 10000;
+constexpr double kBenchmarkDtSec = 0.01;
+constexpr double kMaxVelocityRad = 3.14;
+constexpr double kMaxAccelerationRad = 31.4;
+constexpr double kMaxJerkRad = 31.4;
+constexpr double kPositionGain =2.0;
+constexpr double kTargetPosA = 1.35;
+constexpr double kTargetPosB = 1.35;
+
+volatile double g_planner_sink = 0.0;
+
+void run_position_planner_benchmark(const char* label, rt_int32_t target_switch_period)
+{
+    cmvr::SCurvePositionPlanner1D planner(
+        kMaxVelocityRad,
+        kMaxAccelerationRad,
+        kMaxJerkRad);
+
+    planner.setPositionGain(kPositionGain);
+    planner.initialize(0.0, 0.0, 0.0);
+    planner.setTarget(kTargetPosA);
+
+    for (rt_int32_t i = 0; i < kWarmupIterations; ++i)
+    {
+        if (target_switch_period > 0 && (i % target_switch_period) == 0)
+        {
+            const bool use_target_a = ((i / target_switch_period) & 1) == 0;
+            planner.setTarget(use_target_a ? kTargetPosA : kTargetPosB);
+        }
+        g_planner_sink = planner.update(kBenchmarkDtSec);
+    }
+
+    planner.initialize(0.0, 0.0, 0.0);
+    planner.setTarget(kTargetPosA);
+
+    const rt_tick_t start_tick = rt_tick_get();
+
+    for (rt_int32_t i = 0; i < kBenchmarkIterations; ++i)
+    {
+        if (target_switch_period > 0 && (i % target_switch_period) == 0)
+        {
+            const bool use_target_a = ((i / target_switch_period) & 1) == 0;
+            planner.setTarget(use_target_a ? kTargetPosA : kTargetPosB);
+        }
+        g_planner_sink = planner.update(kBenchmarkDtSec);
+    }
+
+    const rt_tick_t end_tick = rt_tick_get();
+    const rt_uint64_t elapsed_ticks = static_cast<rt_uint64_t>(end_tick - start_tick);
+    const rt_uint64_t elapsed_us =
+        (elapsed_ticks * 1000000ULL) / RT_TICK_PER_SECOND;
+    const rt_uint64_t avg_ns =
+        (elapsed_ticks * 1000000000ULL) /
+        (RT_TICK_PER_SECOND * static_cast<rt_uint64_t>(kBenchmarkIterations));
+
+    rt_kprintf(
+        "[PlannerBench] %s iter=%d dt=%.3fms switch=%d elapsed=%llu us avg=%llu ns pos=%.6f vel=%.6f moving=%d\n",
+        label,
+        kBenchmarkIterations,
+        kBenchmarkDtSec * 1000.0,
+        target_switch_period,
+        elapsed_us,
+        avg_ns,
+        planner.getPosition(),
+        planner.getVelocity(),
+        planner.isMoving() ? 1 : 0);
+}
+
+void run_planner_benchmarks()
+{
+    rt_kprintf("[PlannerBench] start RT_TICK_PER_SECOND=%d\n", RT_TICK_PER_SECOND);
+    run_position_planner_benchmark("hold_target", 0);
+    run_position_planner_benchmark("retarget_20", 20);
+    run_position_planner_benchmark("retarget_5", 5);
+    rt_kprintf("[PlannerBench] done sink=%.6f\n", g_planner_sink);
+}
+
+} // namespace
+
+int main(void)
+{
+    int count = 1;
+
+    rt_pin_mode(LED0_PIN, PIN_MODE_OUTPUT);
+    run_planner_benchmarks();
+
+    while (count++)
+    {
+        rt_pin_write(LED0_PIN, PIN_HIGH);
+        rt_thread_mdelay(100);
+        rt_pin_write(LED0_PIN, PIN_LOW);
+        rt_thread_mdelay(100);
+    }
+
+    return RT_EOK;
+}

applications/planner/SConscript

@@ -0,0 +1,10 @@
+from building import *
+import os
+
+cwd = GetCurrentDir()
+src = Glob('s_curve_planner/src/*.cpp')
+CPPPATH = [cwd]
+
+group = DefineGroup('controller', src, depend = [''], CPPPATH = CPPPATH)
+
+Return('group')

applications/planner/s_curve_planner/include/s_curve_position_planner.h

@@ -0,0 +1,100 @@
+//
+// Created by lgv on 2026/3/10.
+//
+
+#pragma once
+
+#include "s_curve_velocity_planner.h"
+
+#include <algorithm>
+#include <cmath>
+
+namespace cmvr
+{
+
+struct SCurvePositionPlannerState
+{
+  double position;
+  double velocity;
+  double acceleration;
+  double jerk;
+  double target_position;
+  bool is_moving;
+
+  SCurvePositionPlannerState()
+    : position(0.0)
+    , velocity(0.0)
+    , acceleration(0.0)
+    , jerk(0.0)
+    , target_position(0.0)
+    , is_moving(false)
+  {}
+};
+
+/**
+ * @brief 单轴位置跟踪规划器
+ *
+ * 结构：
+ * - 外环：position_error -> desired_velocity
+ * - 内环：SCurveVelocityPlanner1D 负责 jerk-limited 速度规划
+ *
+ * 这个类用于高频变化目标的位置跟踪，不承担严格点到点 profile 语义。
+ */
+class SCurvePositionPlanner1D
+{
+public:
+  SCurvePositionPlanner1D(double max_velocity = 3.0,
+                          double max_acceleration = 10.0,
+                          double max_jerk = 50.0);
+
+  void setConstraints(double max_velocity, double max_acceleration, double max_jerk);
+  void getConstraints(double& max_velocity, double& max_acceleration, double& max_jerk) const;
+
+  void setPositionGain(double position_gain);
+  double getPositionGain() const { return position_gain_; }
+
+  void initialize(double position, double velocity = 0.0, double acceleration = 0.0);
+  void reset();
+
+  void setTarget(double target_position);
+  double update(double dt);
+
+  const SCurvePositionPlannerState& getState() const { return state_; }
+  double getPosition() const { return state_.position; }
+  double getVelocity() const { return state_.velocity; }
+  double getAcceleration() const { return state_.acceleration; }
+  double getJerk() const { return state_.jerk; }
+  double getTargetPosition() const { return state_.target_position; }
+  bool isMoving() const { return state_.is_moving; }
+
+private:
+  double max_velocity_;
+  double max_acceleration_;
+  double max_jerk_;
+
+  SCurvePositionPlannerState state_;
+  SCurveVelocityPlanner1D velocity_planner_;
+  double position_gain_;
+
+  static constexpr double EPSILON = 1e-9;
+  static constexpr double VELOCITY_THRESHOLD = 1e-6;
+  static constexpr double ACCELERATION_THRESHOLD = 1e-6;
+  static constexpr double POSITION_THRESHOLD = 1e-7;
+
+  double computeDesiredVelocity(double position_error) const;
+  double computePlannerStoppingDistance(double velocity, double acceleration) const;
+
+  static double clamp(double value, double min_val, double max_val)
+  {
+    return std::max(min_val, std::min(max_val, value));
+  }
+
+  static double sign(double value)
+  {
+    if (value > EPSILON) return 1.0;
+    if (value < -EPSILON) return -1.0;
+    return 0.0;
+  }
+};
+
+}  // namespace cmvr

applications/planner/s_curve_planner/include/s_curve_velocity_planner.h

@@ -0,0 +1,240 @@
+//
+// Created by lgv on 2026/3/9.
+//
+
+#pragma once
+
+#include <algorithm>
+#include <cmath>
+#include <vector>
+
+namespace cmvr
+{
+
+/**
+ * @brief 单次速度切换的 S 曲线剖面参数
+ *
+ * 本类表示从任意 (v0, a0) 连续规划到 (vf, 0) 的 3 段 jerk-limited profile：
+ *
+ *   seg1: jerk = j1, duration = t1
+ *   seg2: jerk = j2, duration = t2   (当前实现 j2 = 0)
+ *   seg3: jerk = j3, duration = t3
+ *
+ * 其中 j1/j3 为 ±j_max，j2 为 0。
+ */
+struct SCurveVelocityProfile
+{
+  double t1;
+  double t2;
+  double t3;
+
+  double total_time;
+
+  double j1;
+  double j2;
+  double j3;
+
+  double v0;
+  double a0;
+  double vf;
+
+  bool valid;
+
+  SCurveVelocityProfile()
+    : t1(0.0), t2(0.0), t3(0.0)
+    , total_time(0.0)
+    , j1(0.0), j2(0.0), j3(0.0)
+    , v0(0.0), a0(0.0), vf(0.0)
+    , valid(false)
+  {}
+};
+
+/**
+ * @brief S 曲线速度规划器状态
+ */
+struct SCurveVelocityPlannerState
+{
+  double velocity;
+  double acceleration;
+  double jerk;
+
+  double target_velocity;
+
+  double elapsed_time;
+  bool has_active_profile;
+  bool is_moving;
+
+  SCurveVelocityPlannerState()
+    : velocity(0.0)
+    , acceleration(0.0)
+    , jerk(0.0)
+    , target_velocity(0.0)
+    , elapsed_time(0.0)
+    , has_active_profile(false)
+    , is_moving(false)
+  {}
+};
+
+/**
+ * @brief 单轴速度型 S 曲线规划器
+ *
+ * 语义：
+ * - setTargetVelocity(v): 设置新目标并重规划
+ * - update(dt): 只采样当前 active profile
+ * - overwriteState(v,a): 仅覆盖当前状态并清掉旧 profile
+ * - synchronizeAndReplan(v,a): 用测量状态同步，并按当前目标重规划
+ *
+ * 数学目标：
+ * - 从任意 (v0, a0) 连续规划到 (vf, 0)
+ * - 满足 |v| <= v_max, |a| <= a_max, |j| <= j_max
+ */
+class SCurveVelocityPlanner1D
+{
+public:
+  SCurveVelocityPlanner1D(double max_velocity = 1.0,
+                          double max_acceleration = 1.0,
+                          double max_jerk = 5.0);
+
+  void setConstraints(double max_velocity, double max_acceleration, double max_jerk);
+  void getConstraints(double& max_velocity, double& max_acceleration, double& max_jerk) const;
+
+  /**
+   * @brief 设置目标变化阈值
+   *
+   * 目标变化量小于该阈值时，不触发重规划。
+   */
+  void setTargetReplanThreshold(double threshold);
+
+  /**
+   * @brief 设置同步反馈时的重规划阈值
+   *
+   * 测量速度与当前 profile 采样状态的偏差小于该阈值时，
+   * synchronizeAndReplan() 只对齐当前状态，不打断现有 profile。
+   */
+  void setSynchronizeReplanThreshold(double threshold);
+
+  void initialize(double velocity = 0.0, double acceleration = 0.0);
+  void reset();
+
+  /**
+   * @brief 仅覆盖当前状态，并清掉旧 profile
+   *
+   * 注意：
+   * - 这个接口不会自动重规划
+   * - 用于初始化、调试、或显式中断旧轨迹
+   */
+  void overwriteState(double velocity,
+                      double acceleration = 0.0,
+                      bool keep_target_velocity = true);
+
+  /**
+   * @brief 用测量状态同步，并按当前目标重新规划
+   *
+   * 这是闭环场景真正该用的接口。
+   */
+  void synchronizeAndReplan(double velocity,
+                            double acceleration = 0.0);
+
+  /**
+   * @brief 设置目标速度，并立即重规划
+   */
+  void setTargetVelocity(double target_velocity);
+
+  /**
+   * @brief 平滑停下
+   */
+  void stop() { setTargetVelocity(0.0); }
+
+  /**
+   * @brief 更新一步（只采样）
+   * @param dt 时间步长
+   * @return 当前采样后的速度
+   */
+  double update(double dt);
+
+  /**
+   * @brief 计算单次速度切换剖面
+   */
+  SCurveVelocityProfile calculateProfile(double start_velocity,
+                                         double end_velocity,
+                                         double start_acceleration = 0.0) const;
+
+  /**
+   * @brief 给定剖面下，获取时刻 t 的速度
+   */
+  double getVelocityAtTime(const SCurveVelocityProfile& profile, double t) const;
+
+  /**
+   * @brief 给定剖面下，获取时刻 t 的加速度
+   */
+  double getAccelerationAtTime(const SCurveVelocityProfile& profile, double t) const;
+
+  /**
+   * @brief 给定剖面下，获取时刻 t 的 jerk
+   */
+  double getJerkAtTime(const SCurveVelocityProfile& profile, double t) const;
+
+  /**
+   * @brief 按固定时间间隔生成轨迹点
+   */
+  void generateTrajectory(const SCurveVelocityProfile& profile, double dt,
+                          std::vector<double>& velocities,
+                          std::vector<double>& accelerations,
+                          std::vector<double>& jerks) const;
+
+  const SCurveVelocityPlannerState& getState() const { return state_; }
+  const SCurveVelocityProfile& getActiveProfile() const { return active_profile_; }
+
+  double getVelocity() const { return state_.velocity; }
+  double getAcceleration() const { return state_.acceleration; }
+  double getJerk() const { return state_.jerk; }
+  double getTargetVelocity() const { return state_.target_velocity; }
+  bool hasActiveProfile() const { return state_.has_active_profile; }
+  bool isMoving() const { return state_.is_moving; }
+
+private:
+  double max_velocity_;
+  double max_acceleration_;
+  double max_jerk_;
+
+  double target_replan_threshold_;
+  double synchronize_replan_threshold_;
+
+  SCurveVelocityPlannerState state_;
+  SCurveVelocityProfile active_profile_;
+
+  static constexpr double EPSILON = 1e-9;
+  static constexpr double VELOCITY_THRESHOLD = 1e-6;
+  static constexpr double ACCELERATION_THRESHOLD = 1e-6;
+  static constexpr double DEFAULT_TARGET_REPLAN_THRESHOLD = 1e-9;
+  static constexpr double DEFAULT_SYNCHRONIZE_REPLAN_THRESHOLD = 5e-2;
+
+private:
+  void updateIsMovingFlag();
+
+  /**
+   * @brief 判断 profile 是否满足速度上限
+   */
+  bool isVelocityLimitSatisfied(const SCurveVelocityProfile& profile) const;
+
+  /**
+   * @brief 计算完整剖面终点状态
+   */
+  void integrateWholeProfile(const SCurveVelocityProfile& profile,
+                             double& v_end,
+                             double& a_end) const;
+
+  static double clamp(double value, double min_val, double max_val)
+  {
+    return std::max(min_val, std::min(max_val, value));
+  }
+
+  static double sign(double value)
+  {
+    if (value > EPSILON) return 1.0;
+    if (value < -EPSILON) return -1.0;
+    return 0.0;
+  }
+};
+
+} // namespace cmvr

applications/planner/s_curve_planner/src/s_curve_position_planner.cpp

@@ -0,0 +1,193 @@
+//
+// Created by lgv on 2026/3/10.
+//
+
+#include "planner/s_curve_planner/include/s_curve_position_planner.h"
+
+#include <algorithm>
+#include <cmath>
+
+namespace cmvr
+{
+
+SCurvePositionPlanner1D::SCurvePositionPlanner1D(double max_velocity,
+                                                 double max_acceleration,
+                                                 double max_jerk)
+  : max_velocity_(std::abs(max_velocity))
+  , max_acceleration_(std::abs(max_acceleration))
+  , max_jerk_(std::abs(max_jerk))
+  , velocity_planner_(max_velocity_, max_acceleration_, max_jerk_)
+  , position_gain_(1.0)
+{
+  velocity_planner_.setTargetReplanThreshold(std::max(1e-3, 0.02 * max_velocity_));
+  reset();
+}
+
+void SCurvePositionPlanner1D::setConstraints(double max_velocity,
+                                             double max_acceleration,
+                                             double max_jerk)
+{
+  max_velocity_ = std::abs(max_velocity);
+  max_acceleration_ = std::abs(max_acceleration);
+  max_jerk_ = std::abs(max_jerk);
+  velocity_planner_.setConstraints(max_velocity_, max_acceleration_, max_jerk_);
+  velocity_planner_.setTargetReplanThreshold(std::max(1e-3, 0.02 * max_velocity_));
+}
+
+void SCurvePositionPlanner1D::getConstraints(double& max_velocity,
+                                             double& max_acceleration,
+                                             double& max_jerk) const
+{
+  max_velocity = max_velocity_;
+  max_acceleration = max_acceleration_;
+  max_jerk = max_jerk_;
+}
+
+void SCurvePositionPlanner1D::setPositionGain(double position_gain)
+{
+  position_gain_ = std::max(0.0, position_gain);
+}
+
+void SCurvePositionPlanner1D::initialize(double position, double velocity, double acceleration)
+{
+  state_.position = position;
+  velocity_planner_.initialize(velocity, acceleration);
+  state_.velocity = velocity_planner_.getVelocity();
+  state_.acceleration = velocity_planner_.getAcceleration();
+  state_.jerk = 0.0;
+  state_.target_position = position;
+  state_.is_moving = false;
+}
+
+void SCurvePositionPlanner1D::reset()
+{
+  state_ = SCurvePositionPlannerState();
+  velocity_planner_.reset();
+}
+
+void SCurvePositionPlanner1D::setTarget(double target_position)
+{
+  state_.target_position = target_position;
+}
+
+double SCurvePositionPlanner1D::update(double dt)
+{
+  if (dt <= 0.0 || dt > 0.1) {
+    return state_.position;
+  }
+
+  const double position_error = state_.target_position - state_.position;
+  if (std::abs(position_error) < POSITION_THRESHOLD &&
+      std::abs(velocity_planner_.getVelocity()) < VELOCITY_THRESHOLD &&
+      std::abs(velocity_planner_.getAcceleration()) < ACCELERATION_THRESHOLD) {
+    velocity_planner_.overwriteState(0.0, 0.0, false);
+    state_.position = state_.target_position;
+    state_.velocity = 0.0;
+    state_.acceleration = 0.0;
+    state_.jerk = 0.0;
+    state_.is_moving = false;
+    return state_.position;
+  }
+
+  velocity_planner_.setTargetVelocity(computeDesiredVelocity(position_error));
+
+  const double old_velocity = velocity_planner_.getVelocity();
+  const double new_velocity = velocity_planner_.update(dt);
+
+  state_.velocity = new_velocity;
+  state_.acceleration = velocity_planner_.getAcceleration();
+  state_.jerk = velocity_planner_.getJerk();
+  state_.position += 0.5 * (old_velocity + new_velocity) * dt;
+
+  const double updated_error = state_.target_position - state_.position;
+  if (std::abs(updated_error) < POSITION_THRESHOLD &&
+      std::abs(state_.velocity) < VELOCITY_THRESHOLD &&
+      std::abs(state_.acceleration) < ACCELERATION_THRESHOLD) {
+    velocity_planner_.overwriteState(0.0, 0.0, false);
+    state_.position = state_.target_position;
+    state_.velocity = 0.0;
+    state_.acceleration = 0.0;
+    state_.jerk = 0.0;
+    state_.is_moving = false;
+    return state_.position;
+  }
+
+  state_.is_moving = velocity_planner_.isMoving() ||
+                     std::abs(updated_error) > POSITION_THRESHOLD;
+  return state_.position;
+}
+
+double SCurvePositionPlanner1D::computeDesiredVelocity(double position_error) const
+{
+  if (std::abs(position_error) <= POSITION_THRESHOLD) {
+    return 0.0;
+  }
+
+  const double proportional_velocity =
+      clamp(position_gain_ * position_error, -max_velocity_, max_velocity_);
+
+  return proportional_velocity;
+  const double current_velocity = velocity_planner_.getVelocity();
+  const double current_acceleration = velocity_planner_.getAcceleration();
+  const double stopping_distance =
+      computePlannerStoppingDistance(current_velocity, current_acceleration);
+
+  if (std::abs(current_velocity) > VELOCITY_THRESHOLD &&
+      sign(current_velocity) != sign(position_error)) {
+    return 0.0;
+  }
+
+  if (std::abs(current_velocity) > VELOCITY_THRESHOLD &&
+      sign(current_velocity) == sign(position_error) &&
+      std::abs(position_error) <= std::abs(stopping_distance) * 1.05) {
+    return 0.0;
+  }
+
+  double low = 0.0;
+  double high = max_velocity_;
+  for (int i = 0; i < 40; ++i) {
+    const double mid = 0.5 * (low + high);
+    const double distance =
+        std::abs(computePlannerStoppingDistance(sign(position_error) * mid, 0.0));
+    if (distance <= std::abs(position_error)) {
+      low = mid;
+    } else {
+      high = mid;
+    }
+  }
+
+  return sign(position_error) *
+         std::min(std::abs(proportional_velocity), low);
+}
+
+double SCurvePositionPlanner1D::computePlannerStoppingDistance(double velocity,
+                                                               double acceleration) const
+{
+  if (std::abs(velocity) <= VELOCITY_THRESHOLD &&
+      std::abs(acceleration) <= ACCELERATION_THRESHOLD) {
+    return 0.0;
+  }
+
+  const auto profile = velocity_planner_.calculateProfile(velocity, 0.0, acceleration);
+  if (!profile.valid || profile.total_time <= EPSILON) {
+    return 0.0;
+  }
+
+  const int steps = std::max(16, static_cast<int>(std::ceil(profile.total_time / 0.002)));
+  const double dt = profile.total_time / static_cast<double>(steps);
+
+  double distance = 0.0;
+  double previous_time = 0.0;
+  double previous_velocity = velocity_planner_.getVelocityAtTime(profile, 0.0);
+  for (int i = 1; i <= steps; ++i) {
+    const double t = (i == steps) ? profile.total_time : i * dt;
+    const double current_velocity = velocity_planner_.getVelocityAtTime(profile, t);
+    distance += 0.5 * (previous_velocity + current_velocity) * (t - previous_time);
+    previous_time = t;
+    previous_velocity = current_velocity;
+  }
+
+  return distance;
+}
+
+}  // namespace cmvr

applications/planner/s_curve_planner/src/s_curve_velocity_planner.cpp

@@ -0,0 +1,562 @@
+//
+// Created by lgv on 2026/3/9.
+//
+
+#include "../include/s_curve_velocity_planner.h"
+
+#include <cmath>
+#include <limits>
+
+namespace cmvr
+{
+
+SCurveVelocityPlanner1D::SCurveVelocityPlanner1D(double max_velocity,
+                                                 double max_acceleration,
+                                                 double max_jerk)
+  : max_velocity_(std::abs(max_velocity))
+  , max_acceleration_(std::abs(max_acceleration))
+  , max_jerk_(std::abs(max_jerk))
+  , target_replan_threshold_(DEFAULT_TARGET_REPLAN_THRESHOLD)
+  , synchronize_replan_threshold_(DEFAULT_SYNCHRONIZE_REPLAN_THRESHOLD)
+{
+  reset();
+}
+
+void SCurveVelocityPlanner1D::setConstraints(double max_velocity,
+                                             double max_acceleration,
+                                             double max_jerk)
+{
+  max_velocity_ = std::abs(max_velocity);
+  max_acceleration_ = std::abs(max_acceleration);
+  max_jerk_ = std::abs(max_jerk);
+
+  state_.velocity = clamp(state_.velocity, -max_velocity_, max_velocity_);
+  state_.acceleration = clamp(state_.acceleration, -max_acceleration_, max_acceleration_);
+  state_.target_velocity = clamp(state_.target_velocity, -max_velocity_, max_velocity_);
+
+  if (state_.has_active_profile) {
+    active_profile_ = calculateProfile(state_.velocity,
+                                       state_.target_velocity,
+                                       state_.acceleration);
+    state_.elapsed_time = 0.0;
+    state_.has_active_profile = active_profile_.valid && active_profile_.total_time > EPSILON;
+  }
+
+  updateIsMovingFlag();
+}
+
+void SCurveVelocityPlanner1D::getConstraints(double& max_velocity,
+                                             double& max_acceleration,
+                                             double& max_jerk) const
+{
+  max_velocity = max_velocity_;
+  max_acceleration = max_acceleration_;
+  max_jerk = max_jerk_;
+}
+
+void SCurveVelocityPlanner1D::setTargetReplanThreshold(double threshold)
+{
+  target_replan_threshold_ = std::max(0.0, threshold);
+}
+
+void SCurveVelocityPlanner1D::setSynchronizeReplanThreshold(double threshold)
+{
+  synchronize_replan_threshold_ = std::max(0.0, threshold);
+}
+
+void SCurveVelocityPlanner1D::initialize(double velocity, double acceleration)
+{
+  state_ = SCurveVelocityPlannerState();
+  state_.velocity = clamp(velocity, -max_velocity_, max_velocity_);
+  state_.acceleration = clamp(acceleration, -max_acceleration_, max_acceleration_);
+  state_.jerk = 0.0;
+  state_.target_velocity = state_.velocity;
+  state_.elapsed_time = 0.0;
+  state_.has_active_profile = false;
+
+  active_profile_ = SCurveVelocityProfile();
+  active_profile_.v0 = state_.velocity;
+  active_profile_.a0 = state_.acceleration;
+  active_profile_.vf = state_.velocity;
+  active_profile_.valid = false;
+
+  updateIsMovingFlag();
+}
+
+void SCurveVelocityPlanner1D::reset()
+{
+  state_ = SCurveVelocityPlannerState();
+  active_profile_ = SCurveVelocityProfile();
+}
+
+void SCurveVelocityPlanner1D::overwriteState(double velocity,
+                                             double acceleration,
+                                             bool keep_target_velocity)
+{
+  state_.velocity = clamp(velocity, -max_velocity_, max_velocity_);
+  state_.acceleration = clamp(acceleration, -max_acceleration_, max_acceleration_);
+  state_.jerk = 0.0;
+  state_.elapsed_time = 0.0;
+  state_.has_active_profile = false;
+
+  if (!keep_target_velocity) {
+    state_.target_velocity = state_.velocity;
+  } else {
+    state_.target_velocity = clamp(state_.target_velocity, -max_velocity_, max_velocity_);
+  }
+
+  active_profile_ = SCurveVelocityProfile();
+  active_profile_.v0 = state_.velocity;
+  active_profile_.a0 = state_.acceleration;
+  active_profile_.vf = state_.target_velocity;
+  active_profile_.valid = false;
+
+  updateIsMovingFlag();
+}
+
+  void SCurveVelocityPlanner1D::synchronizeAndReplan(double velocity,
+                                                     double acceleration)
+{
+  const double measured_velocity = clamp(velocity, -max_velocity_, max_velocity_);
+  const double measured_acceleration =
+      clamp(acceleration, -max_acceleration_, max_acceleration_);
+
+  if (!state_.has_active_profile &&
+      std::abs(measured_velocity - state_.target_velocity) <= VELOCITY_THRESHOLD) {
+    state_.velocity = state_.target_velocity;
+    state_.acceleration = 0.0;
+    state_.jerk = 0.0;
+    updateIsMovingFlag();
+    return;
+      }
+
+  // 如果测量值已经基本落在当前采样状态上，就继续沿现有 profile 走。
+  // 否则每拍都从同一目标重规划，会把已经进入的 jerk phase 反复打断。
+  if (state_.has_active_profile &&
+      std::abs(measured_velocity - state_.velocity) <= synchronize_replan_threshold_) {
+    state_.velocity = measured_velocity;
+    state_.acceleration = measured_acceleration;
+    state_.jerk = getJerkAtTime(active_profile_, state_.elapsed_time);
+    updateIsMovingFlag();
+    return;
+      }
+
+  state_.velocity = measured_velocity;
+  state_.acceleration = measured_acceleration;
+  state_.jerk = 0.0;
+
+  active_profile_ = calculateProfile(state_.velocity,
+                                     state_.target_velocity,
+                                     state_.acceleration);
+
+  state_.elapsed_time = 0.0;
+  state_.has_active_profile = active_profile_.total_time > EPSILON;
+
+  updateIsMovingFlag();
+}
+
+void SCurveVelocityPlanner1D::setTargetVelocity(double target_velocity)
+{
+  const double new_target = clamp(target_velocity, -max_velocity_, max_velocity_);
+
+  const bool same_target =
+      std::abs(new_target - state_.target_velocity) <= target_replan_threshold_;
+
+  if (same_target &&
+      state_.has_active_profile) {
+    return;
+  }
+
+  if (same_target &&
+      !state_.has_active_profile &&
+      std::abs(state_.velocity - new_target) <= VELOCITY_THRESHOLD &&
+      std::abs(state_.acceleration) <= ACCELERATION_THRESHOLD) {
+    return;
+  }
+
+  state_.target_velocity = new_target;
+
+  active_profile_ = calculateProfile(state_.velocity,
+                                     state_.target_velocity,
+                                     state_.acceleration);
+
+  state_.elapsed_time = 0.0;
+  state_.has_active_profile = active_profile_.valid && active_profile_.total_time > EPSILON;
+
+  updateIsMovingFlag();
+}
+
+double SCurveVelocityPlanner1D::getVelocityAtTime(const SCurveVelocityProfile& profile, double t) const
+{
+  if (!profile.valid) {
+    return profile.v0;
+  }
+
+  if (t <= 0.0) {
+    return profile.v0;
+  }
+  if (t >= profile.total_time) {
+    return profile.vf;
+  }
+
+  double v = profile.v0;
+  double a = profile.a0;
+
+  auto process_segment = [&](double dt, double jerk) {
+    v += a * dt + 0.5 * jerk * dt * dt;
+    a += jerk * dt;
+  };
+
+  // seg1
+  if (t <= profile.t1) {
+    process_segment(t, profile.j1);
+    return v;
+  }
+  process_segment(profile.t1, profile.j1);
+
+  // seg2
+  const double t_after_1 = t - profile.t1;
+  if (t_after_1 <= profile.t2) {
+    process_segment(t_after_1, profile.j2);
+    return v;
+  }
+  process_segment(profile.t2, profile.j2);
+
+  // seg3
+  const double t_after_2 = t_after_1 - profile.t2;
+  process_segment(t_after_2, profile.j3);
+  return v;
+}
+
+double SCurveVelocityPlanner1D::getAccelerationAtTime(const SCurveVelocityProfile& profile, double t) const
+{
+  if (!profile.valid) {
+    return profile.a0;
+  }
+
+  if (t <= 0.0) {
+    return profile.a0;
+  }
+  if (t >= profile.total_time) {
+    return 0.0;
+  }
+
+  double a = profile.a0;
+
+  auto process_segment = [&](double dt, double jerk) {
+    a += jerk * dt;
+  };
+
+  // seg1
+  if (t <= profile.t1) {
+    process_segment(t, profile.j1);
+    return a;
+  }
+  process_segment(profile.t1, profile.j1);
+
+  // seg2
+  const double t_after_1 = t - profile.t1;
+  if (t_after_1 <= profile.t2) {
+    process_segment(t_after_1, profile.j2);
+    return a;
+  }
+  process_segment(profile.t2, profile.j2);
+
+  // seg3
+  const double t_after_2 = t_after_1 - profile.t2;
+  process_segment(t_after_2, profile.j3);
+  return a;
+}
+
+double SCurveVelocityPlanner1D::getJerkAtTime(const SCurveVelocityProfile& profile, double t) const
+{
+  if (!profile.valid) {
+    return 0.0;
+  }
+
+  if (t <= 0.0 || t >= profile.total_time) {
+    return 0.0;
+  }
+
+  if (t <= profile.t1) {
+    return profile.j1;
+  }
+  if (t <= profile.t1 + profile.t2) {
+    return profile.j2;
+  }
+  return profile.j3;
+}
+
+void SCurveVelocityPlanner1D::integrateWholeProfile(const SCurveVelocityProfile& profile,
+                                                    double& v_end,
+                                                    double& a_end) const
+{
+  double v = profile.v0;
+  double a = profile.a0;
+
+  auto process_segment = [&](double dt, double jerk) {
+    v += a * dt + 0.5 * jerk * dt * dt;
+    a += jerk * dt;
+  };
+
+  process_segment(profile.t1, profile.j1);
+  process_segment(profile.t2, profile.j2);
+  process_segment(profile.t3, profile.j3);
+
+  v_end = v;
+  a_end = a;
+}
+
+bool SCurveVelocityPlanner1D::isVelocityLimitSatisfied(const SCurveVelocityProfile& profile) const
+{
+  if (!profile.valid) {
+    return true;
+  }
+
+  std::vector<double> check_times;
+  check_times.reserve(8);
+
+  check_times.push_back(0.0);
+  check_times.push_back(profile.t1);
+  check_times.push_back(profile.t1 + profile.t2);
+  check_times.push_back(profile.total_time);
+
+  // seg1 内部若 acceleration 过零，则 velocity 可能在段内出现极值
+  if (std::abs(profile.j1) > EPSILON && profile.t1 > EPSILON) {
+    const double tz = -profile.a0 / profile.j1;
+    if (tz > EPSILON && tz < profile.t1 - EPSILON) {
+      check_times.push_back(tz);
+    }
+  }
+
+  for (double t : check_times) {
+    const double v = getVelocityAtTime(profile, t);
+    if (std::abs(v) > max_velocity_ + 1e-7) {
+      return false;
+    }
+  }
+
+  return true;
+}
+
+SCurveVelocityProfile SCurveVelocityPlanner1D::calculateProfile(double start_velocity,
+                                                                double end_velocity,
+                                                                double start_acceleration) const
+{
+  SCurveVelocityProfile best;
+  best.v0 = clamp(start_velocity, -max_velocity_, max_velocity_);
+  best.a0 = clamp(start_acceleration, -max_acceleration_, max_acceleration_);
+  best.vf = clamp(end_velocity, -max_velocity_, max_velocity_);
+
+  if (max_jerk_ <= EPSILON || max_acceleration_ <= EPSILON) {
+    // 退化情况：这里不做特殊退化规划，直接返回 invalid
+    return best;
+  }
+
+  const double v0 = best.v0;
+  const double a0 = best.a0;
+  const double vf = best.vf;
+
+  // 目标方向坐标：尽量统一到“signed frame 下 Δv >= 0”
+  double s = sign(vf - v0);
+  if (std::abs(vf - v0) <= VELOCITY_THRESHOLD) {
+    s = 1.0;
+  }
+
+  const double dv = s * (vf - v0);   // >= 0
+  const double a0s = s * a0;         // signed frame 下的初始 acceleration
+  const double j = max_jerk_;
+  const double amax = max_acceleration_;
+
+  auto normalize_nonnegative = [](double x) {
+    return (x < 0.0 && x > -1e-10) ? 0.0 : x;
+  };
+
+  auto try_candidate = [&](double t1, double t2, double t3,
+                           double j1s, double j2s, double j3s) {
+    t1 = normalize_nonnegative(t1);
+    t2 = normalize_nonnegative(t2);
+    t3 = normalize_nonnegative(t3);
+
+    if (t1 < 0.0 || t2 < 0.0 || t3 < 0.0) {
+      return;
+    }
+
+    SCurveVelocityProfile cand;
+    cand.t1 = t1;
+    cand.t2 = t2;
+    cand.t3 = t3;
+    cand.total_time = t1 + t2 + t3;
+
+    cand.j1 = s * j1s;
+    cand.j2 = s * j2s;
+    cand.j3 = s * j3s;
+
+    cand.v0 = v0;
+    cand.a0 = a0;
+    cand.vf = vf;
+    cand.valid = true;
+
+    double vend = 0.0;
+    double aend = 0.0;
+    integrateWholeProfile(cand, vend, aend);
+
+    if (std::abs(vend - vf) > 1e-5) {
+      return;
+    }
+    if (std::abs(aend) > 1e-5) {
+      return;
+    }
+    if (!isVelocityLimitSatisfied(cand)) {
+      return;
+    }
+
+    if (!best.valid || cand.total_time < best.total_time) {
+      best = cand;
+    }
+  };
+
+  // ---------------- UDU 三角形（无平台） ----------------
+  {
+    // Δv = (2A^2 - a0^2)/(2j)
+    const double A2 = 0.5 * (2.0 * j * dv + a0s * a0s);
+    if (A2 >= -1e-10) {
+      const double A = std::sqrt(std::max(0.0, A2));
+      if (A <= amax + 1e-10 && A >= a0s - 1e-10) {
+        const double t1 = (A - a0s) / j;
+        const double t2 = 0.0;
+        const double t3 = A / j;
+        try_candidate(t1, t2, t3, +j, 0.0, -j);
+      }
+    }
+  }
+
+  // ---------------- UDU 梯形（有平台） ----------------
+  {
+    const double A = amax;
+    if (A > EPSILON && A >= a0s - 1e-10) {
+      const double base = (2.0 * A * A - a0s * a0s) / (2.0 * j);
+      const double t1 = (A - a0s) / j;
+      const double t2 = (dv - base) / A;
+      const double t3 = A / j;
+      try_candidate(t1, t2, t3, +j, 0.0, -j);
+    }
+  }
+
+  // ---------------- DUD 三角形（无平台） ----------------
+  {
+    // Δv = (a0^2 - 2B^2)/(2j)
+    const double B2 = 0.5 * (a0s * a0s - 2.0 * j * dv);
+    if (B2 >= -1e-10) {
+      const double B = std::sqrt(std::max(0.0, B2));
+      if (B <= amax + 1e-10 && B >= -a0s - 1e-10) {
+        const double t1 = (a0s + B) / j;
+        const double t2 = 0.0;
+        const double t3 = B / j;
+        try_candidate(t1, t2, t3, -j, 0.0, +j);
+      }
+    }
+  }
+
+  // ---------------- DUD 梯形（有平台） ----------------
+  {
+    const double B = amax;
+    if (B > EPSILON && B >= -a0s - 1e-10) {
+      const double base = (a0s * a0s - 2.0 * B * B) / (2.0 * j);
+      const double t1 = (a0s + B) / j;
+      const double t2 = (base - dv) / B;
+      const double t3 = B / j;
+      try_candidate(t1, t2, t3, -j, 0.0, +j);
+    }
+  }
+
+  // 已到终点附近
+  if (!best.valid &&
+      std::abs(vf - v0) <= VELOCITY_THRESHOLD &&
+      std::abs(a0) <= ACCELERATION_THRESHOLD) {
+    best.valid = false;
+    best.total_time = 0.0;
+    best.v0 = v0;
+    best.a0 = a0;
+    best.vf = vf;
+  }
+
+  return best;
+}
+
+void SCurveVelocityPlanner1D::generateTrajectory(const SCurveVelocityProfile& profile, double dt,
+                                                 std::vector<double>& velocities,
+                                                 std::vector<double>& accelerations,
+                                                 std::vector<double>& jerks) const
+{
+  velocities.clear();
+  accelerations.clear();
+  jerks.clear();
+
+  if (!profile.valid || profile.total_time <= 0.0 || dt <= 0.0) {
+    velocities.push_back(profile.v0);
+    accelerations.push_back(profile.a0);
+    jerks.push_back(0.0);
+    return;
+  }
+
+  const int num_points = static_cast<int>(std::ceil(profile.total_time / dt)) + 1;
+  velocities.reserve(num_points);
+  accelerations.reserve(num_points);
+  jerks.reserve(num_points);
+
+  for (double t = 0.0; t <= profile.total_time; t += dt) {
+    velocities.push_back(getVelocityAtTime(profile, t));
+    accelerations.push_back(getAccelerationAtTime(profile, t));
+    jerks.push_back(getJerkAtTime(profile, t));
+  }
+
+  if (velocities.empty() || std::abs(velocities.back() - profile.vf) > VELOCITY_THRESHOLD) {
+    velocities.push_back(profile.vf);
+    accelerations.push_back(0.0);
+    jerks.push_back(0.0);
+  }
+}
+
+double SCurveVelocityPlanner1D::update(double dt)
+{
+  if (dt <= 0.0) {
+    return state_.velocity;
+  }
+
+  if (!state_.has_active_profile || !active_profile_.valid) {
+    state_.jerk = 0.0;
+    updateIsMovingFlag();
+    return state_.velocity;
+  }
+
+  state_.elapsed_time += dt;
+
+  if (state_.elapsed_time >= active_profile_.total_time) {
+    state_.elapsed_time = active_profile_.total_time;
+    state_.velocity = active_profile_.vf;
+    state_.acceleration = 0.0;
+    state_.jerk = 0.0;
+    state_.has_active_profile = false;
+    updateIsMovingFlag();
+    return state_.velocity;
+  }
+
+  state_.velocity = getVelocityAtTime(active_profile_, state_.elapsed_time);
+  state_.acceleration = getAccelerationAtTime(active_profile_, state_.elapsed_time);
+  state_.jerk = getJerkAtTime(active_profile_, state_.elapsed_time);
+
+  updateIsMovingFlag();
+  return state_.velocity;
+}
+
+void SCurveVelocityPlanner1D::updateIsMovingFlag()
+{
+  state_.is_moving =
+      state_.has_active_profile ||
+      std::abs(state_.velocity) > VELOCITY_THRESHOLD ||
+      std::abs(state_.target_velocity) > VELOCITY_THRESHOLD ||
+      std::abs(state_.acceleration) > ACCELERATION_THRESHOLD;
+}
+
+} // namespace cmvr

applications/servo_manager/SConscript

@@ -5,6 +5,6 @@ cwd = GetCurrentDir()
 src = Glob('src/*.cpp')
 CPPPATH = [cwd]
 
-group = DefineGroup('servo_manager', src, depend = [''], CPPPATH = CPPPATH)
+group = DefineGroup('servo_control', src, depend = [''], CPPPATH = CPPPATH)
 
 Return('group')

applications/servo_manager/include/servo_control.h

@@ -0,0 +1,67 @@
+//
+// Created by Administrator on 2026/4/22.
+//
+
+#ifndef SERVO_CONTROL_H
+#define SERVO_CONTROL_H
+
+#include <stdint.h>
+#include <string>
+
+#include "common/type/servo_types.h"
+#include "common/curve/include/s_curve.h"
+#include "servo_manager/include/servo_driver.h"
+#include <rtthread.h>
+
+
+class ServoControl
+{
+public:
+    explicit ServoControl(const ServoDriver::Config& cfg);
+    ~ServoControl();
+
+    rt_err_t init(bool enable_after_init = true, bool go_home = true);
+
+    rt_err_t move(float angle_rad, MotionMode mode);
+    rt_err_t stop();
+
+    void setConstraints(double max_velocity_rad,
+                        double max_acceleration_rad,
+                        double max_jerk_rad);
+
+    void update(rt_tick_t now_tick);
+
+    const std::string& id() const;
+
+    float currentAngle() const;
+    float targetAngle() const;
+    float currentVelocity() const;
+
+    bool isActive() const;
+
+private:
+    void sampleStateNoLock(rt_tick_t now_tick);
+    double ticksToSeconds(rt_tick_t delta_ticks) const;
+
+private:
+    // 角度变化阈值，rad
+    static constexpr float ESP = 1e-4f;
+    ServoDriver driver_;
+    cmvr::SCurve curve_;
+
+    float current_angle_rad_;
+    float current_velocity_rad_per_sec_;
+    float target_angle_rad_;
+
+    rt_tick_t start_tick_;
+    uint32_t generation_;
+
+    bool active_;
+    bool dirty_;
+
+    cmvr::SCurveProfile profile_;
+
+    rt_mutex_t mutex_;
+};
+
+#endif // SERVO_CONTROL_H

applications/servo_manager/include/servo_driver.h

@@ -0,0 +1,61 @@
+//
+// Created by Administrator on 2026/4/22.
+//
+
+#ifndef RTTHREAD_SERVO_DRIVER_H
+#define RTTHREAD_SERVO_DRIVER_H
+
+
+#include <rtdevice.h>
+#include <rtthread.h>
+
+#include <stdint.h>
+#include <string>
+
+class ServoDriver
+{
+public:
+    struct Config
+    {
+        std::string servo_id;
+        std::string pwm_dev_name;
+        int pwm_channel;
+
+        rt_uint32_t period_ns;
+        rt_uint32_t min_pulse_ns;
+        rt_uint32_t max_pulse_ns;
+
+        float min_angle_rad;
+        float max_angle_rad;
+        float home_angle_rad;
+    };
+
+public:
+    explicit ServoDriver(const Config& cfg);
+    ~ServoDriver();
+
+    rt_err_t init(bool enable_after_init = true, bool go_home = true);
+
+    rt_err_t enable(bool on);
+    rt_err_t setAngle(float angle_rad);
+
+    const std::string& id() const;
+    const Config& config() const;
+
+private:
+    rt_err_t enableNoLock(bool on);
+    rt_err_t setAngleNoLock(float angle_rad);
+
+    static float clampf(float v, float lo, float hi);
+    static rt_uint32_t angleToPulseNs(const Config& cfg, float angle_rad);
+
+private:
+    Config cfg_;
+    rt_device_pwm* pwm_dev_;
+    rt_uint32_t last_pulse_;
+    bool inited_;
+
+    rt_mutex_t mutex_;
+};
+
+#endif //RTTHREAD_SERVO_DRIVER_H

applications/servo_manager/include/servo_manager.h

@@ -1,83 +1,54 @@
 //
-// Created by Administrator on 2026/1/7.
+// Created by Administrator on 2026/4/22.
 //
 
 #ifndef SERVO_MANAGER_H
 #define SERVO_MANAGER_H
 
-extern "C" {
-#include <rtthread.h>
-#include <rtdevice.h>
-}
-
-#include <stdint.h>
 #include <stddef.h>
+#include <stdint.h>
+#include <string>
 #include <vector>
 
+#include "servo_manager/include/servo_control.h"
+#include "servo_manager/include/servo_driver.h"
+
+#include <rtthread.h>
+
 class ServoManager
 {
 public:
-    struct ServoConfig
+    explicit ServoManager(const std::vector<ServoDriver::Config>& cfg);
-    {
-        const char* servo_id;     // 唯一ID
-        const char* pwm_dev_name;
-        int         pwm_channel;
-
-        // ns
-        rt_uint32_t period_ns;
-        rt_uint32_t min_pulse_ns;
-        rt_uint32_t max_pulse_ns;
-
-        float       min_angle_deg;
-        float       max_angle_deg;
-        float       home_angle_deg;
-    };
-
-    struct ServoCmd
-    {
-        const char* id;
-        float       angle_deg;
-    };
-
-public:
-
-    explicit ServoManager(const std::vector<ServoConfig> &cfg);
     ~ServoManager();
 
-    size_t count() const { return _cfg.size(); }
-
     rt_err_t init(bool enable_after_init = true, bool go_home = true);
 
-    rt_err_t enable(const char* servo_id, bool on);
+    ServoControl* get(const std::string& servo_id);
-    rt_err_t setAngle(const char* servo_id, float angle_deg);
+    const ServoControl* get(const std::string& servo_id) const;
 
-    // 全部设置：两种形态（vector / 指针）
+    size_t count() const;
-    rt_err_t setAllAngles(const std::vector<float>& angles_deg);
-    rt_err_t setAllAngles(const float* angles_deg, size_t n);
 
-    // 批量设置：两种形态（vector / 指针）
+    rt_err_t start();
-    rt_err_t setAngles(const std::vector<ServoCmd>& cmds);
+    void stop();
-    rt_err_t setAngles(const ServoCmd* cmds, size_t n);
 
-    const char* idAt(size_t order) const;
+    void setUpdatePeriodMs(rt_uint32_t update_period_ms);
-    int orderOf(const char* servo_id) const;
+    void wake();
 
 private:
-    rt_err_t enableNoLock(size_t order, bool on);
+    static void threadEntry(void* parameter);
-    rt_err_t setAngleNoLock(size_t order, float angle_deg);
+    void run();
 
-    int findOrderByIdNoLock(const char* servo_id) const;
+    int findIndexByIdNoLock(const std::string& servo_id) const;
-
-    static float clampf(float v, float lo, float hi);
-    static rt_uint32_t angleToPulseNs(const ServoConfig& c, float angle_deg);
 
 private:
-    std::vector<ServoConfig>   _cfg;
+    std::vector<ServoControl> controls_;
-    std::vector<rt_device_pwm*> _pwm_dev;
-    std::vector<rt_uint32_t>    _last_pulse;
 
-    rt_mutex_t _mutex;
+    rt_mutex_t mutex_;
+    rt_sem_t wake_sem_;
+    rt_thread_t thread_;
+
+    rt_uint32_t update_period_ms_{1};
+    volatile bool running_;
 };
 
 #endif // SERVO_MANAGER_H
-

applications/servo_manager/src/servo_control.cpp

@@ -0,0 +1,283 @@
+//
+// Created by Administrator on 2026/4/22.
+//
+
+#include "servo_manager/include/servo_control.h"
+
+#include <cmath>
+
+
+ServoControl::ServoControl(const ServoDriver::Config& cfg)
+    : driver_(cfg),
+      curve_(),
+      current_angle_rad_(cfg.home_angle_rad),
+      current_velocity_rad_per_sec_(0.0f),
+      target_angle_rad_(cfg.home_angle_rad),
+      start_tick_(0),
+      generation_(0),
+      active_(false),
+      dirty_(false),
+      profile_(),
+      mutex_(RT_NULL)
+{
+    mutex_ = rt_mutex_create("sv_ctl", RT_IPC_FLAG_PRIO);
+}
+
+ServoControl::~ServoControl()
+{
+    if (mutex_) {
+        rt_mutex_delete(mutex_);
+        mutex_ = RT_NULL;
+    }
+}
+
+rt_err_t ServoControl::init(bool enable_after_init, bool go_home)
+{
+    const rt_err_t err = driver_.init(enable_after_init, go_home);
+    if (err != RT_EOK) {
+        return err;
+    }
+
+    if (!mutex_) {
+        return -RT_ENOMEM;
+    }
+
+    rt_mutex_take(mutex_, RT_WAITING_FOREVER);
+
+    const float init_angle = go_home ? driver_.config().home_angle_rad
+                                     : driver_.config().min_angle_rad;
+
+    current_angle_rad_ = init_angle;
+    current_velocity_rad_per_sec_ = 0.0f;
+    target_angle_rad_ = init_angle;
+    start_tick_ = rt_tick_get();
+    generation_ = 0;
+    active_ = false;
+    dirty_ = false;
+
+    rt_mutex_release(mutex_);
+    return RT_EOK;
+}
+
+rt_err_t ServoControl::move(float angle_rad, MotionMode mode)
+{
+    if (!mutex_) {
+        return -RT_ENOMEM;
+    }
+
+    rt_mutex_take(mutex_, RT_WAITING_FOREVER);
+
+    const rt_tick_t now_tick = rt_tick_get();
+    sampleStateNoLock(now_tick);
+
+    ++generation_;
+    target_angle_rad_ = angle_rad;
+
+    if (mode == MotionMode::Immediate) {
+        current_angle_rad_ = angle_rad;
+        current_velocity_rad_per_sec_ = 0.0f;
+        start_tick_ = now_tick;
+        active_ = false;
+        dirty_ = false;
+
+        rt_mutex_release(mutex_);
+        return driver_.setAngle(angle_rad);
+    }
+
+    if (mode != MotionMode::SCurve) {
+        rt_mutex_release(mutex_);
+        return -RT_EINVAL;
+    }
+
+    const float start_angle_rad = current_angle_rad_;
+    const float start_velocity_rad_per_sec = current_velocity_rad_per_sec_;
+    profile_ = curve_.calculateProfile(
+        current_angle_rad_, angle_rad, current_velocity_rad_per_sec_, 0.0);
+
+    if (profile_.total_time <= 0.0) {
+        profile_ = curve_.calculateProfile(
+            current_angle_rad_, angle_rad, 0.0, 0.0);
+    }
+
+    start_tick_ = now_tick;
+
+    if (profile_.total_time <= 0.0) {
+        if (std::fabs(angle_rad - start_angle_rad) > ESP) {
+            rt_kprintf("[ServoControl][W] scurve fallback to immediate, id=%s start=%.4f target=%.4f vel=%.4f\n",
+                       id().c_str(),
+                       (double)start_angle_rad,
+                       (double)angle_rad,
+                       (double)start_velocity_rad_per_sec);
+        }
+        current_angle_rad_ = angle_rad;
+        current_velocity_rad_per_sec_ = 0.0f;
+        active_ = false;
+        dirty_ = false;
+
+        rt_mutex_release(mutex_);
+        return driver_.setAngle(angle_rad);
+    }
+
+    const double tick_sec = 1.0 / RT_TICK_PER_SECOND;
+    if (profile_.total_time <= tick_sec) {
+        rt_kprintf("[ServoControl][W] scurve duration too short, id=%s duration=%.6f tick=%.6f\n",
+                   id().c_str(),
+                   profile_.total_time,
+                   tick_sec);
+    }
+
+    active_ = true;
+    dirty_ = false;
+
+    rt_mutex_release(mutex_);
+    return RT_EOK;
+}
+
+rt_err_t ServoControl::stop()
+{
+    if (!mutex_) {
+        return -RT_ENOMEM;
+    }
+
+    rt_mutex_take(mutex_, RT_WAITING_FOREVER);
+
+    const rt_tick_t now_tick = rt_tick_get();
+    sampleStateNoLock(now_tick);
+
+    ++generation_;
+    target_angle_rad_ = current_angle_rad_;
+    current_velocity_rad_per_sec_ = 0.0f;
+    start_tick_ = now_tick;
+    active_ = false;
+    dirty_ = false;
+
+    const float hold_angle = current_angle_rad_;
+
+    rt_mutex_release(mutex_);
+    return driver_.setAngle(hold_angle);
+}
+
+void ServoControl::setConstraints(double max_velocity_rad,
+                                  double max_acceleration_rad,
+                                  double max_jerk_rad)
+{
+    if (!mutex_) {
+        curve_.setConstraints(max_velocity_rad, max_acceleration_rad, max_jerk_rad);
+        return;
+    }
+
+    rt_mutex_take(mutex_, RT_WAITING_FOREVER);
+    curve_.setConstraints(max_velocity_rad, max_acceleration_rad, max_jerk_rad);
+    rt_mutex_release(mutex_);
+}
+
+void ServoControl::update(rt_tick_t now_tick)
+{
+    if (!mutex_) {
+        return;
+    }
+
+    float output_angle = 0.0f;
+    bool should_write = false;
+
+    rt_mutex_take(mutex_, RT_WAITING_FOREVER);
+
+    const float previous_angle = current_angle_rad_;
+    sampleStateNoLock(now_tick);
+
+    if (dirty_ || std::fabs(current_angle_rad_ - previous_angle) > ESP) {
+        output_angle = current_angle_rad_;
+        dirty_ = false;
+        should_write = true;
+    }
+
+    rt_mutex_release(mutex_);
+
+    if (should_write) {
+        const rt_err_t err = driver_.setAngle(output_angle);
+        if (err != RT_EOK) {
+            rt_kprintf("[ServoControl][E] setAngle failed, id=%s angle=%.4f err=%d\n",
+                       id().c_str(),
+                       (double)output_angle,
+                       (int)err);
+        }
+    }
+}
+
+const std::string& ServoControl::id() const
+{
+    return driver_.id();
+}
+
+float ServoControl::currentAngle() const
+{
+    if (!mutex_) {
+        return current_angle_rad_;
+    }
+
+    rt_mutex_take(mutex_, RT_WAITING_FOREVER);
+    const float v = current_angle_rad_;
+    rt_mutex_release(mutex_);
+    return v;
+}
+
+float ServoControl::targetAngle() const
+{
+    if (!mutex_) {
+        return target_angle_rad_;
+    }
+
+    rt_mutex_take(mutex_, RT_WAITING_FOREVER);
+    const float v = target_angle_rad_;
+    rt_mutex_release(mutex_);
+    return v;
+}
+
+float ServoControl::currentVelocity() const
+{
+    if (!mutex_) {
+        return current_velocity_rad_per_sec_;
+    }
+
+    rt_mutex_take(mutex_, RT_WAITING_FOREVER);
+    const float v = current_velocity_rad_per_sec_;
+    rt_mutex_release(mutex_);
+    return v;
+}
+
+bool ServoControl::isActive() const
+{
+    if (!mutex_) {
+        return active_;
+    }
+
+    rt_mutex_take(mutex_, RT_WAITING_FOREVER);
+    const bool v = active_;
+    rt_mutex_release(mutex_);
+    return v;
+}
+
+void ServoControl::sampleStateNoLock(rt_tick_t now_tick)
+{
+    if (!active_) {
+        return;
+    }
+
+    const double elapsed_sec = ticksToSeconds(now_tick - start_tick_);
+    if (elapsed_sec >= profile_.total_time) {
+        current_angle_rad_ = target_angle_rad_;
+        current_velocity_rad_per_sec_ = 0.0f;
+        active_ = false;
+        return;
+    }
+
+    current_angle_rad_ = static_cast<float>(
+        curve_.getPositionAtTime(profile_, elapsed_sec));
+    current_velocity_rad_per_sec_ = static_cast<float>(
+        curve_.getVelocityAtTime(profile_, elapsed_sec));
+}
+
+double ServoControl::ticksToSeconds(rt_tick_t delta_ticks) const
+{
+    return static_cast<double>(delta_ticks) / RT_TICK_PER_SECOND;
+}

applications/servo_manager/src/servo_driver.cpp

@@ -0,0 +1,188 @@
+//
+// Created by Administrator on 2026/4/22.
+//
+
+#include "servo_manager/include/servo_driver.h"
+
+ServoDriver::ServoDriver(const Config& cfg)
+    : cfg_(cfg),
+      pwm_dev_(RT_NULL),
+      last_pulse_(0),
+      inited_(false),
+      mutex_(RT_NULL)
+{
+    mutex_ = rt_mutex_create("sv_drv", RT_IPC_FLAG_PRIO);
+}
+
+ServoDriver::~ServoDriver()
+{
+    if (mutex_) {
+        rt_mutex_delete(mutex_);
+        mutex_ = RT_NULL;
+    }
+}
+
+rt_err_t ServoDriver::init(bool enable_after_init, bool go_home)
+{
+    if (cfg_.servo_id.empty() || cfg_.pwm_dev_name.empty()) {
+        rt_kprintf("[ServoDriver][E] init: invalid cfg, id=%s pwm=%s\n",
+                   cfg_.servo_id.c_str(),
+                   cfg_.pwm_dev_name.c_str());
+        return -RT_EINVAL;
+    }
+
+    if (!mutex_) {
+        return -RT_ENOMEM;
+    }
+
+    rt_mutex_take(mutex_, RT_WAITING_FOREVER);
+
+    pwm_dev_ = (rt_device_pwm*)rt_device_find(cfg_.pwm_dev_name.c_str());
+    if (!pwm_dev_) {
+        rt_kprintf("[ServoDriver][E] init: rt_device_find failed, id=%s pwm=%s ch=%d\n",
+                   cfg_.servo_id.c_str(),
+                   cfg_.pwm_dev_name.c_str(),
+                   cfg_.pwm_channel);
+        rt_mutex_release(mutex_);
+        return -RT_ENOSYS;
+    }
+
+    const float init_angle = go_home ? cfg_.home_angle_rad : cfg_.min_angle_rad;
+    const rt_uint32_t pulse = angleToPulseNs(cfg_, init_angle);
+
+    rt_err_t err = rt_pwm_set(pwm_dev_, cfg_.pwm_channel, cfg_.period_ns, pulse);
+    if (err != RT_EOK) {
+        rt_kprintf("[ServoDriver][E] init: rt_pwm_set failed, id=%s ch=%d pulse=%u err=%d\n",
+                   cfg_.servo_id.c_str(),
+                   cfg_.pwm_channel,
+                   (unsigned)pulse,
+                   (int)err);
+        rt_mutex_release(mutex_);
+        return err;
+    }
+
+    last_pulse_ = pulse;
+    inited_ = true;
+
+    if (enable_after_init) {
+        err = rt_pwm_enable(pwm_dev_, cfg_.pwm_channel);
+        if (err != RT_EOK) {
+            rt_kprintf("[ServoDriver][E] init: rt_pwm_enable failed, id=%s ch=%d err=%d\n",
+                       cfg_.servo_id.c_str(),
+                       cfg_.pwm_channel,
+                       (int)err);
+            rt_mutex_release(mutex_);
+            return err;
+        }
+    }
+
+    rt_mutex_release(mutex_);
+
+    rt_kprintf("[ServoDriver] init OK: id=%s pwm=%s ch=%d enable=%d go_home=%d\n",
+               cfg_.servo_id.c_str(),
+               cfg_.pwm_dev_name.c_str(),
+               cfg_.pwm_channel,
+               (int)enable_after_init,
+               (int)go_home);
+
+    return RT_EOK;
+}
+
+rt_err_t ServoDriver::enable(bool on)
+{
+    if (!mutex_) {
+        return -RT_ENOMEM;
+    }
+
+    rt_mutex_take(mutex_, RT_WAITING_FOREVER);
+    const rt_err_t err = enableNoLock(on);
+    rt_mutex_release(mutex_);
+    return err;
+}
+
+rt_err_t ServoDriver::setAngle(float angle_rad)
+{
+    if (!mutex_) {
+        return -RT_ENOMEM;
+    }
+
+    rt_mutex_take(mutex_, RT_WAITING_FOREVER);
+    const rt_err_t err = setAngleNoLock(angle_rad);
+    rt_mutex_release(mutex_);
+    return err;
+}
+
+const std::string& ServoDriver::id() const
+{
+    return cfg_.servo_id;
+}
+
+const ServoDriver::Config& ServoDriver::config() const
+{
+    return cfg_;
+}
+
+rt_err_t ServoDriver::enableNoLock(bool on)
+{
+    if (!inited_ || !pwm_dev_) {
+        return -RT_ERROR;
+    }
+
+    return on ? rt_pwm_enable(pwm_dev_, cfg_.pwm_channel)
+              : rt_pwm_disable(pwm_dev_, cfg_.pwm_channel);
+}
+
+rt_err_t ServoDriver::setAngleNoLock(float angle_rad)
+{
+    if (!inited_ || !pwm_dev_) {
+        return -RT_ERROR;
+    }
+
+    const rt_uint32_t pulse = angleToPulseNs(cfg_, angle_rad);
+    if (pulse == last_pulse_) {
+        return RT_EOK;
+    }
+
+    const rt_err_t err = rt_pwm_set(pwm_dev_, cfg_.pwm_channel, cfg_.period_ns, pulse);
+    if (err != RT_EOK) {
+        return err;
+    }
+
+    last_pulse_ = pulse;
+    return RT_EOK;
+}
+
+float ServoDriver::clampf(float v, float lo, float hi)
+{
+    if (v < lo) {
+        return lo;
+    }
+    if (v > hi) {
+        return hi;
+    }
+    return v;
+}
+
+rt_uint32_t ServoDriver::angleToPulseNs(const Config& cfg, float angle_rad)
+{
+    const float a = clampf(angle_rad, cfg.min_angle_rad, cfg.max_angle_rad);
+
+    const float in_span = cfg.max_angle_rad - cfg.min_angle_rad;
+    const float out_span = (float)(cfg.max_pulse_ns - cfg.min_pulse_ns);
+
+    float t = 0.0f;
+    if (in_span > 1e-6f) {
+        t = (a - cfg.min_angle_rad) / in_span;
+    }
+
+    float pulse = (float)cfg.min_pulse_ns + t * out_span;
+
+    if (pulse < (float)cfg.min_pulse_ns) {
+        pulse = (float)cfg.min_pulse_ns;
+    }
+    if (pulse > (float)cfg.max_pulse_ns) {
+        pulse = (float)cfg.max_pulse_ns;
+    }
+
+    return (rt_uint32_t)(pulse + 0.5f);
+}

applications/servo_manager/src/servo_manager.cpp

@@ -1,283 +1,213 @@
 //
-// Created by Administrator on 2026/1/7.
+// Created by Administrator on 2026/4/22.
 //
 
 #include "servo_manager/include/servo_manager.h"
 
-
+ServoManager::ServoManager(const std::vector<ServoDriver::Config>& cfg)
-
+    : controls_(),
-ServoManager::ServoManager(const std::vector<ServoConfig> &cfg)
+      mutex_(RT_NULL),
-: _cfg(std::move(cfg)),
+      wake_sem_(RT_NULL),
-  _pwm_dev(_cfg.size(), RT_NULL),
+      thread_(RT_NULL),
-  _last_pulse(_cfg.size(), 0),
+      running_(false)
-  _mutex(RT_NULL)
 {
-    _mutex = rt_mutex_create("sv_mtx", RT_IPC_FLAG_PRIO);
+    controls_.reserve(cfg.size());
+    for (const auto& c : cfg) {
+        controls_.emplace_back(c);
+    }
+
+    mutex_ = rt_mutex_create("sv_mgr", RT_IPC_FLAG_PRIO);
+    wake_sem_ = rt_sem_create("sv_wk", 0, RT_IPC_FLAG_PRIO);
 }
 
 ServoManager::~ServoManager()
 {
-    if (_mutex)
+    stop();
-    {
+
-        rt_mutex_delete(_mutex);
+    if (mutex_) {
-        _mutex = RT_NULL;
+        rt_mutex_delete(mutex_);
+        mutex_ = RT_NULL;
+    }
+
+    if (wake_sem_) {
+        rt_sem_delete(wake_sem_);
+        wake_sem_ = RT_NULL;
     }
 }
 
 rt_err_t ServoManager::init(bool enable_after_init, bool go_home)
 {
-    if (_cfg.empty())
+    if (!mutex_) {
-    {
-        rt_kprintf("[ServoManager][E] init: empty cfg\n");
-        return -RT_EINVAL;
-    }
-
-    if (!_mutex)
-    {
-        rt_kprintf("[ServoManager][E] init: mutex not created\n");
         return -RT_ENOMEM;
     }
 
-    rt_mutex_take(_mutex, RT_WAITING_FOREVER);
+    rt_mutex_take(mutex_, RT_WAITING_FOREVER);
 
-    for (size_t i = 0; i < _cfg.size(); ++i)
+    for (size_t i = 0; i < controls_.size(); ++i) {
-    {
+        const rt_err_t err = controls_[i].init(enable_after_init, go_home);
-        const auto& c = _cfg[i];
+        if (err != RT_EOK) {
-
+            rt_mutex_release(mutex_);
-        rt_device_pwm* dev = (rt_device_pwm*)rt_device_find(c.pwm_dev_name);
-        if (!dev)
-        {
-            rt_kprintf("[ServoManager][E] init: rt_device_find failed, i=%d id=%s pwm=%s ch=%d\n",
-                       (int)i, c.servo_id ? c.servo_id : "null", c.pwm_dev_name ? c.pwm_dev_name : "null", c.pwm_channel);
-            rt_mutex_release(_mutex);
-            return -RT_ENOSYS;
-        }
-        _pwm_dev[i] = dev;
-
-        const float init_angle = go_home ? c.home_angle_deg : c.min_angle_deg;
-        const rt_uint32_t pulse = angleToPulseNs(c, init_angle);
-
-        rt_err_t err = rt_pwm_set(_pwm_dev[i], c.pwm_channel, c.period_ns, pulse);
-        if (err != RT_EOK)
-        {
-            rt_kprintf("[ServoManager][E] init: rt_pwm_set failed, i=%d id=%s pwm=%s ch=%d period=%u pulse=%u err=%d\n",
-                       (int)i, c.servo_id ? c.servo_id : "null", c.pwm_dev_name ? c.pwm_dev_name : "null",
-                       c.pwm_channel, (unsigned)c.period_ns, (unsigned)pulse, (int)err);
-            rt_mutex_release(_mutex);
-            return err;
-        }
-        _last_pulse[i] = pulse;
-
-        if (enable_after_init)
-        {
-            err = rt_pwm_enable(_pwm_dev[i], c.pwm_channel);
-            if (err != RT_EOK)
-            {
-                rt_kprintf("[ServoManager][E] init: rt_pwm_enable failed, i=%d id=%s pwm=%s ch=%d err=%d\n",
-                           (int)i, c.servo_id ? c.servo_id : "null", c.pwm_dev_name ? c.pwm_dev_name : "null",
-                           c.pwm_channel, (int)err);
-                rt_mutex_release(_mutex);
-                return err;
-            }
-        }
-
-
-        rt_kprintf("[ServoManager] init OK: i=%d id=%s pwm=%s ch=%d\n", (int)i, c.servo_id, c.pwm_dev_name, c.pwm_channel);
-    }
-
-    rt_mutex_release(_mutex);
-
-    //
-    rt_kprintf("[ServoManager] init OK: count=%d enable=%d go_home=%d\n",
-               (int)_cfg.size(), (int)enable_after_init, (int)go_home);
-
-    return RT_EOK;
-}
-
-
-rt_err_t ServoManager::enable(const char* servo_id, bool on)
-{
-    if (!servo_id) return -RT_EINVAL;
-    if (!_mutex) return -RT_ENOMEM;
-
-    rt_mutex_take(_mutex, RT_WAITING_FOREVER);
-
-    int order = findOrderByIdNoLock(servo_id);
-    if (order < 0)
-    {
-        rt_mutex_release(_mutex);
-        return -RT_ENOSYS;
-    }
-
-    rt_err_t err = enableNoLock((size_t)order, on);
-    rt_mutex_release(_mutex);
-    return err;
-}
-
-rt_err_t ServoManager::setAngle(const char* servo_id, float angle_deg)
-{
-    if (!servo_id) return -RT_EINVAL;
-    if (!_mutex) return -RT_ENOMEM;
-
-    rt_mutex_take(_mutex, RT_WAITING_FOREVER);
-
-    int order = findOrderByIdNoLock(servo_id);
-    if (order < 0)
-    {
-        rt_mutex_release(_mutex);
-        return -RT_ENOSYS;
-    }
-
-    rt_err_t err = setAngleNoLock((size_t)order, angle_deg);
-    rt_mutex_release(_mutex);
-    return err;
-}
-
-rt_err_t ServoManager::setAllAngles(const std::vector<float>& angles_deg)
-{
-    if (angles_deg.size() < _cfg.size()) return -RT_EINVAL;
-    return setAllAngles(angles_deg.data(), angles_deg.size());
-}
-
-rt_err_t ServoManager::setAllAngles(const float* angles_deg, size_t n)
-{
-    if (!angles_deg || n < _cfg.size()) return -RT_EINVAL;
-    if (!_mutex) return -RT_ENOMEM;
-
-    rt_mutex_take(_mutex, RT_WAITING_FOREVER);
-
-    for (size_t i = 0; i < _cfg.size(); ++i)
-    {
-        rt_err_t err = setAngleNoLock(i, angles_deg[i]);
-        if (err != RT_EOK)
-        {
-            rt_mutex_release(_mutex);
             return err;
         }
     }
 
-    rt_mutex_release(_mutex);
+    rt_mutex_release(mutex_);
     return RT_EOK;
 }
 
-rt_err_t ServoManager::setAngles(const std::vector<ServoCmd>& cmds)
+ServoControl* ServoManager::get(const std::string& servo_id)
 {
-    if (cmds.empty()) return -RT_EINVAL;
+    if (!mutex_) {
-    return setAngles(cmds.data(), cmds.size());
+        return RT_NULL;
-}
-
-rt_err_t ServoManager::setAngles(const ServoCmd* cmds, size_t n)
-{
-    if (!cmds || n == 0) return -RT_EINVAL;
-    if (!_mutex) return -RT_ENOMEM;
-
-    rt_mutex_take(_mutex, RT_WAITING_FOREVER);
-
-    for (size_t i = 0; i < n; ++i)
-    {
-        if (!cmds[i].id)
-        {
-            rt_mutex_release(_mutex);
-            return -RT_EINVAL;
-        }
-
-        int order = findOrderByIdNoLock(cmds[i].id);
-        if (order < 0)
-        {
-            rt_mutex_release(_mutex);
-            return -RT_ENOSYS;
-        }
-
-        rt_err_t err = setAngleNoLock((size_t)order, cmds[i].angle_deg);
-        if (err != RT_EOK)
-        {
-            rt_mutex_release(_mutex);
-            return err;
-        }
     }
 
-    rt_mutex_release(_mutex);
+    rt_mutex_take(mutex_, RT_WAITING_FOREVER);
-    return RT_EOK;
+    const int index = findIndexByIdNoLock(servo_id);
+    ServoControl* control = (index >= 0) ? &controls_[(size_t)index] : RT_NULL;
+    rt_mutex_release(mutex_);
+    return control;
 }
 
-const char* ServoManager::idAt(size_t order) const
+const ServoControl* ServoManager::get(const std::string& servo_id) const
 {
-    if (order >= _cfg.size()) return RT_NULL;
+    if (!mutex_) {
-    return _cfg[order].servo_id;
+        return RT_NULL;
-}
-
-int ServoManager::orderOf(const char* servo_id) const
-{
-    if (!servo_id) return -1;
-    for (size_t i = 0; i < _cfg.size(); ++i)
-    {
-        const char* sid = _cfg[i].servo_id;
-        if (sid && (rt_strcmp(sid, servo_id) == 0)) return (int)i;
     }
-    return -1;
+
+    rt_mutex_take(mutex_, RT_WAITING_FOREVER);
+    const int index = findIndexByIdNoLock(servo_id);
+    const ServoControl* control = (index >= 0) ? &controls_[(size_t)index] : RT_NULL;
+    rt_mutex_release(mutex_);
+    return control;
 }
 
-// ---------- private ----------
+size_t ServoManager::count() const
-
-rt_err_t ServoManager::enableNoLock(size_t order, bool on)
 {
-    if (order >= _cfg.size()) return -RT_EINVAL;
+    return controls_.size();
-    if (!_pwm_dev[order]) return -RT_ERROR;
-
-    return on ? rt_pwm_enable(_pwm_dev[order], _cfg[order].pwm_channel)
-              : rt_pwm_disable(_pwm_dev[order], _cfg[order].pwm_channel);
 }
 
-rt_err_t ServoManager::setAngleNoLock(size_t order, float angle_deg)
+rt_err_t ServoManager::start()
 {
-    if (order >= _cfg.size()) return -RT_EINVAL;
+    if (!mutex_ || !wake_sem_) {
-    if (!_pwm_dev[order]) return -RT_ERROR;
+        return -RT_ENOMEM;
+    }
 
-    const rt_uint32_t pulse = angleToPulseNs(_cfg[order], angle_deg);
+    if (running_) {
+        return RT_EOK;
+    }
 
-    if (pulse != _last_pulse[order])
+    running_ = true;
-    {
+
-        rt_err_t err = rt_pwm_set(_pwm_dev[order], _cfg[order].pwm_channel, _cfg[order].period_ns, pulse);
+    thread_ = rt_thread_create("sv_mgr",
-        if (err != RT_EOK) return err;
+                               ServoManager::threadEntry,
-        _last_pulse[order] = pulse;
+                               this,
+                               4096,
+                               20,
+                               1);
+    if (!thread_) {
+        running_ = false;
+        return -RT_ENOMEM;
+    }
+
+    const rt_err_t err = rt_thread_startup(thread_);
+    if (err != RT_EOK) {
+        running_ = false;
+        rt_thread_delete(thread_);
+        thread_ = RT_NULL;
+        return err;
     }
 
     return RT_EOK;
 }
 
-int ServoManager::findOrderByIdNoLock(const char* servo_id) const
+void ServoManager::stop()
 {
-    for (size_t i = 0; i < _cfg.size(); ++i)
+    if (!running_) {
-    {
+        return;
-        const char* sid = _cfg[i].servo_id;
+    }
-        if (sid && (rt_strcmp(sid, servo_id) == 0))
+
+    running_ = false;
+    wake();
+
+    if (thread_) {
+        rt_thread_mdelay((rt_int32_t)(update_period_ms_ + 1));
+        rt_thread_delete(thread_);
+        thread_ = RT_NULL;
+    }
+}
+
+void ServoManager::setUpdatePeriodMs(rt_uint32_t update_period_ms)
+{
+    const rt_uint32_t clamped = (update_period_ms == 0) ? 1 : update_period_ms;
+
+    if (!mutex_) {
+        update_period_ms_ = clamped;
+        return;
+    }
+
+    rt_mutex_take(mutex_, RT_WAITING_FOREVER);
+    update_period_ms_ = clamped;
+    rt_mutex_release(mutex_);
+}
+
+void ServoManager::wake()
+{
+    if (thread_ && wake_sem_) {
+        rt_sem_release(wake_sem_);
+    }
+}
+
+void ServoManager::threadEntry(void* parameter)
+{
+    ServoManager* self = static_cast<ServoManager*>(parameter);
+    if (!self) {
+        return;
+    }
+
+    self->run();
+}
+
+void ServoManager::run()
+{
+    while (running_) {
+        rt_tick_t wait_ticks = rt_tick_from_millisecond(update_period_ms_);
+        if (wait_ticks == 0) {
+            wait_ticks = 1;
+        }
+
+        if (wake_sem_) {
+            rt_sem_take(wake_sem_, wait_ticks);
+        } else {
+            rt_thread_mdelay((rt_int32_t)update_period_ms_);
+        }
+
+        if (!running_) {
+            break;
+        }
+
+        const rt_tick_t now_tick = rt_tick_get();
+
+        rt_mutex_take(mutex_, RT_WAITING_FOREVER);
+        for (size_t i = 0; i < controls_.size(); ++i) {
+            controls_[i].update(now_tick);
+        }
+        rt_mutex_release(mutex_);
+    }
+
+    thread_ = RT_NULL;
+}
+
+int ServoManager::findIndexByIdNoLock(const std::string& servo_id) const
+{
+    if (servo_id.empty()) {
+        return -1;
+    }
+
+    for (size_t i = 0; i < controls_.size(); ++i) {
+        if (controls_[i].id() == servo_id) {
             return (int)i;
+        }
     }
+
     return -1;
 }
-
-float ServoManager::clampf(float v, float lo, float hi)
-{
-    if (v < lo) return lo;
-    if (v > hi) return hi;
-    return v;
-}
-
-rt_uint32_t ServoManager::angleToPulseNs(const ServoConfig& c, float angle_deg)
-{
-    const float a = clampf(angle_deg, c.min_angle_deg, c.max_angle_deg);
-
-    const float in_span  = (c.max_angle_deg - c.min_angle_deg);
-    const float out_span = (float)(c.max_pulse_ns - c.min_pulse_ns);
-
-    float t = 0.0f;
-    if (in_span > 1e-6f)
-        t = (a - c.min_angle_deg) / in_span;
-
-    float pulse = (float)c.min_pulse_ns + t * out_span;
-
-    if (pulse < (float)c.min_pulse_ns) pulse = (float)c.min_pulse_ns;
-    if (pulse > (float)c.max_pulse_ns) pulse = (float)c.max_pulse_ns;
-
-    return (rt_uint32_t)(pulse + 0.5f);
-}

rtconfig.h

@@ -148,6 +148,7 @@
 #define RT_USING_ADC
 #define RT_USING_PWM
 #define RT_USING_SPI
+#define RT_USING_QSPI
 #define RT_USING_PIN
 #define RT_USING_KTIME
 #define RT_USING_CHERRYUSB
@@ -452,6 +453,7 @@
 #define BSP_USING_PWM
 #define BSP_USING_PWM1
 #define BSP_USING_PWM1_CH1
+#define BSP_USING_QSPI
 #define BSP_USING_ADC
 #define BSP_USING_ADC1
 /* end of On-chip Peripheral Drivers */