robofish/rm_balance
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

修陀螺移动方向

Browse Source
This commit is contained in:
Robofish 2026-03-15 17:12:53 +08:00
parent 557a41e8b6
commit a8d01e7f74
1 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
User/module/balance_chassis.c
View File

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