北京快3玩法窍门

单片机论坛

 找回密码
 立即注册

QQ登录

只需一步,快速开始

搜索
查看: 1607|回复: 3
收起左侧

自制MWC飞控2.3固件Arduino源码

[复制链接]
腾飞的龙 发表于 2019-1-14 15:19 | 显示全部楼层 |阅读模式
好用的固件
0.png
  1. #include "Arduino.h"
  2. #include "config.h"
  3. #include "def.h"
  4. #include "types.h"
  5. #include "MultiWii.h"
  6. #include "IMU.h"
  7. #include "Sensors.h"

  8. void getEstimatedAttitude();

  9. void computeIMU () {
  10.   uint8_t axis;
  11.   static int16_t gyroADCprevious[3] = {0,0,0};
  12.   int16_t gyroADCp[3];
  13.   int16_t gyroADCinter[3];
  14.   static uint32_t timeInterleave = 0;

  15.   //we separate the 2 situations because reading gyro values with a gyro only setup can be acchieved at a higher rate
  16.   //gyro+nunchuk: we must wait for a quite high delay betwwen 2 reads to get both WM+ and Nunchuk data. It works with 3ms
  17.   //gyro only: the delay to read 2 consecutive values can be reduced to only 0.65ms
  18.   #if defined(NUNCHUCK)
  19.     annexCode();
  20.     while((uint16_t)(micros()-timeInterleave)<INTERLEAVING_DELAY) ; //interleaving delay between 2 consecutive reads
  21.     timeInterleave=micros();
  22.     ACC_getADC();
  23.     getEstimatedAttitude(); // computation time must last less than one interleaving delay
  24.     while((uint16_t)(micros()-timeInterleave)<INTERLEAVING_DELAY) ; //interleaving delay between 2 consecutive reads
  25.     timeInterleave=micros();
  26.     f.NUNCHUKDATA = 1;
  27.     while(f.NUNCHUKDATA) ACC_getADC(); // For this interleaving reading, we must have a gyro update at this point (less delay)

  28.     for (axis = 0; axis < 3; axis++) {
  29.       // empirical, we take a weighted value of the current and the previous values
  30.       // /4 is to average 4 values, note: overflow is not possible for WMP gyro here
  31.       imu.gyroData[axis] = (imu.gyroADC[axis]*3+gyroADCprevious[axis])>>2;
  32.       gyroADCprevious[axis] = imu.gyroADC[axis];
  33.     }
  34.   #else
  35.     #if ACC
  36.       ACC_getADC();
  37.       getEstimatedAttitude();
  38.     #endif
  39.     #if GYRO
  40.       Gyro_getADC();
  41.     #endif
  42.     for (axis = 0; axis < 3; axis++)
  43.       gyroADCp[axis] =  imu.gyroADC[axis];
  44.     timeInterleave=micros();
  45.     annexCode();
  46.     uint8_t t=0;
  47.     while((uint16_t)(micros()-timeInterleave)<650) t=1; //empirical, interleaving delay between 2 consecutive reads
  48.     if (!t) annex650_overrun_count++;
  49.     #if GYRO
  50.       Gyro_getADC();
  51.     #endif
  52.     for (axis = 0; axis < 3; axis++) {
  53.       gyroADCinter[axis] =  imu.gyroADC[axis]+gyroADCp[axis];
  54.       // empirical, we take a weighted value of the current and the previous values
  55.       imu.gyroData[axis] = (gyroADCinter[axis]+gyroADCprevious[axis])/3;
  56.       gyroADCprevious[axis] = gyroADCinter[axis]>>1;
  57.       if (!ACC) imu.accADC[axis]=0;
  58.     }
  59.   #endif
  60.   #if defined(GYRO_SMOOTHING)
  61.     static int16_t gyroSmooth[3] = {0,0,0};
  62.     for (axis = 0; axis < 3; axis++) {
  63.       imu.gyroData[axis] = (int16_t) ( ( (int32_t)((int32_t)gyroSmooth[axis] * (conf.Smoothing[axis]-1) )+imu.gyroData[axis]+1 ) / conf.Smoothing[axis]);
  64.       gyroSmooth[axis] = imu.gyroData[axis];
  65.     }
  66.   #elif defined(TRI)
  67.     static int16_t gyroYawSmooth = 0;
  68.     imu.gyroData[YAW] = (gyroYawSmooth*2+imu.gyroData[YAW])/3;
  69.     gyroYawSmooth = imu.gyroData[YAW];
  70.   #endif
  71. }

  72. // **************************************************
  73. // Simplified IMU based on "Complementary Filter"
  74. // Inspired by http://starlino.com/imu_guide.html
  75. //
  76. // adapted by ziss_dm : http://www.multiwii.com/forum/viewtopic.php?f=8&t=198
  77. //
  78. // The following ideas was used in this project:
  79. // 1) Rotation matrix: http://en.wikipedia.org/wiki/Rotation_matrix
  80. // 2) Small-angle approximation: http://en.wikipedia.org/wiki/Small-angle_approximation
  81. // 3) C. Hastings approximation for atan2()
  82. // 4) Optimization tricks: http://www.hackersdelight.org/
  83. //
  84. // Currently Magnetometer uses separate CF which is used only
  85. // for heading approximation.
  86. //
  87. // **************************************************

  88. //******  advanced users settings *******************
  89. /* Set the Low Pass Filter factor for ACC
  90.    Increasing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time
  91.    Comment this if  you do not want filter at all.
  92.    unit = n power of 2 */
  93. // this one is also used for ALT HOLD calculation, should not be changed
  94. #ifndef ACC_LPF_FACTOR
  95.   #define ACC_LPF_FACTOR 4 // that means a LPF of 16
  96. #endif

  97. /* Set the Gyro Weight for Gyro/Acc complementary filter
  98.    Increasing this value would reduce and delay Acc influence on the output of the filter*/
  99. #ifndef GYR_CMPF_FACTOR
  100.   #define GYR_CMPF_FACTOR 600
  101. #endif

  102. /* Set the Gyro Weight for Gyro/Magnetometer complementary filter
  103.    Increasing this value would reduce and delay Magnetometer influence on the output of the filter*/
  104. #define GYR_CMPFM_FACTOR 250

  105. //****** end of advanced users settings *************
  106. #define INV_GYR_CMPF_FACTOR   (1.0f / (GYR_CMPF_FACTOR  + 1.0f))
  107. #define INV_GYR_CMPFM_FACTOR  (1.0f / (GYR_CMPFM_FACTOR + 1.0f))

  108. typedef struct fp_vector {               
  109.   float X,Y,Z;               
  110. } t_fp_vector_def;

  111. typedef union {               
  112.   float A[3];               
  113.   t_fp_vector_def V;               
  114. } t_fp_vector;

  115. typedef struct int32_t_vector {
  116.   int32_t X,Y,Z;
  117. } t_int32_t_vector_def;

  118. typedef union {
  119.   int32_t A[3];
  120.   t_int32_t_vector_def V;
  121. } t_int32_t_vector;

  122. int16_t _atan2(int32_t y, int32_t x){
  123.   float z = (float)y / x;
  124.   int16_t a;
  125.   if ( abs(y) < abs(x) ){
  126.      a = 573 * z / (1.0f + 0.28f * z * z);
  127.    if (x<0) {
  128.      if (y<0) a -= 1800;
  129.      else a += 1800;
  130.    }
  131.   } else {
  132.    a = 900 - 573 * z / (z * z + 0.28f);
  133.    if (y<0) a -= 1800;
  134.   }
  135.   return a;
  136. }

  137. float InvSqrt (float x){
  138.   union{  
  139.     int32_t i;  
  140.     float   f;
  141.   } conv;
  142.   conv.f = x;
  143.   conv.i = 0x5f3759df - (conv.i >> 1);
  144.   return 0.5f * conv.f * (3.0f - x * conv.f * conv.f);
  145. }

  146. // Rotate Estimated vector(s) with small angle approximation, according to the gyro data
  147. void rotateV(struct fp_vector *v,float* delta) {
  148.   fp_vector v_tmp = *v;
  149.   v->Z -= delta[ROLL]  * v_tmp.X + delta[PITCH] * v_tmp.Y;
  150.   v->X += delta[ROLL]  * v_tmp.Z - delta[YAW]   * v_tmp.Y;
  151.   v->Y += delta[PITCH] * v_tmp.Z + delta[YAW]   * v_tmp.X;
  152. }


  153. static int32_t accLPF32[3]    = {0, 0, 1};
  154. static float invG; // 1/|G|

  155. static t_fp_vector EstG;
  156. static t_int32_t_vector EstG32;
  157. #if MAG
  158.   static t_int32_t_vector EstM32;
  159.   static t_fp_vector EstM;
  160. #endif

  161. void getEstimatedAttitude(){
  162.   uint8_t axis;
  163.   int32_t accMag = 0;
  164.   float scale, deltaGyroAngle[3];
  165.   uint8_t validAcc;
  166.   static uint16_t previousT;
  167.   uint16_t currentT = micros();

  168.   scale = (currentT - previousT) * GYRO_SCALE; // GYRO_SCALE unit: radian/microsecond
  169.   previousT = currentT;

  170.   // Initialization
  171.   for (axis = 0; axis < 3; axis++) {
  172.     deltaGyroAngle[axis] = imu.gyroADC[axis]  * scale; // radian

  173.     accLPF32[axis]    -= accLPF32[axis]>>ACC_LPF_FACTOR;
  174.     accLPF32[axis]    += imu.accADC[axis];
  175.     imu.accSmooth[axis]    = accLPF32[axis]>>ACC_LPF_FACTOR;

  176.     accMag += (int32_t)imu.accSmooth[axis]*imu.accSmooth[axis] ;
  177.   }

  178.   rotateV(&EstG.V,deltaGyroAngle);
  179.   #if MAG
  180.     rotateV(&EstM.V,deltaGyroAngle);
  181.   #endif

  182.   accMag = accMag*100/((int32_t)ACC_1G*ACC_1G);
  183.   validAcc = 72 < (uint16_t)accMag && (uint16_t)accMag < 133;
  184.   // Apply complimentary filter (Gyro drift correction)
  185.   // If accel magnitude >1.15G or <0.85G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation.
  186.   // To do that, we just skip filter, as EstV already rotated by Gyro
  187.   for (axis = 0; axis < 3; axis++) {
  188.     if ( validAcc )
  189.       EstG.A[axis] = (EstG.A[axis] * GYR_CMPF_FACTOR + imu.accSmooth[axis]) * INV_GYR_CMPF_FACTOR;
  190.     EstG32.A[axis] = EstG.A[axis]; //int32_t cross calculation is a little bit faster than float       
  191.     #if MAG
  192.       EstM.A[axis] = (EstM.A[axis] * GYR_CMPFM_FACTOR  + imu.magADC[axis]) * INV_GYR_CMPFM_FACTOR;
  193.       EstM32.A[axis] = EstM.A[axis];
  194.     #endif
  195.   }
  196.   
  197.   if ((int16_t)EstG32.A[2] > ACCZ_25deg)
  198.     f.SMALL_ANGLES_25 = 1;
  199.   else
  200.     f.SMALL_ANGLES_25 = 0;

  201.   // Attitude of the estimated vector
  202.   int32_t sqGX_sqGZ = sq(EstG32.V.X) + sq(EstG32.V.Z);
  203.   invG = InvSqrt(sqGX_sqGZ + sq(EstG32.V.Y));
  204.   att.angle[ROLL]  = _atan2(EstG32.V.X , EstG32.V.Z);
  205.   att.angle[PITCH] = _atan2(EstG32.V.Y , InvSqrt(sqGX_sqGZ)*sqGX_sqGZ);

  206.   #if MAG
  207.     att.heading = _atan2(
  208.       EstM32.V.Z * EstG32.V.X - EstM32.V.X * EstG32.V.Z,
  209.       (EstM.V.Y * sqGX_sqGZ  - (EstM32.V.X * EstG32.V.X + EstM32.V.Z * EstG32.V.Z) * EstG.V.Y)*invG );
  210.     att.heading += conf.mag_declination; // Set from GUI
  211.     att.heading /= 10;
  212.   #endif

  213.   #if defined(THROTTLE_ANGLE_CORRECTION)
  214.     cosZ = EstG.V.Z / ACC_1G * 100.0f;                                                        // cos(angleZ) * 100
  215.     throttleAngleCorrection = THROTTLE_ANGLE_CORRECTION * constrain(100 - cosZ, 0, 100) >>3;  // 16 bit ok: 200*150 = 30000  
  216.   #endif
  217. }

  218. #define UPDATE_INTERVAL 25000    // 40hz update rate (20hz LPF on acc)
  219. #define BARO_TAB_SIZE   21

  220. #define ACC_Z_DEADBAND (ACC_1G>>5) // was 40 instead of 32 now


  221. #define applyDeadband(value, deadband)  \
  222.   if(abs(value) < deadband) {           \
  223.     value = 0;                          \
  224.   } else if(value > 0){                 \
  225.     value -= deadband;                  \
  226.   } else if(value < 0){                 \
  227.     value += deadband;                  \
  228.   }

  229. #if BARO
  230. uint8_t getEstimatedAltitude(){
  231.   int32_t  BaroAlt;
  232.   static float baroGroundTemperatureScale,logBaroGroundPressureSum;
  233.   static float vel = 0.0f;
  234.   static uint16_t previousT;
  235.   uint16_t currentT = micros();
  236.   uint16_t dTime;

  237.   dTime = currentT - previousT;
  238.   if (dTime < UPDATE_INTERVAL) return 0;
  239.   previousT = currentT;

  240.   if(calibratingB > 0) {
  241.     logBaroGroundPressureSum = log(baroPressureSum);
  242.     baroGroundTemperatureScale = (baroTemperature + 27315) *  29.271267f;
  243.     calibratingB--;
  244.   }

  245.   // baroGroundPressureSum is not supposed to be 0 here
  246.   // see: https://code.google.com/p/ardupilot-mega/source/browse/libraries/AP_Baro/AP_Baro.cpp
  247.   BaroAlt = ( logBaroGroundPressureSum - log(baroPressureSum) ) * baroGroundTemperatureScale;

  248.   alt.EstAlt = (alt.EstAlt * 6 + BaroAlt * 2) >> 3; // additional LPF to reduce baro noise (faster by 30 ?s)

  249.   #if (defined(VARIOMETER) && (VARIOMETER != 2)) || !defined(SUPPRESS_BARO_ALTHOLD)
  250.     //P
  251.     int16_t error16 = constrain(AltHold - alt.EstAlt, -300, 300);
  252.     applyDeadband(error16, 10); //remove small P parametr to reduce noise near zero position
  253.     BaroPID = constrain((conf.pid[PIDALT].P8 * error16 >>7), -150, +150);

  254.     //I
  255.     errorAltitudeI += conf.pid[PIDALT].I8 * error16 >>6;
  256.     errorAltitudeI = constrain(errorAltitudeI,-30000,30000);
  257.     BaroPID += errorAltitudeI>>9; //I in range +/-60
  258. ……………………

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

全部资料51hei下载地址:
MultiWii.zip (152.07 KB, 下载?#38382;? 23)

评分

参与人数 1黑币 +50 收起 理由
admin + 50 共享资料的黑币奖励!

查看全部评分

回复

使用道具 举报

ycb83523 发表于 2019-9-18 19:58 | 显示全部楼层
亲,具体是怎么用呢?
回复

使用道具 举报

 楼主| 腾飞的龙 发表于 2019-9-21 10:48 | 显示全部楼层
ycb83523 发表于 2019-9-18 19:58
亲,具体是怎么用呢?

很简单的飞控制作。
回复

使用道具 举报

310139033 发表于 2019-11-9 17:16 | 显示全部楼层
谢谢分享,拿走了
回复

使用道具 举报

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

本版积分规则

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

Powered by 单片机?#22363;?#32593;

快速回复 返回顶部 返回列表
北京快3玩法窍门
江苏15选5开奖结果走势图 加拿大28有人赢了吗 浙江卖什么赚钱 pk10人工免费计划 农村种植赚钱 11选5胆拖投注规则 广西快3开奖结果查询今天 河北快3遗漏数据图表 pc蛋蛋28论坛 体彩甘肃十一选五前三