diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index 5c45b69..758e3c5 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -820,8 +820,9 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { /* ==================== Yaw轴控制 ==================== */ if (c_cmd->mode == CHASSIS_MODE_BALANCE_ROTOR || c->mode == CHASSIS_MODE_BALANCE_ROTOR) { - /* 小陀螺模式:自动以max_spin_speed恒速旋转 */ - float target_spin = c->param->rotor_ctrl.max_spin_speed; + /* 小陀螺模式:旋转速度在5~8 rad/s之间以0.5Hz正弦变化 */ + float t = (float)BSP_TIME_Get_us() / 1000000.0f; /* 秒 */ + float target_spin = 6.5f + 1.5f * sinf(2.0f * M_PI * 0.5f * t); if (c->rotor_state.exiting) { /* 退出过渡:目标角速度为0,用减速斜坡 */