From 03802352383d44942d2d16cf9cae01a6d2a287b7 Mon Sep 17 00:00:00 2001 From: lgv Date: Wed, 29 Apr 2026 09:33:16 +0800 Subject: [PATCH] feat: add json as config file --- .config | 5 +- CMakeLists.txt | 129 +++++++------- applications/config/servo_config.json | 48 ++++++ .../erpc/service/include/servo_service_impl.h | 2 - .../erpc/service/src/servo_service_impl.cpp | 34 +--- applications/main.cpp | 6 +- .../servo_manager/include/servo_config.h | 26 +++ .../servo_manager/include/servo_control.h | 17 +- .../servo_manager/include/servo_driver.h | 9 +- .../servo_manager/include/servo_manager.h | 7 +- .../servo_manager/src/servo_config.cpp | 159 ++++++++++++++++++ .../servo_manager/src/servo_control.cpp | 32 ++-- .../servo_manager/src/servo_driver.cpp | 40 ++--- .../servo_manager/src/servo_manager.cpp | 37 +++- packages/ArduinoJson-latest | 1 + packages/pkgs.json | 5 + rtconfig.h | 2 + 17 files changed, 419 insertions(+), 140 deletions(-) create mode 100644 applications/config/servo_config.json create mode 100644 applications/servo_manager/include/servo_config.h create mode 100644 applications/servo_manager/src/servo_config.cpp create mode 160000 packages/ArduinoJson-latest diff --git a/.config b/.config index f681dac..80e68e6 100644 --- a/.config +++ b/.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 diff --git a/CMakeLists.txt b/CMakeLists.txt index 3d389d7..9b74bea 100644 --- a/CMakeLists.txt +++ b/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/service/src/servo_service_impl.cpp + applications/erpc/proto/generated/file_service_interface.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 ) diff --git a/applications/config/servo_config.json b/applications/config/servo_config.json new file mode 100644 index 0000000..339c530 --- /dev/null +++ b/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 + } + } + ] +} diff --git a/applications/erpc/service/include/servo_service_impl.h b/applications/erpc/service/include/servo_service_impl.h index 57e6b3c..4fc123b 100644 --- a/applications/erpc/service/include/servo_service_impl.h +++ b/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 kServoCfg; }; #endif // RTTHREAD_HEAD_SERVICE_IMPL_H diff --git a/applications/erpc/service/src/servo_service_impl.cpp b/applications/erpc/service/src/servo_service_impl.cpp index 7fe6e53..8da1f9e 100644 --- a/applications/erpc/service/src/servo_service_impl.cpp +++ b/applications/erpc/service/src/servo_service_impl.cpp @@ -4,19 +4,10 @@ #include "erpc/service/include/servo_service_impl.h" #include -#include "common/type/common_type.h" -const std::vector 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", - kServoCfg[i].servo_id.c_str()); + rt_kprintf("[ServoService][E] moveJ: servo not found, index=%d\n", (int)i); 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); diff --git a/applications/main.cpp b/applications/main.cpp index 5973f42..07fe41d 100644 --- a/applications/main.cpp +++ b/applications/main.cpp @@ -13,12 +13,10 @@ #include #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; diff --git a/applications/servo_manager/include/servo_config.h b/applications/servo_manager/include/servo_config.h new file mode 100644 index 0000000..d6bdb0a --- /dev/null +++ b/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 +#include + +#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 loadServoConfigsFromJsonFile(const std::string& json_file_path); + +#endif // SERVO_CONFIG_H diff --git a/applications/servo_manager/include/servo_control.h b/applications/servo_manager/include/servo_control.h index 973775b..411e0cf 100644 --- a/applications/servo_manager/include/servo_control.h +++ b/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_; diff --git a/applications/servo_manager/include/servo_driver.h b/applications/servo_manager/include/servo_driver.h index 0ed0d75..a6d908d 100644 --- a/applications/servo_manager/include/servo_driver.h +++ b/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_; diff --git a/applications/servo_manager/include/servo_manager.h b/applications/servo_manager/include/servo_manager.h index a349853..6d12c42 100644 --- a/applications/servo_manager/include/servo_manager.h +++ b/applications/servo_manager/include/servo_manager.h @@ -11,18 +11,21 @@ #include #include "servo_manager/include/servo_control.h" -#include "servo_manager/include/servo_driver.h" +#include "servo_manager/include/servo_config.h" #include class ServoManager { public: - explicit ServoManager(const std::vector& cfg); + explicit ServoManager(const std::vector& 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; diff --git a/applications/servo_manager/src/servo_config.cpp b/applications/servo_manager/src/servo_config.cpp new file mode 100644 index 0000000..afd928a --- /dev/null +++ b/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 +#include +#include + +#include "sys/unistd.h" + +std::vector loadServoConfigsFromJsonFile(const std::string& json_file_path) +{ + std::vector 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(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(); + 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 control = servo["control"].as(); + + 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; +} diff --git a/applications/servo_manager/src/servo_control.cpp b/applications/servo_manager/src/servo_control.cpp index c766dd8..b567045 100644 --- a/applications/servo_manager/src/servo_control.cpp +++ b/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 #include -ServoControl::ServoControl(const ServoDriver::Config& cfg) - : driver_(cfg), +ServoControl::ServoControl(const ServoConfig& 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, - 100, - 500), + follow_planner_(cfg.control.max_velocity_rad, + cfg.control.max_acceleration_rad, + 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 diff --git a/applications/servo_manager/src/servo_driver.cpp b/applications/servo_manager/src/servo_driver.cpp index 0a3ad35..d721d45 100644 --- a/applications/servo_manager/src/servo_driver.cpp +++ b/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()) { - rt_kprintf("[ServoDriver][E] init: invalid cfg, id=%s pwm=%s\n", - cfg_.servo_id.c_str(), - cfg_.pwm_dev_name.c_str()); + if (cfg_.pwm_dev_name.empty()) { + rt_kprintf("[ServoDriver][E] init: invalid cfg, pwm is empty\n"); 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", - cfg_.servo_id.c_str(), + rt_kprintf("[ServoDriver][E] init: rt_device_find failed, pwm=%s ch=%d\n", 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); + const float limited_angle = clampf(initial_angle_rad, limit_min_angle_rad_, limit_max_angle_rad_); + 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", - cfg_.servo_id.c_str(), + rt_kprintf("[ServoDriver][E] init: rt_pwm_set failed, ch=%d pulse=%u err=%d\n", 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", - cfg_.servo_id.c_str(), + rt_kprintf("[ServoDriver][E] init: rt_pwm_enable failed, ch=%d err=%d\n", 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", - cfg_.servo_id.c_str(), + rt_kprintf("[ServoDriver] init OK: pwm=%s ch=%d enable=%d initial=%.4f\n", 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; } @@ -185,4 +177,4 @@ rt_uint32_t ServoDriver::angleToPulseNs(const Config& cfg, float angle_rad) } return (rt_uint32_t)(pulse + 0.5f); -} \ No newline at end of file +} diff --git a/applications/servo_manager/src/servo_manager.cpp b/applications/servo_manager/src/servo_manager.cpp index 8914dd8..0ca213a 100644 --- a/applications/servo_manager/src/servo_manager.cpp +++ b/applications/servo_manager/src/servo_manager.cpp @@ -4,7 +4,7 @@ #include "servo_manager/include/servo_manager.h" -ServoManager::ServoManager(const std::vector& cfg) +ServoManager::ServoManager(const std::vector& cfg) : controls_(), mutex_(RT_NULL), wake_sem_(RT_NULL), @@ -20,6 +20,12 @@ ServoManager::ServoManager(const std::vector& 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_) { diff --git a/packages/ArduinoJson-latest b/packages/ArduinoJson-latest new file mode 160000 index 0000000..d4e745a --- /dev/null +++ b/packages/ArduinoJson-latest @@ -0,0 +1 @@ +Subproject commit d4e745ab5490c977b3a64651891c3bba522f807d diff --git a/packages/pkgs.json b/packages/pkgs.json index 4990fec..252e920 100644 --- a/packages/pkgs.json +++ b/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" } ] \ No newline at end of file diff --git a/rtconfig.h b/rtconfig.h index 52fe34d..34a155d 100644 --- a/rtconfig.h +++ b/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 */