lgv/cmvr_head
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: add json as config file

Browse Source
This commit is contained in:
lgv 2026-04-29 09:33:16 +08:00
parent 7813023069
commit 0380235238
17 changed files with 419 additions and 140 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
.config
View File

@ -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

129
CMakeLists.txt
View File

@ -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
)

48
applications/config/servo_config.json Normal file
View File

@ -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
}
}
]
}

2
applications/erpc/service/include/servo_service_impl.h
View File

@ -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

34
applications/erpc/service/src/servo_service_impl.cpp
View File

@ -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",
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);

6
applications/main.cpp
View File

@ -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;

26
applications/servo_manager/include/servo_config.h Normal file
View File

@ -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

17
applications/servo_manager/include/servo_control.h
View File

@ -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_;

9
applications/servo_manager/include/servo_driver.h
View File

@ -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_;

7
applications/servo_manager/include/servo_manager.h
View File

@ -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;

159
applications/servo_manager/src/servo_config.cpp Normal file
View File

@ -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;
}

32
applications/servo_manager/src/servo_control.cpp
View File

@ -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)
: 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

38
applications/servo_manager/src/servo_driver.cpp
View File

@ -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;
}

37
applications/servo_manager/src/servo_manager.cpp
View File

@ -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_) {

1
packages/ArduinoJson-latest Submodule

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

5
packages/pkgs.json
View File

@ -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"
}
]

2
rtconfig.h
View File

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