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

 找回密码
 立即注册

QQ登录

只需一步，快速开始

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

搜索
»论坛 › 嵌入式/单片机论坛 › STM32/8 › 基于STM32F103的mini四轴源码
返回列表 发新帖
查看: 2300|回复: 0
打印 上一主题 下一主题
收起左侧

基于STM32F103的mini四轴源码

[复制链接]
跳转到指定楼层
楼主
ID:344541 发表于 2018-6-4 12:24 | 只看该作者 |只看大图 回帖奖励 |倒序浏览 |阅读模式
mpu6050作为姿态传感器
PID适用在mini四轴上 自己DIY后需要再调参数 大四轴正在试

所有资料51hei提供下载:
STM32F103四轴飞行器源码.rar .7z (283.09 KB, 下载次数: 34)


单片机源程序如下:
  1. #include "Maths.h"
  2. #include "Control.h"
  3. #include "struct_all.h"

  5. //RC
  6. uint8_t Rc_Lock=1;//1上锁；0解锁
  7. /******************************************************************************
  8. 函数原型：        void RC_Limit(struct _Rc *rc)
  9. 功    能：        限制遥控指令范围
  10. *******************************************************************************/
  11. void RC_Limit(struct _Rc *rc)
  12. {
  13.         rc->THROTTLE = (rc->THROTTLE<=1000)?1000:rc->THROTTLE;
  14.         rc->THROTTLE = (rc->THROTTLE>=2000)?2000:rc->THROTTLE;
  15.         rc->PITCH = (rc->PITCH<=1000)?1000:rc->PITCH;
  16.         rc->PITCH = (rc->PITCH>=2000)?2000:rc->PITCH;
  17.         rc->ROLL = (rc->ROLL<=1000)?1000:rc->ROLL;
  18.         rc->ROLL = (rc->ROLL>=2000)?2000:rc->ROLL;
  19.         rc->YAW  = (rc->YAW<=1000)?1000:rc->YAW;
  20.         rc->YAW  = (rc->YAW>=2000)?2000:rc->YAW;
  21.         rc->AUX1 = (rc->AUX1<=1000)?1000:rc->AUX1;
  22.         rc->AUX1 = (rc->AUX1>=2000)?2000:rc->AUX1;
  23.         rc->AUX2 = (rc->AUX2<=1000)?1000:rc->AUX2;
  24.         rc->AUX2 = (rc->AUX2>=2000)?2000:rc->AUX2;
  25.         rc->AUX3 = (rc->AUX3<=1000)?1000:rc->AUX3;
  26.         rc->AUX3 = (rc->AUX3>=2000)?2000:rc->AUX3;
  27. }
  28.         
  29. /******************************************************************************
  30. 函数原型：        void RC_LOCK(void)
  31. 功    能：        遥控手势指令及解锁处理（以下注释针对美国手，遥控默认的就是美国手）
  32. *******************************************************************************/
  33. void RC_LOCK(void)
  34. {
  35.         static uint8_t count0,count1,count2;
  36.         if(Rc.THROTTLE<1300 )//&& Rc.YAW>1700 && Rc.PITCH>1400 && Rc.PITCH<1600)
  37.                 count0++;
  38.         else
  39.                 count0 = 0;
  40.         
  41.         if(count0>15 && Rc_Lock==1)
  42.         {
  43.                 Rc_Lock = 0;//左手油门手的摇杆打向右下角，右手摇杆不动，解锁
  44.                 LED3_OFF;
  45.                 Delay_led(100);
  46.                 LED3_ON;
  47.                 Delay_led(100);
  48.         }
  49. ////////////////////////////////////////////////        
  50.         if(Rc.THROTTLE<1300 && Rc.YAW<1300 && Rc.PITCH>1400 && Rc.PITCH<1600)
  51.                 count1++;
  52.         else
  53.                 count1 = 0;
  54.         
  55.         if(count1>150 && Rc_Lock==0)
  56.         {
  57.                 Rc_Lock = 1;//左手油门手的摇杆打向左下角，右手摇杆不动，上锁
  58.                 LED3_OFF;
  59.                 Delay_led(100);
  60.                 LED3_ON;
  61.                 Delay_led(100);
  62.         }
  63. ////////////////////////////////////////////////        
  64.         if(Rc.THROTTLE<1300 && Rc.YAW<1300 && Rc.PITCH<1300)
  65.                 count2++;
  66.         else
  67.                 count2 = 0;
  68.         
  69.         if(count2>100)//&& Rc_Lock)//上锁状态才能校正
  70.         {
  71.                 count2=0;
  72.                 Do_GYRO_Offset();//左手油门手的摇杆打向左下角，右手摇杆俯仰方向拉到最底，校正陀螺仪
  73.                 Do_ACC_Offset(); //左手油门手的摇杆打向左下角，右手摇杆俯仰方向拉到最底，校正加速度计
  74.                
  75.                 LED3_OFF;
  76.                 Delay_led(50);
  77.                 LED3_ON;
  78.                 Delay_led(50);
  79.                 LED3_OFF;
  80.                 Delay_led(50);
  81.                 LED3_ON;
  82.         }
  83. }

  85. //PID
  86. #define angle_max                          10.0f        
  87. #define angle_integral_max         1000.0f        
  88. /******************************************************************************
  89. 函数原型：        void Control_Angle(struct _out_angle *angle,struct _Rc *rc)
  90. 功    能：        PID控制器(外环)
  91. *******************************************************************************/
  92. void Control_Angle(struct _out_angle *angle,struct _Rc *rc)
  93. {
  94.         static struct _out_angle control_angle;
  95.         static struct _out_angle last_angle;
  96. //////////////////////////////////////////////////////////////////
  97. //                        以下为角度环
  98. //////////////////////////////////////////////////////////////////
  99.         if(rc->ROLL>1490 && rc->ROLL<1510)        
  100.                 rc->ROLL=1500;
  101.         if(rc->PITCH>1490 && rc->PITCH<1510)        
  102.                 rc->PITCH=1500;
  103. //////////////////////////////////////////////////////////////////
  104.         if(rc->AUX1>1495 && rc->AUX1<1505)        
  105.                 rc->AUX1=1500;
  106.         if(rc->AUX2>1495 && rc->AUX2<1505)        
  107.                 rc->AUX2=1500;
  108. //////////////////////////////////////////////////////////////////
  109.         control_angle.roll  = angle->roll  - (rc->ROLL  -1500)/13.0f + (rc->AUX2 -1500)/100.0f;
  110.         control_angle.pitch = angle->pitch - (rc->PITCH -1500)/13.0f - (rc->AUX1 -1500)/100.0f;
  111. //////////////////////////////////////////////////////////////////
  112.         if(control_angle.roll >  angle_max)        //ROLL
  113.                 roll.integral +=  angle_max;
  114.         if(control_angle.roll < -angle_max)
  115.                 roll.integral += -angle_max;
  116.         else
  117.                 roll.integral += control_angle.roll;
  118.         
  119.         if(roll.integral >  angle_integral_max)
  120.            roll.integral =  angle_integral_max;
  121.         if(roll.integral < -angle_integral_max)
  122.            roll.integral = -angle_integral_max;
  123. //////////////////////////////////////////////////////////////////
  124.         if(control_angle.pitch >  angle_max)//PITCH
  125.            pitch.integral +=  angle_max;
  126.         if(control_angle.pitch < -angle_max)
  127.            pitch.integral += -angle_max;
  128.         else
  129.                 pitch.integral += control_angle.pitch;

  131.         if(pitch.integral >  angle_integral_max)
  132.            pitch.integral =  angle_integral_max;
  133.         if(pitch.integral < -angle_integral_max)
  134.            pitch.integral = -angle_integral_max;
  135. //////////////////////////////////////////////////////////////////
  136.         if(rc->THROTTLE<1200)//油门较小时，积分清零
  137.         {
  138.                 roll.integral  = 0;
  139.                 pitch.integral = 0;
  140.         }
  141. //////////////////////////////////////////////////////////////////
  142.         roll.output  = roll.kp *control_angle.roll  + roll.ki *roll.integral  + roll.kd *(control_angle.roll -last_angle.roll );
  143.         pitch.output = pitch.kp*control_angle.pitch + pitch.ki*pitch.integral + pitch.kd*(control_angle.pitch-last_angle.pitch);
  144. //////////////////////////////////////////////////////////////////
  145.         last_angle.roll =control_angle.roll;
  146.         last_angle.pitch=control_angle.pitch;
  147. }

  149. #define gyro_max                          50.0f        
  150. #define gyro_integral_max         5000.0f
  151. /******************************************************************************
  152. 函数原型：        void Control_Gyro(struct _SI_float *gyro,struct _Rc *rc,uint8_t Lock)
  153. 功    能：        PID控制器(内环)
  154. *******************************************************************************/
  155. void Control_Gyro(struct _SI_float *gyro,struct _Rc *rc,uint8_t Lock)
  156. {
  157.         static struct _out_angle control_gyro;
  158.         static struct _out_angle last_gyro;
  159.         int16_t throttle1,throttle2,throttle3,throttle4;
  160. //////////////////////////////////////////////////////////////////
  161. //                        以下为角速度环
  162. //////////////////////////////////////////////////////////////////
  163.         if(rc->YAW>1400 && rc->YAW<1600)
  164.                 rc->YAW=1500;
  165.         if(rc->AUX3>1495 && rc->AUX3<1505)        
  166.                 rc->AUX3=1500;
  167. //////////////////////////////////////////////////////////////////
  168.         control_gyro.roll  = -roll.output - gyro->y*Radian_to_Angle;
  169.         control_gyro.pitch = pitch.output - gyro->x*Radian_to_Angle;
  170.         if(rc->AUX4 & Lock_Mode)
  171.                 control_gyro.yaw   = - gyro->z*Radian_to_Angle - (rc->AUX3 -1500)/100.0f;//锁尾模式
  172.         else
  173.                 control_gyro.yaw   = -(rc->YAW-1500)/2.0f - gyro->z*Radian_to_Angle + (rc->AUX3 -1500)/50.0f;//非锁尾模式
  174. //////////////////////////////////////////////////////////////////
  175.         if(control_gyro.roll >  gyro_max)        //GYRO_ROLL
  176.                 gyro_roll.integral +=  gyro_max;
  177.         if(control_gyro.roll < -gyro_max)
  178.                 gyro_roll.integral += -gyro_max;
  179.         else
  180.                 gyro_roll.integral += control_gyro.roll;
  181.         
  182.         if(gyro_roll.integral >  gyro_integral_max)
  183.            gyro_roll.integral =  gyro_integral_max;
  184.         if(gyro_roll.integral < -gyro_integral_max)
  185.            gyro_roll.integral = -gyro_integral_max;
  186. //////////////////////////////////////////////////////////////////
  187.         if(control_gyro.pitch >  gyro_max)//GYRO_PITCH
  188.                 gyro_pitch.integral +=  gyro_max;
  189.         if(control_gyro.pitch < -gyro_max)
  190.                 gyro_pitch.integral += -gyro_max;
  191.         else
  192.                 gyro_pitch.integral += control_gyro.pitch;
  193.         
  194.         if(gyro_pitch.integral >  gyro_integral_max)
  195.            gyro_pitch.integral =  gyro_integral_max;
  196.         if(gyro_pitch.integral < -gyro_integral_max)
  197.            gyro_pitch.integral = -gyro_integral_max;
  198. //////////////////////////////////////////////////////////////////
  199. //        if(control_gyro.yaw >  gyro_max)//GYRO_YAW
  200. //                gyro_yaw.integral +=  gyro_max;
  201. //        if(control_gyro.yaw < -gyro_max)
  202. //                gyro_yaw.integral += -gyro_max;
  203. //        else
  204.                 gyro_yaw.integral += control_gyro.yaw;
  205.         
  206.         if(gyro_yaw.integral >  gyro_integral_max)
  207.            gyro_yaw.integral =  gyro_integral_max;
  208.         if(gyro_yaw.integral < -gyro_integral_max)
  209.            gyro_yaw.integral = -gyro_integral_max;
  210. //////////////////////////////////////////////////////////////////
  211.         if(rc->THROTTLE<1200)//油门较小时，积分清零
  212.         {
  213.                 gyro_yaw.integral  = 0;
  214. ……………………

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



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

使用道具 举报

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

本版积分规则

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

Powered by 单片机教程网

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