自己写的小四轴程序stm32f103c8t6,采用mpu6050中的dmp直接输出姿态角可不用了解四元数,通行模块为蓝牙模块,程序是裸机代码没有太多变量可以容易看懂,如果有这方面的爱好可以一起探讨qq1244450581,PID算法只用了串级PD(这个PID是由模电稳压的过程得到灵感)不过有引进PID死区,分为俩个PID调节
自己试了感觉挺稳,由于不能发太大文件遥控程序就不发了。
部分源程序预览:
- /*******************************************************************************
- gggi
- 四轴飞控程序
- 由mpu_dmp_get_data(&pitch,&roll,&yaw)这个函数引出欧拉角
- 由RCDataProcess(&exthrottle,&exroll,&expitch,&exyaw);这个引出期望角与油门
-
- *******************************************************************************/
- #include "public.h"
- #include "printf.h"
- #include "stdio.h"
- #include "delay.h"
- #include "dmp.h"
- #include "inv_mpu.h"
- #include "inv_mpu_dmp_motion_driver.h"
- #include "bt.h"
- #include "pwm.h"
- #include "time.h" //把LED初始化的文件一块放里面,恩,我没有偷懒!!!
- #include "pid.h"
- float old;//用于 判断是否进入加速剂调节模式
- float exthrottle,roll_ex,pitch_ex,yaw_ex;
- u16 power1,power2,power3,power4;
- //S_FLOAT_ANGLE Q_ANGLE;
- float pitch,roll,yaw; //欧拉角
- float gyrox,gyroy,gyroz;
- float Gyrox,Gyroy,Gyroz;
- float aacx,aacy,aacz; //加速度传感器原始数据
- //传送数据给匿名四轴上位机软件(V2.6版本)
- //fun:功能字. 0XA0~0XAF
- //data:数据缓存区,最多28字节!!
- //len:data区有效数据个数
- void usart1_niming_report(u8 fun,u8*data,u8 len)
- {
- u8 send_buf[32];
- u8 i;
- if(len>28)return; //最多28字节数据
- send_buf[len+3]=0; //校验数置零
- send_buf[0]=0X88; //帧头
- send_buf[1]=fun; //功能字
- send_buf[2]=len; //数据长度
- for(i=0;i<len;i++)send_buf[3+i]=data[i]; //复制数据
- for(i=0;i<len+3;i++)send_buf[len+3]+=send_buf[i]; //计算校验和
- for(i=0;i<len+4;i++)usart1_send_char(send_buf[i]); //发送数据到串口1
- }
- //匿名四轴上位机发送的数据格式
- void usart1_report_imu(short aacx,short aacy,short aacz,short gyrox,short gyroy,short gyroz,short roll,short pitch,short yaw)
- {
- u8 tbuf[28];
- u8 i;
- for(i=0;i<28;i++)tbuf[i]=0;//清0
- tbuf[0]=(aacx>>8)&0XFF;
- tbuf[1]=aacx&0XFF;
- tbuf[2]=(aacy>>8)&0XFF;
- tbuf[3]=aacy&0XFF;
- tbuf[4]=(aacz>>8)&0XFF;
- tbuf[5]=aacz&0XFF;
- tbuf[6]=(gyrox>>8)&0XFF;
- tbuf[7]=gyrox&0XFF;
- tbuf[8]=(gyroy>>8)&0XFF;
- tbuf[9]=gyroy&0XFF;
- tbuf[10]=(gyroz>>8)&0XFF;
- tbuf[11]=gyroz&0XFF;
- tbuf[18]=(roll>>8)&0XFF;
- tbuf[19]=roll&0XFF;
- tbuf[20]=(pitch>>8)&0XFF;
- tbuf[21]=pitch&0XFF;
- tbuf[22]=(yaw>>8)&0XFF;
- tbuf[23]=yaw&0XFF;
- usart1_niming_report(0XAF,tbuf,28);//飞控显示帧,0XAF
- }
- int main()
- {
- u8 i;
- float j;//用于遥控油门数值
- u8 a,b=0,x=0;//用于加速计的累加求平均值
- u16 flag=0,gyro[3];
- float aac1[10],aac2[10];
- SystemInit();
- delay_init(72);
- LED_Init() ;
- printf_init();
- I2C_GPIO_Config();
- delay_ms(10);
- PIDvalue();//往PID里面塞东西,确定三个姿态的p,i,d值
- MPU_Init();
- mpu_dmp_init();
- pwm_init();
- TIM2_Init();
-
-
- while(1)
- {
- mpu_dmp_get_data(&pitch,&roll,&yaw,&gyrox,&gyroy,&gyroz,&aacx,&aacy,&aacz);
- ////////////////////////////////////加速剂平滑滤波
- // aac1[x]=aacx;
- //aac2[x]=aacy;
- // x++;
- // for(i=0;i<10;i++)
- // aacx=(aacx+aac1[i])/2;
- //
- // for(i=0;i<10;i++)
- // aacy=(aacy+aac2[i])/2;
- // if(x==10)
- // x=0;
- /////////////////////////////////////
-
-
-
-
- if(timer==1)
- {
- timer=0;
- flag++;
-
- gyrox=gyrox/10;
- gyroy=gyroy/10;
- gyroz=gyroz/10;
- // aacx=aacx/40-25;
- // aacy=aacy/40+22.5;
- // aacz=aacz/90-180;
- // RCDataProcess(&exthrottle,&roll_ex,&pitch_ex,&yaw_ex);
-
-
- /////////////////////////翻滚程序/////////////////////
-
- /////////////////////////////////////////////////////////////////////////
-
-
- //////////////////////////////////油门保护程序////////////////////////////////
- // if((old-exthrottle)>400)
- // {
- // while(1)
- // {
- // mpu_dmp_get_data(&pitch,&roll,&yaw,&gyrox,&gyroy,&gyroz);
- // RCDataProcess(&nouse,&roll_ex,&pitch_ex,&yaw_ex);
- // exthrottle=old-10;
- // singlePIDupdate();
- // pwm_in(power1,power2,power3,power4);
- // if(nouse>300)break;
- // }
- // }
- //////////////////////////////////////////////////////////////////////////////////////
- RCDataProcess(&exthrottle,&roll_ex,&pitch_ex,&yaw_ex);
- // if(roll_ex==170)
- // {
- // pwm_in(power1+200,power2-200,power3-200,power4+200);
- // delay_ms(10);
- // pwm_in(power1-200,power2+400,power3+400,power4-200);
- // delay_ms(10);
- //
- // power2=power2+400;
- // if(power2>1000)
- // power2=1000;
- // power4=power4-200;
- // if(power4<0)
- // power4=0;
- //
- // power3=power3+400;
- // if(power3>1000)
- // power3=1000;
- // power1=power1-200;
- // if(power1<0)
- // power1=0;
- // while(1)
- // {
- // pwm_in(power1,power2,power3,power4);
- // mpu_dmp_get_data(&pitch,&roll,&yaw,&gyrox,&gyroy,&gyroz,&aacx,&aacy,&aacz);
- // if(roll_ex>150)
- // {
- // b=1;
- // delay_ms(500);
- // }
- // if((roll>(-40))&&(b==1))
- // {b=0;
- // break;
- // }
- // }
- // RCDataProcess(&exthrottle,&roll_ex,&pitch_ex,&yaw_ex);
- // mpu_dmp_get_data(&pitch,&roll,&yaw,&gyrox,&gyroy,&gyroz,&aacx,&aacy,&aacz);
- // gyrox=gyrox/10;
- // gyroy=gyroy/10;
- // gyroz=gyroz/10;
- // aacx=aacx/40-25;
- // aacy=aacy/40+22.5;
- // aacz=aacz/90-180;
- // }
- //exthrottle=21;
- if(exthrottle>20)
- {
-
- // Q_ANGLE.Pitch=(int)((pitch)*100)/100.0;
- // Q_ANGLE.Roll=(int)(roll*100)/100.0;
- // Q_ANGLE.Yaw=(int)(yaw*100)/100.0;
- singlePIDupdate(); //PID调节
- pwm_in(power1,power2,power3,power4); //油门输出
-
- //pwm_in(power1,0,power3,0); //油门输出
- // pwm_in(0,power2,0,power4);
- }
- else if(exthrottle<20)
- {
- // Q_ANGLE.Pitch=(int)((pitch)*100)/100.0+1.5;
- // Q_ANGLE.Roll=(int)(roll*100)/100.0+0.3;
- // Q_ANGLE.Yaw=(int)(Q_ANGLE.Yaw*100)/100.0;
- pwm_in(0,0,0,0); //油门输出
-
-
-
- }
-
-
-
-
- if(flag==500)
- {
- led_display1();
-
- //MPU_Get_Accelerometer(&aacx,&aacy,&aacz); //得到加速度传感器数据
- //usart1_report_imu(exthrottle,roll_ex,pitch_ex,gyrox,gyrox,gyroz,(int)(roll*100),(int)(pitch*100),(int)(yaw*10));
- // j=exthrottle+1000;
- // i[0]=((int)j>>8)&0XFF;
- // i[1]=(int)j&0XFF;
- //
- // usart1_niming_report(0xAE,i,24);
-
- }
- if(flag==1000)
- {
- flag=0;
- led_display2();
- old=exthrottle;
-
- }
- if(pitch>75||pitch<-75||roll<-75||roll>75)
- {
- pwm_in(0,0,0,0);
- while(1);
- }
- //}
- // if(flag==30)
- // {
- // //led_display1();
- //
- // flag=0;
- // usart1_report_imu(exthrottle,roll_ex,pitch_ex,gyrox,gyrox,gyroz,(int)(roll*100),(int)(pitch*100),(int)(yaw*10));
- //// j=exthrottle+1000;
- //// i[0]=((int)j>>8)&0XFF;
- //// i[1]=(int)j&0XFF;
- ////
- //// usart1_niming_report(0xAE,i,24);
- //
- }
-
- …………余下代码请下载附件…………
复制代码
资料下载:
四轴带遥控.zip
(9.82 MB, 下载次数: 46)
|