找回密碼
 立即注冊

QQ登錄

只需一步，快速開始

搜索
查看: 2783|回復(fù): 0
打印 上一主題 下一主題
收起左側(cè)

基于STM32F103的mini四軸源碼

[復(fù)制鏈接]
跳轉(zhuǎn)到指定樓層
樓主
mpu6050作為姿態(tài)傳感器
PID適用在mini四軸上 自己DIY后需要再調(diào)參數(shù) 大四軸正在試

所有資料51hei提供下載:
STM32F103四軸飛行器源碼.rar .7z (283.09 KB, 下載次數(shù): 34)


單片機(jī)源程序如下:
  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. 函數(shù)原型：        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. 函數(shù)原型：        void RC_LOCK(void)
  31. 功    能：        遙控手勢指令及解鎖處理（以下注釋針對美國手，遙控默認(rèn)的就是美國手）
  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;//左手油門手的搖桿打向右下角，右手搖桿不動(dòng)，解鎖
  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;//左手油門手的搖桿打向左下角，右手搖桿不動(dòng)，上鎖
  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)//上鎖狀態(tài)才能校正
  70.         {
  71.                 count2=0;
  72.                 Do_GYRO_Offset();//左手油門手的搖桿打向左下角，右手搖桿俯仰方向拉到最底，校正陀螺儀
  73.                 Do_ACC_Offset(); //左手油門手的搖桿打向左下角，右手搖桿俯仰方向拉到最底，校正加速度計(jì)
  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. 函數(shù)原型：        void Control_Angle(struct _out_angle *angle,struct _Rc *rc)
  90. 功    能：        PID控制器(外環(huán))
  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. //                        以下為角度環(huán)
  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)//油門較小時(shí)，積分清零
  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. 函數(shù)原型：        void Control_Gyro(struct _SI_float *gyro,struct _Rc *rc,uint8_t Lock)
  153. 功    能：        PID控制器(內(nèi)環(huán))
  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. //                        以下為角速度環(huán)
  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)//油門較小時(shí)，積分清零
  212.         {
  213.                 gyro_yaw.integral  = 0;
  214. ……………………

  216. …………限于本文篇幅 余下代碼請從51黑下載附件…………
復(fù)制代碼



分享到:  QQ好友和群QQ好友和群 QQ空間QQ空間 騰訊微博騰訊微博 騰訊朋友騰訊朋友
收藏收藏1 分享淘帖 頂頂 踩踩
回復(fù)

使用道具 舉報(bào)

您需要登錄后才可以回帖 登錄 | 立即注冊

本版積分規(guī)則

手機(jī)版|小黑屋|51黑電子論壇 |51黑電子論壇6群 QQ 管理員QQ:125739409;技術(shù)交流QQ群281945664

Powered by 單片機(jī)教程網(wǎng)

快速回復(fù) 返回頂部 返回列表