定位定點一鍵返航飛控程序注釋

//mainfunctionpath//mainfunctionpath//thispartistest.c,suggestyoureadthere#include"usart.h" //usart1 //led"#includeinv_mpu_dmp_motion_driver.h"http://MPU6050dmpio初始化模擬iic#include"inv_mpu.h"http://MPU6050dmpio初始化模擬iic"#include"HMC.h"http://電子羅盤HMC5983初始化相關(guān)#include"IMU.h"http://姿態(tài)角融合文件#include"ms5611.h"http://氣壓計MS5611#include"usart3.h"http://gps串口數(shù)據(jù)接收#include"control.h"http://控制所在文件 floatang0_car[3];//角度卡爾曼濾波值 ////externdoublevoidtranslateUSART(void上位機發(fā)送函數(shù)voidanalyse(void);//無線串口數(shù)據(jù)接收解析u8usartre[8]={0};//無線串口數(shù)據(jù)接收數(shù)組u8senddata[36]={0};//上位機發(fā)送的數(shù)據(jù) voidtranslate2401(void2401longgps_lati=0,gps_long=0;//記錄起飛點GPS坐標(biāo)吧得出的GPS坐標(biāo)歸零u8flag_gps=0;//GPS是否搜到衛(wèi)星其實可以用gpsx.fixtype=33D//以及向東向北的速度staticintlast_time=0;//staticdoubletime=0;//變量用于測中斷執(zhí)行用了多久時間doublehighford=0;//用于互補濾波算對地速度之用int{Stm32_Clock_Init(336,8,2,7);//STM32F4的初始化復(fù)位向量表之類的 //延時函數(shù)初始化記住延時函數(shù)不能用在中斷里面uart_init(84,115200);//串口1初始化用于無線數(shù)傳usart2_init(42,100000);//串口2初始化用于遙控接收機SBUS接收usart3_init(42,115200);//串口3初始化用于GPS數(shù)據(jù)接收LED_Init();//LED狀態(tài)燈初始化LED=~LED;//狀態(tài)LEDLCD_init();//OLED初始化LCD_clear();//OLED清屏得得PWM_Init/8路PWM4{ capture();//解析SBUSGet_PositionInfo();//GPS}}externdoublePressure;//氣壓externdoublehighforkp,speed_press,high_pressure;//PIDKP{{{//判斷GPS(gpsx.longitude)>30000000立}ang_c[0]=ang0_car[0]+0.5;//去除飛控板安裝誤差 if(ii>5000&&control_flag==1&&ii%20==0)LED=~LED;//解鎖快閃if(ii%5==0)mpu_dmp_get_data(aaa,www0,ang0);//10ms讀取一次DMP值if(ii%11==0)Exchange_Number(1);//據(jù)if(ii%11==5){Exchange_Number(2);}//氣壓計分時采集不要動會影響氣壓計甚至讀不if(ii%11==5)Exchange_Number(3);//據(jù)if(ii%11==10){Exchange_Number(4);//Control();//這個Control();//這個}}{u8res;int{{}}}externdouble{u8u8{kd_h=((int)usartre[3])*4.0;//PIDkdki_h=((int)usartre[4])/10.0;//PIDki}}extern}}externdoublesum2,sum1,Temperature;//100倍的sum1sum2void{int遙控器{{}{}}}void{u16}void{u16u8temp=1,status=0;{East_Speed=gpsx.East_speed/1000.0;}}sys.csys.hdelay.cdelay.h隸屬stm32intfputc(intch,FILE{USART3->DR=(u8)ch;return}intfputc(intch,FILE{USART3->DR=(u8)ch;return}//1初始化PA9TXPA10RX{floattemp;u16fraction; 組}{}{while(*ch!={}}while(*ch!={}} #include"sys.h"http://這里做出更改實例注意區(qū)別//#define {GPIOE->MODER&=~(3<<(7*2));GPIOE->MODER|=0<<7*2;}//PE3式//#define//#define//#definePEout(7) //PE3 式#define#definePEout(3) //發(fā)送IIC//發(fā)送//發(fā)送IIC//發(fā)送IICvoidvoidu8voidIIC_Ack2(void);voidSingle_Write2(unsignedcharSlaveAddress,unsignedcharunsignedcharSingle_Read2(unsignedcharSlaveAddress,unsignedcharREG_Address);#defineFALSE0 #include"delay.h"{ }void{u8for(i=0;i<1;i++)}void{IIC_SCL2=0;//鉗住I2C}void{IIC_SDA2=0;//STOP:whenCLKishighDATAchangeformlowtohighIIC_SDA2=1;//發(fā)送I2C}u8{u8 {{return1;}}return}//產(chǎn)生ACKvoid{}void{}{u8{ }}u8IIC_Read_Byte2(unsignedcharack){unsignedchari,receive=0;for(i=0;i<8;i++){}return}voidSingle_Write2(unsignedcharSlaveAddress,unsignedchar}return}voidSingle_Write2(unsignedcharSlaveAddress,unsignedchar{}{unsignedcharreturn}#include"IIC.h"#include"HMC.h"#include"lcd.h"#include"oled.h"externfloat #include"IIC.h"#include"HMC.h"#include"lcd.h"#include"oled.h"externfloat externfloat externfloatstruct_angle#define {doubleR=MeasureNoise_R;doubleQ=ProcessNiose_Q;staticdoublex_last;doublex_now;doublep_mid;doublep_now;kg=p_mid/(p_mid+R);//kg為kalmanfilter，R為噪聲p_now=(1-kg)*p_mid;//最優(yōu)值對應(yīng)的x_last=x_now;//更新系統(tǒng)狀態(tài)值returnx_now;} x_last=x_now;//更新系統(tǒng)狀態(tài)值returnx_now;} {doubleR=MeasureNoise_R;doubleQ=ProcessNiose_Q;staticdoublex_last;doublex_now;doublep_mid;doublep_now;kg=p_mid/(p_mid+R);//kg為kalmanfilter，R為噪聲p_last=p_now;//更新covariance值returnx_now;} {doubleR=MeasureNoise_R;doubleQ=ProcessNiose_Q;staticdoublex_last;doublex_now;doublep_mid;doublep_now;kg=p_mid/(p_mid+R);//kg為kalmanfilter，R為噪聲p_last=p_now;//更新covariance值p_last=p_now;//更新covariance值returnx_now;}staticdoubleKalmanFilter_XHMC(constdoubleResrcData,double{doubleR=MeasureNoise_R;doubleQ=ProcessNiose_Q;staticdoublex_last;doublex_now;doublep_mid;doublep_now;kg=p_mid/(p_mid+R);//kg為kalmanfilter，R為噪聲p_last=p_now;//更新covariance值returnx_now;}staticdoubleKalmanFilter_YHMC(constdoubleResrcData,double{doubleR=MeasureNoise_R;doubleQ=ProcessNiose_Q;staticdoublex_last;doublex_now;doublep_mid;doublep_now;;//p_last=p_now;//更新covariance值returnx_now;}staticdoubleKalmanFilter_ZHMC(constdoubleResrcData,double{doubleR=MeasureNoise_R;doubleQ=ProcessNiose_Q;staticdoublex_last;doublex_now;doublep_mid;doublep_now;kg=p_mid/(p_mid+R);//kg為kalmanfilter，R為噪聲p_last=p_now;//更新covariance值returnx_now;} {longi;constfloatthreehalfs=x2=number*yy==*(long*)=0x5f3759df-(i>>1=*(float*)1stiteration（第一次牛頓迭代1stiteration（第一次牛頓迭代 =y*(threehalfs-(x2*y*y)return} {if(x<0) elsereturnx;} {floatresult;result=1-x*x/2;return}{floatresult=y-y*y*y/6;returnresult;} floatVariableParameter(floaterror){ result=if(error<{error=-}{error=}result=1-1.28f*error;if(result<0){result=}return}void{sensor.acc.averag.z=//ACC}return}void{sensor.acc.averag.z=//ACC//ACC//ACC}floatqa0,qa1,qa2,#defineKp0.95f#defineKi//proportionalgaingovernsrateofconvergence //#defineKi//integralgaingovernsrateofconvergenceofgyroscope//#definehalfT0.00125f//采樣周期的一半本程序2.5MS采集一次所以halfT*void}floatq0c=1,q1c=0,q2c=0,}floatq0c=1,q1c=0,q2c=0,q3c=//quaternionelementsrepresentingthefloatexInt=0,eyInt=0,ezInt=double//scaledintegralvoidIMUupdate(floatgx,floatgy,floatgz,floatax,floatay,floatu16floatex,ey,//floatq0q0=q0c*q0c;floatq0q1=q0c*q1c;floatq0q2=q0c*q2c; floatq1q1=q1c*q1c; floatq1q3=q1c*q3c;floatq2q2=q2c*q2c;floatq2q3=q2c*q3c;floatq3q3=norm=Q_rsqrt(ax*ax+ay*ay+ax=ax*norm;ay=ay*az=az*估計重力方向和流量///estimateddirectionofgravityandflux(vand遷vx=2*(q1q3-vy=2*(q0q1+vz=q0q0-q1q1-q2q2+q3q3//errorissumofcrossproductbetweenreferencedirectionoffieldsanddirectionbyex=(ay*vz-az*vy)ey(az*vxax*vzez(ax*vyay*vx對誤差exIntey(az*vxax*vzez(ax*vyay*vx對誤差exInt=exInt+VariableParameter(ex)*ex*eyInt=eyInt+VariableParameter(ey)*ey*Ki;ezInt=ezInt+VariableParameter(ez)*ez*//adjustedgyroscopegx=gx+Kp*VariableParameter(ex)*ex+gy=gy+Kp*VariableParameter(ey)*ey+eyInt;gz=gz+Kp*VariableParameter(ez)*ez+ezInt;程q0c=q0c+(-q1c*gx-q2c*gy-q1c=q1c+(q0c*gx+q2c*gz-q3c*gy)*halfT;q2c=q2c+(q0c*gy-q1c*gz+q3c=q3c+(q0c*gz+q1c*gy-//normalisenorm=Q_rsqrt(q0q0+q1q1+q2q2+q3q3);q0c=q0c*norm;q1c=q1c*norm;q2c=q2c*q3c=q3c*qa0=qa1=q1c;qa2=qa3=angle.roll=atan2(2*q2q3+2*q0q1,-2*q1q1-2*q2q2+1);//angle.pitch=asin(-2*q1q3+2*q0q2);///*參考/view/1239157.htm?fr=aladdin}//mpu6050驅(qū)動struct_sensorsensor;}//mpu6050驅(qū)動struct_sensorsensor;externvs16u8InitMPU6050(void){Single_Write2(MPU6050_ADDRESS,PWR_MGMT_1,0x00);Single_Write2(MPU6050_ADDRESS,SMPLRT_DIV,0x07);Single_Write2(MPU6050_ADDRESS,cONFIG,0x03);Single_Write2(MPU6050_ADDRESS,GYRO_CONFIG,0x10);Single_Write2(MPU6050_ADDRESS,ACCEL_CONFIG,0x09);return}//+-實現(xiàn)函數(shù)//iic讀取到得數(shù)據(jù)分拆,放入相應(yīng)寄存器,更新*void{mpu6050_buffer[0]=Single_Read2(MPU6050_ADDRESS,0x3B);mpu6050_buffer[1]=Single_Read2(MPU6050_ADDRESS,0x3C);mpu6050_buffer[2]=Single_Read2(MPU6050_ADDRESS,0x3D);mpu6050_buffer[3]=Single_Read2(MPU6050_ADDRESS,0x3E);mpu6050_buffer[4]=Single_Read2(MPU6050_ADDRESS,0x3F);mpu6050_buffer[5]=Single_Read2(MPU6050_ADDRESS,0x40);mpu6050_buffer[8]=Single_Read2(MPU6050_ADDRESS,0x43);mpu6050_buffer[9]=Single_Read2(MPU6050_ADDRESS,0x44);}/void{}/void{sensor.acc.origin.x=((((int16_t)mpu6050_buffer[0])<<8)|sensor.acc.origin.y=((((int16_t)mpu6050_buffer[2])<<8)|mpu6050_buffer[3])--=|sensor.gyro.origin.y=<<sensor.gyro.origin.z=((((int16_t)mpu6050_buffer[12])<<8)|mpu6050_buffer[13])}/void{int {{sensor.gyro.origin.z=((((int16_t)mpu6050_buffer[12])<<8)|mpu6050_buffer[13]);tempgx+=sensor.gyro.origin.x;tempgy+=tempgz+=}}#definePIdoubleX_HMC,Y_HMC,Z_HMC,Xr,Yr,Eang2=0;void{X_HMC=KalmanFilter_XHMC(X_HMC_Origin,0.02,6);Y_HMC=KalmanFilter_YHMC(Y_HMC_Origin,0.02,6);Z_HMC=KalmanFilter_ZHMC(Z_HMC_Origin,0.02,6);show_size8float3_1f(0,0,X_HMC);}externfloatang_c[3];externfloatwww[3];doublekppk=0.001;{staticintlast_time=0;{X_HMC=KalmanFilter_XHMC(X_HMC_Origin,0.02,6);Y_HMC=KalmanFilter_YHMC(Y_HMC_Origin,0.02,6);Z_HMC=KalmanFilter_ZHMC(Z_HMC_Origin,0.02,6);試}last_ang2=}#ifndefCONTROL_H#define#includevoidvoidintPID1(void);intintfloaty_control(floatY_Aim);floatx_control(floatX_Aim);voidcal_P_R_YAW(void);voidlvbo_aaa_www_ang(float*www,float*ang,float*aaa);voidgps_control(u8mode);doubleNorth_control(doubleNorth_Aim,doubleNax);voidEastNorth_control(doubleX_Aim,doubleY_Aim);doubleHigh_Control(doublehigh_press,intii_num,doubleAim_high);#include"sys.h"#include"delay.h"#include#include"HMC.h"#include"led.h"#include"math.h"#definePI3.1415fu8control_flag=0;externfloataaa[3];externfloatwww[3]; floatq0,q1,q2,q3;doubleintMotor[5]={0,7300,7300,7300,7300};//存放電機PWMdouble//姿態(tài)橫滾俯仰PIDintMotor[5]={0,7300,7300,7300,7300};//存放電機PWMdouble//姿態(tài)橫滾俯仰PIDdoublekp3=180.0,kd3=220.0,ka3=80.0,ki3=0.2;//偏航PIDdoublesum2=0,sum1=0,sum3=0,youmen=0,high_youmen=0;//sum1sum3是橫滾俯仰和偏航三個軸積分變量youmenhigh_youmen externdoubledoubleu8return_flag=0;//一鍵返航標(biāo)志doubleh_youmen=0;//定高用變量doublesum_East=0;doublesum_North=0;//2D定位所用PIDexternu32doubleAim_high=5,AimShiftEast=0,AimShiftNorth=0;//目標(biāo)定位坐標(biāo)和定高高度voidControl(){staticintstaticdoublebuchang=0;staticint{sum_lock++;if(sum_lock>100){sum_lock=0;control_flag=1;}}{{orkp;}//第一次進入該函數(shù)取當(dāng)前坐標(biāo)orkp;}//第一次進入該函數(shù)取當(dāng)前坐標(biāo)//如果一鍵返航則先水平運動到出發(fā)點上空在下降EastNorth_control(AimShiftEast,AimShiftNorth);//平面分成兩個PID=(}{}{} PID_1PID1()/2;//3PIDPID_2PID2()/2;//3PID if(PID_1<-5200)PID_1=-5200;//PID值限幅if(PID_2<-5200)PID_2=-5200;//PID if(PID_1<-5200)PID_1=-5200;//PID值限幅if(PID_2<-5200)PID_2=-5200;//PID值限幅if(PID_3>2300)PID_3=2300;//PID值限幅if(PID_3<-2300)PID_3=-2300;//PID值限幅Motor[1]=youmen+k*PID_1+Motor[2]=youmen+k*PID_2-k*PID_3;Motor[3]=youmen-k*PID_1+k*PID_3;Motor[4]=youmen-k*PID_2-k*PID_3;{}{}PWM輸出PWM輸出PWMPWM}}{staticdoubler1,r2,r,dw1;staticlongtk=0;}else}return(int)r2;}{}return(int)r2;}{staticdoublehistory_ang2=0;tk++;{}{ang[1]=ang[1]+ROLL_val;}return}{{{}{elseif(ang[2]>=15)sum3+=15;}return}voidlvbo_aaa_www_ang(float*www,float*ang,float*{u8float{}{}{}{{}{}{}{}{}{}{}{}{}}//void{){ifelseif(Capture_CH[2]>=0&&Capture_CH[2]<450)elseif(Capture_CH[1]>550&&Capture_CH[1]<1100)ROLL_val=0-ROLL_val;//偏轉(zhuǎn)ROLL_val+-}else}u8{{}u8{{}}{{} } {}{}returnEast_val;}{{} {}{}{}returnNorth_val;}externfloatang0_car[3];externfloatq0,q1,q2,q3;externfloatang_c[3];doubleEax=0,Nax=0;{doublefloatq0q1=q0*q1;doubleAax_yaw=0,Aay_yaw=0;floatq0q2=q0*q2;floatq1q1=q1*q1;floatq1q3=q1*q3;floatq2q2=q2*q2;floatq2q3=q2*q3;floatq0q0=q0*q0;floatq3q3=q3*q3;floatq1q2=q1*q2;floatq2q0=q2*q0;floatq0q3= }doubleexternfloataaa_car[3];externu8ms5611update;doublespeed_press=0;{static staticdoublevh_k=0.995;intfloatq0q1=q0c*q1c;floatq0q2=q0c*q2c;floatq1q1=q1c*q1c;floatq1q3=q1c*q3c;floatq2q2=q2c*q2c;floatq2q3=q2c*q3c; }Aaz}Aaz=2*aaa_car[0]*(q1q3-q0q2)+2*aaa_car[1]*(q2q3+q0q1)+2*aaa_car[2]*(0.5f- } #include"math.h"http://高度PID