单片机论坛

 找回密码
 立即注册

QQ登录

只需一步,快速开始

搜索
查看: 1598|回复: 1
收起左侧

匿名四轴上位机和飞控经典STM32代码分享

[复制链接]
苏相彬 发表于 2018-10-24 11:57 | 显示全部楼层 |阅读模式
2017最新整理匿名四轴上位机和飞控经典代码分享
0.png 0.png

ANO-MR-F1-140104经典pid

stm32单片机源程序如下:
  1. /* Includes ------------------------------------------------------------------*/
  2. #include "stm32f10x.h"
  3. #include "sysconfig.h"
  4. #include "bsp.h"
  5. #include "led.h"
  6. #include "tim3.h"       
  7. #include "usart.h"
  8. #include "ANO_TC_STM32F1_I2C.h"
  9. #include "MPU6050.h"
  10. #include "moto.h"
  11. #include "spi.h"
  12. #include "nrf24l01.h"
  13. #include "tim_pwm_in.h"
  14. #include "rc.h"
  15. #include "imu.h"
  16. #include "control.h"
  17. #include "data_transfer.h"

  18. u8 SYS_INIT_OK=0;
  19. ////////////////////////////////////////////////////////////////////////////////
  20. void SYS_INIT(void)
  21. {
  22.         LED_INIT();
  23.         LED_FLASH();
  24.         Moto_Init();
  25.         Uart1_Init(115200);       
  26.         Tim3_Init(500);
  27. #ifdef CONTROL_USE_RC
  28.         Tim_Pwm_In_Init();
  29. #endif
  30.         Nvic_Init();
  31.         ANO_TC_I2C2_INIT(0xA6,400000,1,1,3,3);
  32.         MPU6050_Init();
  33.        
  34.         Spi1_Init();
  35.         Nrf24l01_Init(MODEL_TX2,40);
  36.         if(Nrf24l01_Check())       
  37.                 Uart1_Put_String("NRF24L01 IS OK !\r\n");
  38.         else                                                                        
  39.                 Uart1_Put_String("NRF24L01 IS NOT OK !\r\n");
  40.                
  41.         FLASH_Unlock();
  42.         EE_INIT();
  43.         EE_READ_ACC_OFFSET();
  44.         EE_READ_GYRO_OFFSET();
  45.         EE_READ_PID();
  46.        
  47.         Tim3_Control(1);
  48. }
  49. ////////////////////////////////////////////////////////////////////////////////
  50. u8 FLAG_ATT=0;
  51. T_int16_xyz                 Acc,Gyr;        //两次综合后的传感器数据
  52. T_int16_xyz                        Acc_AVG;
  53. T_float_angle                 Att_Angle;        //ATT函数计算出的姿态角
  54. vs32                                Alt;
  55. T_RC_Data                         Rc_D;                //遥控通道数据
  56. T_RC_Control                Rc_C;                //遥控功能数据
  57. int main(void)
  58. {
  59.         static u8 att_cnt=0;
  60.         static u8 rc_cnt=0;
  61.         static T_int16_xyz mpu6050_dataacc1,mpu6050_dataacc2,mpu6050_datagyr1,mpu6050_datagyr2;
  62.         static u8 senser_cnt=0,status_cnt=0,dt_rc_cnt=0,dt_moto_cnt=0;
  63.        
  64.         SYS_INIT();
  65.        
  66.         while (1)
  67.         {                             
  68.                 if(FLAG_ATT)
  69.                 {
  70.                         FLAG_ATT = 0;
  71.                         att_cnt++;
  72.                         rc_cnt++;
  73.                        
  74.                         if(rc_cnt==20)
  75.                         {
  76.                                 rc_cnt = 0;
  77.                                 #ifdef CONTROL_USE_RC
  78.                                 Rc_GetValue(&Rc_D);
  79.                                 #endif
  80.                                 Rc_Fun(&Rc_D,&Rc_C);
  81.                         }
  82.                         if(att_cnt==1)
  83.                                 MPU6050_Dataanl(&mpu6050_dataacc1,&mpu6050_datagyr1);
  84.                         else
  85.                         {
  86.                                 att_cnt = 0;
  87.                                 MPU6050_Dataanl(&mpu6050_dataacc2,&mpu6050_datagyr2);
  88.                                 Acc.X = (mpu6050_dataacc1.X+mpu6050_dataacc2.X)/2;
  89.                                 Acc.Y = (mpu6050_dataacc1.Y+mpu6050_dataacc2.Y)/2;
  90.                                 Acc.Z = (mpu6050_dataacc1.Z+mpu6050_dataacc2.Z)/2;
  91.                                 Gyr.X = (mpu6050_datagyr1.X+mpu6050_datagyr2.X)/2;
  92.                                 Gyr.Y = (mpu6050_datagyr1.Y+mpu6050_datagyr2.Y)/2;
  93.                                 Gyr.Z = (mpu6050_datagyr1.Z+mpu6050_datagyr2.Z)/2;
  94.                                 Prepare_Data(&Acc,&Acc_AVG);
  95.                                 IMUupdate(&Gyr,&Acc_AVG,&Att_Angle);
  96.                                 Control(&Att_Angle,&Gyr,&Rc_D,Rc_C.ARMED);
  97.                                 if(Rc_C.ARMED)
  98.                                         LED1_ONOFF();
  99.                                 else
  100.                                         LED1_OFF;
  101.                                        
  102.                                 senser_cnt++;
  103.                                 status_cnt++;
  104.                                 dt_rc_cnt++;
  105.                                 dt_moto_cnt++;
  106.                                 if(senser_cnt==5)
  107.                                 {
  108.                                         senser_cnt = 0;
  109.                                         Send_Senser = 1;
  110.                                 }
  111.                                 if(status_cnt==5)
  112.                                 {
  113.                                         status_cnt = 0;
  114.                                         Send_Status = 1;
  115.                                 }
  116.                                 if(dt_rc_cnt==10)
  117.                                 {
  118.                                         dt_rc_cnt=0;
  119.                                         Send_RCData = 1;
  120.                                 }
  121.                                 if(dt_moto_cnt==10)
  122.                                 {
  123.                                         dt_moto_cnt=0;
  124.                                         Send_MotoPwm = 1;
  125.                                 }
  126.                         }
  127.                 }
  128.         }
  129. }
  130. ////////////////////////////////////////////////////////////////////////////////
复制代码
  1. #include "control.h"

  2. PID PID_ROL,PID_PIT,PID_YAW,PID_ALT,PID_POS,PID_PID_1,PID_PID_2;

  3. int16_t getlast_roll=0,geilast_pitch=0;
  4. float rol_i=0,pit_i=0,yaw_p=0;

  5. vs16 Moto_PWM_1=0,Moto_PWM_2=0,Moto_PWM_3=0,Moto_PWM_4=0,Moto_PWM_5=0,Moto_PWM_6=0,Moto_PWM_7=0,Moto_PWM_8=0;

  6. void Control(T_float_angle *att_in,T_int16_xyz *gyr_in, T_RC_Data *rc_in, u8 armed)
  7. {
  8.         T_float_angle angle;
  9.         angle.rol = att_in->rol - (rc_in->ROLL-1500)/12;
  10.         angle.pit = att_in->pit + (rc_in->PITCH-1500)/12;
  11.        
  12.         rol_i += angle.rol;
  13.         if(rol_i>2000)
  14.         rol_i=2000;
  15.         if(rol_i<-2000)
  16.         rol_i=-2000;

  17.         PID_ROL.pout = PID_ROL.P * angle.rol;
  18.         PID_ROL.dout = -PID_ROL.D * gyr_in->Y;
  19.         PID_ROL.iout = PID_ROL.I * PID_ROL.dout;

  20.         pit_i += angle.pit;
  21.         if(pit_i>2000)
  22.         pit_i=2000;
  23.         if(pit_i<-2000)
  24.         pit_i=-2000;

  25.         PID_PIT.pout = PID_PIT.P * angle.pit;
  26.         PID_PIT.dout = PID_PIT.D * gyr_in->X;
  27.         PID_PIT.iout = PID_PIT.I * pit_i;
  28.         if(rc_in->YAW<1400||rc_in->YAW>1600)
  29.         {gyr_in->Z=gyr_in->Z+(rc_in->YAW-1500)*2;}
  30.         yaw_p+=gyr_in->Z*0.0609756f*0.002f;// +(Rc_Get.YAW-1500)*30
  31.         if(yaw_p>20)
  32.                 yaw_p=20;
  33.         if(yaw_p<-20)
  34.                 yaw_p=-20;


  35.         PID_YAW.pout=PID_YAW.P*yaw_p;
  36.         PID_YAW.dout = PID_YAW.D * gyr_in->Z;                                  
  37.        
  38.         if(rc_in->THROTTLE<1200)
  39.         {               
  40.                 pit_i=0;
  41.                 rol_i=0;
  42.                 yaw_p=0;
  43.         }

  44.         PID_ROL.OUT =  (-PID_ROL.pout)-PID_ROL.iout +PID_ROL.dout;//
  45.         PID_PIT.OUT = PID_PIT.pout + PID_PIT.iout + PID_PIT.dout;
  46.         PID_YAW.OUT = PID_YAW.pout + PID_YAW.iout + PID_YAW.dout;

  47.         if(rc_in->THROTTLE>1200&&armed)
  48.         {
  49.                 Moto_PWM_1 = rc_in->THROTTLE - 1000 - PID_ROL.OUT - PID_PIT.OUT + PID_YAW.OUT;
  50.                 Moto_PWM_2 = rc_in->THROTTLE - 1000 + PID_ROL.OUT - PID_PIT.OUT - PID_YAW.OUT;
  51.                 Moto_PWM_3 = rc_in->THROTTLE - 1000 + PID_ROL.OUT + PID_PIT.OUT + PID_YAW.OUT;
  52.                 Moto_PWM_4 = rc_in->THROTTLE - 1000 - PID_ROL.OUT + PID_PIT.OUT - PID_YAW.OUT;
  53.         }
  54.         else
  55.         {
  56.                 Moto_PWM_1 = 0;
  57.                 Moto_PWM_2 = 0;
  58.                 Moto_PWM_3 = 0;
  59.                 Moto_PWM_4 = 0;
  60.         }
  61.         Moto_PwmRflash(Moto_PWM_1,Moto_PWM_2,Moto_PWM_3,Moto_PWM_4);
  62. }
复制代码

所有资料51hei提供下载:
2017最新整理匿名四轴上位机和飞控经典代码分享.rar (4.84 MB, 下载次数: 51)
回复

使用道具 举报

gzyy2005 发表于 2019-2-14 21:32 | 显示全部楼层
谢谢分享

回复

使用道具 举报

您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

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

Powered by 单片机教程网

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