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/7] 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/7] 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/7] 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/7] 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); From 6ecf1359d79344576c264376f476855372fcb6ac Mon Sep 17 00:00:00 2001 From: ttwards <12411711@mail.sustech.edu.cn> Date: Wed, 8 Jul 2026 21:35:38 +0800 Subject: [PATCH 5/7] feat(motor): add Gaoqing CAN motor driver --- drivers/motor/CMakeLists.txt | 2 + drivers/motor/Kconfig | 6 + drivers/motor/gq/CMakeLists.txt | 5 + drivers/motor/gq/motor_gq.c | 562 +++++++++++++++++++++++++++++++ drivers/motor/gq/motor_gq.h | 90 +++++ dts/bindings/motor/gq,motor.yaml | 32 ++ dts/bindings/vendor-prefixes.txt | 1 + 7 files changed, 698 insertions(+) create mode 100644 drivers/motor/gq/CMakeLists.txt create mode 100644 drivers/motor/gq/motor_gq.c create mode 100644 drivers/motor/gq/motor_gq.h create mode 100644 dts/bindings/motor/gq,motor.yaml diff --git a/drivers/motor/CMakeLists.txt b/drivers/motor/CMakeLists.txt index 7da3cdf..ea3386d 100644 --- a/drivers/motor/CMakeLists.txt +++ b/drivers/motor/CMakeLists.txt @@ -9,6 +9,8 @@ add_subdirectory_ifdef(CONFIG_MOTOR_LK lk) add_subdirectory_ifdef(CONFIG_MOTOR_DM dm) +add_subdirectory_ifdef(CONFIG_MOTOR_GQ gq) + add_subdirectory_ifdef(CONFIG_MOTOR_RS robstride) add_subdirectory_ifdef(CONFIG_MOTOR_VESC vesc) diff --git a/drivers/motor/Kconfig b/drivers/motor/Kconfig index 813fb4a..35c1b68 100644 --- a/drivers/motor/Kconfig +++ b/drivers/motor/Kconfig @@ -77,6 +77,12 @@ config MOTOR_DM Add support for dm motors select MOTOR_COMMON +config MOTOR_GQ + bool "MOTOR GQ" + help + Add support for Gaoqing motors over classic CAN or CAN FD. + select MOTOR_COMMON + config MOTOR_DM_DEFAULT_FREQ_HZ int "Default DM motor request frequency in Hz" default 200 diff --git a/drivers/motor/gq/CMakeLists.txt b/drivers/motor/gq/CMakeLists.txt new file mode 100644 index 0000000..f5d9a69 --- /dev/null +++ b/drivers/motor/gq/CMakeLists.txt @@ -0,0 +1,5 @@ +# Copyright (c) 2026 ttwards <12411711@mail.sustech.edu.cn> +# SPDX-License-Identifier: Apache-2.0 + +zephyr_library() +zephyr_library_sources_ifdef(CONFIG_MOTOR_GQ motor_gq.c) diff --git a/drivers/motor/gq/motor_gq.c b/drivers/motor/gq/motor_gq.c new file mode 100644 index 0000000..79735f5 --- /dev/null +++ b/drivers/motor/gq/motor_gq.c @@ -0,0 +1,562 @@ +/* + * Copyright (c) 2026 ttwards <12411711@mail.sustech.edu.cn> + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include "motor_gq.h" + +#include +#include +#include + +#include "../common/common.h" +#include "../common/motor_can_sched.h" +#include "../common/motor_link.h" + +#include +#include +#include + +#define DT_DRV_COMPAT gq_motor + +LOG_MODULE_REGISTER(motor_gq, CONFIG_MOTOR_LOG_LEVEL); + +static uint32_t gq_motor_id(const struct gq_motor_config *cfg) +{ + return cfg->common.id & 0x7FU; +} + +static uint32_t gq_classic_tx_id(const struct gq_motor_config *cfg) +{ + return cfg->common.tx_id != 0 ? (uint32_t)cfg->common.tx_id : gq_motor_id(cfg); +} + +static uint32_t gq_reply_id(const struct gq_motor_config *cfg) +{ + return cfg->common.rx_id != 0 ? (uint32_t)cfg->common.rx_id : + (gq_motor_id(cfg) << GQ_REPLY_ID_SHIFT); +} + +static int16_t gq_clamp_i16(float value) +{ + if (isnan(value)) { + return GQ_INT16_NAN; + } + if (value > (float)INT16_MAX) { + motor_stats_inc(MOTOR_STAT_LIMIT_CLAMP); + return INT16_MAX; + } + if (value < (float)INT16_MIN) { + motor_stats_inc(MOTOR_STAT_LIMIT_CLAMP); + return INT16_MIN; + } + return (int16_t)value; +} + +static int16_t gq_angle_to_raw(float angle_deg) +{ + return gq_clamp_i16(angle_deg / 360.0f * GQ_POS_SCALE_INT16); +} + +static int16_t gq_rpm_to_raw(float rpm) +{ + return gq_clamp_i16(rpm / 60.0f * GQ_VEL_SCALE_INT16); +} + +static int16_t gq_torque_to_raw(float torque) +{ + return gq_clamp_i16(torque * GQ_TORQUE_SCALE_INT16); +} + +static int16_t gq_pid_to_raw(float gain) +{ + return gq_clamp_i16(gain * GQ_PID_SCALE_INT16); +} + +static float gq_raw_pos_to_angle(int16_t raw) +{ + return (float)raw / GQ_POS_SCALE_INT16 * 360.0f; +} + +static float gq_raw_vel_to_rpm(int16_t raw) +{ + return (float)raw / GQ_VEL_SCALE_INT16 * 60.0f; +} + +static float gq_raw_torque_to_nm(int16_t raw) +{ + return (float)raw / GQ_TORQUE_SCALE_INT16; +} + +static void gq_put_i16(uint8_t *dst, int16_t value) +{ + sys_put_le16((uint16_t)value, dst); +} + +static int16_t gq_get_i16(const uint8_t *src) +{ + return (int16_t)sys_get_le16(src); +} + +static void gq_init_frame(struct can_frame *frame, uint32_t id, uint8_t len, bool extended, + bool canfd) +{ + *frame = (struct can_frame){ + .id = id, + .dlc = can_bytes_to_dlc(len), + .flags = extended ? CAN_FRAME_IDE : 0, + }; + + if (canfd) { + frame->flags |= CAN_FRAME_FDF | CAN_FRAME_BRS; + } +} + +static int gq_send_frame(const struct device *dev, const struct can_frame *frame, const char *tag) +{ + const struct gq_motor_config *cfg = dev->config; + int ret = motor_can_sched_send_with_priority(cfg->common.phy, frame, + MOTOR_CAN_SCHED_PRIO_CRITICAL, tag); + + if (ret < 0) { + motor_stats_inc(MOTOR_STAT_TX_ERROR); + } + return ret; +} + +static void gq_pack_classic_stop(const struct device *dev, struct can_frame *frame, bool brake) +{ + const struct gq_motor_config *cfg = dev->config; + + gq_init_frame(frame, gq_classic_tx_id(cfg), 3, gq_classic_tx_id(cfg) > CAN_STD_ID_MASK, + false); + frame->data[0] = 0x01; + frame->data[1] = 0x00; + frame->data[2] = brake ? 0x0F : 0x00; +} + +static void gq_pack_classic_zero(const struct device *dev, struct can_frame *frame) +{ + const struct gq_motor_config *cfg = dev->config; + uint32_t id = GQ_CLASSIC_EXT_PREFIX_CONFIG | gq_motor_id(cfg); + const uint8_t cmd[] = {0x40, 0x01, 0x04, 0x64, 0x20, 0x63, 0x0A}; + + gq_init_frame(frame, id, sizeof(cmd), true, false); + memcpy(frame->data, cmd, sizeof(cmd)); +} + +static void gq_pack_classic_control(const struct device *dev, struct can_frame *frame) +{ + const struct gq_motor_config *cfg = dev->config; + struct gq_motor_data *data = dev->data; + int16_t pos = gq_angle_to_raw(data->target_angle); + int16_t vel = gq_rpm_to_raw(data->target_rpm); + int16_t tqe = gq_torque_to_raw(data->target_torque); + + switch (data->common.mode) { + case MIT: + gq_init_frame(frame, GQ_CLASSIC_EXT_PREFIX_MIT | gq_motor_id(cfg), 8, true, false); + int16_t kp = gq_pid_to_raw(data->kp); + int16_t kd = gq_pid_to_raw(data->kd); + + frame->data[0] = pos & 0xFF; + frame->data[1] = (pos >> 8) & 0xFF; + frame->data[2] = vel & 0xFF; + frame->data[3] = ((vel >> 8) & 0x0F) | ((tqe & 0x0F) << 4); + frame->data[4] = (tqe >> 4) & 0xFF; + frame->data[5] = kp & 0xFF; + frame->data[6] = ((kp >> 8) & 0x0F) | ((kd & 0x0F) << 4); + frame->data[7] = kd >> 4; + break; + case PV: + gq_init_frame(frame, gq_classic_tx_id(cfg), 8, + gq_classic_tx_id(cfg) > CAN_STD_ID_MASK, false); + frame->data[0] = 0x07; + frame->data[1] = 0x35; + gq_put_i16(&frame->data[2], vel); + gq_put_i16(&frame->data[4], + data->target_torque == 0.0f ? GQ_INT16_NAN : tqe); + gq_put_i16(&frame->data[6], pos); + break; + case VO: + gq_init_frame(frame, gq_classic_tx_id(cfg), + data->common.target == MOTOR_TARGET_TORQUE ? 4 : 8, + gq_classic_tx_id(cfg) > CAN_STD_ID_MASK, false); + if (data->common.target == MOTOR_TARGET_TORQUE) { + frame->data[0] = 0x05; + frame->data[1] = 0x13; + gq_put_i16(&frame->data[2], tqe); + } else { + frame->data[0] = 0x07; + frame->data[1] = 0x07; + gq_put_i16(&frame->data[2], GQ_INT16_NAN); + gq_put_i16(&frame->data[4], vel); + gq_put_i16(&frame->data[6], GQ_INT16_NAN); + } + break; + default: + break; + } +} + +#ifdef CONFIG_CAN_FD_MODE +static void gq_pack_fd_stop(const struct device *dev, struct can_frame *frame, bool brake) +{ + const struct gq_motor_config *cfg = dev->config; + uint32_t id = GQ_CLASSIC_EXT_PREFIX_CONFIG | gq_motor_id(cfg); + const uint8_t cmd_stop[] = {0x01, 0x00, 0x00, 0x05, 0x06, 0x0F, 0x00}; + const uint8_t cmd_brake[] = {0x01, 0x00, 0x0F, 0x05, 0x06, 0x0F, 0x00}; + const uint8_t *cmd = brake ? cmd_brake : cmd_stop; + + gq_init_frame(frame, id, 7, true, true); + memcpy(frame->data, cmd, 7); +} + +static void gq_pack_fd_zero(const struct device *dev, struct can_frame *frame) +{ + const struct gq_motor_config *cfg = dev->config; + uint32_t id = GQ_CLASSIC_EXT_PREFIX_CONFIG | gq_motor_id(cfg); + const uint8_t cmd[] = {0x40, 0x01, 0x04, 0x64, 0x20, 0x63, 0x0A}; + + gq_init_frame(frame, id, sizeof(cmd), true, true); + memcpy(frame->data, cmd, sizeof(cmd)); +} + +static void gq_pack_fd_control(const struct device *dev, struct can_frame *frame) +{ + const struct gq_motor_config *cfg = dev->config; + struct gq_motor_data *data = dev->data; + uint32_t id = GQ_CLASSIC_EXT_PREFIX_CONFIG | gq_motor_id(cfg); + int16_t pos = gq_angle_to_raw(data->target_angle); + int16_t vel = gq_rpm_to_raw(data->target_rpm); + int16_t tqe = gq_torque_to_raw(data->target_torque); + + switch (data->common.mode) { + case MIT: + gq_init_frame(frame, id, 24, true, true); + memcpy(frame->data, + (uint8_t[]){0x01, 0x00, 0x0A, 0x07, 0x20, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x06, 0x2B, 0x00, 0x00, 0x00, + 0x00, 0x14, 0x04, 0x00, 0x11, 0x0F, 0x50, 0x50}, + 24); + gq_put_i16(&frame->data[5], pos); + gq_put_i16(&frame->data[7], vel); + gq_put_i16(&frame->data[9], tqe); + gq_put_i16(&frame->data[13], gq_pid_to_raw(data->kp)); + gq_put_i16(&frame->data[15], gq_pid_to_raw(data->kd)); + break; + case PV: + gq_init_frame(frame, id, 20, true, true); + memcpy(frame->data, + (uint8_t[]){0x01, 0x00, 0x0A, 0x06, 0x20, 0x00, 0x80, 0x00, + 0x00, 0x06, 0x25, 0x00, 0x00, 0x00, 0x00, 0x14, + 0x04, 0x00, 0x11, 0x0F}, + 20); + gq_put_i16(&frame->data[7], vel); + gq_put_i16(&frame->data[11], + data->target_torque == 0.0f ? GQ_INT16_NAN : tqe); + gq_put_i16(&frame->data[13], pos); + break; + case VO: + if (data->common.target == MOTOR_TARGET_TORQUE) { + gq_init_frame(frame, id, 24, true, true); + memcpy(frame->data, + (uint8_t[]){0x01, 0x00, 0x0A, 0x04, 0x06, 0x20, 0x00, 0x80, + 0x00, 0x00, 0x00, 0x80, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x80, 0x14, 0x04, 0x00, 0x11, 0x0F, 0x50}, + 24); + gq_put_i16(&frame->data[10], tqe); + } else { + gq_init_frame(frame, id, 20, true, true); + memcpy(frame->data, + (uint8_t[]){0x01, 0x00, 0x0A, 0x06, 0x20, 0x00, 0x80, 0x00, + 0x00, 0x14, 0x04, 0x00, 0x11, 0x0F, 0x50, 0x50, + 0x00, 0x00, 0x00, 0x00}, + 20); + gq_put_i16(&frame->data[7], vel); + } + break; + default: + break; + } +} +#endif /* CONFIG_CAN_FD_MODE */ + +static void gq_parse_state_int16(const struct device *dev, uint8_t mode, uint8_t fault, + int16_t pos, int16_t vel, int16_t tqe) +{ + struct gq_motor_data *data = dev->data; + + ARG_UNUSED(mode); + data->common.angle = gq_raw_pos_to_angle(pos); + data->common.rpm = gq_raw_vel_to_rpm(vel); + data->common.torque = gq_raw_torque_to_nm(tqe); + data->error = fault; + if (fault != 0) { + motor_stats_inc(MOTOR_STAT_DRIVER_ERROR); + } + motor_link_observe_reply(dev, &data->common.link); +} + +static void gq_can_rx_handler(const struct device *can_dev, struct can_frame *frame, + void *user_data) +{ + const struct device *dev = user_data; + struct gq_motor_data *data = dev->data; + uint8_t len = can_dlc_to_bytes(frame->dlc); + + motor_can_sched_report_rx(can_dev, frame); + + if ((frame->flags & CAN_FRAME_IDE) == 0 || len < 5) { + motor_stats_inc(MOTOR_STAT_UNKNOWN_RX); + return; + } + + uint8_t id = (frame->id >> GQ_REPLY_ID_SHIFT) & 0xFFU; + const struct gq_motor_config *cfg = dev->config; + + if (id != gq_motor_id(cfg)) { + motor_stats_inc(MOTOR_STAT_UNKNOWN_RX); + return; + } + + if (len == 8 && frame->data[0] == 0x27 && frame->data[1] == 0x01) { + gq_parse_state_int16(dev, data->common.mode, 0, gq_get_i16(&frame->data[2]), + gq_get_i16(&frame->data[4]), gq_get_i16(&frame->data[6])); + return; + } + if (len == 8 && frame->data[0] != 0x27) { + gq_parse_state_int16(dev, frame->data[0], frame->data[1], + gq_get_i16(&frame->data[2]), gq_get_i16(&frame->data[4]), + gq_get_i16(&frame->data[6])); + return; + } +#ifdef CONFIG_CAN_FD_MODE + if (len >= 14 && frame->data[0] == 0x24 && frame->data[1] == 0x04 && + frame->data[2] == 0x00 && frame->data[11] == 0x21 && frame->data[12] == 0x0F) { + gq_parse_state_int16(dev, frame->data[3], frame->data[13], + gq_get_i16(&frame->data[5]), gq_get_i16(&frame->data[7]), + gq_get_i16(&frame->data[9])); + return; + } +#endif + if (len == 7 && frame->data[0] == 0x41 && frame->data[1] == 0x01 && + frame->data[2] == 0x04 && frame->data[3] == 0x4F && frame->data[4] == 0x4B && + frame->data[5] == 0x0D && frame->data[6] == 0x0A) { + motor_link_observe_reply(dev, &data->common.link); + return; + } + + motor_stats_inc(MOTOR_STAT_UNKNOWN_RX); +} + +static int gq_check_canfd_support(const struct device *dev) +{ + const struct gq_motor_config *cfg = dev->config; + + if (!cfg->enable_canfd) { + return 0; + } + +#ifndef CONFIG_CAN_FD_MODE + return -ENOTSUP; +#else + can_mode_t capabilities = 0; + int ret = can_get_capabilities(cfg->common.phy, &capabilities); + + if (ret < 0) { + return ret; + } + if ((capabilities & CAN_MODE_FD) == 0) { + LOG_ERR("%s does not support CAN FD", cfg->common.phy->name); + return -ENOTSUP; + } + + can_mode_t mode = can_get_mode(cfg->common.phy); + + if ((mode & CAN_MODE_FD) != 0) { + return 0; + } + + ret = can_set_mode(cfg->common.phy, mode | CAN_MODE_FD); + if (ret == 0) { + return 0; + } + if (ret == -EBUSY && (can_get_mode(cfg->common.phy) & CAN_MODE_FD) != 0) { + return 0; + } + + LOG_ERR("failed to enable CAN FD mode on %s: %d", cfg->common.phy->name, ret); + return ret; +#endif +} + +int gq_init(const struct device *dev) +{ + const struct gq_motor_config *cfg = dev->config; + struct gq_motor_data *data = dev->data; + int ret; + + if (!device_is_ready(cfg->common.phy)) { + motor_stats_inc(MOTOR_STAT_CONFIG_ERROR); + return -ENODEV; + } + + ret = gq_check_canfd_support(dev); + if (ret < 0) { + motor_stats_inc(MOTOR_STAT_CONFIG_ERROR); + return ret; + } + + ret = motor_can_sched_register_can(cfg->common.phy); + if (ret < 0) { + motor_stats_inc(MOTOR_STAT_CONFIG_ERROR); + return ret; + } + + data->filter = (struct can_filter){ + .id = gq_reply_id(cfg), + .mask = GQ_REPLY_ID_MASK, + .flags = CAN_FILTER_IDE, + }; + data->filter_id = can_add_rx_filter(cfg->common.phy, gq_can_rx_handler, (void *)dev, + &data->filter); + if (data->filter_id < 0) { + motor_stats_inc(MOTOR_STAT_CAN_FILTER_ERROR); + return data->filter_id; + } + + return 0; +} + +void gq_control(const struct device *dev, enum motor_cmd cmd) +{ + const struct gq_motor_config *cfg = dev->config; + struct gq_motor_data *data = dev->data; + struct can_frame frame = {0}; + + switch (cmd) { + case ENABLE_MOTOR: + motor_link_request_enable(&data->common.link); + break; + case DISABLE_MOTOR: + if (cfg->enable_canfd) { +#ifdef CONFIG_CAN_FD_MODE + gq_pack_fd_stop(dev, &frame, false); +#else + return; +#endif + } else { + gq_pack_classic_stop(dev, &frame, false); + } + (void)gq_send_frame(dev, &frame, "gq-disable"); + motor_link_request_disable(&data->common.link); + break; + case SET_ZERO: + if (cfg->enable_canfd) { +#ifdef CONFIG_CAN_FD_MODE + gq_pack_fd_zero(dev, &frame); +#else + return; +#endif + } else { + gq_pack_classic_zero(dev, &frame); + } + (void)gq_send_frame(dev, &frame, "gq-set-zero"); + break; + case CLEAR_ERROR: + if (cfg->enable_canfd) { +#ifdef CONFIG_CAN_FD_MODE + gq_pack_fd_stop(dev, &frame, true); +#else + return; +#endif + } else { + gq_pack_classic_stop(dev, &frame, true); + } + (void)gq_send_frame(dev, &frame, "gq-brake"); + break; + case CLEAR_CONTROLLER: + data->kp = 0.0f; + data->kd = 0.0f; + break; + default: + motor_stats_inc(MOTOR_STAT_UNSUPPORTED_CMD); + break; + } +} + +int gq_set(const struct device *dev, motor_setpoint_t *setpoint) +{ + const struct gq_motor_config *cfg = dev->config; + struct gq_motor_data *data = dev->data; + motor_controller_info_t controller = {0}; + struct can_frame frame = {0}; + int ret; + + ret = motor_resolve_controller(dev, setpoint, &controller); + if (ret < 0) { + motor_stats_inc(MOTOR_STAT_UNSUPPORTED_MODE); + return ret; + } + if (setpoint->target == MOTOR_TARGET_NONE) { + return 0; + } + + data->common.mode = setpoint->mode; + data->common.target = setpoint->target; + data->common.controller_id = setpoint->controller_id; + data->target_angle = setpoint->angle; + data->target_rpm = setpoint->rpm; + data->target_torque = setpoint->torque; + data->kp = 0.0f; + data->kd = 0.0f; + + if (cfg->common.controllers[setpoint->controller_id].param_count > 0) { + struct motor_controller_params params = {0}; + + if (motor_controller_get_params(&cfg->common.controllers[setpoint->controller_id], 0, + ¶ms) == 0) { + data->kp = params.k_p; + data->kd = params.k_d; + } + } + + if (cfg->enable_canfd) { +#ifdef CONFIG_CAN_FD_MODE + gq_pack_fd_control(dev, &frame); +#else + return -ENOTSUP; +#endif + } else { + gq_pack_classic_control(dev, &frame); + } + + return gq_send_frame(dev, &frame, "gq-control"); +} + +int gq_get(const struct device *dev, motor_status_t *status) +{ + struct gq_motor_data *data = dev->data; + + status->online = data->common.link.online; + status->enabled = data->common.link.requested_enabled; + status->error = data->error; + status->angle = data->common.angle; + status->rpm = data->common.rpm; + status->torque = data->common.torque; + status->temperature = data->common.temperature; + status->sum_angle = data->common.angle; + status->mode = data->common.mode; + status->target = data->common.target; + status->controller_id = data->common.controller_id; + status->speed_limit[0] = data->common.speed_limit[0]; + status->speed_limit[1] = data->common.speed_limit[1]; + status->torque_limit[0] = data->common.torque_limit[0]; + status->torque_limit[1] = data->common.torque_limit[1]; + + return data->common.link.online ? 0 : -ENODEV; +} + +DT_INST_FOREACH_STATUS_OKAY(GQ_MOTOR_INST) diff --git a/drivers/motor/gq/motor_gq.h b/drivers/motor/gq/motor_gq.h new file mode 100644 index 0000000..f3b1a28 --- /dev/null +++ b/drivers/motor/gq/motor_gq.h @@ -0,0 +1,90 @@ +/* + * Copyright (c) 2026 ttwards <12411711@mail.sustech.edu.cn> + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#ifndef MOTOR_GQ_H_ +#define MOTOR_GQ_H_ + +#include +#include + +#include +#include +#include +#include + +#define DT_DRV_COMPAT gq_motor + +#define GQ_CLASSIC_EXT_PREFIX_CONFIG 0x8000U +#define GQ_CLASSIC_EXT_PREFIX_MIT 0x18000U +#define GQ_REPLY_ID_SHIFT 8U +#define GQ_REPLY_ID_MASK 0x1FFFFF00U + +#define GQ_POS_SCALE_INT16 10000.0f +#define GQ_VEL_SCALE_INT16 4000.0f +#define GQ_TORQUE_SCALE_INT16 100.0f +#define GQ_PID_SCALE_INT16 10.0f + +#define GQ_INT16_NAN ((int16_t)0x8000) + +struct gq_motor_data { + struct motor_driver_data common; + struct can_filter filter; + int filter_id; + int error; + + float target_angle; + float target_rpm; + float target_torque; + float kp; + float kd; +}; + +struct gq_motor_config { + struct motor_driver_config common; + bool enable_canfd; +}; + +int gq_init(const struct device *dev); +int gq_set(const struct device *dev, motor_setpoint_t *setpoint); +int gq_get(const struct device *dev, motor_status_t *status); +void gq_control(const struct device *dev, enum motor_cmd cmd); + +static const struct motor_driver_api gq_motor_api = { + .motor_get = gq_get, + .motor_set = gq_set, + .motor_control = gq_control, +}; + +#define GQ_MOTOR_DATA_INST(inst) \ + static struct gq_motor_data gq_motor_data_##inst = { \ + .common = MOTOR_DT_DRIVER_DATA_INST_GET(inst), \ + .filter_id = -1, \ + .error = 0, \ + .target_angle = 0.0f, \ + .target_rpm = 0.0f, \ + .target_torque = 0.0f, \ + .kp = 0.0f, \ + .kd = 0.0f, \ + } + +#define GQ_MOTOR_CONFIG_INST(inst) \ + static const struct gq_motor_config gq_motor_cfg_##inst = { \ + .common = MOTOR_DT_DRIVER_CONFIG_INST_GET(inst), \ + .enable_canfd = DT_PROP(DT_DRV_INST(inst), enable_canfd), \ + } + +#define GQ_MOTOR_DEFINE_INST(inst) \ + BUILD_ASSERT(!DT_PROP(DT_DRV_INST(inst), enable_canfd) || IS_ENABLED(CONFIG_CAN_FD_MODE), \ + "gq,motor enable-canfd requires CONFIG_CAN_FD_MODE=y"); \ + DEVICE_DT_INST_DEFINE(inst, gq_init, NULL, &gq_motor_data_##inst, &gq_motor_cfg_##inst, \ + POST_KERNEL, CONFIG_MOTOR_INIT_PRIORITY, &gq_motor_api) + +#define GQ_MOTOR_INST(inst) \ + GQ_MOTOR_CONFIG_INST(inst); \ + GQ_MOTOR_DATA_INST(inst); \ + GQ_MOTOR_DEFINE_INST(inst); + +#endif /* MOTOR_GQ_H_ */ diff --git a/dts/bindings/motor/gq,motor.yaml b/dts/bindings/motor/gq,motor.yaml new file mode 100644 index 0000000..ec81bdc --- /dev/null +++ b/dts/bindings/motor/gq,motor.yaml @@ -0,0 +1,32 @@ +description: Gaoqing Motor + +compatible: "gq,motor" + +include: [base.yaml] + +properties: + id: + type: int + required: true + tx_id: + type: int + required: false + description: > + CAN transmit ID override. If omitted, the driver uses the motor id for + classic CAN control frames and protocol-defined extended IDs for + configuration or CAN FD frames. + rx_id: + type: int + required: false + description: > + CAN receive ID override. If omitted, the driver expects replies on + motor id shifted left by 8, matching the vendor examples. + can_channel: + type: phandle + required: true + controllers: + type: phandles + required: false + enable-canfd: + type: boolean + description: Use the Gaoqing CAN FD protocol instead of classic CAN 2.0. diff --git a/dts/bindings/vendor-prefixes.txt b/dts/bindings/vendor-prefixes.txt index 12b6ec3..aff9f51 100644 --- a/dts/bindings/vendor-prefixes.txt +++ b/dts/bindings/vendor-prefixes.txt @@ -1,5 +1,6 @@ dji DaJiang Innovation Inc. dm DaMiao Bot Inc. +gq Gaoqing Motor. mi Xiaomi Corp. motor-controller ARES motor controller profiles pid PID Controller From 1ade9614665a0602254ec048f3c0aec860e84fec Mon Sep 17 00:00:00 2001 From: ttwards <12411711@mail.sustech.edu.cn> Date: Wed, 8 Jul 2026 21:35:49 +0800 Subject: [PATCH 6/7] test(motor): add Gaoqing sample coverage --- samples/motor/gq_demo/CMakeLists.txt | 9 +++ samples/motor/gq_demo/boards/dm_mc02.conf | 2 + samples/motor/gq_demo/boards/dm_mc02.overlay | 37 +++++++++++ .../gq_demo/boards/robomaster_board_c.overlay | 31 +++++++++ samples/motor/gq_demo/prj.conf | 16 +++++ samples/motor/gq_demo/sample.yaml | 21 ++++++ samples/motor/gq_demo/src/main.c | 66 +++++++++++++++++++ tests/native_sim/motor_driver_sim/app.overlay | 8 +++ tests/native_sim/motor_driver_sim/prj.conf | 1 + 9 files changed, 191 insertions(+) create mode 100644 samples/motor/gq_demo/CMakeLists.txt create mode 100644 samples/motor/gq_demo/boards/dm_mc02.conf create mode 100644 samples/motor/gq_demo/boards/dm_mc02.overlay create mode 100644 samples/motor/gq_demo/boards/robomaster_board_c.overlay create mode 100644 samples/motor/gq_demo/prj.conf create mode 100644 samples/motor/gq_demo/sample.yaml create mode 100644 samples/motor/gq_demo/src/main.c diff --git a/samples/motor/gq_demo/CMakeLists.txt b/samples/motor/gq_demo/CMakeLists.txt new file mode 100644 index 0000000..c0a5e86 --- /dev/null +++ b/samples/motor/gq_demo/CMakeLists.txt @@ -0,0 +1,9 @@ +# Copyright (c) 2026 ttwards <12411711@mail.sustech.edu.cn> +# SPDX-License-Identifier: Apache-2.0 + +cmake_minimum_required(VERSION 3.20.0) + +find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE}) +project(gq_motor_demo) + +target_sources(app PRIVATE src/main.c) diff --git a/samples/motor/gq_demo/boards/dm_mc02.conf b/samples/motor/gq_demo/boards/dm_mc02.conf new file mode 100644 index 0000000..cfd53ee --- /dev/null +++ b/samples/motor/gq_demo/boards/dm_mc02.conf @@ -0,0 +1,2 @@ +# dm_mc02 uses STM32H7 FDCAN for this sample. +CONFIG_CAN_FD_MODE=y diff --git a/samples/motor/gq_demo/boards/dm_mc02.overlay b/samples/motor/gq_demo/boards/dm_mc02.overlay new file mode 100644 index 0000000..df7d00e --- /dev/null +++ b/samples/motor/gq_demo/boards/dm_mc02.overlay @@ -0,0 +1,37 @@ +/ { + motor { + gq_motor: gq_motor { + compatible = "gq,motor"; + status = "okay"; + id = <1>; + can_channel = <&can1>; + enable-canfd; + controllers = <&gq_mit &gq_speed &gq_position>; + }; + }; + + controllers { + gq_mit: gq_mit { + compatible = "motor-controller,mit"; + k_p = "2"; + k_d = "0.5"; + }; + + gq_speed: gq_speed { + compatible = "motor-controller,vo"; + target = "speed"; + k_p = "0"; + }; + + gq_position: gq_position { + compatible = "motor-controller,pv"; + pos_k_p = "0"; + vel_k_p = "0"; + }; + }; +}; + +&can1 { + bitrate = <1000000>; + bitrate-data = <1000000>; +}; diff --git a/samples/motor/gq_demo/boards/robomaster_board_c.overlay b/samples/motor/gq_demo/boards/robomaster_board_c.overlay new file mode 100644 index 0000000..c854956 --- /dev/null +++ b/samples/motor/gq_demo/boards/robomaster_board_c.overlay @@ -0,0 +1,31 @@ +/ { + motor { + gq_motor: gq_motor { + compatible = "gq,motor"; + status = "okay"; + id = <1>; + can_channel = <&can1>; + controllers = <&gq_mit &gq_speed &gq_position>; + }; + }; + + controllers { + gq_mit: gq_mit { + compatible = "motor-controller,mit"; + k_p = "2"; + k_d = "0.5"; + }; + + gq_speed: gq_speed { + compatible = "motor-controller,vo"; + target = "speed"; + k_p = "0"; + }; + + gq_position: gq_position { + compatible = "motor-controller,pv"; + pos_k_p = "0"; + vel_k_p = "0"; + }; + }; +}; diff --git a/samples/motor/gq_demo/prj.conf b/samples/motor/gq_demo/prj.conf new file mode 100644 index 0000000..44b6c75 --- /dev/null +++ b/samples/motor/gq_demo/prj.conf @@ -0,0 +1,16 @@ +# Gaoqing motor demo common configuration. + +CONFIG_MAIN_THREAD_PRIORITY=7 +CONFIG_SYSTEM_WORKQUEUE_STACK_SIZE=2048 + +CONFIG_CONSOLE=y +CONFIG_UART_CONSOLE=y +CONFIG_LOG=y +CONFIG_LOG_BACKEND_UART=y +CONFIG_CBPRINTF_FP_SUPPORT=y + +CONFIG_MOTOR=y +CONFIG_MOTOR_GQ=y + +CONFIG_CAN=y +CONFIG_CAN_INIT_PRIORITY=80 diff --git a/samples/motor/gq_demo/sample.yaml b/samples/motor/gq_demo/sample.yaml new file mode 100644 index 0000000..38c57c4 --- /dev/null +++ b/samples/motor/gq_demo/sample.yaml @@ -0,0 +1,21 @@ +# Copyright (c) 2026 ttwards <12411711@mail.sustech.edu.cn> +# SPDX-License-Identifier: Apache-2.0 + +sample: + name: Gaoqing motor demo +tests: + sample.motor.gq: + tags: + - motor + - can + platform_allow: + - dm_mc02 + - robomaster_board_c + integration_platforms: + - dm_mc02 + - robomaster_board_c + harness: console + harness_config: + type: one_line + regex: + - "Gaoqing motor demo started" diff --git a/samples/motor/gq_demo/src/main.c b/samples/motor/gq_demo/src/main.c new file mode 100644 index 0000000..3337947 --- /dev/null +++ b/samples/motor/gq_demo/src/main.c @@ -0,0 +1,66 @@ +/* + * Copyright (c) 2026 ttwards <12411711@mail.sustech.edu.cn> + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include + +#include +#include +#include +#include + +LOG_MODULE_REGISTER(gq_motor_demo, LOG_LEVEL_INF); + +#define GQ_MOTOR_NODE DT_PATH(motor, gq_motor) + +BUILD_ASSERT(DT_NODE_HAS_STATUS(GQ_MOTOR_NODE, okay), "gq_motor node must be enabled"); + +static const struct device *const gq_motor = DEVICE_DT_GET(GQ_MOTOR_NODE); + +static void log_motor_status(void) +{ + motor_status_t status = {0}; + int ret = motor_get(gq_motor, &status); + + if (ret < 0) { + LOG_INF("status unavailable: %d", ret); + return; + } + + LOG_INF("status: angle %.1f deg, speed %.1f rpm, torque %.2f Nm, error %d", + (double)status.angle, (double)status.rpm, (double)status.torque, status.error); +} + +int main(void) +{ + LOG_INF("Gaoqing motor demo started"); + + if (!device_is_ready(gq_motor)) { + LOG_ERR("Gaoqing motor device is not ready"); + return -ENODEV; + } + + motor_control(gq_motor, ENABLE_MOTOR); + k_msleep(200); + + while (true) { + LOG_INF("command: speed 60 rpm"); + motor_set_speed(gq_motor, 60.0f); + k_msleep(1000); + log_motor_status(); + + LOG_INF("command: position 90 deg"); + motor_set_angle(gq_motor, 90.0f); + k_msleep(1000); + log_motor_status(); + + LOG_INF("command: MIT hold near zero"); + motor_set_mit(gq_motor, 0.0f, 0.0f, 0.0f); + k_msleep(1000); + log_motor_status(); + } + + return 0; +} diff --git a/tests/native_sim/motor_driver_sim/app.overlay b/tests/native_sim/motor_driver_sim/app.overlay index 3ba7153..db7eb78 100644 --- a/tests/native_sim/motor_driver_sim/app.overlay +++ b/tests/native_sim/motor_driver_sim/app.overlay @@ -61,6 +61,14 @@ auto_report; }; + gq0: gq0 { + compatible = "gq,motor"; + status = "okay"; + id = <5>; + can_channel = <&fake_can>; + controllers = <&test_mit &test_speed>; + }; + lk0: lk0 { compatible = "lk,motor"; status = "okay"; diff --git a/tests/native_sim/motor_driver_sim/prj.conf b/tests/native_sim/motor_driver_sim/prj.conf index 4f69f44..eee9120 100644 --- a/tests/native_sim/motor_driver_sim/prj.conf +++ b/tests/native_sim/motor_driver_sim/prj.conf @@ -12,6 +12,7 @@ CONFIG_MOTOR_DJI=y CONFIG_MOTOR_DM=y CONFIG_MOTOR_MI=y CONFIG_MOTOR_RS=y +CONFIG_MOTOR_GQ=y CONFIG_MOTOR_LK=y CONFIG_MOTOR_INIT_PRIORITY=90 CONFIG_MOTOR_LOG_LEVEL=0 From 6aa8409a66e27200bde343d0dc96cb130dcf178a Mon Sep 17 00:00:00 2001 From: ttwards <12411711@mail.sustech.edu.cn> Date: Fri, 10 Jul 2026 22:31:44 +0800 Subject: [PATCH 7/7] style(motor): format Gaoqing driver --- drivers/motor/gq/motor_gq.c | 41 ++++++++++++++++--------------------- drivers/motor/gq/motor_gq.h | 30 +++++++++++++-------------- 2 files changed, 33 insertions(+), 38 deletions(-) diff --git a/drivers/motor/gq/motor_gq.c b/drivers/motor/gq/motor_gq.c index 79735f5..27f105a 100644 --- a/drivers/motor/gq/motor_gq.c +++ b/drivers/motor/gq/motor_gq.c @@ -34,8 +34,8 @@ static uint32_t gq_classic_tx_id(const struct gq_motor_config *cfg) static uint32_t gq_reply_id(const struct gq_motor_config *cfg) { - return cfg->common.rx_id != 0 ? (uint32_t)cfg->common.rx_id : - (gq_motor_id(cfg) << GQ_REPLY_ID_SHIFT); + return cfg->common.rx_id != 0 ? (uint32_t)cfg->common.rx_id + : (gq_motor_id(cfg) << GQ_REPLY_ID_SHIFT); } static int16_t gq_clamp_i16(float value) @@ -175,8 +175,7 @@ static void gq_pack_classic_control(const struct device *dev, struct can_frame * frame->data[0] = 0x07; frame->data[1] = 0x35; gq_put_i16(&frame->data[2], vel); - gq_put_i16(&frame->data[4], - data->target_torque == 0.0f ? GQ_INT16_NAN : tqe); + gq_put_i16(&frame->data[4], data->target_torque == 0.0f ? GQ_INT16_NAN : tqe); gq_put_i16(&frame->data[6], pos); break; case VO: @@ -235,10 +234,9 @@ static void gq_pack_fd_control(const struct device *dev, struct can_frame *frame switch (data->common.mode) { case MIT: gq_init_frame(frame, id, 24, true, true); - memcpy(frame->data, - (uint8_t[]){0x01, 0x00, 0x0A, 0x07, 0x20, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x06, 0x2B, 0x00, 0x00, 0x00, - 0x00, 0x14, 0x04, 0x00, 0x11, 0x0F, 0x50, 0x50}, + memcpy(frame->data, (uint8_t[]){0x01, 0x00, 0x0A, 0x07, 0x20, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x06, 0x2B, 0x00, 0x00, 0x00, + 0x00, 0x14, 0x04, 0x00, 0x11, 0x0F, 0x50, 0x50}, 24); gq_put_i16(&frame->data[5], pos); gq_put_i16(&frame->data[7], vel); @@ -249,13 +247,11 @@ static void gq_pack_fd_control(const struct device *dev, struct can_frame *frame case PV: gq_init_frame(frame, id, 20, true, true); memcpy(frame->data, - (uint8_t[]){0x01, 0x00, 0x0A, 0x06, 0x20, 0x00, 0x80, 0x00, - 0x00, 0x06, 0x25, 0x00, 0x00, 0x00, 0x00, 0x14, - 0x04, 0x00, 0x11, 0x0F}, + (uint8_t[]){0x01, 0x00, 0x0A, 0x06, 0x20, 0x00, 0x80, 0x00, 0x00, 0x06, + 0x25, 0x00, 0x00, 0x00, 0x00, 0x14, 0x04, 0x00, 0x11, 0x0F}, 20); gq_put_i16(&frame->data[7], vel); - gq_put_i16(&frame->data[11], - data->target_torque == 0.0f ? GQ_INT16_NAN : tqe); + gq_put_i16(&frame->data[11], data->target_torque == 0.0f ? GQ_INT16_NAN : tqe); gq_put_i16(&frame->data[13], pos); break; case VO: @@ -269,10 +265,9 @@ static void gq_pack_fd_control(const struct device *dev, struct can_frame *frame gq_put_i16(&frame->data[10], tqe); } else { gq_init_frame(frame, id, 20, true, true); - memcpy(frame->data, - (uint8_t[]){0x01, 0x00, 0x0A, 0x06, 0x20, 0x00, 0x80, 0x00, - 0x00, 0x14, 0x04, 0x00, 0x11, 0x0F, 0x50, 0x50, - 0x00, 0x00, 0x00, 0x00}, + memcpy(frame->data, (uint8_t[]){0x01, 0x00, 0x0A, 0x06, 0x20, 0x00, 0x80, + 0x00, 0x00, 0x14, 0x04, 0x00, 0x11, 0x0F, + 0x50, 0x50, 0x00, 0x00, 0x00, 0x00}, 20); gq_put_i16(&frame->data[7], vel); } @@ -283,8 +278,8 @@ static void gq_pack_fd_control(const struct device *dev, struct can_frame *frame } #endif /* CONFIG_CAN_FD_MODE */ -static void gq_parse_state_int16(const struct device *dev, uint8_t mode, uint8_t fault, - int16_t pos, int16_t vel, int16_t tqe) +static void gq_parse_state_int16(const struct device *dev, uint8_t mode, uint8_t fault, int16_t pos, + int16_t vel, int16_t tqe) { struct gq_motor_data *data = dev->data; @@ -420,8 +415,8 @@ int gq_init(const struct device *dev) .mask = GQ_REPLY_ID_MASK, .flags = CAN_FILTER_IDE, }; - data->filter_id = can_add_rx_filter(cfg->common.phy, gq_can_rx_handler, (void *)dev, - &data->filter); + data->filter_id = + can_add_rx_filter(cfg->common.phy, gq_can_rx_handler, (void *)dev, &data->filter); if (data->filter_id < 0) { motor_stats_inc(MOTOR_STAT_CAN_FILTER_ERROR); return data->filter_id; @@ -516,8 +511,8 @@ int gq_set(const struct device *dev, motor_setpoint_t *setpoint) if (cfg->common.controllers[setpoint->controller_id].param_count > 0) { struct motor_controller_params params = {0}; - if (motor_controller_get_params(&cfg->common.controllers[setpoint->controller_id], 0, - ¶ms) == 0) { + if (motor_controller_get_params(&cfg->common.controllers[setpoint->controller_id], + 0, ¶ms) == 0) { data->kp = params.k_p; data->kd = params.k_d; } diff --git a/drivers/motor/gq/motor_gq.h b/drivers/motor/gq/motor_gq.h index f3b1a28..fc4c7b7 100644 --- a/drivers/motor/gq/motor_gq.h +++ b/drivers/motor/gq/motor_gq.h @@ -59,27 +59,27 @@ static const struct motor_driver_api gq_motor_api = { }; #define GQ_MOTOR_DATA_INST(inst) \ - static struct gq_motor_data gq_motor_data_##inst = { \ - .common = MOTOR_DT_DRIVER_DATA_INST_GET(inst), \ - .filter_id = -1, \ - .error = 0, \ - .target_angle = 0.0f, \ - .target_rpm = 0.0f, \ - .target_torque = 0.0f, \ - .kp = 0.0f, \ - .kd = 0.0f, \ + static struct gq_motor_data gq_motor_data_##inst = { \ + .common = MOTOR_DT_DRIVER_DATA_INST_GET(inst), \ + .filter_id = -1, \ + .error = 0, \ + .target_angle = 0.0f, \ + .target_rpm = 0.0f, \ + .target_torque = 0.0f, \ + .kp = 0.0f, \ + .kd = 0.0f, \ } #define GQ_MOTOR_CONFIG_INST(inst) \ - static const struct gq_motor_config gq_motor_cfg_##inst = { \ - .common = MOTOR_DT_DRIVER_CONFIG_INST_GET(inst), \ - .enable_canfd = DT_PROP(DT_DRV_INST(inst), enable_canfd), \ + static const struct gq_motor_config gq_motor_cfg_##inst = { \ + .common = MOTOR_DT_DRIVER_CONFIG_INST_GET(inst), \ + .enable_canfd = DT_PROP(DT_DRV_INST(inst), enable_canfd), \ } #define GQ_MOTOR_DEFINE_INST(inst) \ - BUILD_ASSERT(!DT_PROP(DT_DRV_INST(inst), enable_canfd) || IS_ENABLED(CONFIG_CAN_FD_MODE), \ - "gq,motor enable-canfd requires CONFIG_CAN_FD_MODE=y"); \ - DEVICE_DT_INST_DEFINE(inst, gq_init, NULL, &gq_motor_data_##inst, &gq_motor_cfg_##inst, \ + BUILD_ASSERT(!DT_PROP(DT_DRV_INST(inst), enable_canfd) || IS_ENABLED(CONFIG_CAN_FD_MODE), \ + "gq,motor enable-canfd requires CONFIG_CAN_FD_MODE=y"); \ + DEVICE_DT_INST_DEFINE(inst, gq_init, NULL, &gq_motor_data_##inst, &gq_motor_cfg_##inst, \ POST_KERNEL, CONFIG_MOTOR_INIT_PRIORITY, &gq_motor_api) #define GQ_MOTOR_INST(inst) \