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

 找回密码
 立即注册

QQ登录

只需一步，快速开始

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

搜索
»论坛 › 电子技术分类讨论区 › 资料共享 › stm32四轴飞控源程序
返回列表 发新帖
查看: 2034|回复: 0
打印 上一主题 下一主题
收起左侧

stm32四轴飞控源程序

[复制链接]
跳转到指定楼层
楼主
ID:347767 发表于 2018-9-25 22:33 | 只看该作者 回帖奖励 |倒序浏览 |阅读模式
编译,下载,运行,连接飞控串口和FTDI串口,串口波特率500K,上位机打开高级收码,在上位机飞控状态标签可以看到变化的传感器数据,3D显示可以跟随roll和pitch的变化而变化,因为没有写yaw的上传,所以yaw保持零度...此时打开飞控波形按钮,打开波形显示页面,并打开相应波形开关,1到3为加速度,4到6为陀螺仪,10和11为roll和pitch,就可以看到变化的波形.

单片机源程序如下:
  1. #include "uart1.h"
  2. #include "app/rc/rc.h"
  3. #include "bsp/MPU6050.h"
  4. #include "bsp/led.h"
  5. #include "app/imu/imu.h"
  6. #include "control.h"
  7. #include "bsp/bsp.h"
  8. #define BYTE0(dwTemp)       (*(char *)(&dwTemp))
  9. #define BYTE1(dwTemp)       (*((char *)(&dwTemp) + 1))
  10. #define BYTE2(dwTemp)       (*((char *)(&dwTemp) + 2))
  11. #define BYTE3(dwTemp)       (*((char *)(&dwTemp) + 3))
  12. u8 ARMED = 0;
  13. char flag_send;

  15. RC_GETDATA Rc_Get;//接收到的RC数据,1000~2000
  16. float RC_Target_ROL=0,RC_Target_PIT=0,RC_Target_YAW=0;
  17. //void RC_FUN(void)
  18. //{
  19. ////判断解锁
  20. //        if(ARMED==0 && Rc_Get.THROTTLE < 1100 && Rc_Get.YAW <1100)
  21. //  {ARMED = 1;LED2_ON;}                //解锁
  22. //        if(ARMED==1 && Rc_Get.THROTTLE <1100 && Rc_Get.YAW >1900)
  23. //        {ARMED = 0;LED2_OFF;}                //锁定
  24. //}
  25. void BlueToothDeal(u8 buf_num)
  26. {
  27.         if(Rx_Buf[buf_num][1]==0x80)                //蓝牙上位机指令
  28.         {               
  29.                 ARMED=1;
  30.                 if(Rx_Buf[buf_num][2]==0x81)flag_send=1;
  31.                 //16位数据  低位在前
  32.                                 Rc_Get.THROTTLE = Rx_Buf[buf_num][2]  ;Rc_Get.THROTTLE <<= 8;Rc_Get.THROTTLE += Rx_Buf[buf_num][3];
  33.                                 Rc_Get.YAW      = Rx_Buf[buf_num][4]  ;Rc_Get.YAW      <<= 8;Rc_Get.YAW      += Rx_Buf[buf_num][5];
  34.                                 Rc_Get.ROLL     = Rx_Buf[buf_num][6]  ;Rc_Get.ROLL     <<= 8;Rc_Get.ROLL     += Rx_Buf[buf_num][7];
  35.                                 Rc_Get.PITCH    = Rx_Buf[buf_num][8]  ;Rc_Get.PITCH    <<= 8;Rc_Get.PITCH    += Rx_Buf[buf_num][9];
  36.                                 Rc_Get.AUX1     = Rx_Buf[buf_num][10] ;Rc_Get.AUX1     <<= 8;Rc_Get.AUX1     += Rx_Buf[buf_num][11];
  37.                                 Rc_Get.AUX2     = Rx_Buf[buf_num][12] ;Rc_Get.AUX2     <<= 8;Rc_Get.AUX2     += Rx_Buf[buf_num][13];
  38.                                 Rc_Get.AUX3     = Rx_Buf[buf_num][14] ;Rc_Get.AUX3     <<= 8;Rc_Get.AUX3     += Rx_Buf[buf_num][15];
  39.                                 Rc_Get.AUX4     = Rx_Buf[buf_num][16] ;Rc_Get.AUX4     <<= 8;Rc_Get.AUX4     += Rx_Buf[buf_num][17];
  40.                                 Rc_Get.AUX5     = Rx_Buf[buf_num][18] ;Rc_Get.AUX5     <<= 8;Rc_Get.AUX5     += Rx_Buf[buf_num][19];
  41.                
  42.                         //RC_FUN();
  43.                
  44.                                 RC_Target_ROL = (Rc_Get.ROLL-1500)/30;
  45.                                 RC_Target_PIT = (Rc_Get.PITCH-1500)/30;
  46.                                 RC_Target_YAW = (Rc_Get.YAW-1500)/30;
  47.                                 LED2_ONOFF();
  48.         }
  49.         else if(Rx_Buf[buf_num][1]==0x77)        //加锁解锁
  50.         {
  51.                 Rc_Get.THROTTLE = Rx_Buf[buf_num][2]  ;Rc_Get.THROTTLE <<= 8;Rc_Get.THROTTLE += Rx_Buf[buf_num][3];
  52.                 Rc_Get.YAW      = Rx_Buf[buf_num][4]  ;Rc_Get.YAW      <<= 8;Rc_Get.YAW      += Rx_Buf[buf_num][5];
  53.                         //判断解锁
  54.                 if(ARMED==0 && Rc_Get.THROTTLE < 1100 && Rc_Get.YAW <1100)
  55.                 {ARMED = 1;LED2_ON;}                //解锁
  56.                 if(ARMED==1 && Rc_Get.THROTTLE <1100 && Rc_Get.YAW >1900)
  57.                 {ARMED = 0;LED2_OFF;}                //锁定
  58.         }
  59.         else if(Rx_Buf[buf_num][1]==0x70)        //保存PID数据
  60.         {
  61.                         PID_ROL.P = (float)((vs16)(Rx_Buf[buf_num][2]<<8) |Rx_Buf[buf_num][3])/100;
  62.                         PID_ROL.I = (float)((vs16)(Rx_Buf[buf_num][4]<<8) |Rx_Buf[buf_num][5])/100;
  63.                         PID_ROL.D = (float)((vs16)(Rx_Buf[buf_num][6]<<8) |Rx_Buf[buf_num][7]) /10000;
  64.                         PID_PIT.P = (float)((vs16)(Rx_Buf[buf_num][8]<<8) |Rx_Buf[buf_num][9])/100;
  65.                         PID_PIT.I = (float)((vs16)(Rx_Buf[buf_num][10]<<8)|Rx_Buf[buf_num][11])/100;
  66.                         PID_PIT.D = (float)((vs16)(Rx_Buf[buf_num][12]<<8)|Rx_Buf[buf_num][13])/10000;
  67.                         PID_YAW.P = (float)((vs16)(Rx_Buf[buf_num][14]<<8)|Rx_Buf[buf_num][15])/100;
  68.                         PID_YAW.I = (float)((vs16)(Rx_Buf[buf_num][16]<<8)|Rx_Buf[buf_num][17])/100;
  69.                         PID_YAW.D = (float)((vs16)(Rx_Buf[buf_num][18]<<8)|Rx_Buf[buf_num][19])/10000;
  70.                         EE_SAVE_PID();       
  71.                         Uart1_Put_Char(0x71);//帧头       
  72.                         Uart1_Put_Char(0x88);//帧头
  73.                
  74.         }
  75.         else if(Rx_Buf[buf_num][1]==0x71)                //发送PID数据给上位机
  76.         {
  77.                 //反馈数据
  78.                 EE_READ_PID_Send();
  79.         }
  80. }
  81. void Uart_DataAnl(u8 buf_num)                //串口缓存数据分析
  82. {
  83. if(Rx_Buf[buf_num][1]==0x8A)                //串口收到的是上位机的遥控数据
  84.         {
  85.                 Uart1_Put_Char(0x30+buf_num);
  86.         }
  87. }
  88. void BlueToothCheckEvent(void)
  89. {
  90.         //0-9位 校验和
  91.         if(Rx_Ok0)
  92.         {
  93.                 Rx_Ok0 = 0;
  94. //                u8 sum = 0;
  95. //                for(int i=0;i<20;i++)
  96. //                        sum += Rx_Buf[0][i];
  97. //                if(sum == Rx_Buf[0][20])                //和校验通过
  98. //                {
  99.                         BlueToothDeal(0);
  100. //                }
  101.                
  102.         }
  103.         if(Rx_Ok1)
  104.         {
  105.                 Rx_Ok1 = 0;
  106. //                u8 sum = 0;
  107. //                for(int i=0;i<20;i++)
  108. //                        sum += Rx_Buf[1][i];
  109. //                if(sum == Rx_Buf[1][20])                //和校验通过
  110. //                {
  111.                         BlueToothDeal(1);
  112. //                }
  113.         }       
  114. }
  115. void Uart_CheckEvent(void)
  116. {
  117.         if(Rx_Ok0)
  118.         {
  119.                 Rx_Ok0 = 0;
  120.                 u8 sum = 0;
  121.                 for(int i=0;i<31;i++)
  122.                         sum += Rx_Buf[0][i];
  123.                 if(sum == Rx_Buf[0][31])                //和校验通过
  124.                 {
  125.                         Uart_DataAnl(0);
  126.                 }
  127.         }
  128.         if(Rx_Ok1)
  129.         {
  130.                 Rx_Ok1 = 0;
  131.                 u8 sum = 0;
  132.                 for(int i=0;i<31;i++)
  133.                         sum += Rx_Buf[1][i];
  134.                 if(sum == Rx_Buf[1][31])                //和校验通过
  135.                 {
  136.                         Uart_DataAnl(1);
  137.                 }
  138.         }
  139. }

  141. void Uart1_Send_Buf(u8 *buf,u8 len)                //发送buf,长度len,返回字节和sum
  142. {
  143.         while(len)
  144.         {
  145.                 Uart1_Put_Char(*buf);
  146.                 buf++;
  147.                 len--;
  148.         }
  149. }
  150. void Uart1_Send_RCdata(void)
  151. {
  152.         uint8_t sum = 0;
  153.         sum += Uart1_Put_Char(0x88);
  154.         sum += Uart1_Put_Char(0xAE);
  155.         sum += Uart1_Put_Char(28);
  156.         sum += Uart1_Put_Int16(Rc_Get.THROTTLE);
  157.         sum += Uart1_Put_Int16(Rc_Get.YAW);
  158.         sum += Uart1_Put_Int16(Rc_Get.ROLL);
  159.         sum += Uart1_Put_Int16(Rc_Get.PITCH);
  160.         sum += Uart1_Put_Int16(Rc_Get.AUX1);
  161.         sum += Uart1_Put_Int16(Rc_Get.AUX2);
  162.         sum += Uart1_Put_Int16(Rc_Get.AUX3);
  163.         sum += Uart1_Put_Int16(Rc_Get.AUX4);
  164.         sum += Uart1_Put_Int16(0);
  165.         sum += Uart1_Put_Int16(0);
  166.         sum += Uart1_Put_Int16(0);
  167.         sum += Uart1_Put_Int16(0);
  168.         sum += Uart1_Put_Int16(0);
  169.         sum += Uart1_Put_Int16(0);
  170.         Uart1_Put_Char(sum);
  171. }

  173. void PC_Debug_Show(u8 num,u16 sta)//sta=0 熄灭 sta=1 点亮  >1 取反
  174. {
  175.         static uint8_t led_s[6] = {0,0,0,0,0,0};
  176.         uint8_t sum = 0;
  177.         if(0<num && num<7)
  178.         {
  179.                 sum += Uart1_Put_Char(0x88);
  180.                 sum += Uart1_Put_Char(0xAD);
  181.                 sum += Uart1_Put_Char(0x02);
  182.                 sum += Uart1_Put_Char(num);
  183.                 if(sta==0)
  184.                         sum += Uart1_Put_Char(0x00);
  185.                 else if(sta==1)
  186.                         sum += Uart1_Put_Char(0x01);
  187.                 else
  188.                 {
  189.                         if(led_s[num])        led_s[num] = 0;
  190.                         else                         led_s[num] = 1;
  191.                         sum += Uart1_Put_Char(led_s[num]);
  192.                 }
  193.                 Uart1_Put_Char(sum);
  194.         }
  195.         else if(6<num && num<13)
  196.         {
  197.                 sum += Uart1_Put_Char(0x88);
  198.                 sum += Uart1_Put_Char(0xAD);
  199.                 sum += Uart1_Put_Char(0x03);
  200.                 sum += Uart1_Put_Char(num);
  201.                 sum += Uart1_Put_Int16(sta);
  202.                 Uart1_Put_Char(sum);
  203.         }
  204. }
  205. void Uart1_Send_AF(void)
  206. {
  207.         uint8_t sum = 0;
  208.         uint16_t _temp;
  209.         sum += Uart1_Put_Char(0x88);
  210.         sum += Uart1_Put_Char(0xAF);
  211.         sum += Uart1_Put_Char(0x1c);
  212.         sum += Uart1_Put_Char(BYTE1(MPU6050_ACC_LAST.X));
  213.         sum += Uart1_Put_Char(BYTE0(MPU6050_ACC_LAST.X));
  214.         sum += Uart1_Put_Char(BYTE1(MPU6050_ACC_LAST.Y));
  215.         sum += Uart1_Put_Char(BYTE0(MPU6050_ACC_LAST.Y));
  216.         sum += Uart1_Put_Char(BYTE1(MPU6050_ACC_LAST.Z));
  217.         sum += Uart1_Put_Char(BYTE0(MPU6050_ACC_LAST.Z));
  218.         sum += Uart1_Put_Char(BYTE1(MPU6050_GYRO_LAST.X));
  219.         sum += Uart1_Put_Char(BYTE0(MPU6050_GYRO_LAST.X));
  220.         sum += Uart1_Put_Char(BYTE1(MPU6050_GYRO_LAST.Y));
  221.         sum += Uart1_Put_Char(BYTE0(MPU6050_GYRO_LAST.Y));
  222.         sum += Uart1_Put_Char(BYTE1(MPU6050_GYRO_LAST.Z));
  223.         sum += Uart1_Put_Char(BYTE0(MPU6050_GYRO_LAST.Z));
  224.         sum += Uart1_Put_Char(0);
  225.         sum += Uart1_Put_Char(0);
  226.         sum += Uart1_Put_Char(0);
  227.         sum += Uart1_Put_Char(0);
  228.         sum += Uart1_Put_Char(0);
  229.         sum += Uart1_Put_Char(0);
  230.        
  231.         _temp = (int)(Q_ANGLE.X*100);
  232.         sum += Uart1_Put_Char(BYTE1(_temp));
  233.         sum += Uart1_Put_Char(BYTE0(_temp));
  234.         _temp = (int)(Q_ANGLE.Y*100);
  235.         sum += Uart1_Put_Char(BYTE1(_temp));
  236.         sum += Uart1_Put_Char(BYTE0(_temp));
  237.   _temp = (int)(Q_ANGLE.Z*100);
  238.         sum += Uart1_Put_Char(BYTE1(_temp));
  239.         sum += Uart1_Put_Char(BYTE0(_temp));
  240.        
  241.         //sum += Uart1_Put_Char(0);
  242.   //sum += Uart1_Put_Char(0);
  243.         sum += Uart1_Put_Char(0);
  244.         sum += Uart1_Put_Char(0);
  245.         sum += Uart1_Put_Char(0);
  246.         sum += Uart1_Put_Char(0);
  247.         Uart1_Put_Char(sum);
  248. }

  250. void Uart1_Send_AE(void)
  251. {
  252.         uint8_t sum=0;
  253.         uint16_t _temp;
  254.         sum = 0;       
  255.         sum =  Uart1_Put_Char(0x88);
  256.         sum += Uart1_Put_Char(0xAE);
  257.         sum += Uart1_Put_Char(0x1c);
  258.         sum += Uart1_Put_Char(0);
  259.         sum += Uart1_Put_Char(0);
  260.         sum += Uart1_Put_Char(0);
  261.         sum += Uart1_Put_Char(0);
  262.         sum += Uart1_Put_Char(0);
  263.         sum += Uart1_Put_Char(0);
  264.         sum += Uart1_Put_Char(0);
  265. ……………………

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

所有资料51hei提供下载:
bee_source.rar (386.55 KB, 下载次数: 13)


分享到:  QQ好友和群QQ好友和群 QQ空间QQ空间 腾讯微博腾讯微博 腾讯朋友腾讯朋友
收藏收藏 分享淘帖 顶顶 踩踩
回复

使用道具 举报

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

本版积分规则

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

Powered by 单片机教程网

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