From 0f4dfbd61d8920f98179670d752a775277338f8b Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Tue, 17 Mar 2026 02:44:50 +0800 Subject: [PATCH] =?UTF-8?q?fix=E4=BD=8D=E7=A7=BB=E6=8E=A7=E5=88=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/module/balance_chassis.c | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index 090702a..07cbcc1 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -1041,10 +1041,16 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { float world_vy = c_cmd->move_vec.vy * c->param->rotor_ctrl.max_trans_speed; c->chassis_state.target_velocity_x = -world_vx * sinf(gimbal_offset) + world_vy * cosf(gimbal_offset); c->chassis_state.last_target_velocity_x = c->chassis_state.target_velocity_x; - c->chassis_state.target_x += c->chassis_state.target_velocity_x * c->dt; } else { c->chassis_state.target_velocity_x = c_cmd->move_vec.vx * 2.0f; c->chassis_state.last_target_velocity_x = c->chassis_state.target_velocity_x; + } + + /* 有速度指令时：纯速度控制，目标位移跟随当前位移（消除位移误差） + * 无速度指令时：位移+速度双环控制，积分保持位置锁定 */ + if (fabsf(c->chassis_state.target_velocity_x) > 0.01f) { + c->chassis_state.target_x = c->chassis_state.position_x; + } else { c->chassis_state.target_x += c->chassis_state.target_velocity_x * c->dt; }