久久久久久久999_99精品久久精品一区二区爱城_成人欧美一区二区三区在线播放_国产精品日本一区二区不卡视频_国产午夜视频_欧美精品在线观看免费

標題: 基于STM32F103的mini四軸源碼 [打印本頁]
作者: BoykaC    時間: 2018-6-4 12:24
標題: 基于STM32F103的mini四軸源碼
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黑下載附件…………
復制代碼







歡迎光臨 (http://m.zg4o1577.cn/bbs/) Powered by Discuz! X3.1
主站蜘蛛池模板： 中文字幕视频一区 | 狠狠躁日日躁夜夜躁2022麻豆 | 91高清国产 | 91狠狠综合 | 日本伊人 | 91午夜精品亚洲一区二区三区 | 天天干天天操天天插 | 91av在线看| av福利在线 | 国产成人在线视频 | 日韩亚洲欧美在线 | 久久夜视频| 老女人性生活视频 | 日韩欧美大片 | 国产精品成人一区二区三区 | 欧美日韩国产三级 | 久久在线免费观看 | 久久精品一区二区三区四区 | 欧美性色网 | 国产成人精品一区二区三区在线 | 黄色裸体视频 | 久久精品视频一区二区 | 久久男人天堂 | 亚洲在线播放 | 色一情一乱一乱一区91av | 手机av网站| 在线播放a| 影音先锋中文字幕在线 | 久久久成人精品 | 亚洲福利影院 | 国产一区二区三区视频在线 | 国产主播av | 情侣av | 九色av | 久久久久久免费 | 久久精品欧美一区二区三区不卡 | 成人欧美一区二区三区黑人免费 | 羞羞的视频网站 | 久久99精品久久久久久琪琪 | 三年中文在线看免费观看 | 国产三级做爰高清在线 |