<<<<<<< HEAD
/*
 * Balance Chassis Module
 */

/* Includes ----------------------------------------------------------------- */
#include "module/balance_chassis.h"
#include "bsp/can.h"
#include "bsp/time.h"
#include "component/user_math.h"
#include "device/motor_lk.h"
#include "module/config.h"
#include <string.h>
#include <math.h>

/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
#define CAN_CMD_ENABLE_ID      121  // 使能命令ID
#define CAN_CMD_JOINT_ID       122  // 关节控制命令ID  
#define CAN_CMD_WHEEL_LEFT_ID  0x128  // 左轮控制命令ID
#define CAN_CMD_WHEEL_RIGHT_ID 0x129  // 右轮控制命令ID

#define CAN_FEEDBACK_JOINT_BASE_ID 124  // 关节反馈基础ID (124-127)
#define CAN_FEEDBACK_WHEEL_LEFT_ID 130  // 左轮反馈ID
#define CAN_FEEDBACK_WHEEL_RIGHT_ID 131 // 右轮反馈ID

/* Private macro ------------------------------------------------------------ */
#define CMD_TIMEOUT_MS 50  // 50ms超时时间，允许控制频率在10-20Hz之间

/* Private variables -------------------------------------------------------- */
static bool joint_command_received = false;
static bool wheel_command_received[2] = {false, false};

// 超时管理 - 防止控制频率不同导致的控制/relax交替
static uint32_t joint_last_cmd_time = 0;
static uint32_t wheel_last_cmd_time[2] = {0, 0};

/* Private function --------------------------------------------------------- */

/**
 * @brief 检查并处理CAN控制命令
 * @param chassis 底盘实例
 * @return 成功返回DEVICE_OK
 */
static int8_t Chassis_ProcessCANCommands(Chassis_t *chassis) {
    BSP_CAN_Message_t rx_msg;
    joint_command_received = false;
    wheel_command_received[0] = false;
    wheel_command_received[1] = false;
  
        // 检查ID 128 - 左轮控制命令 (与电机发送格式一致)
    if (BSP_CAN_GetMessage(BSP_CAN_1, CAN_CMD_WHEEL_LEFT_ID, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
        wheel_command_received[0] = true;
        wheel_last_cmd_time[0] = BSP_TIME_Get_ms(); // 更新左轮命令时间戳
        float left_out = (float)((int16_t)(rx_msg.data[4] | (rx_msg.data[5] << 8))) / 2048.0f;
        chassis->output.wheel[0] = left_out;
        MOTOR_LK_SetOutput(&chassis->param.wheel_param[0], left_out);

    }
    
    // 检查ID 129 - 右轮控制命令 (与电机发送格式一致) 
    if (BSP_CAN_GetMessage(BSP_CAN_1, CAN_CMD_WHEEL_RIGHT_ID, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
        wheel_command_received[1] = true;
        wheel_last_cmd_time[1] = BSP_TIME_Get_ms(); // 更新右轮命令时间戳
        float right_out = (float)((int16_t)(rx_msg.data[4] | (rx_msg.data[5] << 8))) / 2048.0f;
        chassis->output.wheel[1] = right_out;
        MOTOR_LK_SetOutput(&chassis->param.wheel_param[1], right_out);
    }

    BSP_TIME_Delay_ms(1); // 短暂延时，避免总线冲突


    // 检查ID 121 - 使能4个关节电机
    if (BSP_CAN_GetMessage(BSP_CAN_1, CAN_CMD_ENABLE_ID, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
        joint_command_received = true;
        for (int i = 0; i < 4; i++) {
            MOTOR_LZ_Enable(&chassis->param.joint_param[i]);
        }
    }
    
    // 检查ID 122 - 运控模式控制4个关节电机
    if (BSP_CAN_GetMessage(BSP_CAN_1, CAN_CMD_JOINT_ID, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
        joint_command_received = true;
        joint_last_cmd_time = BSP_TIME_Get_ms(); // 更新关节命令时间戳

        
        // 8字节数据分别是4个电机的力矩 (每个电机2字节，有符号整数，精度0.01 Nm)
        for (int i = 0; i < 4; i++) {
            int16_t torque_raw;
            memcpy(&torque_raw, &rx_msg.data[i * 2], sizeof(int16_t));
            float torque = (float)torque_raw / 100.0f; // 转换为浮点数力矩值
            chassis->output.joint[i] = torque;
            // 使用运控模式控制电机，只设置力矩，其他参数为0
            MOTOR_LZ_MotionParam_t motion_param = {
                .target_angle = 0.0f,
                .target_velocity = 0.0f,
                .kp = 0.0f,
                .kd = 0.0f,
                .torque = torque
            };
            MOTOR_LZ_MotionControl(&chassis->param.joint_param[i], &motion_param);
        }
    }

    return DEVICE_OK;
}

/**
 * @brief 发送关节电机反馈数据
 * @param chassis 底盘实例  
 * @return 成功返回DEVICE_OK
 */
static int8_t Chassis_SendJointFeedback(Chassis_t *chassis) {
    // 发送4个关节电机的反馈数据，ID分别为124、125、126、127
    for (int i = 0; i < 4; i++) {
        MOTOR_LZ_t* motor = MOTOR_LZ_GetMotor(&chassis->param.joint_param[i]);
        if (motor != NULL) {
            BSP_CAN_StdDataFrame_t motor_frame = {
                .id = CAN_FEEDBACK_JOINT_BASE_ID + i,  // ID: 124, 125, 126, 127
                .dlc = 8,
                .data = {0}
            };
            
            // 数据重构：转矩电流(2字节) + 位置(3字节) + 速度(3字节) = 8字节
            // 转矩电流 - 转换为16位整数 (精度0.01 Nm)
            int16_t torque_int = (int16_t)(motor->lz_feedback.current_torque * 100);
            memcpy(&motor_frame.data[0], &torque_int, sizeof(int16_t));
            
            // 位置 - 转换为24位整数，使用3字节 (精度0.0001 rad)
            int32_t angle_int = (int32_t)(motor->lz_feedback.current_angle * 10000) & 0xFFFFFF;
            motor_frame.data[2] = (angle_int >> 16) & 0xFF;
            motor_frame.data[3] = (angle_int >> 8) & 0xFF;
            motor_frame.data[4] = angle_int & 0xFF;
            
            // 速度 - 转换为24位整数，使用3字节 (精度0.001 rad/s)
            int32_t velocity_int = (int32_t)(motor->lz_feedback.current_velocity * 1000) & 0xFFFFFF;
            motor_frame.data[5] = (velocity_int >> 16) & 0xFF;
            motor_frame.data[6] = (velocity_int >> 8) & 0xFF;
            motor_frame.data[7] = velocity_int & 0xFF;
            
            BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &motor_frame);
        }
    }
    
    return DEVICE_OK;
}

/**
 * @brief 发送轮子电机反馈数据
 * @param chassis 底盘实例
 * @return 成功返回DEVICE_OK  
 */
static int8_t Chassis_SendWheelFeedback(Chassis_t *chassis) {
    // 发送2个轮子电机的反馈数据，ID分别为130、131
    for (int i = 0; i < 2; i++) {
        MOTOR_LK_t* motor = MOTOR_LK_GetMotor(&chassis->param.wheel_param[i]);
        if (motor != NULL) {
            BSP_CAN_StdDataFrame_t wheel_frame = {
                .id = CAN_FEEDBACK_WHEEL_LEFT_ID + i,  // ID: 130, 131
                .dlc = 8,
                .data = {0}
            };
            
            // 使用LK电机的标准反馈格式
            // 角度(2字节) + 速度(2字节) + 力矩电流(2字节) + 编码器(2字节)
            
            // 角度 - 转换为16位整数 (精度0.01度)
            int16_t angle_int = (int16_t)(motor->motor.feedback.rotor_abs_angle * 180.0f / M_PI * 100);
            wheel_frame.data[0] = (angle_int >> 8) & 0xFF;
            wheel_frame.data[1] = angle_int & 0xFF;
            
            // 速度 - 转换为16位整数 (精度1dps)  
            int16_t speed_int = (int16_t)(motor->motor.feedback.rotor_speed);
            wheel_frame.data[2] = (speed_int >> 8) & 0xFF;
            wheel_frame.data[3] = speed_int & 0xFF;
            
            // 力矩电流 - 转换为16位整数
            int16_t current_int = (int16_t)(motor->motor.feedback.torque_current);
            wheel_frame.data[4] = (current_int >> 8) & 0xFF;
            wheel_frame.data[5] = current_int & 0xFF;
            
            // 编码器值 - 直接使用角度值作为编码器
            uint16_t encoder = (uint16_t)(motor->motor.feedback.rotor_abs_angle / (2 * M_PI) * 65535);
            wheel_frame.data[6] = (encoder >> 8) & 0xFF;
            wheel_frame.data[7] = encoder & 0xFF;
            
            BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &wheel_frame);
        }
    }
    
    return DEVICE_OK;
}

/* Exported functions ------------------------------------------------------- */

/**
 * @brief 底盘初始化
 * @param chassis 底盘实例
 * @param param 底盘参数  
 * @return 成功返回DEVICE_OK
 */
int8_t Chassis_Init(Chassis_t *chassis, Chassis_Param_t *param) {
    if (chassis == NULL || param == NULL) {
        return DEVICE_ERR_NULL;
    }
    
    // 复制参数
    memcpy(&chassis->param, param, sizeof(Chassis_Param_t));
    
    // 初始化CAN
    BSP_CAN_Init();
    
    // 初始化电机驱动
    MOTOR_LZ_Init();
    
    // 注册关节电机
    for (int i = 0; i < 4; i++) {
        if (MOTOR_LZ_Register(&chassis->param.joint_param[i]) != DEVICE_OK) {
            return DEVICE_ERR;
        }
    }
    
    // 注册轮子电机
    for (int i = 0; i < 2; i++) {
        if (MOTOR_LK_Register(&chassis->param.wheel_param[i]) != DEVICE_OK) {
            return DEVICE_ERR;
        }
    }
    
    // 注册CAN接收ID
    BSP_CAN_RegisterId(BSP_CAN_1, CAN_CMD_ENABLE_ID, 0);      // 使能命令
    BSP_CAN_RegisterId(BSP_CAN_1, CAN_CMD_JOINT_ID, 0);       // 关节控制命令
    BSP_CAN_RegisterId(BSP_CAN_1, CAN_CMD_WHEEL_LEFT_ID, 0);  // 左轮控制命令
    BSP_CAN_RegisterId(BSP_CAN_1, CAN_CMD_WHEEL_RIGHT_ID, 0); // 右轮控制命令
    
    return DEVICE_OK;
}

/**
 * @brief 更新底盘数据
 * @param chassis 底盘实例
 * @return 成功返回DEVICE_OK
 */
int8_t Chassis_Update(Chassis_t *chassis) {
    if (chassis == NULL) {
        return DEVICE_ERR_NULL;
    }
    
    // 更新所有电机数据
    MOTOR_LZ_UpdateAll();
    MOTOR_LK_UpdateAll();
    
    // 更新反馈数据
    for (int i = 0; i < 4; i++) {
        MOTOR_LZ_t* joint_motor = MOTOR_LZ_GetMotor(&chassis->param.joint_param[i]);
        if (joint_motor != NULL) {
            chassis->data.joint[i] = joint_motor->motor.feedback;
        }
    }
    
    for (int i = 0; i < 2; i++) {
        MOTOR_LK_t* wheel_motor = MOTOR_LK_GetMotor(&chassis->param.wheel_param[i]);
        if (wheel_motor != NULL) {
            chassis->data.wheel[i] = wheel_motor->motor.feedback;
        }
    }
    
    return DEVICE_OK;
}

/**
 * @brief 使能底盘
 * @param chassis 底盘实例
 * @return 成功返回DEVICE_OK
 */
int8_t Chassis_Enable(Chassis_t *chassis) {
    if (chassis == NULL) {
        return DEVICE_ERR_NULL;
    }
    
    // 使能关节电机
    for (int i = 0; i < 4; i++) {
        MOTOR_LZ_Enable(&chassis->param.joint_param[i]);
    }
    
    // 开启轮子电机
    for (int i = 0; i < 2; i++) {
        MOTOR_LK_MotorOn(&chassis->param.wheel_param[i]);
    }
    
    return DEVICE_OK;
}

/**
 * @brief 底盘松弛
 * @param chassis 底盘实例
 * @return 成功返回DEVICE_OK
 */
int8_t Chassis_Relax(Chassis_t *chassis) {
    if (chassis == NULL) {
        return DEVICE_ERR_NULL;
    }
    
    // 关节电机松弛
    for (int i = 0; i < 4; i++) {
        MOTOR_LZ_Relax(&chassis->param.joint_param[i]);
    }
    
    // 轮子电机松弛
    for (int i = 0; i < 2; i++) {
        MOTOR_LK_Relax(&chassis->param.wheel_param[i]);
    }
    
    return DEVICE_OK;
}

/**
 * @brief 底盘控制处理
 * @param chassis 底盘实例
 * @return 成功返回DEVICE_OK
 */
int8_t Chassis_Ctrl(Chassis_t *chassis) {
    if (chassis == NULL) {
        return DEVICE_ERR_NULL;
    }
    
    // 处理CAN控制命令
    Chassis_ProcessCANCommands(chassis);
    
    uint32_t current_time = BSP_TIME_Get_ms();
    
    // 关节电机超时检查 - 只有在超时时才执行relax
    if (!joint_command_received && 
        (current_time - joint_last_cmd_time) > CMD_TIMEOUT_MS) {
        for (int i = 0; i < 4; i++) {
            MOTOR_LZ_Relax(&chassis->param.joint_param[i]);

            chassis->output.joint[i] = 0.0f; // 松弛时输出力矩设为0
        }
    }
    
    // 轮子电机超时检查 - 只有在超时时才执行relax
    for (int i = 0; i < 2; i++) {
        if (!wheel_command_received[i] && 
            (current_time - wheel_last_cmd_time[i]) > CMD_TIMEOUT_MS) {
            MOTOR_LK_Relax(&chassis->param.wheel_param[i]);

            chassis->output.wheel[i] = 0.0f; // 松弛时输出设为0
        }
    }
    
    // 发送反馈数据
    Chassis_SendJointFeedback(chassis);
    Chassis_SendWheelFeedback(chassis);
    
    return DEVICE_OK;
=======
#include "module/balance_chassis.h"
#include "bsp/can.h"
#include "bsp/time.h"
#include "component/kinematics.h"
#include "component/limiter.h"
#include "component/user_math.h"
#include "device/motor_lk.h"
#include "device/motor_lz.h"
#include <math.h>
#include <stdint.h>
#include <string.h>

/**
 * @brief 使能所有电机
 * @param c 底盘结构体指针
 * @return
 */
static int8_t Chassis_MotorEnable(Chassis_t *c) {
  if (c == NULL)
    return CHASSIS_ERR_NULL;
  for (int i = 0; i < 4; i++) {
    MOTOR_LZ_Enable(&c->param->joint_motors[i]);
  }
  BSP_TIME_Delay_us(200);
  for (int i = 0; i < 2; i++) {
    MOTOR_LK_MotorOn(&c->param->wheel_motors[i]);
  }
  return CHASSIS_OK;
}

/**
 * @brief 关闭所有电机
 * @param c 底盘结构体指针
 * @return
 */
static int8_t Chassis_MotorRelax(Chassis_t *c) {
  if (c == NULL)
    return CHASSIS_ERR_NULL;
  for (int i = 0; i < 2; i++) {
    MOTOR_LK_Relax(&c->param->wheel_motors[i]);
  }
  BSP_TIME_Delay_us(200);
  for (int i = 0; i < 4; i++) {
    MOTOR_LZ_Relax(&c->param->joint_motors[i]);
  }
  return CHASSIS_OK;
}

static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) {
  if (c == NULL)
    return CHASSIS_ERR_NULL; /* 主结构体不能为空 */
  if (mode == c->mode)
    return CHASSIS_OK; /* 模式未改变直接返回 */
  Chassis_MotorEnable(c);

  PID_Reset(&c->pid.leg_length[0]);
  PID_Reset(&c->pid.leg_length[1]);
  PID_Reset(&c->pid.yaw);
  PID_Reset(&c->pid.roll);
  PID_Reset(&c->pid.tp[0]);
  PID_Reset(&c->pid.tp[1]);

  c->yaw_control.target_yaw = c->feedback.imu.euler.yaw;

  // 清空位移
  c->chassis_state.position_x = 0.0f;
  c->chassis_state.velocity_x = 0.0f;
  c->chassis_state.last_velocity_x = 0.0f;
  c->chassis_state.target_x = 0.0f;

  LQR_Reset(&c->lqr[0]);
  LQR_Reset(&c->lqr[1]);
  c->mode = mode;
  c->state = 0; // 重置状态，确保每次切换模式时都重新初始化

  return CHASSIS_OK;
}

/* 更新机体状态估计 */
static void Chassis_UpdateChassisState(Chassis_t *c) {
  if (c == NULL)
    return;

  // 从轮子编码器估计机体速度 (参考C++代码)
  float left_wheel_speed_dps = c->feedback.wheel[0].rotor_speed; // dps (度每秒)
  float right_wheel_speed_dps =
      c->feedback.wheel[1].rotor_speed; // dps (度每秒)

  // 将dps转换为rad/s
  float left_wheel_speed = left_wheel_speed_dps * M_PI / 180.0f;   // rad/s
  float right_wheel_speed = right_wheel_speed_dps * M_PI / 180.0f; // rad/s

  float wheel_radius = 0.072f;

  float left_wheel_linear_vel = left_wheel_speed * wheel_radius;
  float right_wheel_linear_vel = right_wheel_speed * wheel_radius;

  // 机体x方向速度 (轮子中心速度)
  c->chassis_state.last_velocity_x = c->chassis_state.velocity_x;
  c->chassis_state.velocity_x =
      (left_wheel_linear_vel + right_wheel_linear_vel) / 2.0f;

  // 积分得到位置
  c->chassis_state.position_x += c->chassis_state.velocity_x * c->dt;
}

int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) {
  if (c == NULL || param == NULL || target_freq <= 0.0f) {
    return -1; // 参数错误
  }
  c->param = param;

  /*初始化can*/
  BSP_CAN_Init();

  /*初始化和注册所有电机*/
  MOTOR_LZ_Init();

  for (int i = 0; i < 4; i++) {
    MOTOR_LZ_Register(&c->param->joint_motors[i]);
  }
  for (int i = 0; i < 2; i++) {
    MOTOR_LK_Register(&c->param->wheel_motors[i]);
  }
  MOTOR_DM_Register(&c->param->yaw_motor);

  // Chassis_MotorEnable(c);

  /*初始化VMC控制器*/
  VMC_Init(&c->vmc_[0], &param->vmc_param[0], target_freq);
  VMC_Init(&c->vmc_[1], &param->vmc_param[1], target_freq);

  /*初始化pid*/
  PID_Init(&c->pid.leg_length[0], KPID_MODE_CALC_D, target_freq,
           &param->leg_length);
  PID_Init(&c->pid.leg_length[1], KPID_MODE_CALC_D, target_freq,
           &param->leg_length);
  PID_Init(&c->pid.yaw, KPID_MODE_CALC_D, target_freq, &param->yaw);
  PID_Init(&c->pid.roll, KPID_MODE_CALC_D, target_freq, &param->roll);
  PID_Init(&c->pid.tp[0], KPID_MODE_CALC_D, target_freq, &param->tp);
  PID_Init(&c->pid.tp[1], KPID_MODE_CALC_D, target_freq, &param->tp);

  /*初始化LQR控制器*/
  LQR_Init(&c->lqr[0], &param->lqr_gains);
  LQR_Init(&c->lqr[1], &param->lqr_gains);

  /*初始化机体状态*/
  c->chassis_state.position_x = 0.0f;
  c->chassis_state.velocity_x = 0.0f;
  c->chassis_state.last_velocity_x = 0.0f;
  c->chassis_state.target_x = 0.0f;

  /*初始化yaw控制状态*/
  c->yaw_control.target_yaw = 0.0f;
  c->yaw_control.current_yaw = 0.0f;
  c->yaw_control.yaw_force = 0.0f;

  return CHASSIS_OK;
}

int8_t Chassis_UpdateFeedback(Chassis_t *c) {
  if (c == NULL) {
    return -1; // 参数错误
  }
  /*更新电机反馈*/
  for (int i = 0; i < 4; i++) {
    MOTOR_LZ_Update(&c->param->joint_motors[i]);
  }
  MOTOR_LK_Update(&c->param->wheel_motors[0]);
  MOTOR_LK_Update(&c->param->wheel_motors[1]);

  // 更新关节电机反馈并检查离线状态
  for (int i = 0; i < 4; i++) {
    MOTOR_LZ_t *joint_motor = MOTOR_LZ_GetMotor(&c->param->joint_motors[i]);
    if (joint_motor != NULL) {
      if (joint_motor->motor.feedback.rotor_abs_angle > M_PI) {
        joint_motor->motor.feedback.rotor_abs_angle -= M_2PI;
      }
      joint_motor->motor.feedback.rotor_abs_angle =
          -joint_motor->motor.feedback.rotor_abs_angle; // 机械零点调整

      c->feedback.joint[i] = joint_motor->motor.feedback;
    }
  }

  // 更新轮子电机反馈并检查离线状态
  for (int i = 0; i < 2; i++) {
    MOTOR_LK_t *wheel_motor = MOTOR_LK_GetMotor(&c->param->wheel_motors[i]);
    if (wheel_motor != NULL) {
      c->feedback.wheel[i] = wheel_motor->motor.feedback;
    }
  }

  /* 更新云台电机反馈数据（yaw轴） */
  MOTOR_DM_Update(&(c->param->yaw_motor));
  MOTOR_DM_t *dm_motor = MOTOR_DM_GetMotor(&(c->param->yaw_motor));
  if (dm_motor != NULL) {
    c->feedback.yaw = dm_motor->motor.feedback;
  }

  // 更新机体状态估计
  Chassis_UpdateChassisState(c);

  return 0;
}

int8_t Chassis_UpdateIMU(Chassis_t *c, const Chassis_IMU_t imu) {
  if (c == NULL) {
    return -1; // 参数错误
  }
  c->feedback.imu = imu;
  return 0;
}

int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
  if (c == NULL || c_cmd == NULL) {
    return CHASSIS_ERR_NULL; // 参数错误
  }
  c->dt = (BSP_TIME_Get_us() - c->lask_wakeup) /
          1000000.0f; /* 计算两次调用的时间间隔，单位秒 */
  c->lask_wakeup = BSP_TIME_Get_us();

  /*设置底盘模式*/
  if (Chassis_SetMode(c, c_cmd->mode) != CHASSIS_OK) {
    return CHASSIS_ERR_MODE; // 设置模式失败
  }

  // 更新VMC正解算用于状态估计
  VMC_ForwardSolve(&c->vmc_[0], c->feedback.joint[0].rotor_abs_angle,
                   c->feedback.joint[1].rotor_abs_angle,
                   c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
  VMC_ForwardSolve(&c->vmc_[1], c->feedback.joint[3].rotor_abs_angle,
                   c->feedback.joint[2].rotor_abs_angle,
                   c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);

  /*根据底盘模式执行不同的控制逻辑*/
  switch (c->mode) {
  case CHASSIS_MODE_RELAX:
    // 放松模式，电机不输出
    Chassis_MotorRelax(c);

    Chassis_LQRControl(c, c_cmd);

    break;

  case CHASSIS_MODE_RECOVER: {
    float fn;
    fn = -25.0f;

    VMC_InverseSolve(&c->vmc_[0], fn, 0.0f);
    VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0].torque,
                        &c->output.joint[1].torque);
    VMC_InverseSolve(&c->vmc_[1], fn, 0.0f);
    VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3].torque,
                        &c->output.joint[2].torque);
    // Chassis_MotorEnable(c);
    c->output.wheel[0] = 0.0f;
    c->output.wheel[1] = 0.0f;
    Chassis_Output(c); // 统一输出
  } break;

  case CHASSIS_MODE_WHELL_LEG_BALANCE:
    // 轮腿平衡模式，使用LQR控制和PID腿长控制
    // 执行LQR控制（包含PID腿长控制）
    // Chassis_MotorRelax(c);
    Chassis_LQRControl(c, c_cmd);
    Chassis_Output(c); // 统一输出
    break;

  default:
    return CHASSIS_ERR_MODE;
  }

  return CHASSIS_OK;
}

int8_t Chassis_Output(Chassis_t *c) {
  if (c == NULL)
    return CHASSIS_ERR_NULL;
  for (int i = 0; i < 4; i++) {
    MOTOR_LZ_MotionParam_t param = {0};
    param.torque = c->output.joint[i].torque;
    MOTOR_LZ_MotionControl(&c->param->joint_motors[i], &param);
  }
  BSP_TIME_Delay_us(200); // 等待CAN总线空闲，确保前面的命令发送完成
  for (int i = 0; i < 2; i++) {
    MOTOR_LK_SetOutput(&c->param->wheel_motors[i], c->output.wheel[i]);
    // MOTOR_LK_SetOutput(&c->param->wheel_motors[i], 0.0f);
  }
    return CHASSIS_OK;
}

int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
  if (c == NULL || c_cmd == NULL) {
    return CHASSIS_ERR_NULL;
  }

  /* 运动参数（参考C++版本的状态估计） */
  static float xhat = 0.0f, x_dot_hat = 0.0f;
  float target_dot_x = 0.0f;

  // 简化的速度估计（后续可以改进为C++版本的复杂滤波）
  x_dot_hat = c->chassis_state.velocity_x;
  xhat = c->chassis_state.position_x;

  // 目标设定
  target_dot_x = c_cmd->move_vec.vx * 2;
  c->chassis_state.target_x += target_dot_x * c->dt;

  /*更新当前状态*/
  LQR_State_t current_state = {0};
  current_state.theta = c->vmc_[0].leg.theta;     // 左腿摆杆角度
  current_state.d_theta = c->vmc_[0].leg.d_theta; // 左腿摆杆角速度
  current_state.x = xhat;                         // 机体位置
  current_state.d_x = x_dot_hat;                  // 机体速度
  current_state.phi = -c->feedback.imu.euler.pit; // 机体俯仰角
  current_state.d_phi = -c->feedback.imu.gyro.y;  // 机体俯仰角速度
  LQR_UpdateState(&c->lqr[0], &current_state);
  LQR_CalculateGainMatrix(&c->lqr[0], c->vmc_[0].leg.L0);

  current_state.theta = c->vmc_[1].leg.theta;     // 右腿摆杆角度
  current_state.d_theta = c->vmc_[1].leg.d_theta; // 右腿摆杆角速度
  LQR_UpdateState(&c->lqr[1], &current_state);
  LQR_CalculateGainMatrix(&c->lqr[1], c->vmc_[1].leg.L0);

  LQR_State_t target_state = {0};
  target_state.theta = 0.0f;                  // 目标摆杆角度
  target_state.d_theta = 0.0f;                // 目标摆杆角速度
  target_state.phi = -0.2f;                   // 目标俯仰角
  target_state.d_phi = 0.0f;                  // 目标俯仰角速度
  target_state.x = c->chassis_state.target_x; // 目标位置
  target_state.d_x = target_dot_x;            // 目标速度
  LQR_SetTargetState(&c->lqr[0], &target_state);
  LQR_SetTargetState(&c->lqr[1], &target_state);

  LQR_Control(&c->lqr[0]);
  LQR_Control(&c->lqr[1]);

  //   /* 分别计算左右腿的LQR控制 */
  //   float Tw[2] = {0.0f, 0.0f}; // 轮毂力矩
  //   float Tp[2] = {0.0f, 0.0f}; // 髋关节力矩

  //   for (int leg = 0; leg < 2; leg++) {
  //     /* 构建当前状态 */
  //     LQR_State_t current_state = {0};
  //     current_state.theta = leg_theta[leg];
  //     current_state.d_theta = leg_d_theta[leg];
  //     current_state.x = xhat;
  //     current_state.d_x = x_dot_hat;
  //     current_state.phi = -c->feedback.imu.euler.pit;
  //     current_state.d_phi = -c->feedback.imu.gyro.y;

  //     /* 构建目标状态 */
  //     LQR_State_t target_state = {0};
  //     target_state.theta = 0.0f;   // 目标摆杆角度
  //     target_state.d_theta = 0.0f; // 目标摆杆角速度
  //     // target_state.x = 0; // 目标位置
  //     // target_state.d_x = 0.0f; // 目标速度
  //     target_state.phi = -0.2f;  // 目标俯仰角
  //     target_state.d_phi = 0.0f; // 目标俯仰角速度
  //     // target_state.theta = -0.8845f * leg_L0[leg] + 0.40663f; //
  //     目标摆杆角度
  //     // target_state.d_theta = 0.0f; // 目标摆杆角速度
  //     target_state.x = c->chassis_state.target_x; // 目标位置
  //     target_state.d_x = target_dot_x;            // 目标速度
  //     // target_state.phi = 0.16f; // 目标俯仰角
  //     // target_state.d_phi = 0.0f; // 目标俯仰角速度

  //     /* 根据当前腿长更新增益矩阵 */
  //     LQR_CalculateGainMatrix(&c->lqr[leg], leg_L0[leg]);

  //     /* 更新LQR状态 */
  //     LQR_SetTargetState(&c->lqr[leg], &target_state);
  //     LQR_UpdateState(&c->lqr[leg], &current_state);

  //     if (onground_flag[leg]) {
  //       /* 接地状态：使用LQR控制器计算输出 */
  //       if (LQR_Control(&c->lqr[leg]) == 0) {
  //         LQR_Input_t lqr_output = {0};
  //         if (LQR_GetOutput(&c->lqr[leg], &lqr_output) == 0) {
  //           Tw[leg] = lqr_output.T;
  //           Tp[leg] = lqr_output.Tp;
  //           // Tw[leg] = 0.0f; // 暂时屏蔽轮毂力矩输出
  //           // Tp[leg] = -2.5f; // 暂时屏蔽腿部力矩输出
  //         } else {
  //           Tw[leg] = 0.0f;
  //           Tp[leg] = 0.0f;
  //         }
  //       } else {
  //         Tw[leg] = 0.0f;
  //         Tp[leg] = 0.0f;
  //       }
  //     } else {
  //       /* 离地状态：简化控制，只控制腿部摆动 */
  //       Tw[leg] = 0.0f;
  //       // 只控制摆杆角度
  //       float theta_error = current_state.theta - target_state.theta;
  //       float d_theta_error = current_state.d_theta - target_state.d_theta;
  //       Tp[leg] = -(c->lqr[leg].K_matrix[1][0] * theta_error +
  //                   c->lqr[leg].K_matrix[1][1] * d_theta_error);
  //     }
  //   }

  // c->yaw_control.current_yaw = c->feedback.imu.euler.yaw;

  // c->yaw_control.target_yaw -= c_cmd->move_vec.vy * 0.005f; //
  // 目标偏航角，假设遥控器输入范围为[-10, 10]，映射到[-0.02, 0.02] rad/s

  // // 修正目标yaw角度到[-pi, pi]
  // if (c->yaw_control.target_yaw > M_PI) {
  //     c->yaw_control.target_yaw -= M_2PI;
  // } else if (c->yaw_control.target_yaw < -M_PI) {
  //     c->yaw_control.target_yaw += M_2PI;
  // }

  // c->yaw_control.yaw_force = PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw,
  // c->feedback.imu.euler.yaw, c->feedback.imu.gyro.z, c->dt);

  c->yaw_control.current_yaw = c->feedback.yaw.rotor_abs_angle;
  c->yaw_control.target_yaw = c->param->mech_zero_yaw + c_cmd->move_vec.vy;
  c->yaw_control.yaw_force =
      PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw,
               c->feedback.yaw.rotor_abs_angle, 0.0f, c->dt);

  /* 轮毂力矩输出（参考C++版本的减速比） */
  c->output.wheel[0] = c->lqr[0].control_output.T / 4.5f + c->yaw_control.yaw_force;
  c->output.wheel[1] = c->lqr[1].control_output.T / 4.5f - c->yaw_control.yaw_force;

  /* 腿长控制和VMC逆解算（使用PID控制） */

  float target_L0[2];
  float leg_d_length[2]; // 腿长变化率

  /* 横滚角PID补偿计算 */
  float roll_compensation =
      PID_Calc(&c->pid.roll, 0.0f, c->feedback.imu.euler.rol,
               c->feedback.imu.gyro.x, c->dt);

  // 目标腿长设定
  target_L0[0] = 0.15f + c_cmd->height * 0.2f +
                 roll_compensation; // 左腿：基础腿长 + 高度调节 + 横滚角补偿
  target_L0[1] = 0.15f + c_cmd->height * 0.2f -
                 roll_compensation; // 右腿：基础腿长 + 高度调节 - 横滚角补偿

  // 获取腿长变化率
  VMC_GetVirtualLegState(&c->vmc_[0], NULL, NULL, &leg_d_length[0], NULL);
  VMC_GetVirtualLegState(&c->vmc_[1], NULL, NULL, &leg_d_length[1], NULL);

  /* 左右腿摆角相互补偿（参考C++版本的Delta_Tp机制） */
  float Delta_Tp[2];
  // 使用tp_pid进行左右腿摆角同步控制
  // 左腿补偿力矩：使左腿theta向右腿theta靠拢
  Delta_Tp[0] = c->vmc_[0].leg.L0 * 10.0f *
                PID_Calc(&c->pid.tp[0], c->vmc_[1].leg.theta,
                         c->vmc_[0].leg.theta, 0, c->dt);
  // 右腿补偿力矩：使右腿theta向左腿theta靠拢（符号相反）
  Delta_Tp[1] = -Delta_Tp[0];

  float out = PID_Calc(&c->pid.leg_length[0], target_L0[0], c->vmc_[0].leg.L0,
                       leg_d_length[0], c->dt);
  c->vmc_[0].leg.F_virtual = out;
  out = PID_Calc(&c->pid.leg_length[1], target_L0[1], c->vmc_[1].leg.L0,
                 leg_d_length[1], c->dt);
  c->vmc_[1].leg.F_virtual = out;

  c->vmc_[0].leg.T_virtual = c->lqr[0].control_output.Tp + Delta_Tp[0];
  c->vmc_[1].leg.T_virtual = c->lqr[1].control_output.Tp + Delta_Tp[1];

  VMC_InverseSolve(&c->vmc_[0], c->vmc_[0].leg.F_virtual,
                   c->vmc_[0].leg.T_virtual);
  VMC_InverseSolve(&c->vmc_[1], c->vmc_[1].leg.F_virtual,
                   c->vmc_[1].leg.T_virtual);

  VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0].torque,
                      &c->output.joint[1].torque);
  VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3].torque,
                      &c->output.joint[2].torque);

  /* 安全限制 */
  for (int i = 0; i < 2; i++) {
    if (fabsf(c->output.wheel[i]) > 0.8f) {
      c->output.wheel[i] = (c->output.wheel[i] > 0) ? 0.8f : -0.8f;
    }
  }

  for (int i = 0; i < 4; i++) {
    if (fabsf(c->output.joint[i].torque) > 20.0f) {
      c->output.joint[i].torque =
          (c->output.joint[i].torque > 0) ? 20.0f : -20.0f;
    }
  }

  return CHASSIS_OK;
>>>>>>> main
}