From 559046d41c47f5a1f8496b32a062ae20ff08634d Mon Sep 17 00:00:00 2001 From: ttwards <12411711@mail.sustech.edu.cn> Date: Wed, 8 Jul 2026 16:10:59 +0800 Subject: [PATCH 1/4] test(wheel): add native_sim chassis coverage --- drivers/chassis/chassis.c | 2 +- drivers/wheel/mecanum.c | 3 +- tests/native_sim/wheel_chassis/CMakeLists.txt | 7 + tests/native_sim/wheel_chassis/app.overlay | 83 +++++ .../dts/bindings/motor/ares,test-motor.yaml | 7 + .../dts/bindings/wheel/ares,test-wheel.yaml | 11 + tests/native_sim/wheel_chassis/prj.conf | 15 + tests/native_sim/wheel_chassis/src/main.c | 350 ++++++++++++++++++ tests/native_sim/wheel_chassis/testcase.yaml | 15 + 9 files changed, 491 insertions(+), 2 deletions(-) create mode 100644 tests/native_sim/wheel_chassis/CMakeLists.txt create mode 100644 tests/native_sim/wheel_chassis/app.overlay create mode 100644 tests/native_sim/wheel_chassis/dts/bindings/motor/ares,test-motor.yaml create mode 100644 tests/native_sim/wheel_chassis/dts/bindings/wheel/ares,test-wheel.yaml create mode 100644 tests/native_sim/wheel_chassis/prj.conf create mode 100644 tests/native_sim/wheel_chassis/src/main.c create mode 100644 tests/native_sim/wheel_chassis/testcase.yaml diff --git a/drivers/chassis/chassis.c b/drivers/chassis/chassis.c index aa0d47b..3b4a4cd 100644 --- a/drivers/chassis/chassis.c +++ b/drivers/chassis/chassis.c @@ -264,7 +264,7 @@ void chassis_thread_entry(void *arg1, void *arg2, void *arg3) int32_t deltaTimeUs = k_cyc_to_us_floor32(data->currTime - data->prevTime); float error = 0; - if (!isnanf(data->chassis_sensor_data.Yaw) && data->angleControl) { + if (!isnan(data->chassis_sensor_data.Yaw) && data->angleControl) { float delta_Yaw = data->chassis_sensor_data.Yaw - data->target_status.angle; delta_Yaw = fmodf(delta_Yaw, 360.0f); if (delta_Yaw > 180) { diff --git a/drivers/wheel/mecanum.c b/drivers/wheel/mecanum.c index e0f1ef0..40ba7b7 100644 --- a/drivers/wheel/mecanum.c +++ b/drivers/wheel/mecanum.c @@ -146,6 +146,7 @@ struct wheel_driver_api mecanum_driver_api = { DT_STRING_UNQUOTED(DT_DRV_INST(inst), angle_offset) + \ DT_STRING_UNQUOTED(DT_DRV_INST(inst), free_angle) - 90.0f, \ false}, \ + .static_angle = NAN, \ }; \ static const mecanum_cfg_t mecanum_cfg_##inst = { \ .common = DT_WHEEL_CONFIG_GET(inst), \ @@ -156,6 +157,6 @@ struct wheel_driver_api mecanum_driver_api = { MECANUM_DEVICE_DT_DEFINE(DT_DRV_INST(inst), NULL, NULL, &mecanum_data_##inst, \ &mecanum_cfg_##inst, POST_KERNEL, 90, &mecanum_driver_api); -DT_INST_FOREACH_STATUS_OKAY(mecanum_DEVICE_DEFINE) +DT_INST_FOREACH_STATUS_OKAY(MECANUM_DEVICE_DEFINE) #endif // MECANUM_C diff --git a/tests/native_sim/wheel_chassis/CMakeLists.txt b/tests/native_sim/wheel_chassis/CMakeLists.txt new file mode 100644 index 0000000..b5f292d --- /dev/null +++ b/tests/native_sim/wheel_chassis/CMakeLists.txt @@ -0,0 +1,7 @@ +# SPDX-License-Identifier: Apache-2.0 + +cmake_minimum_required(VERSION 3.20.0) +find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE}) +project(ares_native_sim_wheel_chassis) + +target_sources(app PRIVATE src/main.c) diff --git a/tests/native_sim/wheel_chassis/app.overlay b/tests/native_sim/wheel_chassis/app.overlay new file mode 100644 index 0000000..b08b83e --- /dev/null +++ b/tests/native_sim/wheel_chassis/app.overlay @@ -0,0 +1,83 @@ +/* + * SPDX-License-Identifier: Apache-2.0 + */ + +/ { + test_pid: test_pid { + compatible = "pid,single"; + status = "okay"; + k_p = "1.0"; + k_i = "0.0"; + k_d = "0.0"; + out_max = "10.0"; + }; + + mecanum_motor: mecanum_motor { + compatible = "ares,test-motor"; + status = "okay"; + }; + + mecanum0: mecanum0 { + compatible = "ares,mecanum"; + status = "okay"; + wheel-motor = <&mecanum_motor>; + wheel-radius = "0.1"; + angle-offset = "0"; + free-angle = "0"; + }; + + steer_motor: steer_motor { + compatible = "ares,test-motor"; + status = "okay"; + }; + + steer_drive_motor: steer_drive_motor { + compatible = "ares,test-motor"; + status = "okay"; + }; + + steer0: steer0 { + compatible = "ares,steerwheel"; + status = "okay"; + steer-motor = <&steer_motor>; + wheel-motor = <&steer_drive_motor>; + wheel-radius = "0.1"; + angle-offset = "0"; + }; + + chassis_wheel0: chassis_wheel0 { + compatible = "ares,test-wheel"; + status = "okay"; + #wheel-cells = <2>; + }; + + chassis_wheel1: chassis_wheel1 { + compatible = "ares,test-wheel"; + status = "okay"; + #wheel-cells = <2>; + }; + + chassis_wheel2: chassis_wheel2 { + compatible = "ares,test-wheel"; + status = "okay"; + #wheel-cells = <2>; + }; + + chassis_wheel3: chassis_wheel3 { + compatible = "ares,test-wheel"; + status = "okay"; + #wheel-cells = <2>; + }; + + chassis0: chassis0 { + compatible = "ares,chassis"; + status = "okay"; + angle-pid = <&test_pid>; + max_gyro = "5.0"; + max_linear_accel = "20.0"; + wheels = <&chassis_wheel0 10000 10000>, + <&chassis_wheel1 10000 (-10000)>, + <&chassis_wheel2 (-10000) (-10000)>, + <&chassis_wheel3 (-10000) 10000>; + }; +}; diff --git a/tests/native_sim/wheel_chassis/dts/bindings/motor/ares,test-motor.yaml b/tests/native_sim/wheel_chassis/dts/bindings/motor/ares,test-motor.yaml new file mode 100644 index 0000000..fc8e47c --- /dev/null +++ b/tests/native_sim/wheel_chassis/dts/bindings/motor/ares,test-motor.yaml @@ -0,0 +1,7 @@ +# SPDX-License-Identifier: Apache-2.0 + +description: Test motor device + +compatible: "ares,test-motor" + +include: [base.yaml] diff --git a/tests/native_sim/wheel_chassis/dts/bindings/wheel/ares,test-wheel.yaml b/tests/native_sim/wheel_chassis/dts/bindings/wheel/ares,test-wheel.yaml new file mode 100644 index 0000000..dc8c31e --- /dev/null +++ b/tests/native_sim/wheel_chassis/dts/bindings/wheel/ares,test-wheel.yaml @@ -0,0 +1,11 @@ +# SPDX-License-Identifier: Apache-2.0 + +description: Test wheel device + +compatible: "ares,test-wheel" + +include: [base.yaml] + +wheel-cells: + - offset_x + - offset_y diff --git a/tests/native_sim/wheel_chassis/prj.conf b/tests/native_sim/wheel_chassis/prj.conf new file mode 100644 index 0000000..9ef9b17 --- /dev/null +++ b/tests/native_sim/wheel_chassis/prj.conf @@ -0,0 +1,15 @@ +# SPDX-License-Identifier: Apache-2.0 + +CONFIG_ZTEST=y +CONFIG_CBPRINTF_FP_SUPPORT=y +CONFIG_MAIN_STACK_SIZE=4096 + +CONFIG_MOTOR=y +CONFIG_MOTOR_LOG_LEVEL=0 +CONFIG_PID=y +CONFIG_CMSIS_DSP=y +CONFIG_CMSIS_DSP_FASTMATH=y +CONFIG_WHEEL=y +CONFIG_CHASSIS=y +CONFIG_CHASSIS_LOG_LEVEL=0 +CONFIG_CHASSIS_INIT_PRIORITY=90 diff --git a/tests/native_sim/wheel_chassis/src/main.c b/tests/native_sim/wheel_chassis/src/main.c new file mode 100644 index 0000000..3dca21d --- /dev/null +++ b/tests/native_sim/wheel_chassis/src/main.c @@ -0,0 +1,350 @@ +/* + * Copyright (c) 2026 ttwards + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#define FLOAT_TOLERANCE 0.0001f + +#define MECANUM_DEV DEVICE_DT_GET(DT_NODELABEL(mecanum0)) +#define MECANUM_MOTOR_DEV DEVICE_DT_GET(DT_NODELABEL(mecanum_motor)) +#define STEER_DEV DEVICE_DT_GET(DT_NODELABEL(steer0)) +#define STEER_MOTOR_DEV DEVICE_DT_GET(DT_NODELABEL(steer_motor)) +#define STEER_DRIVE_DEV DEVICE_DT_GET(DT_NODELABEL(steer_drive_motor)) +#define CHASSIS_DEV DEVICE_DT_GET(DT_NODELABEL(chassis0)) +#define CHASSIS_WHEEL0_DEV DEVICE_DT_GET(DT_NODELABEL(chassis_wheel0)) +#define CHASSIS_WHEEL1_DEV DEVICE_DT_GET(DT_NODELABEL(chassis_wheel1)) +#define CHASSIS_WHEEL2_DEV DEVICE_DT_GET(DT_NODELABEL(chassis_wheel2)) +#define CHASSIS_WHEEL3_DEV DEVICE_DT_GET(DT_NODELABEL(chassis_wheel3)) +#define EXPECTED_RPM_1MPS_R01 (RADPS_TO_RPM / 0.1f) + +void cchassis_resolve(chassis_data_t *data, const chassis_cfg_t *cfg); + +struct test_motor_data { + motor_status_t status; + motor_setpoint_t last_setpoint; + enum motor_cmd last_cmd; + uint32_t set_count; + uint32_t control_count; +}; + +struct test_wheel_data { + wheel_status_t status; + wheel_status_t target; + float last_speed; + float last_angle; + float last_static_angle; + uint32_t set_speed_count; + uint32_t set_static_count; + uint32_t disable_count; + int static_return; +}; + +static void assert_float_close(float actual, float expected) +{ + zassert_true(fabsf(actual - expected) < FLOAT_TOLERANCE, "actual=%f expected=%f", + (double)actual, (double)expected); +} + +static struct test_motor_data *test_motor_data(const struct device *dev) +{ + return dev->data; +} + +static struct test_wheel_data *test_wheel_data(const struct device *dev) +{ + return dev->data; +} + +static void test_motor_reset(const struct device *dev) +{ + struct test_motor_data *data = test_motor_data(dev); + motor_status_t status = data->status; + + memset(data, 0, sizeof(*data)); + data->status = status; +} + +static void test_motor_set_status(const struct device *dev, float angle, float rpm) +{ + struct test_motor_data *data = test_motor_data(dev); + + data->status.angle = angle; + data->status.rpm = rpm; +} + +static void test_wheel_reset(const struct device *dev) +{ + struct test_wheel_data *data = test_wheel_data(dev); + + memset(data, 0, sizeof(*data)); +} + +static int test_motor_get(const struct device *dev, motor_status_t *status) +{ + struct test_motor_data *data = test_motor_data(dev); + + *status = data->status; + return 0; +} + +static int test_motor_set(const struct device *dev, motor_setpoint_t *setpoint) +{ + struct test_motor_data *data = test_motor_data(dev); + + data->last_setpoint = *setpoint; + data->set_count++; + return 0; +} + +static void test_motor_control(const struct device *dev, enum motor_cmd cmd) +{ + struct test_motor_data *data = test_motor_data(dev); + + data->last_cmd = cmd; + data->control_count++; +} + +static const struct motor_driver_api test_motor_api = { + .motor_control = test_motor_control, + .motor_set = test_motor_set, + .motor_get = test_motor_get, +}; + +#define DT_DRV_COMPAT ares_test_motor + +#define TEST_MOTOR_DEFINE(inst) \ + static struct test_motor_data test_motor_data_##inst; \ + DEVICE_DT_INST_DEFINE(inst, NULL, NULL, &test_motor_data_##inst, NULL, POST_KERNEL, 80, \ + &test_motor_api); + +DT_INST_FOREACH_STATUS_OKAY(TEST_MOTOR_DEFINE) + +#undef DT_DRV_COMPAT + +static void test_wheel_set_speed(const struct device *dev, float speed, float angle) +{ + struct test_wheel_data *data = test_wheel_data(dev); + + data->last_speed = speed; + data->last_angle = angle; + data->target.speed = speed; + data->target.angle = angle; + data->set_speed_count++; +} + +static int test_wheel_set_static(const struct device *dev, float angle) +{ + struct test_wheel_data *data = test_wheel_data(dev); + + data->last_static_angle = angle; + data->target.speed = 0.0f; + data->target.angle = angle; + data->set_static_count++; + return data->static_return; +} + +static wheel_status_t *test_wheel_get_speed(const struct device *dev) +{ + struct test_wheel_data *data = test_wheel_data(dev); + + return &data->status; +} + +static wheel_status_t *test_wheel_get_target(const struct device *dev) +{ + struct test_wheel_data *data = test_wheel_data(dev); + + return &data->target; +} + +static void test_wheel_disable(const struct device *dev) +{ + struct test_wheel_data *data = test_wheel_data(dev); + + data->disable_count++; +} + +static const struct wheel_driver_api test_wheel_api = { + .wheel_set_speed = test_wheel_set_speed, + .wheel_set_static = test_wheel_set_static, + .wheel_get_speed = test_wheel_get_speed, + .wheel_get_target = test_wheel_get_target, + .wheel_disable = test_wheel_disable, +}; + +#define DT_DRV_COMPAT ares_test_wheel + +#define TEST_WHEEL_DEFINE(inst) \ + static struct test_wheel_data test_wheel_data_##inst; \ + DEVICE_DT_INST_DEFINE(inst, NULL, NULL, &test_wheel_data_##inst, NULL, POST_KERNEL, 80, \ + &test_wheel_api); + +DT_INST_FOREACH_STATUS_OKAY(TEST_WHEEL_DEFINE) + +#undef DT_DRV_COMPAT + +ZTEST(ares_native_sim_wheel_chassis, test_devices_are_ready) +{ + zassert_true(device_is_ready(MECANUM_DEV)); + zassert_true(device_is_ready(MECANUM_MOTOR_DEV)); + zassert_true(device_is_ready(STEER_DEV)); + zassert_true(device_is_ready(STEER_MOTOR_DEV)); + zassert_true(device_is_ready(STEER_DRIVE_DEV)); + zassert_true(device_is_ready(CHASSIS_DEV)); + zassert_true(device_is_ready(CHASSIS_WHEEL0_DEV)); + zassert_true(device_is_ready(CHASSIS_WHEEL1_DEV)); + zassert_true(device_is_ready(CHASSIS_WHEEL2_DEV)); + zassert_true(device_is_ready(CHASSIS_WHEEL3_DEV)); +} + +ZTEST(ares_native_sim_wheel_chassis, test_mecanum_projects_speed_and_latches_static_angle) +{ + test_motor_reset(MECANUM_MOTOR_DEV); + test_motor_set_status(MECANUM_MOTOR_DEV, 12.5f, 60.0f); + + wheel_set_speed(MECANUM_DEV, 2.0f, 0.0f); + + struct test_motor_data *motor = test_motor_data(MECANUM_MOTOR_DEV); + wheel_status_t *target = wheel_get_target(MECANUM_DEV); + wheel_status_t *status = wheel_get_speed(MECANUM_DEV); + + zassert_equal(motor->last_setpoint.mode, VO); + zassert_equal(motor->last_setpoint.target, MOTOR_TARGET_SPEED); + assert_float_close(target->speed, 2.0f); + assert_float_close(motor->last_setpoint.rpm, EXPECTED_RPM_1MPS_R01 * 2.0f); + assert_float_close(status->speed, RPM2RADPS(60.0f) * 0.1f); + + int ret = wheel_set_static(MECANUM_DEV, 45.0f); + + zassert_ok(ret); + zassert_equal(motor->last_setpoint.target, MOTOR_TARGET_POSITION); + assert_float_close(motor->last_setpoint.angle, 12.5f); +} + +ZTEST(ares_native_sim_wheel_chassis, test_steerwheel_short_path_stop_and_disable) +{ + test_motor_reset(STEER_MOTOR_DEV); + test_motor_reset(STEER_DRIVE_DEV); + test_motor_set_status(STEER_MOTOR_DEV, 120.0f, 0.0f); + test_motor_set_status(STEER_DRIVE_DEV, 0.0f, 0.0f); + + wheel_set_speed(STEER_DEV, 1.0f, 0.0f); + + struct test_motor_data *steer = test_motor_data(STEER_MOTOR_DEV); + struct test_motor_data *drive = test_motor_data(STEER_DRIVE_DEV); + wheel_status_t *target = wheel_get_target(STEER_DEV); + + assert_float_close(target->speed, 1.0f); + assert_float_close(target->angle, -180.0f); + assert_float_close(steer->last_setpoint.angle, -180.0f); + assert_float_close(drive->last_setpoint.rpm, -EXPECTED_RPM_1MPS_R01); + + wheel_set_speed(STEER_DEV, 0.0f, 30.0f); + + target = wheel_get_target(STEER_DEV); + assert_float_close(target->speed, 0.0f); + assert_float_close(drive->last_setpoint.rpm, 0.0f); + + wheel_disable(STEER_DEV); + + zassert_equal(steer->last_setpoint.target, MOTOR_TARGET_TORQUE); + zassert_equal(drive->last_setpoint.target, MOTOR_TARGET_TORQUE); + assert_float_close(steer->last_setpoint.torque, 0.0f); + assert_float_close(drive->last_setpoint.torque, 0.0f); +} + +ZTEST(ares_native_sim_wheel_chassis, test_chassis_public_commands_and_resolve) +{ + chassis_data_t *data = CHASSIS_DEV->data; + const chassis_cfg_t *cfg = CHASSIS_DEV->config; + const struct device *wheels[] = { + CHASSIS_WHEEL0_DEV, + CHASSIS_WHEEL1_DEV, + CHASSIS_WHEEL2_DEV, + CHASSIS_WHEEL3_DEV, + }; + + chassis_set_speed(CHASSIS_DEV, 1.25f, -0.5f); + assert_float_close(data->target_status.speedX, 0.5f); + assert_float_close(data->target_status.speedY, 1.25f); + + chassis_set_gyro(CHASSIS_DEV, 2.5f); + assert_float_close(data->target_status.gyro, 2.5f); + zassert_false(data->angleControl); + + chassis_set_angle(CHASSIS_DEV, 45.0f); + assert_float_close(data->target_status.angle, 45.0f); + zassert_true(data->angleControl); + + for (size_t i = 0; i < ARRAY_SIZE(wheels); i++) { + test_wheel_reset(wheels[i]); + } + + data->static_angle = false; + data->track_angle = false; + data->set_status.speedX = 1.0f; + data->set_status.speedY = 0.0f; + data->set_status.gyro = 0.0f; + + cchassis_resolve(data, cfg); + + for (size_t i = 0; i < ARRAY_SIZE(wheels); i++) { + struct test_wheel_data *wheel = test_wheel_data(wheels[i]); + + zassert_equal(wheel->set_speed_count, 1); + assert_float_close(wheel->last_speed, 1.0f); + assert_float_close(wheel->last_angle, 90.0f); + } +} + +ZTEST(ares_native_sim_wheel_chassis, test_chassis_static_mode_uses_wheel_static_angles) +{ + chassis_data_t *data = CHASSIS_DEV->data; + const chassis_cfg_t *cfg = CHASSIS_DEV->config; + const struct device *wheels[] = { + CHASSIS_WHEEL0_DEV, + CHASSIS_WHEEL1_DEV, + CHASSIS_WHEEL2_DEV, + CHASSIS_WHEEL3_DEV, + }; + const float expected_angles[] = { + 45.0f, + 135.0f, + 225.0f, + -45.0f, + }; + + for (size_t i = 0; i < ARRAY_SIZE(wheels); i++) { + test_wheel_reset(wheels[i]); + } + + data->static_angle = true; + data->set_status.speedX = 0.0f; + data->set_status.speedY = 0.0f; + data->set_status.gyro = 0.0f; + + cchassis_resolve(data, cfg); + + for (size_t i = 0; i < ARRAY_SIZE(wheels); i++) { + struct test_wheel_data *wheel = test_wheel_data(wheels[i]); + + zassert_equal(wheel->set_static_count, 1); + zassert_equal(wheel->set_speed_count, 0); + assert_float_close(wheel->last_static_angle, expected_angles[i]); + } +} + +ZTEST_SUITE(ares_native_sim_wheel_chassis, NULL, NULL, NULL, NULL, NULL); diff --git a/tests/native_sim/wheel_chassis/testcase.yaml b/tests/native_sim/wheel_chassis/testcase.yaml new file mode 100644 index 0000000..09d0301 --- /dev/null +++ b/tests/native_sim/wheel_chassis/testcase.yaml @@ -0,0 +1,15 @@ +# SPDX-License-Identifier: Apache-2.0 + +tests: + ares.native_sim.wheel_chassis: + tags: + - ares + - native_sim + - wheel + - chassis + platform_allow: + - native_sim + - native_sim/native/64 + integration_platforms: + - native_sim + - native_sim/native/64 From d67003e8bbed996bbef255464c2789fc6adc420c Mon Sep 17 00:00:00 2001 From: ttwards <12411711@mail.sustech.edu.cn> Date: Wed, 8 Jul 2026 16:11:05 +0800 Subject: [PATCH 2/4] test(motor): cover LK latency and DJI CAN load --- drivers/motor/lk/motor_lk.c | 97 +++++---- drivers/motor/lk/motor_lk.h | 3 + tests/native_sim/motor_driver_sim/app.overlay | 56 +++++- tests/native_sim/motor_driver_sim/src/main.c | 188 +++++++++++++++++- 4 files changed, 298 insertions(+), 46 deletions(-) diff --git a/drivers/motor/lk/motor_lk.c b/drivers/motor/lk/motor_lk.c index 54d29c9..3070b8e 100644 --- a/drivers/motor/lk/motor_lk.c +++ b/drivers/motor/lk/motor_lk.c @@ -15,6 +15,25 @@ LOG_MODULE_REGISTER(motor_lk, CONFIG_MOTOR_LOG_LEVEL); +static bool lk_motor_pack(const struct device *dev, struct can_frame *frame); + +static void lk_send_control_frame(const struct device *dev) +{ + struct can_frame tx_frame = {0}; + struct lk_motor_data *data = dev->data; + const struct lk_motor_cfg *cfg = dev->config; + + if (!data->common.link.requested_enabled) { + return; + } + + if (!lk_motor_pack(dev, &tx_frame)) { + return; + } + motor_can_sched_send_reply(cfg->common.phy, &tx_frame, LK_CMD_ID_BASE + cfg->id, + CAN_STD_ID_MASK, 5U, "lk-control"); +} + int float_to_int(float x, float x_min, float x_max, int bits) { @@ -101,11 +120,13 @@ void lk_motor_control(const struct device *dev, enum motor_cmd cmd) // } } -static void lk_motor_pack(const struct device *dev, struct can_frame *frame) +static bool lk_motor_pack(const struct device *dev, struct can_frame *frame) { struct lk_motor_data *data = (struct lk_motor_data *)(dev->data); const struct lk_motor_cfg *cfg = (struct lk_motor_cfg *)(dev->config); + bool control_frame = true; + memset(frame, 0, sizeof(*frame)); frame->id = LK_CMD_ID_BASE + cfg->id; // 标准ID: 0x140 + ID frame->dlc = 8; frame->flags = 0; // 标准帧 @@ -132,6 +153,7 @@ static void lk_motor_pack(const struct device *dev, struct can_frame *frame) } if (data->common.target != MOTOR_TARGET_TORQUE) { frame->data[0] = LK_CMD_READ_STAT; + control_frame = false; break; } frame->data[0] = LK_CMD_TORQUE_LOOP; @@ -150,6 +172,7 @@ static void lk_motor_pack(const struct device *dev, struct can_frame *frame) case PV: if (data->common.target != MOTOR_TARGET_POSITION) { frame->data[0] = LK_CMD_READ_STAT; + control_frame = false; break; } frame->data[0] = LK_CMD_POS_LOOP_MULTI; @@ -171,8 +194,11 @@ static void lk_motor_pack(const struct device *dev, struct can_frame *frame) default: frame->data[0] = LK_CMD_READ_STAT; + control_frame = false; break; } + + return control_frame; } int lk_set(const struct device *dev, motor_setpoint_t *status) @@ -264,6 +290,8 @@ int lk_set(const struct device *dev, motor_setpoint_t *status) } } + lk_send_control_frame(dev); + return 0; } @@ -297,6 +325,9 @@ static void lk_can_rx_handler(const struct device *can_dev, struct can_frame *fr if (!data->common.link.online) { data->need_init_frames = true; } + if (frame->data[0] == LK_CMD_READ_STAT) { + data->last_status_reply_ms = k_uptime_get_32(); + } motor_link_observe_reply(motor_devices[idx], &data->common.link); // Data[0]: 命令字节 // Data[1]: 温度 @@ -340,60 +371,52 @@ void lk_rx_data_handler(struct k_work *work) void lk_tx_isr_handler(struct k_timer *dummy) { - k_work_submit_to_queue(&lk_work_queue, &lk_tx_data_handle); + uint32_t now = k_uptime_get_32(); + + ARG_UNUSED(dummy); + + for (int i = 0; i < MOTOR_COUNT; i++) { + struct lk_motor_data *data = motor_devices[i]->data; + + if (!data->common.link.requested_enabled) { + continue; + } + if (data->last_status_reply_ms != 0U && + now - data->last_status_reply_ms < LK_STATUS_REPLY_FRESH_MS) { + continue; + } + k_work_submit_to_queue(&lk_work_queue, &lk_tx_data_handle); + return; + } } -// 发送处理线程 void lk_tx_data_handler(struct k_work *work) { struct can_frame tx_frame = {0}; + uint32_t now = k_uptime_get_32(); + + ARG_UNUSED(work); for (int i = 0; i < MOTOR_COUNT; i++) { struct lk_motor_data *data = motor_devices[i]->data; const struct lk_motor_cfg *cfg = motor_devices[i]->config; if (data->common.link.requested_enabled) { - if (motor_link_note_missed_reply(motor_devices[i], &data->common.link, - 10)) { - data->offline_tx_cnt = 0; - } - if (!data->common.link.online) { - if ((data->offline_tx_cnt++ % 3U) == 0U) { - memset(&tx_frame, 0, sizeof(tx_frame)); - tx_frame.id = LK_CMD_ID_BASE + cfg->id; - tx_frame.dlc = 8; - tx_frame.flags = 0; - tx_frame.data[0] = LK_CMD_MOTOR_RUN; - motor_can_sched_send_with_priority( - cfg->common.phy, &tx_frame, - MOTOR_CAN_SCHED_PRIO_CRITICAL, "lk-retry-enable"); - } + if (data->last_status_reply_ms != 0U && + now - data->last_status_reply_ms < LK_STATUS_REPLY_FRESH_MS) { continue; } - if (data->need_init_frames) { - memset(&tx_frame, 0, sizeof(tx_frame)); - tx_frame.id = LK_CMD_ID_BASE + cfg->id; - tx_frame.dlc = 8; - tx_frame.flags = 0; - tx_frame.data[0] = LK_CMD_MOTOR_RUN; - motor_can_sched_send_with_priority(cfg->common.phy, &tx_frame, - MOTOR_CAN_SCHED_PRIO_CRITICAL, - "lk-reenable"); - data->params_update[0] = true; - data->params_update[1] = true; - data->params_update[2] = true; - data->need_init_frames = false; + if (motor_link_note_missed_reply(motor_devices[i], &data->common.link, + 10)) { data->offline_tx_cnt = 0; - k_work_submit_to_queue(&lk_work_queue, &lk_tx_params_data_handle); } lk_motor_pack(motor_devices[i], &tx_frame); motor_can_sched_send_reply(cfg->common.phy, &tx_frame, LK_CMD_ID_BASE + cfg->id, CAN_STD_ID_MASK, 5U, - "lk-control"); + tx_frame.data[0] == LK_CMD_READ_STAT ? + "lk-read-stat" : + "lk-control"); } - // if (i % 2 == 1) { - // k_usleep(500); - // } } } void lk_tx_params_data_handler(struct k_work *work) @@ -537,7 +560,7 @@ void lk_init_handler(struct k_work *work) } k_work_submit_to_queue(&lk_work_queue, &lk_tx_params_data_handle); lk_tx_timer.expiry_fn = lk_tx_isr_handler; - k_timer_start(&lk_tx_timer, K_MSEC(600), K_MSEC(8)); + k_timer_start(&lk_tx_timer, K_MSEC(600), K_MSEC(LK_STATUS_POLL_PERIOD_MS)); k_timer_user_data_set(&lk_tx_timer, &lk_tx_data_handle); } diff --git a/drivers/motor/lk/motor_lk.h b/drivers/motor/lk/motor_lk.h index 3724f6c..bd244c1 100644 --- a/drivers/motor/lk/motor_lk.h +++ b/drivers/motor/lk/motor_lk.h @@ -40,6 +40,8 @@ #define CAN_SEND_STACK_SIZE 4096 #define CAN_SEND_PRIORITY -1 +#define LK_STATUS_POLL_PERIOD_MS 8 +#define LK_STATUS_REPLY_FRESH_MS 3 #define PI 3.14159265f #ifdef RAD2DEG #undef RAD2DEG @@ -82,6 +84,7 @@ struct lk_motor_data { bool update; bool need_init_frames; uint16_t offline_tx_cnt; + uint32_t last_status_reply_ms; struct motor_controller_params params[3]; bool params_update[3]; }; diff --git a/tests/native_sim/motor_driver_sim/app.overlay b/tests/native_sim/motor_driver_sim/app.overlay index 7476a05..3ba7153 100644 --- a/tests/native_sim/motor_driver_sim/app.overlay +++ b/tests/native_sim/motor_driver_sim/app.overlay @@ -74,9 +74,9 @@ dji0: dji0 { compatible = "dji,motor"; status = "okay"; - id = <5>; + id = <1>; tx_id = <0x200>; - rx_id = <0x205>; + rx_id = <0x201>; can_channel = <&fake_can>; gear_ratio = "1.0"; is_m3508; @@ -86,9 +86,9 @@ dji1: dji1 { compatible = "dji,motor"; status = "okay"; - id = <6>; + id = <2>; tx_id = <0x200>; - rx_id = <0x206>; + rx_id = <0x202>; can_channel = <&fake_can>; gear_ratio = "1.0"; is_m3508; @@ -96,6 +96,54 @@ }; dji2: dji2 { + compatible = "dji,motor"; + status = "okay"; + id = <5>; + tx_id = <0x1ff>; + rx_id = <0x205>; + can_channel = <&fake_can>; + gear_ratio = "1.0"; + is_m3508; + controllers = <&test_speed>; + }; + + dji3: dji3 { + compatible = "dji,motor"; + status = "okay"; + id = <3>; + tx_id = <0x200>; + rx_id = <0x203>; + can_channel = <&fake_can>; + gear_ratio = "1.0"; + is_m3508; + controllers = <&test_speed>; + }; + + dji4: dji4 { + compatible = "dji,motor"; + status = "okay"; + id = <4>; + tx_id = <0x200>; + rx_id = <0x204>; + can_channel = <&fake_can>; + gear_ratio = "1.0"; + is_m3508; + controllers = <&test_speed>; + }; + + dji5: dji5 { + compatible = "dji,motor"; + status = "okay"; + id = <6>; + tx_id = <0x1ff>; + rx_id = <0x206>; + can_channel = <&fake_can>; + gear_ratio = "1.0"; + is_m3508; + controllers = <&test_speed>; + }; + + dji6: dji6 { compatible = "dji,motor"; status = "okay"; id = <7>; diff --git a/tests/native_sim/motor_driver_sim/src/main.c b/tests/native_sim/motor_driver_sim/src/main.c index 0065f74..095604d 100644 --- a/tests/native_sim/motor_driver_sim/src/main.c +++ b/tests/native_sim/motor_driver_sim/src/main.c @@ -24,6 +24,10 @@ #define DJI_NODE DT_NODELABEL(dji0) #define DJI2_NODE DT_NODELABEL(dji1) #define DJI3_NODE DT_NODELABEL(dji2) +#define DJI4_NODE DT_NODELABEL(dji3) +#define DJI5_NODE DT_NODELABEL(dji4) +#define DJI6_NODE DT_NODELABEL(dji5) +#define DJI7_NODE DT_NODELABEL(dji6) #define DM_DEV DEVICE_DT_GET(DM_NODE) #define MI_DEV DEVICE_DT_GET(MI_NODE) @@ -32,6 +36,10 @@ #define DJI_DEV DEVICE_DT_GET(DJI_NODE) #define DJI2_DEV DEVICE_DT_GET(DJI2_NODE) #define DJI3_DEV DEVICE_DT_GET(DJI3_NODE) +#define DJI4_DEV DEVICE_DT_GET(DJI4_NODE) +#define DJI5_DEV DEVICE_DT_GET(DJI5_NODE) +#define DJI6_DEV DEVICE_DT_GET(DJI6_NODE) +#define DJI7_DEV DEVICE_DT_GET(DJI7_NODE) #define DM_TX_ID DT_PROP(DM_NODE, tx_id) #define DM_RX_ID DT_PROP(DM_NODE, rx_id) @@ -44,6 +52,10 @@ #define DJI2_RX_ID DT_PROP(DJI2_NODE, rx_id) #define DJI3_TX_ID DT_PROP(DJI3_NODE, tx_id) #define DJI3_RX_ID DT_PROP(DJI3_NODE, rx_id) +#define DJI4_RX_ID DT_PROP(DJI4_NODE, rx_id) +#define DJI5_RX_ID DT_PROP(DJI5_NODE, rx_id) +#define DJI6_RX_ID DT_PROP(DJI6_NODE, rx_id) +#define DJI7_RX_ID DT_PROP(DJI7_NODE, rx_id) #define MI_MODE_FEEDBACK 0x02U #define RS_MODE_FEEDBACK 0x02U @@ -51,7 +63,7 @@ #define RS_MODE_MOTOR_REPORT 0x18U #define SIM_FILTER_MAX 16 -#define SIM_TX_MAX 128 +#define SIM_TX_MAX 512 #define SIM_CAN_DLEN 8U #define DM_RATE_WINDOW_MS 300 @@ -59,8 +71,15 @@ #define RS_RATE_WINDOW_MS 300 #define DJI_RATE_WINDOW_MS 300 -#define CONTROL_LATENCY_MS 1000 -#define DJI_CONTROL_LATENCY_MS 30 +#define CONTROL_LATENCY_MS 50 +#define DJI_CONTROL_LATENCY_MS 2 +#define DJI_7_MOTOR_LOAD_COUNT 7 +#define DJI_7_MOTOR_LOAD_ROUNDS 100 +#define DJI_7_MOTOR_MIN_CONTROL_HZ 950 +/* Includes SOF through IFS for classic CAN; bit stuffing is intentionally not modeled. */ +#define SIM_CAN_CLASSIC_STD_BASE_BITS 47U +#define SIM_CAN_CLASSIC_EXT_BASE_BITS 67U +#define SIM_CAN_DEFAULT_BITRATE 1000000U #define ONLINE_RECOVERY_MS 30 #define REPLY_RESPONDER_STACK_SIZE 1024 @@ -81,6 +100,16 @@ static struct sim_filter sim_filters[SIM_FILTER_MAX]; static struct sim_tx_record sim_tx_history[SIM_TX_MAX]; static uint32_t sim_tx_count; static struct k_spinlock sim_lock; +static uint32_t sim_bus_free_cycle; +static bool sim_bus_reserved; + +static const struct device *const dji_load_devs[DJI_7_MOTOR_LOAD_COUNT] = { + DJI_DEV, DJI2_DEV, DJI4_DEV, DJI5_DEV, DJI3_DEV, DJI6_DEV, DJI7_DEV, +}; + +static const uint32_t dji_load_rx_ids[DJI_7_MOTOR_LOAD_COUNT] = { + DJI_RX_ID, DJI2_RX_ID, DJI4_RX_ID, DJI5_RX_ID, DJI3_RX_ID, DJI6_RX_ID, DJI7_RX_ID, +}; struct test_can_config { struct can_driver_config common; @@ -118,6 +147,55 @@ static bool frame_matches_filter(const struct can_frame *frame, const struct can return (frame->id & filter->mask) == (filter->id & filter->mask); } +static uint32_t sim_can_frame_bits(const struct can_frame *frame) +{ + uint32_t data_bytes = MIN(frame->dlc, SIM_CAN_DLEN); + uint32_t base_bits = ((frame->flags & CAN_FRAME_IDE) != 0U) ? + SIM_CAN_CLASSIC_EXT_BASE_BITS : + SIM_CAN_CLASSIC_STD_BASE_BITS; + + if ((frame->flags & CAN_FRAME_RTR) != 0U) { + data_bytes = 0U; + } + + return base_bits + (data_bytes * 8U); +} + +static uint32_t sim_can_frame_time_us(const struct device *dev, const struct can_frame *frame) +{ + const struct test_can_config *cfg = dev->config; + uint32_t bitrate = cfg->common.bitrate; + uint32_t bits = sim_can_frame_bits(frame); + + if (bitrate == 0U) { + bitrate = SIM_CAN_DEFAULT_BITRATE; + } + + return (bits * USEC_PER_SEC + bitrate - 1U) / bitrate; +} + +static void sim_can_wait_for_bus_frame(const struct device *dev, const struct can_frame *frame) +{ + uint32_t duration_us = sim_can_frame_time_us(dev, frame); + uint32_t duration_cycles = k_us_to_cyc_ceil32(duration_us); + uint32_t wait_cycles; + k_spinlock_key_t key = k_spin_lock(&sim_lock); + uint32_t now = k_cycle_get_32(); + uint32_t start = now; + + if (sim_bus_reserved && (int32_t)(sim_bus_free_cycle - now) > 0) { + start = sim_bus_free_cycle; + } + sim_bus_free_cycle = start + duration_cycles; + sim_bus_reserved = true; + wait_cycles = sim_bus_free_cycle - now; + k_spin_unlock(&sim_lock, key); + + if (wait_cycles > 0U) { + k_busy_wait(k_cyc_to_us_ceil32(wait_cycles)); + } +} + static int sim_can_add_rx_filter(const struct device *dev, can_rx_callback_t cb, void *user_data, const struct can_filter *filter) { @@ -147,6 +225,8 @@ static int sim_can_send(const struct device *dev, const struct can_frame *frame, { ARG_UNUSED(timeout); + sim_can_wait_for_bus_frame(dev, frame); + k_spinlock_key_t key = k_spin_lock(&sim_lock); sim_tx_history[sim_tx_count % ARRAY_SIZE(sim_tx_history)] = (struct sim_tx_record){ .frame = *frame, @@ -273,6 +353,8 @@ static void sim_reset_tx_history(void) memset(sim_tx_history, 0, sizeof(sim_tx_history)); sim_tx_count = 0; + sim_bus_free_cycle = k_cycle_get_32(); + sim_bus_reserved = false; k_spin_unlock(&sim_lock, key); } @@ -352,6 +434,44 @@ static uint32_t sim_count_tx_since(uint32_t start, bool (*match)(const struct ca return count; } +static void sim_count_dji_7_motor_controls_since(uint32_t start, + uint32_t counts[DJI_7_MOTOR_LOAD_COUNT]) +{ + k_spinlock_key_t key = k_spin_lock(&sim_lock); + uint32_t end = sim_tx_count; + + memset(counts, 0, sizeof(uint32_t) * DJI_7_MOTOR_LOAD_COUNT); + for (uint32_t i = start; i < end; i++) { + const struct can_frame *frame = + &sim_tx_history[i % ARRAY_SIZE(sim_tx_history)].frame; + + if ((frame->flags & CAN_FRAME_IDE) != 0U || frame->dlc != SIM_CAN_DLEN) { + continue; + } + if (frame->id == 0x200U) { + for (uint8_t slot = 0; slot < 4U; slot++) { + uint16_t value = ((uint16_t)frame->data[slot * 2U] << 8) | + frame->data[slot * 2U + 1U]; + + if (value != 0U) { + counts[slot]++; + } + } + } else if (frame->id == 0x1ffU) { + for (uint8_t slot = 0; slot < 3U; slot++) { + uint16_t value = ((uint16_t)frame->data[slot * 2U] << 8) | + frame->data[slot * 2U + 1U]; + + if (value != 0U) { + counts[4U + slot]++; + } + } + } + } + + k_spin_unlock(&sim_lock, key); +} + static void sim_dump_tx_since(uint32_t start) { k_spinlock_key_t key = k_spin_lock(&sim_lock); @@ -482,6 +602,8 @@ static void sim_emit_frame(const struct device *can_dev, const struct can_frame { struct sim_filter filters[SIM_FILTER_MAX]; + sim_can_wait_for_bus_frame(can_dev, frame); + k_spinlock_key_t key = k_spin_lock(&sim_lock); memcpy(filters, sim_filters, sizeof(filters)); k_spin_unlock(&sim_lock, key); @@ -831,6 +953,13 @@ static void emit_dji_report_for(uint32_t rx_id, int16_t rpm) sim_emit_frame(DEVICE_DT_GET(DT_NODELABEL(fake_can)), &frame); } +static void emit_all_dji_load_reports(int16_t rpm) +{ + for (uint8_t i = 0; i < ARRAY_SIZE(dji_load_rx_ids); i++) { + emit_dji_report_for(dji_load_rx_ids[i], rpm); + } +} + static void emit_dji_report_with_rpm(int16_t rpm) { emit_dji_report_for(DJI_RX_ID, rpm); @@ -1109,7 +1238,6 @@ static void verify_lk_payload_sequence(void) int ret = motor_set_speed(LK_DEV, speeds[i]); zassert_equal(ret, 0, "LK rejected speed setpoint %d", ret); - lk_tx_data_handler(NULL); expected_lk_speed(speeds[i], payload); expect_payload_sequence_step("LK", match_lk_tx, 0x140U + LK_ID, CAN_STD_ID_MASK, payload, set_at_ms, emit_lk_feedback, @@ -1214,7 +1342,7 @@ ZTEST(motor_driver_sim, test_dji_distinct_tx_ids_send_independent_frames) &previous_match); previous_match = UINT32_MAX; - expected_dji_current_slot(dji_expected_current(700.0f, 0.0f), 2, payload2); + expected_dji_current_slot(dji_expected_current(700.0f, 0.0f), 0, payload2); report_at_ms = (uint32_t)k_uptime_get(); emit_dji_report_for(DJI3_RX_ID, 0); service_dji_tx_work(); @@ -1223,6 +1351,52 @@ ZTEST(motor_driver_sim, test_dji_distinct_tx_ids_send_independent_frames) &previous_match); } +ZTEST(motor_driver_sim, test_dji_7_motors_on_one_can_keep_950hz_control) +{ + uint32_t counts[DJI_7_MOTOR_LOAD_COUNT]; + uint32_t start; + uint32_t start_cycle; + uint32_t end_cycle; + uint32_t elapsed_us; + + for (uint8_t i = 0; i < ARRAY_SIZE(dji_load_devs); i++) { + zassert_true(device_is_ready(dji_load_devs[i]), + "DJI load motor %u device is not ready", i); + driver_motor_control(dji_load_devs[i], ENABLE_MOTOR); + configure_dji_speed_test_limits(dji_load_devs[i]); + zassert_equal(motor_set_speed(dji_load_devs[i], 1000.0f + (float)(i * 100U)), + 0, "DJI load motor %u rejected speed setpoint", i); + } + + emit_all_dji_load_reports(0); + service_dji_tx_work(); + k_sleep(K_MSEC(2)); + sim_reset_tx_history(); + start = sim_current_tx_count(); + start_cycle = k_cycle_get_32(); + + for (uint32_t round = 0; round < DJI_7_MOTOR_LOAD_ROUNDS; round++) { + emit_all_dji_load_reports(0); + service_dji_tx_work(); + } + + end_cycle = k_cycle_get_32(); + elapsed_us = k_cyc_to_us_near32(end_cycle - start_cycle); + zassert_true(elapsed_us > 0U, "DJI load elapsed time was zero"); + + sim_count_dji_7_motor_controls_since(start, counts); + for (uint8_t i = 0; i < ARRAY_SIZE(counts); i++) { + uint32_t hz = (counts[i] * 1000000U) / elapsed_us; + + if (hz < DJI_7_MOTOR_MIN_CONTROL_HZ) { + sim_dump_tx_since(start); + } + zassert_true(hz >= DJI_7_MOTOR_MIN_CONTROL_HZ, + "DJI load motor %u control rate %u Hz below %u Hz (%u frames/%u us)", + i, hz, DJI_7_MOTOR_MIN_CONTROL_HZ, counts[i], elapsed_us); + } +} + static void *motor_driver_sim_setup(void) { k_sleep(K_MSEC(1300)); @@ -1241,6 +1415,10 @@ static void motor_driver_sim_before(void *fixture) driver_motor_control(DJI_DEV, DISABLE_MOTOR); driver_motor_control(DJI2_DEV, DISABLE_MOTOR); driver_motor_control(DJI3_DEV, DISABLE_MOTOR); + driver_motor_control(DJI4_DEV, DISABLE_MOTOR); + driver_motor_control(DJI5_DEV, DISABLE_MOTOR); + driver_motor_control(DJI6_DEV, DISABLE_MOTOR); + driver_motor_control(DJI7_DEV, DISABLE_MOTOR); drain_all_reply_motors_until_quiet(50, 1200); sim_reset_tx_history(); } From 61c942cd31174f14a8c4cf1dff0dba0ba069bf1c Mon Sep 17 00:00:00 2001 From: ttwards <12411711@mail.sustech.edu.cn> Date: Wed, 8 Jul 2026 16:11:11 +0800 Subject: [PATCH 3/4] docs: update native_sim test coverage --- Documents/drivers/wheel.md | 20 ++++++++++++++++ Documents/motor/drivers.md | 5 +++- Documents/tests/native_sim.md | 43 +++++++++++++++++++++++++++++------ 3 files changed, 60 insertions(+), 8 deletions(-) diff --git a/Documents/drivers/wheel.md b/Documents/drivers/wheel.md index f8ffb39..ab37164 100644 --- a/Documents/drivers/wheel.md +++ b/Documents/drivers/wheel.md @@ -194,6 +194,26 @@ Kconfig： - 计算出的 RPM 命令 - 已缓存的静态角 +### 麦克纳姆轮示意 + +俯视单个麦克纳姆轮时，驱动电机提供轮周切向速度，滚子方向提供自由滚动轴。驱动会把底盘请求速度向量投影到自由滚动轴，再折算成电机 RPM。 + +```text + target vector + ^ + | + | + wheel face | free rolling axis + +-------------+ | / + | / / / / / | | / free-angle + | / / / / / | | / + |/ / / / / | +----+------> motor tangential axis + +-------------+ + + target projection on free axis + = target speed * cos(target angle - free axis angle) +``` + ### 运行时行为 `mecanum_set_speed()` 将请求的底盘运动向量投影到轮子的自由滚动轴，再把投影线速度转换为电机 RPM。 diff --git a/Documents/motor/drivers.md b/Documents/motor/drivers.md index d98d2b0..3d58b77 100644 --- a/Documents/motor/drivers.md +++ b/Documents/motor/drivers.md @@ -273,7 +273,10 @@ LK 使用标准帧，通过不同命令字区分： 驱动有两条工作路径： -- 周期发送控制命令。 +- `motor_set_*()` 更新目标后立即发送当前控制命令。 +- `lk_tx_timer` 每 8ms 重新发送当前控制命令；如果当前没有可打包的速度、扭矩或位置命令， + 则 fallback 到 `0x9C` 状态读取。提交 handler 前若 3ms 内已收到新的 `0x9C` + 状态回复，则跳过本轮发送。 - 独立发送参数更新命令。 这也是 `lk_tx_data_handler()` 和 `lk_tx_params_data_handler()` 分开的原因。 diff --git a/Documents/tests/native_sim.md b/Documents/tests/native_sim.md index 2a8a13a..30fc016 100644 --- a/Documents/tests/native_sim.md +++ b/Documents/tests/native_sim.md @@ -27,6 +27,7 @@ - `tests/native_sim/pid/` - `tests/native_sim/sbus/` - `tests/native_sim/motor_driver_sim/` +- `tests/native_sim/wheel_chassis/` ### 常用命令 @@ -43,6 +44,7 @@ west twister -T tests/native_sim/module_smoke --platform native_sim/native/64 -- west twister -T tests/native_sim/pid --platform native_sim/native/64 --inline-logs west twister -T tests/native_sim/sbus --platform native_sim/native/64 --inline-logs west twister -T tests/native_sim/motor_driver_sim --platform native_sim/native/64 --inline-logs +west twister -T tests/native_sim/wheel_chassis --platform native_sim/native/64 --inline-logs ``` 单测试构建（快速检查）： @@ -112,6 +114,25 @@ ares.native_sim.sbus `prj.conf` 启用 `CONFIG_ARES_SBUS`、UART async API 与 runtime configure，平台限定 `native_sim/native/64`。 +## wheel_chassis + +测试路径：`tests/native_sim/wheel_chassis/src/main.c` 与 `app.overlay` + +测试行为： + +- 使用测试专用 motor 设备承载 `ares,mecanum` 与 `ares,steerwheel`，验证轮级 API 到电机 setpoint 的转换。 +- 覆盖麦克纳姆轮速度投影、状态读取和静态角锁存。 +- 覆盖舵轮短路径反向驱动、零速停止与禁能扭矩清零。 +- 使用测试专用 wheel 设备承载 `ares,chassis`，直接验证底盘公共命令映射、运动解析与静态角下发。 + +testcase： + +```text +ares.native_sim.wheel_chassis +``` + +`prj.conf` 启用 `CONFIG_MOTOR`、`CONFIG_PID`、`CONFIG_WHEEL`、`CONFIG_CHASSIS` 与 `CONFIG_ZTEST`，平台允许 `native_sim` 与 `native_sim/native/64`。 + ## motor_driver_sim 测试路径：`tests/native_sim/motor_driver_sim/src/main.c` 与 `app.overlay` @@ -124,7 +145,7 @@ ares.native_sim.sbus - `mi0`：`mi,motor` - `rs0`：`rs,motor` - `lk0`：`lk,motor` -- `dji0`：`dji,motor` +- `dji0`~`dji6`：`dji,motor` 驱动配置包含 `fake_can`，并通过 `fake_can` 通道复用 `can_channel`。 @@ -142,15 +163,17 @@ ares.native_sim.sbus `send` 行为： -- 每帧记录到环形缓冲 `SIM_TX_MAX = 128` +- 按 `bitrate` 和经典 CAN 帧格式占用虚拟总线时间 +- 标准帧 8 字节按约 111 bit 计算，扩展帧 8 字节按约 131 bit 计算 +- 每帧记录到环形缓冲 `SIM_TX_MAX = 512` - 记录 `can_frame`、`uptime`、`cycle` -- 同步回调立即触发 tx 回调（`callback` 非空时） +- TX 记录时间为帧完成时间，tx 回调在虚拟帧发送完成后触发 `add_rx_filter` 与回调分发行为： - 过滤注册按 `can_filter` 匹配 -- `sim_emit_frame` 遍历过滤器并触发回调 -- 不模拟总线仲裁、mailbox 竞争、bus-off、误码状态 +- `sim_emit_frame` 先占用虚拟总线时间，再遍历过滤器并触发回调 +- 不模拟 bit stuffing、总线仲裁、mailbox 竞争、bus-off、误码状态 该模型适合协议与驱动逻辑验证，不适合硬件级收发行为还原。 @@ -195,6 +218,8 @@ ares.native_sim.sbus - DM/MI/RS：`motor_set_mit` 的连续 setpoint（10、20、30）； - LK：`motor_set_speed` 的连续 setpoint（25、50、75）； - 每次下发前/后都有 `emit_*_feedback` 的配套； +- LK 速度 setpoint 期望直接触发 `0xA2` 控制帧；8ms timer 平常重发当前命令，只有无可用 + 控制目标时才 fallback 到 `0x9C` 状态读取。 - 使用期望帧匹配函数比较： - 帧 ID（标准/扩展） - DLC（固定 8） @@ -205,8 +230,8 @@ ares.native_sim.sbus #### 延迟阈值 -- `CONTROL_LATENCY_MS = 1000` -- `DJI_CONTROL_LATENCY_MS = 30` +- `CONTROL_LATENCY_MS = 50` +- `DJI_CONTROL_LATENCY_MS = 2` 延迟由下发时间戳到匹配控制帧到达时间计算。测试使用 `expect_payload_sequence_step` 在窗口内等待；超时给出完整帧转储用于诊断。 @@ -231,6 +256,10 @@ ares.native_sim.sbus `test_dji_control_not_starved_by_reply_backlog` 构造 DM backlog 后验证 DJI 控制帧仍能发送，确保请求回复流量不会永久压住周期/关键控制链路。 +`test_dji_7_motors_on_one_can_keep_950hz_control` 将 7 个 DJI 电机挂在同一个 +fake CAN 上，按 `0x200`/`0x1ff` 聚合帧槽位统计每个电机的控制更新频率， +要求每个电机不低于 950Hz。 + ## 配置与 testcase 约定 ### `testcase.yaml`（当前约定） From 9194d94bcaca95657d88cbda98807551e4855f25 Mon Sep 17 00:00:00 2001 From: ttwards <12411711@mail.sustech.edu.cn> Date: Wed, 8 Jul 2026 16:21:38 +0800 Subject: [PATCH 4/4] style: apply pre-commit formatting --- drivers/motor/lk/motor_lk.c | 6 ++--- drivers/motor/lk/motor_lk.h | 6 ++--- drivers/wheel/mecanum.c | 2 +- tests/native_sim/motor_driver_sim/src/main.c | 23 ++++++++++---------- tests/native_sim/wheel_chassis/src/main.c | 20 ++++++++--------- 5 files changed, 28 insertions(+), 29 deletions(-) diff --git a/drivers/motor/lk/motor_lk.c b/drivers/motor/lk/motor_lk.c index 3070b8e..a6e85e1 100644 --- a/drivers/motor/lk/motor_lk.c +++ b/drivers/motor/lk/motor_lk.c @@ -413,9 +413,9 @@ void lk_tx_data_handler(struct k_work *work) lk_motor_pack(motor_devices[i], &tx_frame); motor_can_sched_send_reply(cfg->common.phy, &tx_frame, LK_CMD_ID_BASE + cfg->id, CAN_STD_ID_MASK, 5U, - tx_frame.data[0] == LK_CMD_READ_STAT ? - "lk-read-stat" : - "lk-control"); + tx_frame.data[0] == LK_CMD_READ_STAT + ? "lk-read-stat" + : "lk-control"); } } } diff --git a/drivers/motor/lk/motor_lk.h b/drivers/motor/lk/motor_lk.h index bd244c1..f33e519 100644 --- a/drivers/motor/lk/motor_lk.h +++ b/drivers/motor/lk/motor_lk.h @@ -38,11 +38,11 @@ #define SIZE_OF_ARRAY(x) (sizeof(x) / sizeof(x[0])) -#define CAN_SEND_STACK_SIZE 4096 -#define CAN_SEND_PRIORITY -1 +#define CAN_SEND_STACK_SIZE 4096 +#define CAN_SEND_PRIORITY -1 #define LK_STATUS_POLL_PERIOD_MS 8 #define LK_STATUS_REPLY_FRESH_MS 3 -#define PI 3.14159265f +#define PI 3.14159265f #ifdef RAD2DEG #undef RAD2DEG #endif diff --git a/drivers/wheel/mecanum.c b/drivers/wheel/mecanum.c index 40ba7b7..0fecae2 100644 --- a/drivers/wheel/mecanum.c +++ b/drivers/wheel/mecanum.c @@ -146,7 +146,7 @@ struct wheel_driver_api mecanum_driver_api = { DT_STRING_UNQUOTED(DT_DRV_INST(inst), angle_offset) + \ DT_STRING_UNQUOTED(DT_DRV_INST(inst), free_angle) - 90.0f, \ false}, \ - .static_angle = NAN, \ + .static_angle = NAN, \ }; \ static const mecanum_cfg_t mecanum_cfg_##inst = { \ .common = DT_WHEEL_CONFIG_GET(inst), \ diff --git a/tests/native_sim/motor_driver_sim/src/main.c b/tests/native_sim/motor_driver_sim/src/main.c index 095604d..6fb5aa9 100644 --- a/tests/native_sim/motor_driver_sim/src/main.c +++ b/tests/native_sim/motor_driver_sim/src/main.c @@ -71,17 +71,17 @@ #define RS_RATE_WINDOW_MS 300 #define DJI_RATE_WINDOW_MS 300 -#define CONTROL_LATENCY_MS 50 -#define DJI_CONTROL_LATENCY_MS 2 -#define DJI_7_MOTOR_LOAD_COUNT 7 -#define DJI_7_MOTOR_LOAD_ROUNDS 100 -#define DJI_7_MOTOR_MIN_CONTROL_HZ 950 +#define CONTROL_LATENCY_MS 50 +#define DJI_CONTROL_LATENCY_MS 2 +#define DJI_7_MOTOR_LOAD_COUNT 7 +#define DJI_7_MOTOR_LOAD_ROUNDS 100 +#define DJI_7_MOTOR_MIN_CONTROL_HZ 950 /* Includes SOF through IFS for classic CAN; bit stuffing is intentionally not modeled. */ #define SIM_CAN_CLASSIC_STD_BASE_BITS 47U #define SIM_CAN_CLASSIC_EXT_BASE_BITS 67U #define SIM_CAN_DEFAULT_BITRATE 1000000U -#define ONLINE_RECOVERY_MS 30 -#define REPLY_RESPONDER_STACK_SIZE 1024 +#define ONLINE_RECOVERY_MS 30 +#define REPLY_RESPONDER_STACK_SIZE 1024 struct sim_filter { bool used; @@ -150,9 +150,8 @@ static bool frame_matches_filter(const struct can_frame *frame, const struct can static uint32_t sim_can_frame_bits(const struct can_frame *frame) { uint32_t data_bytes = MIN(frame->dlc, SIM_CAN_DLEN); - uint32_t base_bits = ((frame->flags & CAN_FRAME_IDE) != 0U) ? - SIM_CAN_CLASSIC_EXT_BASE_BITS : - SIM_CAN_CLASSIC_STD_BASE_BITS; + uint32_t base_bits = ((frame->flags & CAN_FRAME_IDE) != 0U) ? SIM_CAN_CLASSIC_EXT_BASE_BITS + : SIM_CAN_CLASSIC_STD_BASE_BITS; if ((frame->flags & CAN_FRAME_RTR) != 0U) { data_bytes = 0U; @@ -1364,8 +1363,8 @@ ZTEST(motor_driver_sim, test_dji_7_motors_on_one_can_keep_950hz_control) "DJI load motor %u device is not ready", i); driver_motor_control(dji_load_devs[i], ENABLE_MOTOR); configure_dji_speed_test_limits(dji_load_devs[i]); - zassert_equal(motor_set_speed(dji_load_devs[i], 1000.0f + (float)(i * 100U)), - 0, "DJI load motor %u rejected speed setpoint", i); + zassert_equal(motor_set_speed(dji_load_devs[i], 1000.0f + (float)(i * 100U)), 0, + "DJI load motor %u rejected speed setpoint", i); } emit_all_dji_load_reports(0); diff --git a/tests/native_sim/wheel_chassis/src/main.c b/tests/native_sim/wheel_chassis/src/main.c index 3dca21d..b598064 100644 --- a/tests/native_sim/wheel_chassis/src/main.c +++ b/tests/native_sim/wheel_chassis/src/main.c @@ -18,16 +18,16 @@ #define FLOAT_TOLERANCE 0.0001f -#define MECANUM_DEV DEVICE_DT_GET(DT_NODELABEL(mecanum0)) -#define MECANUM_MOTOR_DEV DEVICE_DT_GET(DT_NODELABEL(mecanum_motor)) -#define STEER_DEV DEVICE_DT_GET(DT_NODELABEL(steer0)) -#define STEER_MOTOR_DEV DEVICE_DT_GET(DT_NODELABEL(steer_motor)) -#define STEER_DRIVE_DEV DEVICE_DT_GET(DT_NODELABEL(steer_drive_motor)) -#define CHASSIS_DEV DEVICE_DT_GET(DT_NODELABEL(chassis0)) -#define CHASSIS_WHEEL0_DEV DEVICE_DT_GET(DT_NODELABEL(chassis_wheel0)) -#define CHASSIS_WHEEL1_DEV DEVICE_DT_GET(DT_NODELABEL(chassis_wheel1)) -#define CHASSIS_WHEEL2_DEV DEVICE_DT_GET(DT_NODELABEL(chassis_wheel2)) -#define CHASSIS_WHEEL3_DEV DEVICE_DT_GET(DT_NODELABEL(chassis_wheel3)) +#define MECANUM_DEV DEVICE_DT_GET(DT_NODELABEL(mecanum0)) +#define MECANUM_MOTOR_DEV DEVICE_DT_GET(DT_NODELABEL(mecanum_motor)) +#define STEER_DEV DEVICE_DT_GET(DT_NODELABEL(steer0)) +#define STEER_MOTOR_DEV DEVICE_DT_GET(DT_NODELABEL(steer_motor)) +#define STEER_DRIVE_DEV DEVICE_DT_GET(DT_NODELABEL(steer_drive_motor)) +#define CHASSIS_DEV DEVICE_DT_GET(DT_NODELABEL(chassis0)) +#define CHASSIS_WHEEL0_DEV DEVICE_DT_GET(DT_NODELABEL(chassis_wheel0)) +#define CHASSIS_WHEEL1_DEV DEVICE_DT_GET(DT_NODELABEL(chassis_wheel1)) +#define CHASSIS_WHEEL2_DEV DEVICE_DT_GET(DT_NODELABEL(chassis_wheel2)) +#define CHASSIS_WHEEL3_DEV DEVICE_DT_GET(DT_NODELABEL(chassis_wheel3)) #define EXPECTED_RPM_1MPS_R01 (RADPS_TO_RPM / 0.1f) void cchassis_resolve(chassis_data_t *data, const chassis_cfg_t *cfg);