//#include "xm_quadctrl.h"
//#include "component/pid.h"
//#include "bsp/pwm.h"


//// PID控制器实例
//KPID_t roll_angle_pid, pitch_angle_pid;  // 角度环PID (外环)
//KPID_t roll_rate_pid, pitch_rate_pid;    // 角速度环PID (内环)
//KPID_t yaw_rate_pid;                     // 偏航角速度环PID
//KPID_t altitude_pos_pid;                     // 高度环PID
//KPID_t altitude_vel_pid;                     // 高度环PID

//// PID参数配置
//KPID_Params_t angle_pid_params = {
//    .k = 1.0f,          // 控制器增益
//    .p = 0.8f,          // 比例增益
//    .i = 0.05f,         // 积分增益
//    .d = 0.0f,          // 微分增益 (角度环通常不需要D)
//    .i_limit = 0.3f,     // 积分限幅
//    .out_limit = 1.0f,   // 输出限幅
//    .d_cutoff_freq = 0.0f, // 截止频率(不使用)
//    .range = 0.0f        // 误差范围(不使用)
//};

//KPID_Params_t rate_pid_params = {
//    .k = 1.0f,
//    .p = 0.25f,          // 比例增益
//    .i = 0.05f,         // 积分增益
//    .d = 0.01f,         // 微分增益
//    .i_limit = 0.05f,
//    .out_limit = 0.2f,
//    .d_cutoff_freq = 30.0f, // 微分项低通截止频率(Hz)
//    .range = 0.0f
//};

//KPID_Params_t alt_pos_pid_params = {
//    .k = 0.25f,
//    .p = 1.0f,          // 比例增益
//    .i = 0.05f,          // 积分增益
//    .d = 0.00f,          // 微分增益
//    .i_limit = 0.5f,
//    .out_limit = 3.0f,   // 输出限幅(油门变化量)
//    .d_cutoff_freq = 0.0f, // 截止频率(Hz)
//    .range = 0.0f
//};

//KPID_Params_t alt_vel_pid_params = {
//    .k = 0.08f,
//    .p = 2.2f,          // 比例增益
//    .i = 0.05f,          // 积分增益
//    .d = 0.01f,          // 微分增益
//    .i_limit = 0.05f,
//    .out_limit = 0.2f,   // 输出限幅(油门变化量)
//    .d_cutoff_freq = -1.0f, // 截止频率(Hz)
//    .range = 0.0f
//};

//// 初始化卡尔曼滤波器
//void Kalman_Init(KalmanFilter* kf) {
//    kf->Q_angle = 0.001f;
//    kf->Q_bias = 0.003f;
//    kf->R_measure = 0.03f;
//    kf->angle = 0.0f;
//    kf->bias = 0.0f;
//    kf->P[0][0] = 0.0f;
//    kf->P[0][1] = 0.0f;
//    kf->P[1][0] = 0.0f;
//    kf->P[1][1] = 0.0f;
//}

//// 卡尔曼滤波器更新
//float Kalman_Update(KalmanFilter* kf, float newAngle, float newRate, float dt) {
//    // 预测步骤
//    kf->angle += dt * (newRate - kf->bias);
//    kf->P[0][0] += dt * (dt * kf->P[1][1] - kf->P[0][1] - kf->P[1][0] + kf->Q_angle);
//    kf->P[0][1] -= dt * kf->P[1][1];
//    kf->P[1][0] -= dt * kf->P[1][1];
//    kf->P[1][1] += kf->Q_bias * dt;

//    // 更新步骤
//    float y = newAngle - kf->angle;
//    float S = kf->P[0][0] + kf->R_measure;
//    float K[2];
//    K[0] = kf->P[0][0] / S;
//    K[1] = kf->P[1][0] / S;

//    // 更新状态和协方差
//    kf->angle += K[0] * y;
//    kf->bias += K[1] * y;
//    float P00_temp = kf->P[0][0];
//    float P01_temp = kf->P[0][1];
//    kf->P[0][0] -= K[0] * P00_temp;
//    kf->P[0][1] -= K[0] * P01_temp;
//    kf->P[1][0] -= K[1] * P00_temp;
//    kf->P[1][1] -= K[1] * P01_temp;

//    return kf->angle;
//}

//// 全局变量
//KalmanFilter roll_kf, pitch_kf;

//// 控制器初始化函数
//void FlightController_Init(float sample_freq) {
//    // 初始化卡尔曼滤波器
//    Kalman_Init(&roll_kf);
//    Kalman_Init(&pitch_kf);
//    
//    
//    // 初始化PID控制器
//    PID_Init(&roll_angle_pid, KPID_MODE_NO_D, sample_freq, &angle_pid_params);
//    PID_Init(&pitch_angle_pid, KPID_MODE_NO_D, sample_freq, &angle_pid_params);
//    PID_Init(&roll_rate_pid, KPID_MODE_CALC_D, sample_freq, &rate_pid_params);
//    PID_Init(&pitch_rate_pid, KPID_MODE_CALC_D, sample_freq, &rate_pid_params);
//    PID_Init(&yaw_rate_pid, KPID_MODE_SET_D, sample_freq, &rate_pid_params);
//    PID_Init(&altitude_pos_pid, KPID_MODE_NO_D, sample_freq, &alt_pos_pid_params);
//	PID_Init(&altitude_vel_pid, KPID_MODE_CALC_D, sample_freq, &alt_vel_pid_params);
//    
//    // 重置所有PID状态
//    PID_Reset(&roll_angle_pid);
//    PID_Reset(&pitch_angle_pid);
//    PID_Reset(&roll_rate_pid);
//    PID_Reset(&pitch_rate_pid);
//    PID_Reset(&yaw_rate_pid);
//    PID_Reset(&altitude_pos_pid);
//	PID_Reset(&altitude_vel_pid);
//}

//float target_yaw_rate;
//float expected_speed;
//float expect_thrust;
//float target_roll_rate;
//float target_pitch_rate;
//float base_throttle = 0.4f;    // 基础油门
//float target_altitude = 0.0f;  // 目标高度1米
//float roll_angle;
//float pitch_angle;
//// 主控制循环（在固定时间间隔调用	
//void FlightController_Update(SensorData sensors, Control_t* output, float dt, PS2_TypeDef rc) {
//    // 1. 姿态估计（使用卡尔曼滤波）
//     roll_angle = Kalman_Update(&roll_kf, 
//                                    atan2f(sensors.accel[1], sensors.accel[2]), 
//                                    sensors.gyro[0], 
//                                    dt)+0.0302813556;
//    
//     pitch_angle = Kalman_Update(&pitch_kf, 
//                                     atan2f(-sensors.accel[0], sensors.accel[2]), 
//                                     sensors.gyro[1], 
//                                     dt)+0.0181136467;
//    
//    // 2. 高度估计（融合加速度计和气压计）
//    //AltKalman_Update(&alt_kf, sensors.accel[2], sensors.baro_alt, dt);
//    
//    // 3. 遥控器输入
////    float target_yaw_rate, target_throttle;
//	
//    target_altitude += (rc.Rocker_LY-0.5)*0.03;
//	if(target_altitude<=-13){target_altitude=-13;}else if(target_altitude>=11){target_altitude=11;}
//	
////	expected_speed = 
//	
//	output->Controltarget.pitch = (rc.Rocker_RY-0.5)*0.349065f;
//	output->Controltarget.roll = (rc.Rocker_RX-0.5)*0.1745329f;
//	target_yaw_rate = (rc.Rocker_LX-0.5)*0.349065f;
//	
//    // 4. 高度控制（单环PID）
//	expected_speed = PID_Calc(&altitude_pos_pid, target_altitude, -sensors.baro_alt, 0, dt);
//    expect_thrust = PID_Calc(&altitude_vel_pid, expected_speed, sensors.speed_z, 0, dt);
//    if(expect_thrust<-1.0f){expect_thrust=-1.0f;}
//	output->ControlOutput.throttle = base_throttle+expect_thrust;
//    // 5. 双环姿态控制
//    // 外环：角度控制（输出目标角速度）
////    float target_roll_rate = PID_Calc(&roll_angle_pid, output->Controltarget.roll, roll_angle, 0, dt);
////    float target_pitch_rate = PID_Calc(&pitch_angle_pid, output->Controltarget.pitch, pitch_angle, 0, dt);
//	target_roll_rate = PID_Calc(&roll_angle_pid, output->Controltarget.roll, roll_angle, 0, dt);
//    target_pitch_rate = PID_Calc(&pitch_angle_pid, output->Controltarget.pitch, pitch_angle, 0, dt);
//    
//    // 内环：角速度控制（输出控制量）
//    output->ControlOutput.roll = 2*PID_Calc(&roll_rate_pid, target_roll_rate, sensors.gyro[0], 0, dt);
//    output->ControlOutput.pitch = 2*PID_Calc(&pitch_rate_pid, target_pitch_rate, sensors.gyro[1], 0, dt);
//    output->ControlOutput.yaw = 1.5*PID_Calc(&yaw_rate_pid, target_yaw_rate, sensors.gyro[2], 0, dt);
//    
//    // 6. 输出限幅保护
//    output->ControlOutput.throttle = fmaxf(0.0f, fminf(1.0f, output->ControlOutput.throttle));
//    output->ControlOutput.roll = fmaxf(-1.0f, fminf(1.0f, output->ControlOutput.roll));
//    output->ControlOutput.pitch = fmaxf(-1.0f, fminf(1.0f, output->ControlOutput.pitch));
//    output->ControlOutput.yaw = fmaxf(-1.0f, fminf(1.0f, output->ControlOutput.yaw));
//	
//}

//// 重置所有控制器（在模式切换或紧急情况下调用）
//void FlightController_Reset() {
//    PID_Reset(&roll_angle_pid);
//    PID_Reset(&pitch_angle_pid);
//    PID_Reset(&roll_rate_pid);
//    PID_Reset(&pitch_rate_pid);
//    PID_Reset(&yaw_rate_pid);
//    PID_Reset(&altitude_pos_pid);
//    PID_Reset(&altitude_vel_pid);
//    // 重置卡尔曼滤波器
//    Kalman_Init(&roll_kf);
//    Kalman_Init(&pitch_kf);
//    //AltKalman_Init(&alt_kf);
//}

///*********************************************************** */
///*                           混控器                           */
///*********************************************************** */

//// 全局混控器实例
////static Mixer mixer;
//Mixer mixer;
//// 初始化混控器
//void MotorMixer_Init() {
//    // 电机1配置 (前右)
//    mixer.motors[0].min_output = 0.1f;  
//    mixer.motors[0].max_output = 1.0f;  
//    
//    // 电机2配置 (前左)
//    mixer.motors[1].min_output = 0.1f;
//    mixer.motors[1].max_output = 1.0f;
//    
//    // 电机3配置 (后左)
//    mixer.motors[2].min_output = 0.1f;
//    mixer.motors[2].max_output = 1.0f;
//    
//    // 电机4配置 (后右)
//    mixer.motors[3].min_output = 0.1f;
//    mixer.motors[3].max_output = 1.0f;
//    
//    // 初始输出为0
//    for (int i = 0; i < 4; i++) {
//        mixer.motor_output[i] = 0.0f;
//    }
//}
//float mix[4];
//// 应用电机输出
//void Apply_Motor_Outputs(Control_t ctrl) {
//    // 1. 基本混控公式 (X型四轴布局)
//    // 电机布局:
//    //   [0] 前右 (CW)
//    //   [1] 前左 (CCW)
//    //   [2] 后左 (CW)
//    //   [3] 后右 (CCW)
//    
//    // 混控公式:
////    float mix[4];
//    mix[0] = ctrl.ControlOutput.throttle + ctrl.ControlOutput.roll - ctrl.ControlOutput.pitch + ctrl.ControlOutput.yaw;  // 前右
//    mix[1] = ctrl.ControlOutput.throttle - ctrl.ControlOutput.roll - ctrl.ControlOutput.pitch - ctrl.ControlOutput.yaw;  // 前左
//    mix[2] = ctrl.ControlOutput.throttle - ctrl.ControlOutput.roll + ctrl.ControlOutput.pitch + ctrl.ControlOutput.yaw;  // 后左
//    mix[3] = ctrl.ControlOutput.throttle + ctrl.ControlOutput.roll + ctrl.ControlOutput.pitch - ctrl.ControlOutput.yaw;  // 后右
//    
//    // 2. 输出限幅和约束
//    for (int i = 0; i < 4; i++) {
//        // 应用电机特定限幅
//        if (mix[i] < mixer.motors[i].min_output) {
//            mix[i] = mixer.motors[i].min_output;
//        } else if (mix[i] > mixer.motors[i].max_output) {
//            mix[i] = mixer.motors[i].max_output;
//        }
//        
//        // 更新当前输出值
//        mixer.motor_output[i] = 0.275+0.8*mix[i]*0.21; // 转换为PWM计数值
//    }
//    
//    // 3. 输出到PWM
//    BSP_PWM_Set(BSP_PWM_MOTOR_OUT1, mixer.motor_output[0]);
//    BSP_PWM_Set(BSP_PWM_MOTOR_OUT2, mixer.motor_output[1]); 
//    BSP_PWM_Set(BSP_PWM_MOTOR_OUT3, mixer.motor_output[2]);
//    BSP_PWM_Set(BSP_PWM_MOTOR_OUT4, mixer.motor_output[3]);
//}

//// 安全停止所有电机
//void MotorMixer_OutStop() {
//	BSP_PWM_Stop(BSP_PWM_MOTOR_OUT1);
//	BSP_PWM_Stop(BSP_PWM_MOTOR_OUT2);
//	BSP_PWM_Stop(BSP_PWM_MOTOR_OUT3);
//	BSP_PWM_Stop(BSP_PWM_MOTOR_OUT4);
//}