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

 找回密碼
 立即注冊

QQ登錄

只需一步,快速開始

帖子
查看: 7290|回復: 3
收起左側

自制MWC飛控2.3固件Arduino源碼

[復制鏈接]
ID:79544 發表于 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, 下載次數: 62)

評分

參與人數 1黑幣 +50 收起 理由
admin + 50 共享資料的黑幣獎勵!

查看全部評分

回復

使用道具 舉報

ID:205027 發表于 2019-9-18 19:58 | 顯示全部樓層
親,具體是怎么用呢?
回復

使用道具 舉報

ID:79544 發表于 2019-9-21 10:48 | 顯示全部樓層
ycb83523 發表于 2019-9-18 19:58
親,具體是怎么用呢?

很簡單的飛控制作。
回復

使用道具 舉報

ID:514567 發表于 2019-11-9 17:16 | 顯示全部樓層
謝謝分享,拿走了
回復

使用道具 舉報

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

本版積分規則

小黑屋|51黑電子論壇 |51黑電子論壇6群 QQ 管理員QQ:125739409;技術交流QQ群281945664

Powered by 單片機教程網

快速回復 返回頂部 返回列表
主站蜘蛛池模板: av基地网 | 国产午夜视频在线观看 | 亚洲精选在线观看 | 亚洲成人毛片 | 成人黄色一级片 | 97国产视频 | 超碰免费在线观看 | 亚洲精品18在线观看 | 日本少妇做爰全过程毛片 | 99精品欧美一区二区蜜桃免费 | 久久免费国产视频 | 日韩国产一区二区三区 | 日本视频在线免费观看 | 超碰国产在线 | 亚洲高清在线视频 | 久久一区二区视频 | 在线免费黄色网址 | h片免费 | 亚洲国产精品一区二区三区 | 亚洲人天堂 | 日韩视频在线免费观看 | 日皮视频免费看 | 69久久久 | 欧美视频一二三区 | 亚洲精品在线视频观看 | 三级av片| 国产精品一级 | 亚洲综合视频在线观看 | 日韩一区二区在线视频 | 97视频在线播放 | 视频一区在线播放 | 亚洲青涩 | 日韩视频免费在线观看 | 日韩av在线免费播放 | 黑人操亚洲人 | 一区二区高清视频 | 欧美日韩免费 | 欧美日韩免费一区二区三区 | 日韩一级片视频 | 中文字幕偷拍 | 99在线播放 |