diff --git a/User/bsp/can.c b/User/bsp/can.c index e094ea1..e401cdd 100644 --- a/User/bsp/can.c +++ b/User/bsp/can.c @@ -45,9 +45,7 @@ static osMessageQueueId_t BSP_CAN_FindQueue(BSP_CAN_t can, uint32_t can_id); static int8_t BSP_CAN_CreateIdQueue(BSP_CAN_t can, uint32_t can_id, uint8_t queue_size); static void BSP_CAN_RxFifo0Callback(void); static void BSP_CAN_RxFifo1Callback(void); -static void BSP_CAN_TxCompleteCallback_Internal(BSP_CAN_t can); -static void BSP_CAN_1_TxCompleteCallback(void); -static void BSP_CAN_2_TxCompleteCallback(void); +static void BSP_CAN_TxCompleteCallback(void); static BSP_CAN_FrameType_t BSP_CAN_GetFrameType(CAN_RxHeaderTypeDef *header); static uint32_t BSP_CAN_DefaultIdParser(uint32_t original_id, BSP_CAN_FrameType_t frame_type); static void BSP_CAN_TxQueueInit(BSP_CAN_t can); @@ -214,50 +212,39 @@ static bool BSP_CAN_TxQueueIsEmpty(BSP_CAN_t can) { } /** - * @brief 处理指定CAN实例的发送队列 + * @brief 处理所有CAN实例的发送队列 */ -static void BSP_CAN_TxCompleteCallback_Internal(BSP_CAN_t can) { - CAN_HandleTypeDef *hcan = BSP_CAN_GetHandle(can); - if (hcan == NULL) return; - - BSP_CAN_TxMessage_t msg; - uint32_t mailbox; - - // 尝试发送队列中的消息 - while (!BSP_CAN_TxQueueIsEmpty(can)) { - // 检查是否有空闲邮箱 - if (HAL_CAN_GetTxMailboxesFreeLevel(hcan) == 0) { - break; // 没有空闲邮箱，等待下次中断 - } +static void BSP_CAN_TxCompleteCallback(void) { + // 处理所有CAN实例的发送队列 + for (int i = 0; i < BSP_CAN_NUM; i++) { + BSP_CAN_t can = (BSP_CAN_t)i; + CAN_HandleTypeDef *hcan = BSP_CAN_GetHandle(can); + if (hcan == NULL) continue; - // 从队列中取出消息 - if (!BSP_CAN_TxQueuePop(can, &msg)) { - break; - } + BSP_CAN_TxMessage_t msg; + uint32_t mailbox; - // 发送消息 - if (HAL_CAN_AddTxMessage(hcan, &msg.header, msg.data, &mailbox) != HAL_OK) { - // 发送失败，消息已经从队列中移除，直接丢弃 - break; + // 尝试发送队列中的消息 + while (!BSP_CAN_TxQueueIsEmpty(can)) { + // 检查是否有空闲邮箱 + if (HAL_CAN_GetTxMailboxesFreeLevel(hcan) == 0) { + break; // 没有空闲邮箱，等待下次中断 + } + + // 从队列中取出消息 + if (!BSP_CAN_TxQueuePop(can, &msg)) { + break; + } + + // 发送消息 + if (HAL_CAN_AddTxMessage(hcan, &msg.header, msg.data, &mailbox) != HAL_OK) { + // 发送失败，消息已经从队列中移除，直接丢弃 + break; + } } } } -/** - * @brief 1发送完成回调 - */ -static void BSP_CAN_1_TxCompleteCallback(void) { - BSP_CAN_TxCompleteCallback_Internal(BSP_CAN_1); -} - -/** - * @brief 2发送完成回调 - */ -static void BSP_CAN_2_TxCompleteCallback(void) { - BSP_CAN_TxCompleteCallback_Internal(BSP_CAN_2); -} - - /** * @brief FIFO0接收处理函数 @@ -478,9 +465,9 @@ int8_t BSP_CAN_Init(void) { // 自动注册CAN1接收回调函数 BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_RX_FIFO0_MSG_PENDING_CB, BSP_CAN_RxFifo0Callback); - BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_TX_MAILBOX0_CPLT_CB, BSP_CAN_1_TxCompleteCallback); - BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_TX_MAILBOX1_CPLT_CB, BSP_CAN_1_TxCompleteCallback); - BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_TX_MAILBOX2_CPLT_CB, BSP_CAN_1_TxCompleteCallback); + BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_TX_MAILBOX0_CPLT_CB, BSP_CAN_TxCompleteCallback); + BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_TX_MAILBOX1_CPLT_CB, BSP_CAN_TxCompleteCallback); + BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_TX_MAILBOX2_CPLT_CB, BSP_CAN_TxCompleteCallback); // 激活CAN1中断 HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING | @@ -494,9 +481,9 @@ int8_t BSP_CAN_Init(void) { // 自动注册CAN2接收回调函数 BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_RX_FIFO1_MSG_PENDING_CB, BSP_CAN_RxFifo1Callback); - BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_TX_MAILBOX0_CPLT_CB, BSP_CAN_2_TxCompleteCallback); - BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_TX_MAILBOX1_CPLT_CB, BSP_CAN_2_TxCompleteCallback); - BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_TX_MAILBOX2_CPLT_CB, BSP_CAN_2_TxCompleteCallback); + BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_TX_MAILBOX0_CPLT_CB, BSP_CAN_TxCompleteCallback); + BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_TX_MAILBOX1_CPLT_CB, BSP_CAN_TxCompleteCallback); + BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_TX_MAILBOX2_CPLT_CB, BSP_CAN_TxCompleteCallback); // 激活CAN2中断 HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO1_MSG_PENDING | diff --git a/User/component/limiter.h b/User/component/limiter.h index dde71f3..d0aa92a 100644 --- a/User/component/limiter.h +++ b/User/component/limiter.h @@ -61,4 +61,3 @@ float PowerLimit_TargetPower(float power_limit, float power_buffer); */ float HeatLimit_ShootFreq(float heat, float heat_limit, float cooling_rate, float heat_increase, bool is_big); - diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index 97d198e..1e20db8 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -409,9 +409,9 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { LQR_State_t target_state = {0}; target_state.theta = c->param->theta; // 目标摆杆角度 target_state.d_theta = 0.0f; // 目标摆杆角速度 - target_state.phi = 11.9601*pow((0.13f + c_cmd->height * 0.25f),3) + -11.8715*pow((0.13f + c_cmd->height * 0.25f),2) + 3.87083*(0.13f + c_cmd->height * 0.25f) + -0.414154; // 目标俯仰角 + target_state.phi = 11.9601*pow((0.15f + c_cmd->height * 0.25f),3) + -11.8715*pow((0.15f + c_cmd->height * 0.25f),2) + 3.87083*(0.15f + c_cmd->height * 0.25f) + -0.414154; // 目标俯仰角 target_state.d_phi = 0.0f; // 目标俯仰角速度 - target_state.x = c->chassis_state.target_x -2.07411f*(0.13f + c_cmd->height * 0.25f) + 0.41182f; + target_state.x = c->chassis_state.target_x -2.07411f*(0.15f + c_cmd->height * 0.25f) + 0.41182f; target_state.d_x = target_dot_x; // 目标速度 if (c->mode != CHASSIS_MODE_ROTOR){ @@ -434,7 +434,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { LQR_SetTargetState(&c->lqr[0], &target_state); LQR_Control(&c->lqr[0]); } else { - target_state.theta = 0.13f; // 非接触时，腿摆角目标为0.1rad，防止腿完全悬空 + target_state.theta = 0.15f; // 非接触时，腿摆角目标为0.1rad，防止腿完全悬空 LQR_SetTargetState(&c->lqr[0], &target_state); c->lqr[0].control_output.T = 0.0f; c->lqr[0].control_output.Tp = @@ -446,7 +446,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { LQR_SetTargetState(&c->lqr[1], &target_state); LQR_Control(&c->lqr[1]); }else { - target_state.theta = 0.13f; // 非接触时，腿摆角目标为0.1rad，防止腿完全悬空 + target_state.theta = 0.15f; // 非接触时，腿摆角目标为0.1rad，防止腿完全悬空 LQR_SetTargetState(&c->lqr[1], &target_state); c->lqr[1].control_output.T = 0.0f; c->lqr[1].control_output.Tp = @@ -472,20 +472,23 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { c->feedback.imu.gyro.x, c->dt); // 限制补偿力范围，防止过度补偿 - if (roll_compensation_force > 20.0f) - roll_compensation_force = 20.0f; - if (roll_compensation_force < -20.0f) - roll_compensation_force = -20.0f; + // 如果任一腿离地，限制补偿力为20 + if (!c->vmc_[0].leg.is_ground_contact || !c->vmc_[1].leg.is_ground_contact) { + if (roll_compensation_force > 20.0f) + roll_compensation_force = 20.0f; + if (roll_compensation_force < -20.0f) + roll_compensation_force = -20.0f; + } // 目标腿长设定（移除横滚角补偿） switch (c->state) { case 0: // 正常状态，根据高度指令调节腿长 - target_L0[0] = 0.13f + c_cmd->height * 0.22f; // 左腿：基础腿长 + 高度调节 - target_L0[1] = 0.13f + c_cmd->height * 0.22f; // 右腿：基础腿长 + 高度调节 + target_L0[0] = 0.15f + c_cmd->height * 0.22f; // 左腿：基础腿长 + 高度调节 + target_L0[1] = 0.15f + c_cmd->height * 0.22f; // 右腿：基础腿长 + 高度调节 break; case 1: // 准备阶段，腿长收缩 - target_L0[0] = 0.13f; // 左腿：收缩到基础腿长 - target_L0[1] = 0.13f; // 右腿：收缩到基础腿长 + target_L0[0] = 0.15f; // 左腿：收缩到基础腿长 + target_L0[1] = 0.15f; // 右腿：收缩到基础腿长 break; case 2: // 起跳阶段，腿长快速伸展 target_L0[0] = 0.55f; // 左腿：伸展到最大腿长 @@ -496,8 +499,8 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { target_L0[1] = 0.1f; // 右腿：缓冲腿长 break; default: - target_L0[0] = 0.13f + c_cmd->height * 0.22f; - target_L0[1] = 0.13f + c_cmd->height * 0.22f; + target_L0[0] = 0.15f + c_cmd->height * 0.22f; + target_L0[1] = 0.15f + c_cmd->height * 0.22f; break; } diff --git a/User/module/config.c b/User/module/config.c index f4906e6..ef9d1b4 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -179,12 +179,12 @@ Config_RobotParam_t robot_config = { }, .roll={ - .k=20.0f, - .p=5.0f, /* 横滚角比例系数 */ - .i=0.0f, /* 横滚角积分系数 */ - .d=0.2f, /* 横滚角微分系数 */ - .i_limit=0.0f, /* 积分限幅 */ - .out_limit=50.0f, /* 输出限幅，腿长差值限制 */ + .k=30.0f, + .p=20.0f, /* 横滚角比例系数 */ + .i=1.0f, /* 横滚角积分系数 */ + .d=0.5f, /* 横滚角微分系数 */ + .i_limit=20.0f, /* 积分限幅 */ + .out_limit=100.0f, /* 输出限幅，腿长差值限制 */ .d_cutoff_freq=30.0f, /* 微分截止频率 */ .range=-1.0f, /* 不使用循环误差处理 */ },