四轴飞行器 PID 调优测试代码（可直接嵌入主程序）

四轴飞行器PID调优测试代码（可直接嵌入主程序）一、测试模式定义（新增头文件pid_debug.h）c运行#ifndef__PID_DEBUG_H#define__PID_DEBUG_H#include"common.h"#include"pid.h"//调试模式枚举typedefenum{DEBUG_OFF,//正常飞行模式DEBUG_ROLL_RATE,//Roll速率PID调试DEBUG_PITCH_RATE,//Pitch速率PID调试DEBUG_YAW_RATE,//Yaw速率PID调试DEBUG_ROLL_ANGLE,//Roll姿态PID调试DEBUG_PITCH_ANGLE,//Pitch姿态PID调试DEBUG_YAW_ANGLE//Yaw姿态PID调试}Debug_Mode;externDebug_Modecurrent_mode;externfloattest_setpoint;//调试目标值//函数声明voidPID_Debug_Init(void);voidPID_Debug_Update(void);voidPID_Debug_Set_Mode(Debug_Modemode);voidPID_Debug_Set_Setpoint(floatsetpoint);#endif二、调试模块实现（pid_debug.c）c运行#include"pid_debug.h"#include"attitude.h"#include"motor.h"#include"remote.h"Debug_Modecurrent_mode=DEBUG_OFF;floattest_setpoint=0.0f;uint16_tfixed_throttle=1200;//固定油门值（调试时使用）voidPID_Debug_Init(void){//初始化串口用于调试输出USART_InitTypeDefUSART_InitStruct;GPIO_InitTypeDefGPIO_InitStruct;RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1,ENABLE);RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA,ENABLE);GPIO_InitStruct.GPIO_Pin=GPIO_Pin_9|GPIO_Pin_10;GPIO_InitStruct.GPIO_Mode=GPIO_Mode_AF;GPIO_InitStruct.GPIO_Speed=GPIO_Speed_50MHz;GPIO_InitStruct.GPIO_OType=GPIO_OType_PP;GPIO_InitStruct.GPIO_PuPd=GPIO_PuPd_UP;GPIO_Init(GPIOA,&GPIO_InitStruct);GPIO_PinAFConfig(GPIOA,GPIO_PinSource9,GPIO_AF_USART1);GPIO_PinAFConfig(GPIOA,GPIO_PinSource10,GPIO_AF_USART1);USART_InitStruct.USART_BaudRate=115200;USART_InitStruct.USART_WordLength=USART_WordLength_8b;USART_InitStruct.USART_StopBits=USART_StopBits_1;USART_InitStruct.USART_Parity=USART_Parity_No;USART_InitStruct.USART_HardwareFlowControl=USART_HardwareFlowControl_None;USART_InitStruct.USART_Mode=USART_Mode_Rx|USART_Mode_Tx;USART_Init(USART1,&USART_InitStruct);USART_Cmd(USART1,ENABLE);}voidPID_Debug_Update(void){staticuint32_tcnt=0;cnt++;if(cnt%200==0)//500ms输出一次调试信息{switch(current_mode){caseDEBUG_ROLL_RATE:USART_SendString("RollRate:Set=");USART_SendFloat(test_setpoint);USART_SendString("Actual=");USART_SendFloat(gyro_x_filt);USART_SendString("\r\n");break;caseDEBUG_PITCH_RATE:USART_SendString("PitchRate:Set=");USART_SendFloat(test_setpoint);USART_SendFloat(gyro_y_filt);USART_SendString("Actual=");USART_SendString("\r\n");break;caseDEBUG_YAW_RATE:USART_SendString("YawRate:Set=");USART_SendFloat(test_setpoint);USART_SendString("Actual=");USART_SendFloat(gyro_z_filt);USART_SendString("\r\n");break;caseDEBUG_ROLL_ANGLE:USART_SendString("RollAngle:Set=");USART_SendFloat(test_setpoint);USART_SendString("Actual=");USART_SendFloat(roll);USART_SendString("\r\n");break;caseDEBUG_PITCH_ANGLE:USART_SendString("PitchAngle:Set=");USART_SendFloat(test_setpoint);USART_SendString("Actual=");USART_SendFloat(pitch);USART_SendString("\r\n");break;caseDEBUG_YAW_ANGLE:USART_SendString("YawAngle:Set=");USART_SendFloat(test_setpoint);USART_SendString("Actual=");USART_SendFloat(yaw);USART_SendString("\r\n");break;default:break;}}//调试模式下的PID计算floatroll_out=0,pitch_out=0,yaw_out=0;switch(current_mode){caseDEBUG_ROLL_RATE:roll_out=PID_Calculate(&pid_roll_rate,test_setpoint,gyro_x_filt);break;caseDEBUG_PITCH_RATE:pitch_out=PID_Calculate(&pid_pitch_rate,test_setpoint,gyro_y_filt);break;caseDEBUG_YAW_RATE:yaw_out=PID_Calculate(&pid_yaw_rate,test_setpoint,gyro_z_filt);break;caseDEBUG_ROLL_ANGLE:roll_out=PID_Calculate(&pid_roll,test_setpoint,roll);break;caseDEBUG_PITCH_ANGLE:pitch_out=PID_Calculate(&pid_pitch,test_setpoint,pitch);break;caseDEBUG_YAW_ANGLE:yaw_out=PID_Calculate(&pid_yaw,test_setpoint,yaw);break;caseDEBUG_OFF://正常飞行模式，使用遥控器指令return;}//电机混合输出（固定油门）motor1=fixed_throttle+pitch_out-roll_out+yaw_out;motor2=fixed_throttle-pitch_out-roll_out-yaw_out;motor3=fixed_throttle-pitch_out+roll_out+yaw_out;motor4=fixed_throttle+pitch_out+roll_out-yaw_out;//电机输出限幅motor1=motor1<MIN_THROTTLE?MIN_THROTTLE:(motor1>MAX_THROTTLE?MAX_THROTTLE:motor1);motor2=motor2<MIN_THROTTLE?MIN_THROTTLE:(motor2>MAX_THROTTLE?MAX_THROTTLE:motor2);motor3=motor3<MIN_THROTTLE?MIN_THROTTLE:(motor3>MAX_THROTTLE?MAX_THROTTLE:motor3);motor4=motor4<MIN_THROTTLE?MIN_THROTTLE:(motor4>MAX_THROTTLE?MAX_THROTTLE:motor4);}voidPID_Debug_Set_Mode(Debug_Modemode){current_mode=mode;//切换模式时重置PID积分项pid_egral=0;pid_egral=0;pid_egral=0;pid_roll_egral=0;pid_pitch_egral=0;pid_yaw_egral=0;}voidPID_Debug_Set_Setpoint(floatsetpoint){test_setpoint=setpoint;}//串口发送字符串voidUSART_SendString(char*str){while(*str!='\0'){while(USART_GetFlagStatus(USART1,USART_FLAG_TXE)==RESET);USART_SendData(USART1,*str);str++;}}//串口发送浮点数voidUSART_SendFloat(floatnum){charbuf[20];sprintf(buf,"%.2f",num);USART_SendString(buf);}三、主程序集成（修改main.c）c运行#include"pid_debug.h"intmain(void){System_Init();Delay_ms(100);//初始化调试模块PID_Debug_Init();MPU6050_Init();NRF24L01_Init();PID_Init();Motor_