设为首页收藏本站
开启辅助访问 切换到宽版

 找回密码
 立即注册

QQ登录

只需一步，快速开始

热门： 51单片机 | 24小时必答区 | 单片机教程 | 单片机DIY制作 | STM32 | Cortex M3 | 模数电子 | 电子DIY制作 | 音响/功放 | 拆机乐园 | Arduino | 嵌入式OS | 程序设计

搜索
»论坛 › 电子技术分类讨论区 › 四轴/航模/飞行器DIY › 四轴四元数的51单片机源程序
返回列表 发新帖
查看: 3612|回复: 0
打印 上一主题 下一主题
收起左侧

四轴四元数的51单片机源程序

[复制链接]
跳转到指定楼层
楼主
ID:184171 发表于 2017-5-16 20:14 | 只看该作者 |只看大图 回帖奖励 |倒序浏览 |阅读模式
分享个四轴四元数的51单片机代码：



单片机源程序如下:
  1. //四川 绵阳      西南科技大学 信息工程学院 电气13级 刘其民  
  2. //硬件参数：
  3. //电池:1S/3.7V电池 推荐300-500mAh左右
  4. //电机/桨:720空心杯/59MM桨
  5. //MCU IAP15W4K61S4@28.000MHZ  
  6. //陀螺仪加速度计：MPU-6050
  7. //磁场传感计:HMC5883L
  8. //无线芯片:NRF24L01
  9. //电机驱动MOS管:AO3400
  10. //MOS管保护用肖特基:BAT54ST
  11. //下载口保护用瞬态抑制二极管:RCLAMP0524P
  12. //机架尺寸:94mm*94mm
  13. //标准PID控制公式:PID=P*e(n)+I*[(e(n)+e(n-1)+...+e(0)]+D*[e(n)-e(n-1)]

  15. //数据定义说明：
  16. //data 51单片机片内RAM最前面128字节RAM 用ACC读写，速度最快
  17. //idata 片内RAM最前面256字节的RAM 包括data 用类似指针模式访问 适合用于指针操作
  18. //pdata 外部扩展RAM的前256字节的RAM
  19. //xdata 外部扩展RAM 用DPTR访问
  20. #include <STC15W4K60S4.H>
  21. #include <intrins.h>
  22. #include <NRF24L01.H>
  23. #include <MPU6050.H>
  24. #include <math.h>
  25. #include <STC15W4KPWM.H>
  26. void Time0_Init();   //定时器初始化
  27. void update();       //陀螺仪矫正
  28. void Kalman_Filter(float Accel,float Gyro);   //X轴卡尔曼滤波
  29. void Kalman_Filter_Y(float Accel,float Gyro);        //Y轴卡尔曼滤波       
  30. //*************************************自整定PID相关参数************************************************
  31. //unsigned char xdata ERRORPID[512]={0};
  32. //unsigned char xdata ZSYPID[100]={0};
  33. //unsigned long int xdata ALLERROR[100]={0};
  34. //unsigned long int TE=0,TZA=0,ADD=0,THEEND=0,LESTERROR=0;
  35. //float BESTPID=0;
  36. //******************************************************************************************************
  37. unsigned char PID=0;                         //无线串口PID调整延时用
  38. int ich1=0,ich2=0,ich3=0,ich4=0;                                     //无线串口待发送数据
  39. int speed0=0,speed1=0,speed2=0,speed3=0,V=0; //电机控制参数
  40. int PWM0=0,PWM1=0,PWM2=0,PWM3=0;             //电机控制参数
  41. unsigned char TxBuf[20]={0};
  42. unsigned char RxBuf[20]={0};
  43. double g_x=0,g_y=0,g_z=0;                    //陀螺仪静差消除参数
  44. double PID_x=0,PID_y=0,PID_z=0;              //每根轴PID控制量输出
  45. //******角度参数****************************************************************************************
  46. double Angle_ax=0,Angle_ay=0,Angle_az=0;    //由加速度计算的倾斜角度
  47. double Angle_gy=0,Angle_gx=0,Angle_gz=0;    //由角速度计算的倾斜角度
  48. double AngleAx=0,AngleAy=0;                 //反三角函数处理后的角度值
  49. double Angle=0,Angley=0;                    //最终倾斜角度
  50. double Anglelate=0,Anglelatey=0,Anglezlate=0;//存储上一次角度数据
  51. double Ax=0,Ay=0;                            //加入遥控器控制量后的角度
  52. float idata dt=0.008;                              //系统周期
  53. //******卡尔曼滤波参数-X轴******************************************************************************
  54. float data Q_angle=0.001;     //对加速度计的信任度
  55. float data Q_gyro=0.003;      //对陀螺仪的信任度
  56. float data R_angle=0.5;
  57. char  data C_0 = 1;
  58. float data Q_bias, Angle_err;
  59. float data PCt_0, PCt_1, E;
  60. float data K_0, K_1, t_0, t_1;
  61. float data Pdot[4] ={0,0,0,0};
  62. float data PP[2][2] = { { 1, 0 },{ 0, 1 } };
  63. void Kalman_Filter(float Accel,float Gyro)               
  64. {
  65.         Angle+=(Gyro - Q_bias) * dt; //陀螺仪积分角度（测量值-陀螺仪偏差)*dt  
  66. //Angle相当于是系统的预测值
  67.         Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // 先验估计误差协方差的微分
  68.         Pdot[1]=- PP[1][1];
  69.         Pdot[2]=- PP[1][1];
  70.         Pdot[3]=  Q_gyro;
  71.         PP[0][0] += Pdot[0] * dt;   // 先验估计误差协方差微分的积分
  72.         PP[0][1] += Pdot[1] * dt;   
  73.         PP[1][0] += Pdot[2] * dt;
  74.         PP[1][1] += Pdot[3] * dt;
  75.         Angle_err = Accel - Angle;//估计值与预测值之间的偏差
  76.         PCt_0 = C_0 * PP[0][0];
  77.         PCt_1 = C_0 * PP[1][0];
  78.         E = R_angle + C_0 * PCt_0;
  79.         K_0 = PCt_0 / E;//卡尔曼增益1 用于计算最优估计值
  80.         K_1 = PCt_1 / E;//卡尔曼增益2 用于计算最后估计的偏差
  81.         t_0 = PCt_0;
  82.         t_1 = C_0 * PP[0][1];
  83.         PP[0][0] -= K_0 * t_0;                 //更新协方差矩阵
  84.         PP[0][1] -= K_0 * t_1;
  85.         PP[1][0] -= K_1 * t_0;
  86.         PP[1][1] -= K_1 * t_1;
  87.         Angle        += K_0 * Angle_err;//根据卡尔曼增益1算出最优角度
  88.         Q_bias        += K_1 * Angle_err;//根据卡尔曼增益2算出预测值偏差
  89. }
  90. //******卡尔曼滤波参数-Y轴***************************************************************************
  91.                
  92. float data Q_angley=0.001;  
  93. float data Q_gyroy=0.003;
  94. float data R_angley=0.5;
  95. char  data C_0y = 1;
  96. float idata Q_biasy, Angle_erry;
  97. float idata PCt_0y, PCt_1y, Ey;
  98. float idata K_0y, K_1y, t_0y, t_1y;
  99. float idata Pdoty[4] ={0,0,0,0};
  100. float idata PPy[2][2] = { { 1, 0 },{ 0, 1 } };

  102. void Kalman_Filter_Y(float Accely,float Gyroy)               
  103. {
  104.         Angley+=(Gyroy - Q_biasy) * dt;
  105.         Pdoty[0]=Q_angley - PPy[0][1] - PPy[1][0];
  106.         Pdoty[1]=- PPy[1][1];
  107.         Pdoty[2]=- PPy[1][1];
  108.         Pdoty[3]=Q_gyroy;
  109.         PPy[0][0] += Pdoty[0] * dt;   
  110.         PPy[0][1] += Pdoty[1] * dt;   
  111.         PPy[1][0] += Pdoty[2] * dt;
  112.         PPy[1][1] += Pdoty[3] * dt;
  113.         Angle_erry = Accely - Angley;       
  114.         PCt_0y = C_0y * PPy[0][0];
  115.         PCt_1y = C_0y * PPy[1][0];
  116.         Ey = R_angley + C_0y * PCt_0y;
  117.         K_0y = PCt_0y / Ey;
  118.         K_1y = PCt_1y / Ey;
  119.         t_0y = PCt_0y;
  120.         t_1y = C_0y * PPy[0][1];
  121.         PPy[0][0] -= K_0y * t_0y;               
  122.         PPy[0][1] -= K_0y * t_1y;
  123.         PPy[1][0] -= K_1y * t_0y;
  124.         PPy[1][1] -= K_1y * t_1y;
  125.         Angley        += K_0y * Angle_erry;       
  126.         Q_biasy        += K_1y * Angle_erry;       
  127. }
  128. //************姿态处理和PID**************************************************************************
  129. float idata YP=4.5,YD=115.0,YI=0.05,ERRORX;
  130. float idata XP=4.5,XD=115.0,XI=0.05,ERRORY;
  131. float idata ZP=4.0,ZD=2.0;
  132. void Angle_Calcu(void)
  133. {
  134.         Angle_ax=(GetData(ACCEL_XOUT_H))/8192;  //加速度处理
  135.         Angle_az=(GetData(ACCEL_ZOUT_H))/8192;  //加速度量程 +-4g/S
  136.         Angle_ay=(GetData(ACCEL_YOUT_H))/8192;        //转换关系8192LSB/g

  138.         Angle_gx=(GetData(GYRO_XOUT_H)-g_x)/65.5;   //陀螺仪处理
  139.         Angle_gy=(GetData(GYRO_YOUT_H)-g_y)/65.5;   //陀螺仪量程 +-500度/S
  140.         Angle_gz=(GetData(GYRO_ZOUT_H)-g_z)/65.5;   //转换关系65.5LSB/度
  141. //**************X轴指向*******************************************************************************
  142.         AngleAx=atan(Angle_ax/sqrt(Angle_ay*Angle_ay+Angle_az*Angle_az))*180/3.141592657;
  143.   
  144.         Anglelate=Ax;
  145.        
  146.         //Angle=0.95*(Angle-Angle_gy*dt)+0.05*AngleAx;//一阶互补滤波       这边是-Angle_gy
  147.        
  148.         Kalman_Filter(AngleAx,-Angle_gy);       //卡尔曼滤波
  149.        
  150.         Ax=Angle+((float)RxBuf[1]-128)/7;  
  151.         ERRORX+=3*Ax;                 //加强静差积分强度但是不改变输出控制量的范围
  152.         if(ERRORX>1000){ERRORX=1000;}if(ERRORX<-1000){ERRORX=-1000;}
  153.         PID_y=Ax*XP+ERRORX*XI+(Ax-Anglelate)*XD;          
  154.         speed0=0+PID_y,speed2=0-PID_y;
  155. //**************Y轴指向*******************************************************************************
  156.   AngleAy=atan(Angle_ay/sqrt(Angle_ax*Angle_ax+Angle_az*Angle_az))*180/3.141592657;

  158.         Anglelatey=Ay;
  159.        
  160.         //Angley=0.95*(Angley+Angle_gx*dt)+0.05*AngleAy;//一阶互补滤波     这边是+Angle_gx
  161.        
  162.         Kalman_Filter_Y(AngleAy,Angle_gx);      //卡尔曼滤波
  163.        
  164.         Ay=Angley+((float)RxBuf[2]-128)/7-RxBuf[4]/20;           //由于结构问题，油门越大，Y轴加速度计输出偏差越大，RxBuf[4]补偿之
  165.         ERRORY+=3*Ay;                 //加强静差积分强度但是不改变输出控制量的范围
  166.         if(ERRORY>1000){ERRORY=1000;}if(ERRORY<-1000){ERRORY=-1000;}
  167.         PID_x=Ay*YP+ERRORY*YI+(Ay-Anglelatey)*YD;          
  168.         speed3=0+PID_x,speed1=0-PID_x;          
  169. //**************Z轴指向*******************************************************************************
  170.         Angle_gz+=(RxBuf[3]-128)/10;
  171.         PID_z=(Angle_gz)*ZP+(Angle_gz-Anglezlate)*ZD;
  172.         Anglezlate=Angle_gz;
  173.         speed0=speed0+PID_z,speed2=speed2+PID_z;
  174.         speed1=speed1-PID_z,speed3=speed3-PID_z;
  175. //*****************无线串口相关***********************************************************************
  176.         ich1=Ax;
  177.         ich2=Ay;
  178.         ich3=AngleAy;
  179.         ich4=XD;
  180. //**************速度更新******************************************************************************       
  181.         if((1000-RxBuf[4]*4+speed0)>1000)PWM0=1000;
  182.         else if((1000-RxBuf[4]*4+speed0)<0)PWM0=0;
  183.         else PWM0=(1000-RxBuf[4]*4+speed0);

  185.         if((1000-RxBuf[4]*4+speed1)>1000)PWM1=1000;
  186.         else if((1000-RxBuf[4]*4+speed1)<0)PWM1=0;
  187.         else PWM1=(1000-RxBuf[4]*4+speed1);

  189.         if((1000-RxBuf[4]*4+speed2)>1000)PWM2=1000;
  190.         else if((1000-RxBuf[4]*4+speed2)<0)PWM2=0;
  191.         else PWM2=(1000-RxBuf[4]*4+speed2);

  193.         if((1000-RxBuf[4]*4+speed3)>1000)PWM3=1000;
  194.         else if((1000-RxBuf[4]*4+speed3)<0)PWM3=0;
  195.         else PWM3=(1000-RxBuf[4]*4+speed3);
  196.   if(RxBuf[4]>=10)
  197.         {PWM(PWM1,PWM2,PWM0,PWM3);}//1203
  198.         else
  199.         {PWM(1000,1000,1000,1000);}
  200. //******************************************以下注释内容为PID中D的自动整定模式********************************************
  201.         /*         
  202.         if(RxBuf[4]>60&&THEEND==0)
  203.         {
  204.                 if(Ax<0&&Ay>0)
  205.                 {
  206.                 ERRORPID[TE]=-Ax+Ay;
  207.                 }
  208.                 else if(Ax>0&&Ay<0)
  209.                 {
  210.                 ERRORPID[TE]=Ax-Ay;
  211.                 }       
  212.                 else if(Ax<0&&Ay<0)
  213.                 {
  214.                 ERRORPID[TE]=-Ax-Ay;
  215.                 }       
  216.                 else
  217.                 {
  218.                 ERRORPID[TE]=Ax+Ay;
  219.                 }
  220.                 TE++;
  221.                 if(TE==511)
  222.                 {
  223.                         TE=0;
  224.                         ZSYPID[TZA]=XD;
  225.                         XD++;YD++;
  226.                         for(ADD=0;ADD<=510;ADD++)
  227.                                 {
  228.                              ALLERROR[TZA]+=ERRORPID[ADD];
  229.                                 }
  230.                         TZA++;
  231.                         if(TZA==100)
  232.                         {
  233.                                 THEEND=1;
  234.                                 LESTERROR=ALLERROR[1];
  235.                                 for(TE=1;TE<=99;TE++)
  236.                                 {
  237.                                         if(LESTERROR>=ALLERROR[TE])
  238.                                         {
  239.                                                 LESTERROR=ALLERROR[TE];
  240.                                                 BESTPID=ZSYPID[TE];
  241.                                         }
  242.                                 }
  243.                                 XD=YD=BESTPID;
  244.                         }
  245.                 }
  246.         }       
  247.         */
  248. }
  249. void main()
  250. {
  251. Delay(1000);
  252. PWMGO();
  253. init_NRF24L01();
  254. InitMPU6050();
  255. Time0_Init();
  256. RxBuf[1]=128;
  257. RxBuf[2]=128;
  258. RxBuf[3]=128;
  259. RxBuf[4]=0;
  260.         //SetRX_Mode();
  261. while(1)
  262. {          
  263.                     //以下为无线串口助手发送程序
  264.                                 TxBuf[1]=13; TxBuf[6]=13; TxBuf[11]=15; TxBuf[16]=15;
  265.                           if(ich1<0)         {TxBuf[1]=12; ich1=-ich1;}
  266.                                 if(ich2<0)         {TxBuf[6]=12; ich2=-ich2;}
  267.                                 if(ich3<0)         {TxBuf[11]=12; ich3=-ich3;}
  268.                                 if(ich4<0)         {TxBuf[16]=12; ich4=-ich4;}
  269.                                 TxBuf[2]=ich1/1000;        TxBuf[3]=ich1/100%10; TxBuf[4]=ich1%100/10;        TxBuf[5]=ich1%10;         
  270.                                 TxBuf[7]=ich2/1000;        TxBuf[8]=ich2/100%10; TxBuf[9]=ich2%100/10;        TxBuf[10]=ich2%10;
  271.                                 TxBuf[12]=ich3/1000; TxBuf[13]=ich3/100%10;        TxBuf[14]=ich3%100/10; TxBuf[15]=ich3%10;
  272.                                 TxBuf[17]=ich4/1000; TxBuf[18]=ich4/100%10;        TxBuf[19]=ich4%100/10; TxBuf[0]=ich4%10;
  273.                                 //以上为无线串口助手发送程序
  274.                                 nRF24L01_TxPacket(TxBuf);
  275. ……………………

  277. …………限于本文篇幅 余下代码请从51黑下载附件…………
复制代码

所有资料51hei提供下载:
四轴-四元数测试版.rar (197.51 KB, 下载次数: 28)


源程序
分享到:  QQ好友和群QQ好友和群 QQ空间QQ空间 腾讯微博腾讯微博 腾讯朋友腾讯朋友
收藏收藏2 分享淘帖 顶顶 踩踩

相关帖子
回复

使用道具 举报

返回列表 发新帖
高级模式
B Color Image Link Quote Code Smilies
您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

手机版|小黑屋|51黑电子论坛 |51黑电子论坛6群 QQ 管理员QQ:125739409;技术交流QQ群281945664

Powered by 单片机教程网

快速回复 返回顶部 返回列表