【導(dǎo)讀】出飛行器實(shí)時(shí)的俯仰角、翻滾角、偏航角。在解算出飛行姿態(tài)角度的基礎(chǔ)上應(yīng)用PID算法?？刂扑男盹w行器進(jìn)行自平衡懸停及相關(guān)的運(yùn)動(dòng)姿態(tài)控制。硬件上，采用STM32F103作為。本文將從硬件、軟件初始化、控制算法及調(diào)試等幾個(gè)篇幅詳細(xì)展。示整個(gè)微型四旋翼飛行器的制作過程。

　　

【正文】 //沒有插 MPU6050 的時(shí)候此函數(shù)一定要屏蔽，否則程序停在此處 NRF_RX_Mode()。 NRF_Check()。 while(1) { debug0()。 } } void Init_MPU6050() { I2C_Write(PWR_MGMT_1, 0x00)。 //解除休眠狀態(tài) I2C_Write(SMPLRT_DIV, 0x07)。 //設(shè)置陀螺儀的采樣頻率 I2C_Write(CONFIG, 0x06)。 //設(shè)置低通濾波頻率 I2C_Write(GYRO_CONFIG, 0x18)。 //設(shè)置陀螺儀的量程為177。 2020 176。 /s I2C_Write(ACCEL_CONFIG, 0x01)。 //設(shè)置加速 度計(jì)的量程為177。 2g } 煙臺(tái)大學(xué)畢業(yè)論文（設(shè)計(jì)） 28 short getdata(uint8_t Addr) { short geted_date=0。 geted_date=I2C_Read(Addr)8。 //讀取高 8 位數(shù)據(jù) geted_date|=I2C_Read(Addr+1)。 //讀取低 8 位數(shù)據(jù)并合并成 16 位數(shù) return geted_date。 // } void NRF_Check(void) { u8 buf[5]={0xC2,0xC2,0xC2,0xC2,0xC2}。 u8 buf1[5]。 u8 i。 /*寫入 5 個(gè)字節(jié)的地址 . */ SPI_NRF_WriteBuf(NRF_WRITE_REG+TX_ADDR,buf,5)。 /*讀出寫入的地址 */ SPI_NRF_ReadBuf(TX_ADDR,buf1,5)。 /*比較 */ for(i=0。i5。i++) { if(buf1[i]!=0xC2) break。 } if(i==5) Uart1Purchar(0x11)。 //MCU 與 NRF 成功連接 else Uart1Purchar(0x22)。 //MCU 與 NRF 不正常連接 } void NRF_Rx_Dat(u8 *rxbuf) { u8 state。 /*讀取 status 寄存器的值 */ state=SPI_NRF_ReadReg(STATUS)。 /*判斷是否接收到數(shù)據(jù) */ if(stateamp。RX_DR) //接收到數(shù)據(jù) { SPI_NRF_ReadBuf(RD_RX_PLOAD,rxbuf,RX_PLOAD_WIDTH)。//讀取數(shù)據(jù) Uart1Purchar(rxbuf[0])。 //把接收到的數(shù)據(jù)通過串口發(fā)送過去 if(rxbuf[0]==0xFE) { START=1。 STOP=0。 UP=0。 DOWN=0。 煙臺(tái)大學(xué)畢業(yè)論文（設(shè)計(jì)） 29 FORWARD=0。 BACK=0。 HOVER=0。 } if(rxbuf[0]==0xFD) { START=0。 STOP=1。 UP=0。 DOWN=0。 FORWARD=0。 BACK=0。 HOVER=0。 } if(rxbuf[0]==0xFB) { START=1。 STOP=0。 UP=1。 DOWN=0。 FORWARD=0。 BACK=0。 HOVER=0。 } if(rxbuf[0]==0xF7) { START=1。 STOP=0。 UP=0。 DOWN=1。 FORWARD=0。 BACK=0。 HOVER=0。 } if(rxbuf[0]==0xEF) { START=1。 STOP=0。 UP=0。 DOWN=0。 FORWARD=1。 BACK=0。 HOVER=0。 } 煙臺(tái)大學(xué)畢業(yè)論文（設(shè)計(jì)） 30 if(rxbuf[0]==0xDF) { START=1。 STOP=0。 UP=0。 DOWN=0。 FORWARD=0。 BACK=1。 HOVER=0。 } if(rxbuf[0]==0xBF) { START=1。 STOP=0。 UP=0。 DOWN=0。 FORWARD=0。 BACK=0。 HOVER=1。 } } SPI_NRF_WriteReg(NRF_WRITE_REG+STATUS,state)。/* 清除中斷標(biāo)志 */ } include define Acce_Revise_Val_X 40 define Gyro_Revise_Val_X 0 //修正值定義 float Air_AcceVal_X。 //修正后的角速度 float Air_GyroVal_X。 //修正后的角加速度 float Aircraft_Angle_X。 float Error_Angle_X[4]={0,0,0}。 float Integration_Error_X=0。 float Set_Angle_X=0。 float Speed_Out_X_L,Speed_Out_X_R,Speed_Out_Y_L,Speed_Out_Y_R。 float Speed_Out_X_L_Old,Speed_Out_X_R_Old,Speed_Out_Y_L_Old,Speed_Out_Y_R_Old。 //Speed_Z_L,Speed_Z_R, define Acce_Revise_Val_Y 40 define Gyro_Revise_Val_Y 0 //修正值定義 float Air_AcceVal_Y。 //修正后的角速度 float Air_GyroVal_Y。 //修正后的角加速度 float Aircraft_Angle_Y。 煙臺(tái)大學(xué)畢業(yè)論文（設(shè)計(jì)） 31 float Error_Angle_Y[4]={0}。 float Set_Angle_Y=0。 float Speed_X=0。 float Speed_Y=0。 define Filter_Method 1 // 濾波方式選擇， 0：卡爾曼濾波 其他：互補(bǔ)濾波 if Filter_Method == 0 //卡爾曼濾波 else //互補(bǔ)濾波 float Aircraft_P_X=。//。 float Aircraft_I_X=。 float Aircraft_D_X=。//。 float Aircraft_P_Y=0。 float Aircraft_I_Y=0。 float Aircraft_D_Y=0。 endif void Angle_calculate(void) { float Value_Register。 if Filter_Method == 0 //卡爾曼濾波 else //互補(bǔ)濾波 Value_Register = Get_ACCE_X*。 //數(shù)值歸一 Air_AcceVal_X =(Value_RegisterAcce_Revise_Val_X)。 Value_Register =Get_GYRO_Y*。 //數(shù)值歸一 Air_GyroVal_X =(Gyro_Revise_Val_XValue_Register)。 Value_Register =(Air_AcceVal_X Aircraft_Angle_X)。 Error_Angle_X[2]=Error_Angle_X[1]。 Error_Angle_X[1]=Error_Angle_X[0]。 Aircraft_Angle_X+=(Air_GyroVal_X+Value_Register)*。//反饋系數(shù) Error_Angle_X[0]=Aircraft_Angle_XSet_Angle_X。 Value_Register =Get_ACCE_Y*。 Air_AcceVal_Y =(Value_RegisterAcce_Revise_Val_Y)。 煙臺(tái)大學(xué)畢業(yè)論文（設(shè)計(jì)） 32 Value_Register =Get_GYRO_X*。 Air_GyroVal_Y =(Value_RegisterGyro_Revise_Val_Y)。 Value_Register = (Air_AcceVal_Y Aircraft_Angle_Y)。 Aircraft_Angle_Y+=(Air_GyroVal_Y+Value_Register)*。//。 endif } void AngleControl(void){ float nP_X,nI_X,nD_X,n_Speed_X,nP_Y,nD_Y,n_Speed_Y。 nP_X=Aircraft_Angle_X*Aircraft_P_X。 //比例 OutData[0]=Aircraft_Angle_X。 Integration_Error_X+=Error_Angle_X[0]。 if(Integration_Error_X1600) Integration_Error_X=1600。 if(Integration_Error_X1600)Integration_Error_X=1600。 nI_X=Integration_Error_X*Aircraft_I_X。 OutData[1]=nI_X 。 nD_X=(Error_Angle_X[0]Error_Angle_X[1]+Air_GyroVal_X)*Aircraft_D_X。 //微分 OutData[2]=nD_X。 n_Speed_X=nP_X+nI_XnD_X。 //角度融合 OutData[3]=n_Speed_X。 if(n_Speed_X0){ Speed_Out_X_L=n_Speed_X。} //矢量輸出 if(n_Speed_X0) {Speed_Out_X_R=n_Speed_X。 } // if(Error_Angle_X[0]==0) Speed_X=n_Speed_X。 nP_Y=Aircraft_Angle_Y*Aircraft_P_Y。 nD_Y=Air_GyroVal_Y*Aircraft_D_Y。 n_Speed_Y=nP_Y+nD_Y。 if(n_Speed_Y0) Speed_Out_Y_L=n_Speed_Y。 if(n_Speed_Y0) Speed_Out_Y_R=n_Speed_Y。 } void Motor_Output(void){ CCR1_Va