diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index ea7b3ef..f35a311 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -767,11 +767,11 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { /* ==================== 速度控制 ==================== */ if (c_cmd->mode == CHASSIS_MODE_BALANCE_ROTOR) { - /* 小陀螺平移:将世界坐标系期望速度投影到机体坐标系 */ - float yaw_angle = c->feedback.imu.euler.yaw; + /* 小陀螺平移:将云台坐标系(用户视角)速度投影到机体坐标系 */ + float gimbal_offset = c->feedback.yaw.rotor_abs_angle - c->param->mech.mech_zero_yaw; float world_vx = c_cmd->move_vec.vx * c->param->rotor_ctrl.max_trans_speed; float world_vy = c_cmd->move_vec.vy * c->param->rotor_ctrl.max_trans_speed; - c->chassis_state.target_velocity_x = world_vx * cosf(yaw_angle) + world_vy * sinf(yaw_angle); + c->chassis_state.target_velocity_x = world_vx * cosf(gimbal_offset) + world_vy * sinf(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 {