/* Includes ----------------------------------------------------------------- */ #include "xm_quadctrl.h" #include "component/pid.h" #include "bsp/pwm.h" #include "bsp/time.h" #include "module/HeightEstimation.h" #include "component/filter.h" // 电机布局: // [0] 前右 (CW) // [1] 前左 (CCW) // [2] 后左 (CW) // [3] 后右 (CCW) //低通滤波初始化和计算 //// 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 //}; /* Private typedef ---------------------------------------------------------- */ /* Private define ----------------------------------------------------------- */ /* Private macro ------------------------------------------------------------ */ /* Private variables -------------------------------------------------------- */ /* Private function -------------------------------------------------------- */ static inline void Quad_Math_LimitFloat(float* value, float min, float max) { if (*value < min) { *value = min; } else if (*value > max) { *value = max; } } static inline void Quad_Math_ScaleSumTo1(float *a, float *b, float *c, float *d) { float sum = *a + *b + *c + *d; if (sum > 1.0f) { float scale = 1.0f / sum; *a *= scale; *b *= scale; *c *= scale; *d *= scale; } } // 初始化卡尔曼滤波器 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; } static bool Quad_UnlockDetection(Quad_t *q, Quad_CMD_t *cmd){ static bool arm_flag=false; float thr = cmd->throttle; float yaw = cmd->yaw; float pit = cmd->pit; float rol = cmd->rol; /* 2. 内八判定 */ bool inside_8 = (thr < 0.1f) && (yaw > 0.9f) && (pit < -0.9f) && (rol < -0.9f); uint32_t now_ms = BSP_TIME_Get_us() / 1000.0f; if (!inside_8) { // 一旦出界，计时清零 q->timer_ms = 0; return arm_flag; } if (q->timer_ms == 0) // 第一次进入 q->timer_ms = now_ms; if ((now_ms - q->timer_ms) >= 1000 && !arm_flag) { arm_flag = true; q->timer_ms = 0; } return arm_flag; } static bool Quad_OutZeroDetection(Quad_CMD_t *cmd) { return (cmd->throttle < 0.05f) && (fabsf(cmd->yaw) < 0.05f) && (fabsf(cmd->pit) < 0.05f) && (fabsf(cmd->rol) < 0.05f); } int8_t Quad_UpdateStatus(Quad_t *q) { // 1. 姿态估计（使用卡尔曼滤波）精度不好 // q->current_status.eulr.rol = Kalman_Update(&q->kalman.rol_kf, // atan2f(q->current_status.accl.y, q->current_status.accl.z), // q->current_status.gyro.x, // q->dt); // // q->current_status.eulr.pit = Kalman_Update(&q->kalman.pit_kf, // atan2f(-q->current_status.accl.x, q->current_status.accl.z), // q->current_status.gyro.y, // q->dt); LowPassFilter2p_Apply(&q->filter.gyro_x,q->current_status.gyro.x); LowPassFilter2p_Apply(&q->filter.gyro_y,q->current_status.gyro.y); LowPassFilter2p_Apply(&q->filter.gyro_z,q->current_status.gyro.z); LowPassFilter2p_Apply(&q->filter.eulr_rol,q->current_status.eulr.rol); LowPassFilter2p_Apply(&q->filter.eulr_pit,q->current_status.eulr.pit); LowPassFilter2p_Apply(&q->filter.alt,q->current_status.alt); LowPassFilter2p_Apply(&q->filter.vel_z,q->current_status.vel_z); return 0; } int8_t Quad_SetTargetStates(Quad_t *q, Quad_CMD_t *cmd) { q->expect_status.vel = cmd->throttle; q->expect_status.yaw_omg = cmd->yaw; q->expect_status.rol_agl = cmd->rol+q->calibration_status.eulr.rol; q->expect_status.pit_agl = cmd->pit+q->calibration_status.eulr.pit; Quad_Math_LimitFloat(&q->expect_status.yaw_omg, -q->param->expect_status_limit.yaw_omg, q->param->expect_status_limit.yaw_omg); Quad_Math_LimitFloat(&q->expect_status.rol_agl, q->expect_status.rol_agl-q->param->expect_status_limit.rol_agl, q->expect_status.rol_agl+q->param->expect_status_limit.rol_agl); Quad_Math_LimitFloat(&q->expect_status.pit_agl, -q->param->expect_status_limit.pit_agl, q->param->expect_status_limit.pit_agl); return 0; } //位置控制 int8_t Quad_PosCtrl(Quad_t *q,float dt) { // q->expect_status.vel=PID_Calc(&q->pid.alt_pos, // q->expect_status.alt, // q->current_status.alt, // 0, // dt); q->expect_status.thrust=PID_Calc(&q->pid.alt_vel, q->expect_status.vel, q->current_status.vel_z, 0, dt); Quad_Math_LimitFloat(&q->expect_status.thrust, -1.0f, 1.0f); q->output.throttle=q->param->baseThrottle +q->expect_status.thrust; return 0; } //姿态控制 int8_t Quad_AttCtrl_agl(Quad_t *q, float dt) { //角度控制 q->expect_status.rol_omg = PID_Calc(&q->pid.rol_agl, q->expect_status.rol_agl, q->current_status.eulr.rol, 0, q->dt); q->expect_status.pit_omg = PID_Calc(&q->pid.pit_agl, q->expect_status.pit_agl, q->current_status.eulr.pit, 0, q->dt); return 0; } int8_t Quad_AttCtrl_omg(Quad_t *q, float dt) { //角速度控制 q->output.pit = PID_Calc(&q->pid.pit_omg, q->expect_status.pit_omg, q->current_status.gyro.x, 0, q->dt); q->output.yaw = PID_Calc(&q->pid.yaw_omg, q->expect_status.yaw_omg, -q->current_status.gyro.z, 0, q->dt); q->output.rol = PID_Calc(&q->pid.rol_omg, q->expect_status.rol_omg, -q->current_status.gyro.y, 0, q->dt); LowPassFilter2p_Apply(&q->filter.output_pit,q->output.pit); return 0; } int8_t Quad_Mixer(Quad_t *q) { Quad_Math_ScaleSumTo1(&q->output.throttle, &q->output.rol, &q->output.pit, &q->output.yaw); q->output.mix[0] = q->output.throttle - q->output.rol - q->output.pit - q->output.yaw;// 前右 q->output.mix[1] = q->output.throttle + q->output.rol - q->output.pit + q->output.yaw;// 前左 q->output.mix[2] = q->output.throttle + q->output.rol + q->output.pit - q->output.yaw;// 后左 q->output.mix[3] = q->output.throttle - q->output.rol + q->output.pit + q->output.yaw;// 后右 for(int i=0;i<4;i++){ Quad_Math_LimitFloat(&q->output.mix[i], q->param->motor_limit.min_output, q->param->motor_limit.max_output); q->output.motor[i] = 0.275+q->output.mix[i]*0.21; LowPassFilter2p_Apply(&q->filter.motor[i],q->output.motor[i]); } return 0; } int8_t Quad_OutPut(Quad_Output_t *output){ if(output == NULL){ return -1; } BSP_PWM_SetComp(BSP_PWM_MOTOR_OUT1, output->motor[0]); BSP_PWM_SetComp(BSP_PWM_MOTOR_OUT2, output->motor[1]); BSP_PWM_SetComp(BSP_PWM_MOTOR_OUT3, output->motor[2]); BSP_PWM_SetComp(BSP_PWM_MOTOR_OUT4, output->motor[3]); return 0; } int8_t Quad_ResetStatus(Quad_CurrentStatus_t *currentStatus) { return 0; // 返回0表示成功 } int8_t Quad_ResetOutput(Quad_Output_t *output) { for (int i = 0; i < 4; i++) { output->motor[i] = 0.0f; } return 0; // 返回0表示成功 } /* Exported functions ------------------------------------------------------- */ int8_t Quad_Init(Quad_t *q, Quad_Params_t *param, Quad_CMD_t *cmd, float freq) { if(param == NULL) { return -1; // 参数错误 } q->param = param; // 初始化卡尔曼滤波器 Kalman_Init(&q->kalman.rol_kf); Kalman_Init(&q->kalman.pit_kf); uint16_t pos_alt=param->ctrl_freq.pos_alt; uint16_t pos_vel=param->ctrl_freq.pos_vel; uint16_t att_agl=param->ctrl_freq.att_agl; uint16_t att_omg=param->ctrl_freq.att_omg; LowPassFilter2p_Init(&q->filter.accl_x, freq, 30.0f); LowPassFilter2p_Init(&q->filter.accl_y, freq, 30.0f); LowPassFilter2p_Init(&q->filter.accl_z, freq, 30.0f); LowPassFilter2p_Init(&q->filter.gyro_x, freq, 20.0f); LowPassFilter2p_Init(&q->filter.gyro_y, freq, 20.0f); LowPassFilter2p_Init(&q->filter.gyro_z, freq, 20.0f); LowPassFilter2p_Init(&q->filter.eulr_yaw, freq, 20.0f); LowPassFilter2p_Init(&q->filter.eulr_pit, freq, 10.0f); LowPassFilter2p_Init(&q->filter.eulr_rol, freq, 10.0f); LowPassFilter2p_Init(&q->filter.alt, freq, 10.0f); LowPassFilter2p_Init(&q->filter.vel_z, freq, 15.0f); LowPassFilter2p_Init(&q->filter.output_pit, freq, 5.0f); for(int i=0;i<4;i++) LowPassFilter2p_Init(&q->filter.motor[i],freq,30.0f); PID_Init(&q->pid.yaw_agl, KPID_MODE_CALC_D, att_agl, ¶m->pid.yaw_agl); PID_Init(&q->pid.pit_agl, KPID_MODE_CALC_D, att_agl, ¶m->pid.pit_agl); PID_Init(&q->pid.rol_agl, KPID_MODE_CALC_D, att_agl, ¶m->pid.rol_agl); PID_Init(&q->pid.yaw_omg, KPID_MODE_CALC_D, att_omg, ¶m->pid.yaw_omg); PID_Init(&q->pid.pit_omg, KPID_MODE_CALC_D, att_omg, ¶m->pid.pit_omg); PID_Init(&q->pid.rol_omg, KPID_MODE_CALC_D, att_omg, ¶m->pid.rol_omg); PID_Init(&q->pid.alt_pos, KPID_MODE_CALC_D, pos_alt, ¶m->pid.alt_pos); PID_Init(&q->pid.alt_vel, KPID_MODE_CALC_D, pos_vel, ¶m->pid.alt_vel); q->expect_status.alt = q->current_status.alt; q->expect_status.pit_agl = q->current_status.eulr.pit; q->expect_status.rol_agl = q->current_status.eulr.rol; q->expect_status.yaw_agl = q->current_status.eulr.yaw; q->calibration_status.eulr.rol = q->current_status.eulr.rol; q->calibration_status.eulr.pit = q->current_status.eulr.pit; q->calibration_status.eulr.yaw = q->current_status.eulr.yaw; BSP_PWM_Start(BSP_PWM_MOTOR_OUT1); BSP_PWM_Start(BSP_PWM_MOTOR_OUT2); BSP_PWM_Start(BSP_PWM_MOTOR_OUT3); BSP_PWM_Start(BSP_PWM_MOTOR_OUT4); return 0; } uint16_t dt; int8_t Quad_Ctrl(Quad_t *q, Quad_CMD_t *cmd) { if(q == NULL) { return -1; // 参数错误 } q->now =BSP_TIME_Get_us() / 1000000.0f; q->dt =(BSP_TIME_Get_us() - q->last) / 1000000.0f; q->last=BSP_TIME_Get_us(); switch(q->fsm_states){ case JOYSTICKDETECTION: BSP_PWM_SetComp(BSP_PWM_MOTOR_OUT1, 0.275+0.21*cmd->throttle); BSP_PWM_SetComp(BSP_PWM_MOTOR_OUT2, 0.275+0.21*cmd->throttle); BSP_PWM_SetComp(BSP_PWM_MOTOR_OUT3, 0.275+0.21*cmd->throttle); BSP_PWM_SetComp(BSP_PWM_MOTOR_OUT4, 0.275+0.21*cmd->throttle); if(!Quad_UnlockDetection(q,cmd)){ Quad_ResetOutput(&q->output); return -1; } q->fsm_states=UNLOCKMOTOR; q->timer_ms=q->now; case UNLOCKMOTOR: BSP_PWM_SetComp(BSP_PWM_MOTOR_OUT1, 0.275); BSP_PWM_SetComp(BSP_PWM_MOTOR_OUT2, 0.275); BSP_PWM_SetComp(BSP_PWM_MOTOR_OUT3, 0.275); BSP_PWM_SetComp(BSP_PWM_MOTOR_OUT4, 0.275); dt=1000*q->now-q->timer_ms; if(dt>2000){ q->fsm_states=RUNNING; q->timer_ms=1000*q->now; } break; case RUNNING: Quad_UpdateStatus(q); Quad_SetTargetStates(q, cmd); static uint8_t cnt_1ms = 0; cnt_1ms = (cnt_1ms + 1) % 10; /* 位置环 100 Hz */ if (cnt_1ms == 0) { Quad_PosCtrl(q,10*q->dt); } /* 角度环 250 Hz */ if (cnt_1ms % 4 == 0) { Quad_AttCtrl_agl(q,4*q->dt); } /* 角速度环 1000 Hz */ Quad_AttCtrl_omg(q,q->dt); /* 混控 & 输出（1000 Hz） */ Quad_Mixer(q); if(cmd->lockoutput){ for(int i=0;i<4;i++){ q->output.motor[i]=0; } }else if(cmd->stop){ Quad_stop(); } Quad_OutPut(&q->output); return 0; break; } } void Quad_stop() { 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); }