280 lines
10 KiB
C
280 lines
10 KiB
C
//#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);
|
||
//}
|