feat: add json as config file

.config

@@ -1493,7 +1493,10 @@ CONFIG_PKG_STM32H7_CMSIS_DRIVER_VER="latest"
 # Data Processing
 #
 # CONFIG_PKG_USING_ARDUINO_KALMANFILTER is not set
-# CONFIG_PKG_USING_ARDUINO_ARDUINOJSON is not set
+CONFIG_PKG_USING_ARDUINO_ARDUINOJSON=y
+CONFIG_PKG_ARDUINO_ARDUINOJSON_PATH="/packages/arduino/dataprocessing/ArduinoJson"
+CONFIG_PKG_USING_ARDUINO_ARDUINOJSON_LATEST_VERSION=y
+CONFIG_PKG_ARDUINO_ARDUINOJSON_VER="latest"
 # CONFIG_PKG_USING_ARDUINO_TENSORFLOW_LITE_MICRO is not set
 # CONFIG_PKG_USING_ARDUINO_RUNNINGMEDIAN is not set
 # end of Data Processing

CMakeLists.txt

@@ -43,6 +43,7 @@ INCLUDE_DIRECTORIES(
 	board/ports/usb
 	board/ports/fal
 	libraries/HAL_Drivers/CMSIS/Include
+	packages/ArduinoJson-latest/src
 	packages/erpc-1.14.0/erpc_c
 	packages/erpc-1.14.0/erpc_c/config
 	packages/erpc-1.14.0/erpc_c/infra
@@ -112,28 +113,31 @@ ADD_DEFINITIONS(
 
 # Library source files
 SET(RT_APP_ERPC_SOURCES
-	applications/erpc/proto/generated/file_service_interface.cpp
-	applications/erpc/proto/generated/servo_service_server.cpp
-	applications/erpc/service/src/file_service_impl.cpp
 	applications/erpc/service/src/erpc_server_main.cpp
-	applications/erpc/proto/generated/file_service_server.cpp
+	applications/erpc/proto/generated/file_service_interface.cpp
-	applications/erpc/service/src/servo_service_impl.cpp
 	applications/erpc/proto/generated/servo_service_interface.cpp
+	applications/erpc/proto/generated/file_service_server.cpp
+	applications/erpc/service/src/file_service_impl.cpp
 	applications/erpc/common/src/erpc_error_handler.cpp
+	applications/erpc/service/src/servo_service_impl.cpp
+	applications/erpc/proto/generated/servo_service_server.cpp
 )
 
 SET(RT_APPLICATIONS_SOURCES
 	applications/main.cpp
 )
 
+SET(RT_ARDUINOJSON_SOURCES
+)
+
 SET(RT_CHERRYUSB_SOURCES
+	rt-thread/components/drivers/usb/cherryusb/platform/rtthread/usb_check.c
+	rt-thread/components/drivers/usb/cherryusb/port/dwc2/usb_dc_dwc2.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/class/msc/usbd_msc.c
-	rt-thread/components/drivers/usb/cherryusb/port/dwc2/usb_dc_dwc2.c
-	rt-thread/components/drivers/usb/cherryusb/platform/rtthread/usb_msh.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/platform/rtthread/usb_msh.c
+	rt-thread/components/drivers/usb/cherryusb/class/msc/usbd_msc.c
 	rt-thread/components/drivers/usb/cherryusb/osal/usb_osal_rtthread.c
 )
 
@@ -158,8 +162,8 @@ SET(RT_CONTROLLER_SOURCES
 )
 
 SET(RT_CPP_SOURCES
-	rt-thread/components/libc/cplusplus/cxx_crt_init.c
 	rt-thread/components/libc/cplusplus/cxx_crt.cpp
+	rt-thread/components/libc/cplusplus/cxx_crt_init.c
 )
 
 SET(RT_DEVICEDRIVERS_SOURCES
@@ -202,30 +206,30 @@ SET(RT_DRIVERS_SOURCES
 )
 
 SET(RT_ERPC_SOURCES
+	packages/erpc-1.14.0/erpc_c/infra/erpc_transport_arbitrator.cpp
+	packages/erpc-1.14.0/erpc_c/port/erpc_port_rtt.cpp
+	packages/erpc-1.14.0/erpc_c/infra/erpc_basic_codec.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/port/erpc_threading_pthreads.cpp
+	packages/erpc-1.14.0/erpc_c/setup/erpc_setup_mbf_dynamic.cpp
+	packages/erpc-1.14.0/erpc_c/transports/erpc_usb_cdc_transport.cpp
+	packages/erpc-1.14.0/erpc_c/infra/erpc_message_loggers.cpp
 	packages/erpc-1.14.0/erpc_c/infra/erpc_server.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_transport_arbitrator.cpp
-	packages/erpc-1.14.0/erpc_c/setup/erpc_setup_mbf_dynamic.cpp
 	packages/erpc-1.14.0/erpc_c/setup/erpc_server_setup.cpp
-	packages/erpc-1.14.0/erpc_c/infra/erpc_message_loggers.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_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_pre_post_action.cpp
 	packages/erpc-1.14.0/erpc_c/infra/erpc_message_buffer.cpp
-	packages/erpc-1.14.0/erpc_c/infra/erpc_basic_codec.cpp
-	packages/erpc-1.14.0/erpc_c/infra/erpc_framed_transport.cpp
 	packages/erpc-1.14.0/erpc_c/infra/erpc_crc16.cpp
+	packages/erpc-1.14.0/erpc_c/infra/erpc_framed_transport.cpp
+	packages/erpc-1.14.0/erpc_c/port/erpc_threading_pthreads.cpp
+	packages/erpc-1.14.0/erpc_c/infra/erpc_pre_post_action.cpp
+	packages/erpc-1.14.0/erpc_c/setup/erpc_setup_mbf_static.cpp
 )
 
 SET(RT_FAL_SOURCES
-	rt-thread/components/fal/src/fal.c
+	rt-thread/components/fal/src/fal_rtt.c
 	rt-thread/components/fal/src/fal_flash.c
 	rt-thread/components/fal/src/fal_partition.c
-	rt-thread/components/fal/src/fal_rtt.c
+	rt-thread/components/fal/src/fal.c
 )
 
 SET(RT_FILESYSTEM_SOURCES
@@ -237,11 +241,11 @@ SET(RT_FILESYSTEM_SOURCES
 )
 
 SET(RT_FINSH_SOURCES
-	rt-thread/components/finsh/msh.c
-	rt-thread/components/finsh/msh_parse.c
-	rt-thread/components/finsh/shell.c
 	rt-thread/components/finsh/cmd.c
 	rt-thread/components/finsh/msh_file.c
+	rt-thread/components/finsh/msh.c
+	rt-thread/components/finsh/shell.c
+	rt-thread/components/finsh/msh_parse.c
 )
 
 SET(RT_KERNEL_SOURCES
@@ -263,16 +267,16 @@ SET(RT_KERNEL_SOURCES
 )
 
 SET(RT_KLIBC_SOURCES
-	rt-thread/src/klibc/kstdio.c
-	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/cputimer.c
 	rt-thread/components/drivers/ktime/src/hrtimer.c
 	rt-thread/components/drivers/ktime/src/boottime.c
-	rt-thread/components/drivers/ktime/src/cputimer.c
 )
 
 SET(RT_LIBCPU_SOURCES
@@ -285,45 +289,45 @@ SET(RT_LIBCPU_SOURCES
 )
 
 SET(RT_LIBRARIES_SOURCES
-	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_flash_ex.c
-	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_crc.c
-	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_cryp_ex.c
-	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_sram.c
-	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_dma_ex.c
-	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_cortex.c
-	libraries/STM32H7xx_HAL/CMSIS/Device/ST/STM32H7xx/Source/Templates/system_stm32h7xx.c
-	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_rng.c
-	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_nor.c
-	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_mdma.c
-	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_crc_ex.c
 	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_pwr.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_usart.c
-	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_rcc_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_uart_ex.c
-	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_tim_ex.c
-	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_lptim.c
-	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_tim.c
-	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_flash.c
-	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_comp.c
-	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_dma.c
-	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_cec.c
-	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_gpio.c
-	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal.c
 	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_cryp.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_mdma.c
+	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_gpio.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_nor.c
+	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal.c
+	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_dma_ex.c
+	libraries/STM32H7xx_HAL/CMSIS/Device/ST/STM32H7xx/Source/Templates/system_stm32h7xx.c
+	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_spi.c
+	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_comp.c
+	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_flash.c
+	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_cortex.c
+	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_crc.c
+	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_lptim.c
+	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_cryp_ex.c
+	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_sram.c
+	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_rng.c
+	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_tim.c
+	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_tim_ex.c
+	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_adc.c
+	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_usart.c
+	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_crc_ex.c
+	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_uart_ex.c
+	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_flash_ex.c
+	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_uart.c
 	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_qspi.c
+	libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_rcc.c
 )
 
 SET(RT_LITTLEFS_SOURCES
-	packages/littlefs-latest/lfs_util.c
-	packages/littlefs-latest/lfs_crc.c
-	packages/littlefs-latest/dfs_lfs.c
 	packages/littlefs-latest/lfs.c
+	packages/littlefs-latest/dfs_lfs.c
+	packages/littlefs-latest/lfs_crc.c
+	packages/littlefs-latest/lfs_util.c
 )
 
 SET(RT_POSIX_SOURCES
@@ -341,9 +345,10 @@ SET(RT_POSIX_SOURCES
 )
 
 SET(RT_SERVO_CONTROL_SOURCES
+	applications/servo_manager/src/servo_driver.cpp
 	applications/servo_manager/src/servo_manager.cpp
 	applications/servo_manager/src/servo_control.cpp
-	applications/servo_manager/src/servo_driver.cpp
+	applications/servo_manager/src/servo_config.cpp
 )
 
 SET(RT_UTC_UTEST_SOURCES
@@ -391,6 +396,7 @@ ADD_LIBRARY(rtt_POSIX OBJECT ${RT_POSIX_SOURCES})
 ADD_LIBRARY(rtt_servo_control OBJECT ${RT_SERVO_CONTROL_SOURCES})
 
 # Interface libraries
+ADD_LIBRARY(rtt_ArduinoJson INTERFACE)
 ADD_LIBRARY(rtt_utc_UTest INTERFACE)
 ADD_LIBRARY(rtt_utestcases INTERFACE)
 
@@ -423,6 +429,7 @@ TARGET_LINK_LIBRARIES(${CMAKE_PROJECT_NAME}.elf
 	rtt_littlefs
 	rtt_POSIX
 	rtt_servo_control
+	rtt_ArduinoJson
 	rtt_utc_UTest
 	rtt_utestcases
 )

applications/config/servo_config.json

@@ -0,0 +1,48 @@
+{
+  "version": 1,
+  "servo_count": 2,
+  "servos": [
+    {
+      "id": "eye_l_up",
+      "limit_min_angle_rad": -1.5707963,
+      "limit_max_angle_rad": 1.5707963,
+      "home_angle_rad": 0.0,
+      "driver": {
+        "pwm_dev_name": "pwm1",
+        "pwm_channel": 1,
+        "period_ns": 20000000,
+        "min_pulse_ns": 500000,
+        "max_pulse_ns": 2500000,
+        "min_angle_rad": -1.5707963,
+        "max_angle_rad": 1.5707963
+      },
+      "control": {
+        "max_velocity_rad": 6.28,
+        "max_acceleration_rad": 100.0,
+        "max_jerk_rad": 500.0,
+        "position_gain": 8.0
+      }
+    },
+    {
+      "id": "eye_r_up",
+      "limit_min_angle_rad": -1.5707963,
+      "limit_max_angle_rad": 1.5707963,
+      "home_angle_rad": 0.0,
+      "driver": {
+        "pwm_dev_name": "pwm1",
+        "pwm_channel": 2,
+        "period_ns": 20000000,
+        "min_pulse_ns": 500000,
+        "max_pulse_ns": 2500000,
+        "min_angle_rad": -1.5707963,
+        "max_angle_rad": 1.5707963
+      },
+      "control": {
+        "max_velocity_rad": 6.28,
+        "max_acceleration_rad": 100.0,
+        "max_jerk_rad": 500.0,
+        "position_gain": 8.0
+      }
+    }
+  ]
+}

applications/erpc/service/include/servo_service_impl.h

@@ -37,8 +37,6 @@ private:
     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/servo_service_impl.cpp

@@ -4,19 +4,10 @@
 
 #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.5f * PI, 0.5f * PI, 0.0f},
-{"eye_l_down", "pwm1", 3, 20000000, 500000, 2500000, -0.5f * PI, 0.5f * PI, 0.0f},
-{"eye_r_down", "pwm1", 4, 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),
+    : manager_("/config/servo_config.json"),
       init_err_(RT_EOK),
       mode_(MotionMode::Immediate),
       mutex_(RT_NULL)
@@ -36,20 +27,6 @@ ServoServiceImpl::ServoServiceImpl()
 
     manager_.setUpdatePeriodMs(10);
 
-    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(
-            6.28,
-            100,
-            500);
-    }
-
     init_err_ = manager_.start();
     if (init_err_ != RT_EOK) {
         rt_kprintf("[ServoService][E] manager start FAILED, err=%d\n", (int)init_err_);
@@ -273,7 +250,7 @@ int32_t ServoServiceImpl::moveJ(const list_float_1_t* angles_rad)
         return -RT_EINVAL;
     }
 
-    const size_t need = kServoCfg.size();
+    const size_t need = manager_.count();
     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",
@@ -292,17 +269,16 @@ int32_t ServoServiceImpl::moveJ(const list_float_1_t* angles_rad)
     rt_mutex_release(mutex_);
 
     for (size_t i = 0; i < need; ++i) {
-        ServoControl* servo = manager_.get(kServoCfg[i].servo_id);
+        ServoControl* servo = manager_.get(i);
         if (!servo) {
-            rt_kprintf("[ServoService][E] moveJ: servo not found, id=%s\n",
+            rt_kprintf("[ServoService][E] moveJ: servo not found, index=%d\n", (int)i);
-                       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(),
+                       servo->id().c_str(),
                        (double)angles_rad->elements[i],
                        (int)motion_mode,
                        (int)err);

applications/main.cpp

@@ -13,12 +13,10 @@
 #include <board.h>
 
 #include "fal.h"
-#include "planner/s_curve_planner/include/s_curve_position_planner.h"
 
 /* defined the LED0 pin: PC13 */
 #define LED0_PIN    GET_PIN(C, 13)
 
-
 int main(void)
 {
     int count = 1;
@@ -29,9 +27,9 @@ int main(void)
     while (count++)
     {
         rt_pin_write(LED0_PIN, PIN_HIGH);
-        rt_thread_mdelay(100);
+        rt_thread_mdelay(500);
         rt_pin_write(LED0_PIN, PIN_LOW);
-        rt_thread_mdelay(100);
+        rt_thread_mdelay(500);
     }
 
     return RT_EOK;

applications/servo_manager/include/servo_config.h

@@ -0,0 +1,26 @@
+//
+// Created by Administrator on 2026/4/28.
+//
+
+#ifndef SERVO_CONFIG_H
+#define SERVO_CONFIG_H
+
+#include <string>
+#include <vector>
+
+#include "servo_manager/include/servo_control.h"
+#include "servo_manager/include/servo_driver.h"
+
+struct ServoConfig
+{
+    std::string id;
+    float limit_min_angle_rad;
+    float limit_max_angle_rad;
+    float home_angle_rad;
+    ServoDriver::Config driver;
+    ServoControl::Config control;
+};
+
+std::vector<ServoConfig> loadServoConfigsFromJsonFile(const std::string& json_file_path);
+
+#endif // SERVO_CONFIG_H

applications/servo_manager/include/servo_control.h

@@ -15,10 +15,20 @@
 #include "planner/s_curve_planner/include/s_curve_position_planner.h"
 #include "servo_manager/include/servo_driver.h"
 
+struct ServoConfig;
+
 class ServoControl
 {
 public:
-    explicit ServoControl(const ServoDriver::Config& cfg);
+    struct Config
+    {
+        double max_velocity_rad{6.28};
+        double max_acceleration_rad{100.0};
+        double max_jerk_rad{500.0};
+        double position_gain{8.0};
+    };
+
+    explicit ServoControl(const ServoConfig& cfg);
     ~ServoControl();
 
     rt_err_t init(bool enable_after_init = true, bool go_home = true);
@@ -48,6 +58,11 @@ private:
 private:
     static constexpr float ESP = 1e-4f;
 
+    std::string id_;
+    float limit_min_angle_rad_;
+    float limit_max_angle_rad_;
+    float home_angle_rad_;
+    Config cfg_;
     ServoDriver driver_;
     cmvr::SCurve curve_;
     cmvr::SCurvePositionPlanner1D follow_planner_;

applications/servo_manager/include/servo_driver.h

@@ -17,7 +17,6 @@ class ServoDriver
 public:
     struct Config
     {
-        std::string servo_id;
         std::string pwm_dev_name;
         int pwm_channel;
 
@@ -27,19 +26,17 @@ public:
 
         float min_angle_rad;
         float max_angle_rad;
-        float home_angle_rad;
     };
 
 public:
-    explicit ServoDriver(const Config& cfg);
+    ServoDriver(const Config& cfg, float limit_min_angle_rad, float limit_max_angle_rad);
     ~ServoDriver();
 
-    rt_err_t init(bool enable_after_init = true, bool go_home = true);
+    rt_err_t init(float initial_angle_rad, bool enable_after_init = true);
 
     rt_err_t enable(bool on);
     rt_err_t setAngle(float angle_rad);
 
-    const std::string& id() const;
     const Config& config() const;
 
 private:
@@ -51,6 +48,8 @@ private:
 
 private:
     Config cfg_;
+    float limit_min_angle_rad_;
+    float limit_max_angle_rad_;
     rt_device_pwm* pwm_dev_;
     rt_uint32_t last_pulse_;
     bool inited_;

applications/servo_manager/include/servo_manager.h

@@ -11,18 +11,21 @@
 #include <vector>
 
 #include "servo_manager/include/servo_control.h"
-#include "servo_manager/include/servo_driver.h"
+#include "servo_manager/include/servo_config.h"
 
 #include <rtthread.h>
 
 class ServoManager
 {
 public:
-    explicit ServoManager(const std::vector<ServoDriver::Config>& cfg);
+    explicit ServoManager(const std::vector<ServoConfig>& cfg);
+    explicit ServoManager(const std::string& json_file_path);
     ~ServoManager();
 
     rt_err_t init(bool enable_after_init = true, bool go_home = true);
 
+    ServoControl* get(size_t index);
+    const ServoControl* get(size_t index) const;
     ServoControl* get(const std::string& servo_id);
     const ServoControl* get(const std::string& servo_id) const;
 

applications/servo_manager/src/servo_config.cpp

@@ -0,0 +1,159 @@
+//
+// Created by Administrator on 2026/4/29.
+//
+
+#include "servo_manager/include/servo_config.h"
+
+#include <ArduinoJson.h>
+#include <dfs.h>
+#include <sys/stat.h>
+
+#include "sys/unistd.h"
+
+std::vector<ServoConfig> loadServoConfigsFromJsonFile(const std::string& json_file_path)
+{
+    std::vector<ServoConfig> configs;
+
+    if (json_file_path.empty()) {
+        rt_kprintf("json file path is empty\n");
+        return configs;
+    }
+
+    struct stat st;
+    if (stat(json_file_path.c_str(), &st) != 0) {
+        rt_kprintf("stat json file failed: %s\n", json_file_path.c_str());
+        return configs;
+    }
+
+    if (st.st_size <= 0) {
+        rt_kprintf("json file is empty: %s\n", json_file_path.c_str());
+        return configs;
+    }
+
+    char* json_buf = static_cast<char*>(rt_malloc((rt_size_t)st.st_size + 1));
+    if (json_buf == RT_NULL) {
+        rt_kprintf("malloc json buffer failed, size=%d\n", (int)st.st_size);
+        return configs;
+    }
+
+    int fd = open(json_file_path.c_str(), O_RDONLY);
+    if (fd < 0) {
+        rt_kprintf("open json file failed: %s\n", json_file_path.c_str());
+        rt_free(json_buf);
+        return configs;
+    }
+
+    int len = read(fd, json_buf, (size_t)st.st_size);
+    close(fd);
+
+    if (len <= 0) {
+        rt_kprintf("read json file failed: %s\n", json_file_path.c_str());
+        rt_free(json_buf);
+        return configs;
+    }
+
+    json_buf[len] = '\0';
+
+    JsonDocument doc;
+
+    DeserializationError error = deserializeJson(doc, json_buf);
+    rt_free(json_buf);
+
+    if (error) {
+        rt_kprintf("parse json failed: %s\n", error.c_str());
+        return configs;
+    }
+
+    const int version = doc["version"] | 1;
+    if (version != 1) {
+        rt_kprintf("unsupported servo config version: %d\n", version);
+        return configs;
+    }
+
+    JsonArray servos = doc["servos"].as<JsonArray>();
+    if (servos.isNull()) {
+        rt_kprintf("json field 'servos' missing or invalid\n");
+        return configs;
+    }
+
+    const int servo_count = doc["servo_count"] | -1;
+    if (servo_count >= 0 && servo_count != (int)servos.size()) {
+        rt_kprintf("servo_count mismatch, declared=%d actual=%d\n",
+                   servo_count,
+                   (int)servos.size());
+        return configs;
+    }
+
+    configs.reserve(servos.size());
+
+    for (JsonObject servo : servos) {
+        const char* servo_id = servo["id"] | "";
+        JsonObject driver = servo["driver"].as<JsonObject>();
+        JsonObject control = servo["control"].as<JsonObject>();
+
+        if (servo_id[0] == '\0' || driver.isNull()) {
+            rt_kprintf("invalid servo config: id empty or driver missing\n");
+            continue;
+        }
+
+        ServoConfig cfg;
+        cfg.id = servo_id;
+
+        cfg.driver.pwm_dev_name = driver["pwm_dev_name"] | "";
+        cfg.driver.pwm_channel = driver["pwm_channel"] | 0;
+
+        cfg.driver.period_ns = driver["period_ns"] | 20000000;
+        cfg.driver.min_pulse_ns = driver["min_pulse_ns"] | 500000;
+        cfg.driver.max_pulse_ns = driver["max_pulse_ns"] | 2500000;
+
+        cfg.driver.min_angle_rad = driver["min_angle_rad"] | -1.5707963f;
+        cfg.driver.max_angle_rad = driver["max_angle_rad"] | 1.5707963f;
+
+        cfg.limit_min_angle_rad = servo["limit_min_angle_rad"] | cfg.driver.min_angle_rad;
+        cfg.limit_max_angle_rad = servo["limit_max_angle_rad"] | cfg.driver.max_angle_rad;
+        cfg.home_angle_rad = servo["home_angle_rad"] | 0.0f;
+
+        cfg.control.max_velocity_rad = control["max_velocity_rad"] | 6.28;
+        cfg.control.max_acceleration_rad = control["max_acceleration_rad"] | 100.0;
+        cfg.control.max_jerk_rad = control["max_jerk_rad"] | 500.0;
+        cfg.control.position_gain = control["position_gain"] | 8.0;
+
+        if (cfg.driver.pwm_dev_name.empty() ||
+            cfg.driver.pwm_channel <= 0 ||
+            cfg.driver.period_ns == 0 ||
+            cfg.driver.min_pulse_ns >= cfg.driver.max_pulse_ns ||
+            cfg.driver.min_angle_rad >= cfg.driver.max_angle_rad ||
+            cfg.limit_min_angle_rad < cfg.driver.min_angle_rad ||
+            cfg.limit_max_angle_rad > cfg.driver.max_angle_rad ||
+            cfg.limit_min_angle_rad >= cfg.limit_max_angle_rad ||
+            cfg.home_angle_rad < cfg.limit_min_angle_rad ||
+            cfg.home_angle_rad > cfg.limit_max_angle_rad ||
+            cfg.control.max_velocity_rad <= 0.0 ||
+            cfg.control.max_acceleration_rad <= 0.0 ||
+            cfg.control.max_jerk_rad <= 0.0 ||
+            cfg.control.position_gain < 0.0) {
+            rt_kprintf("invalid servo config values, id=%s\n", cfg.id.c_str());
+            continue;
+        }
+
+        bool duplicated = false;
+        for (const auto& existing : configs) {
+            if (existing.id == cfg.id) {
+                duplicated = true;
+                break;
+            }
+        }
+        if (duplicated) {
+            rt_kprintf("duplicate servo id in config: %s\n", cfg.id.c_str());
+            continue;
+        }
+
+        configs.push_back(cfg);
+    }
+
+    rt_kprintf("load servo config from %s, count=%d\n",
+               json_file_path.c_str(),
+               (int)configs.size());
+
+    return configs;
+}

applications/servo_manager/src/servo_control.cpp

@@ -3,15 +3,22 @@
 //
 
 #include "servo_manager/include/servo_control.h"
+#include "servo_manager/include/servo_config.h"
 
+#include <algorithm>
 #include <cmath>
 
-ServoControl::ServoControl(const ServoDriver::Config& cfg)
+ServoControl::ServoControl(const ServoConfig& cfg)
-    : driver_(cfg),
+    : id_(cfg.id),
+      limit_min_angle_rad_(cfg.limit_min_angle_rad),
+      limit_max_angle_rad_(cfg.limit_max_angle_rad),
+      home_angle_rad_(cfg.home_angle_rad),
+      cfg_(cfg.control),
+      driver_(cfg.driver, cfg.limit_min_angle_rad, cfg.limit_max_angle_rad),
       curve_(),
-      follow_planner_(6.28,
+      follow_planner_(cfg.control.max_velocity_rad,
-                      100,
+                      cfg.control.max_acceleration_rad,
-                      500),
+                      cfg.control.max_jerk_rad),
       current_angle_rad_(cfg.home_angle_rad),
       current_velocity_rad_per_sec_(0.0f),
       target_angle_rad_(cfg.home_angle_rad),
@@ -24,7 +31,10 @@ ServoControl::ServoControl(const ServoDriver::Config& cfg)
       profile_(),
       mutex_(RT_NULL)
 {
-    follow_planner_.setPositionGain(8.0);
+    curve_.setConstraints(cfg_.max_velocity_rad,
+                          cfg_.max_acceleration_rad,
+                          cfg_.max_jerk_rad);
+    follow_planner_.setPositionGain(cfg_.position_gain);
     mutex_ = rt_mutex_create("sv_ctl", RT_IPC_FLAG_PRIO);
 }
 
@@ -38,7 +48,9 @@ ServoControl::~ServoControl()
 
 rt_err_t ServoControl::init(bool enable_after_init, bool go_home)
 {
-    const rt_err_t err = driver_.init(enable_after_init, go_home);
+    const float init_angle = go_home ? home_angle_rad_
+                                     : driver_.config().min_angle_rad;
+    const rt_err_t err = driver_.init(init_angle, enable_after_init);
     if (err != RT_EOK) {
         return err;
     }
@@ -49,8 +61,6 @@ rt_err_t ServoControl::init(bool enable_after_init, bool go_home)
 
     rt_mutex_take(mutex_, RT_WAITING_FOREVER);
 
-    const float init_angle = go_home ? driver_.config().home_angle_rad
-                                     : driver_.config().min_angle_rad;
     const rt_tick_t now_tick = rt_tick_get();
 
     current_angle_rad_ = init_angle;
@@ -79,6 +89,8 @@ rt_err_t ServoControl::move(float angle_rad, MotionMode mode)
 
     rt_mutex_take(mutex_, RT_WAITING_FOREVER);
 
+    angle_rad = std::max(limit_min_angle_rad_, std::min(angle_rad, limit_max_angle_rad_));
+
     const rt_tick_t now_tick = rt_tick_get();
     sampleStateNoLock(now_tick);
 
@@ -259,7 +271,7 @@ void ServoControl::update(rt_tick_t now_tick)
 
 const std::string& ServoControl::id() const
 {
-    return driver_.id();
+    return id_;
 }
 
 float ServoControl::currentAngle() const

applications/servo_manager/src/servo_driver.cpp

@@ -4,8 +4,10 @@
 
 #include "servo_manager/include/servo_driver.h"
 
-ServoDriver::ServoDriver(const Config& cfg)
+ServoDriver::ServoDriver(const Config& cfg, float limit_min_angle_rad, float limit_max_angle_rad)
     : cfg_(cfg),
+      limit_min_angle_rad_(limit_min_angle_rad),
+      limit_max_angle_rad_(limit_max_angle_rad),
       pwm_dev_(RT_NULL),
       last_pulse_(0),
       inited_(false),
@@ -22,12 +24,10 @@ ServoDriver::~ServoDriver()
     }
 }
 
-rt_err_t ServoDriver::init(bool enable_after_init, bool go_home)
+rt_err_t ServoDriver::init(float initial_angle_rad, bool enable_after_init)
 {
-    if (cfg_.servo_id.empty() || cfg_.pwm_dev_name.empty()) {
+    if (cfg_.pwm_dev_name.empty()) {
-        rt_kprintf("[ServoDriver][E] init: invalid cfg, id=%s pwm=%s\n",
+        rt_kprintf("[ServoDriver][E] init: invalid cfg, pwm is empty\n");
-                   cfg_.servo_id.c_str(),
-                   cfg_.pwm_dev_name.c_str());
         return -RT_EINVAL;
     }
 
@@ -39,21 +39,19 @@ rt_err_t ServoDriver::init(bool enable_after_init, bool go_home)
 
     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",
+        rt_kprintf("[ServoDriver][E] init: rt_device_find failed, 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 float limited_angle = clampf(initial_angle_rad, limit_min_angle_rad_, limit_max_angle_rad_);
-    const rt_uint32_t pulse = angleToPulseNs(cfg_, init_angle);
+    const rt_uint32_t pulse = angleToPulseNs(cfg_, limited_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",
+        rt_kprintf("[ServoDriver][E] init: rt_pwm_set failed, ch=%d pulse=%u err=%d\n",
-                   cfg_.servo_id.c_str(),
                    cfg_.pwm_channel,
                    (unsigned)pulse,
                    (int)err);
@@ -67,8 +65,7 @@ rt_err_t ServoDriver::init(bool enable_after_init, bool go_home)
     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",
+            rt_kprintf("[ServoDriver][E] init: rt_pwm_enable failed, ch=%d err=%d\n",
-                       cfg_.servo_id.c_str(),
                        cfg_.pwm_channel,
                        (int)err);
             rt_mutex_release(mutex_);
@@ -78,12 +75,11 @@ rt_err_t ServoDriver::init(bool enable_after_init, bool go_home)
 
     rt_mutex_release(mutex_);
 
-    rt_kprintf("[ServoDriver] init OK: id=%s pwm=%s ch=%d enable=%d go_home=%d\n",
+    rt_kprintf("[ServoDriver] init OK: pwm=%s ch=%d enable=%d initial=%.4f\n",
-               cfg_.servo_id.c_str(),
                cfg_.pwm_dev_name.c_str(),
                cfg_.pwm_channel,
                (int)enable_after_init,
-               (int)go_home);
+               (double)initial_angle_rad);
 
     return RT_EOK;
 }
@@ -112,11 +108,6 @@ rt_err_t ServoDriver::setAngle(float angle_rad)
     return err;
 }
 
-const std::string& ServoDriver::id() const
-{
-    return cfg_.servo_id;
-}
-
 const ServoDriver::Config& ServoDriver::config() const
 {
     return cfg_;
@@ -138,7 +129,8 @@ rt_err_t ServoDriver::setAngleNoLock(float angle_rad)
         return -RT_ERROR;
     }
 
-    const rt_uint32_t pulse = angleToPulseNs(cfg_, angle_rad);
+    const float limited_angle = clampf(angle_rad, limit_min_angle_rad_, limit_max_angle_rad_);
+    const rt_uint32_t pulse = angleToPulseNs(cfg_, limited_angle);
     if (pulse == last_pulse_) {
         return RT_EOK;
     }

applications/servo_manager/src/servo_manager.cpp

@@ -4,7 +4,7 @@
 
 #include "servo_manager/include/servo_manager.h"
 
-ServoManager::ServoManager(const std::vector<ServoDriver::Config>& cfg)
+ServoManager::ServoManager(const std::vector<ServoConfig>& cfg)
     : controls_(),
       mutex_(RT_NULL),
       wake_sem_(RT_NULL),
@@ -20,6 +20,12 @@ ServoManager::ServoManager(const std::vector<ServoDriver::Config>& cfg)
     wake_sem_ = rt_sem_create("sv_wk", 0, RT_IPC_FLAG_PRIO);
 }
 
+
+ServoManager::ServoManager(const std::string& json_file_path)
+    : ServoManager(loadServoConfigsFromJsonFile(json_file_path))
+{
+}
+
 ServoManager::~ServoManager()
 {
     stop();
@@ -41,6 +47,11 @@ rt_err_t ServoManager::init(bool enable_after_init, bool go_home)
         return -RT_ENOMEM;
     }
 
+    if (controls_.empty()) {
+        rt_kprintf("[ServoManager][E] init: no servo config\n");
+        return -RT_EINVAL;
+    }
+
     rt_mutex_take(mutex_, RT_WAITING_FOREVER);
 
     for (size_t i = 0; i < controls_.size(); ++i) {
@@ -55,6 +66,30 @@ rt_err_t ServoManager::init(bool enable_after_init, bool go_home)
     return RT_EOK;
 }
 
+ServoControl* ServoManager::get(size_t index)
+{
+    if (!mutex_) {
+        return RT_NULL;
+    }
+
+    rt_mutex_take(mutex_, RT_WAITING_FOREVER);
+    ServoControl* control = (index < controls_.size()) ? &controls_[index] : RT_NULL;
+    rt_mutex_release(mutex_);
+    return control;
+}
+
+const ServoControl* ServoManager::get(size_t index) const
+{
+    if (!mutex_) {
+        return RT_NULL;
+    }
+
+    rt_mutex_take(mutex_, RT_WAITING_FOREVER);
+    const ServoControl* control = (index < controls_.size()) ? &controls_[index] : RT_NULL;
+    rt_mutex_release(mutex_);
+    return control;
+}
+
 ServoControl* ServoManager::get(const std::string& servo_id)
 {
     if (!mutex_) {

packages/ArduinoJson-latest

@@ -0,0 +1 @@
+Subproject commit d4e745ab5490c977b3a64651891c3bba522f807d

packages/pkgs.json

@@ -18,5 +18,10 @@
   "name": "STM32H7_CMSIS_DRIVER",
   "path": "/packages/peripherals/hal-sdk/stm32/stm32h7_cmsis_driver",
   "ver": "latest"
+ },
+ {
+  "name": "ARDUINO_ARDUINOJSON",
+  "path": "/packages/arduino/dataprocessing/ArduinoJson",
+  "ver": "latest"
  }
 ]

rtconfig.h

@@ -423,6 +423,8 @@
 
 /* Data Processing */
 
+#define PKG_USING_ARDUINO_ARDUINOJSON
+#define PKG_USING_ARDUINO_ARDUINOJSON_LATEST_VERSION
 /* end of Data Processing */
 
 /* Data Storage */