定位定点一键返航飞控程序注释

//mainfunctionpath//mainfunctionpath//thispartistest.c,suggestyoureadthere#include"usart.h" //usart1 //led"#includeinv_mpu_dmp_motion_driver.h"//MPU6050dmpio初始化模拟iic#include"inv_mpu.h"//MPU6050dmpio初始化模拟iic"#include"HMC.h"//电子罗盘HMC5983初始化相关#include"IMU.h"//姿态角融合文件#include"ms5611.h"//气压计MS5611#include"usart3.h"//gps串口数据接收#include"control.h"//控制所在文件 floatang0_car[3];//角度卡尔曼滤波值 ////externdoublevoidtranslateUSART(void上位机发送函数voidanalyse(void);//无线串口数据接收解析u8usartre[8]={0};//无线串口数据接收数组u8senddata[36]={0};//上位机发送的数据 voidtranslate2401(void2401longgps_lati=0,gps_long=0;//记录起飞点GPS坐标吧得出的GPS坐标归零u8flag_gps=0;//GPS是否搜到卫星其实可以用gpsx.fixtype=33D//以及向东向北的速度staticintlast_time=0;//staticdoubletime=0;//变量用于测中断执行用了多久时间doublehighford=0;//用于互补滤波算对地速度之用int{Stm32_Clock_Init(336,8,2,7);//STM32F4的初始化复位向量表之类的 //延时函数初始化记住延时函数不能用在中断里面uart_init(84,115200);//串口1初始化用于无线数传usart2_init(42,100000);//串口2初始化用于遥控接收机SBUS接收usart3_init(42,115200);//串口3初始化用于GPS数据接收LED_Init();//LED状态灯初始化LED=~LED;//状态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);//据if(ii%11==5){Exchange_Number(2);}//气压计分时采集不要动会影响气压计甚至读不if(ii%11==5)Exchange_Number(3);//据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"//这里做出更改实例注意区别//#define {GPIOE->MODER&=~(3<<(7*2));GPIOE->MODER|=0<<7*2;}//PE3式//#define//#define//#definePEout(7) //PE3 式#define#definePEout(3) //发送IIC//发送//发送IIC//发送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;//发送I2C}u8{u8 {{return1;}}return}//产生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;//最优值对应的x_last=x_now;//更新系统状态值returnx_now;} x_last=x_now;//更新系统状态值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驱动struct_sensorsensor;}//mpu6050驱动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}//+-实现函数//iic读取到得数据分拆,放入相应寄存器,更新*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//姿态横滚俯仰PIDintMotor[5]={0,7300,7300,7300,7300};//存放电机PWMdouble//姿态横滚俯仰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;//一键返航标志doubleh_youmen=0;//定高用变量doublesum_East=0;doublesum_North=0;//2D定位所用PIDexternu32doubleAim_high=5,AimShiftEast=0,AimShiftNorth=0;//目标定位坐标和定高高度voidControl(){staticintstaticdoublebuchang=0;staticint{sum_lock++;if(sum_lock>100){sum_lock=0;control_flag=1;}}{{orkp;}//第一次进入该函数取当前坐标orkp;}//第一次进入该函数取当前坐标//如果一键返航则先水平运动到出发点上空在下降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;//偏转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"//高度PID