標題: 自制MWC飛控2.3固件Arduino源碼 [打印本頁]
作者: 騰飛的龍    時間: 2019-1-14 15:19
標題: 自制MWC飛控2.3固件Arduino源碼
好用的固件
  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"

  9. void getEstimatedAttitude();

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

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

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

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

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

  104. /* Set the Gyro Weight for Gyro/Acc complementary filter
  105.    Increasing this value would reduce and delay Acc influence on the output of the filter*/
  106. #ifndef GYR_CMPF_FACTOR
  107.   #define GYR_CMPF_FACTOR 600
  108. #endif

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

  114. //****** end of advanced users settings *************
  115. #define INV_GYR_CMPF_FACTOR   (1.0f / (GYR_CMPF_FACTOR  + 1.0f))
  116. #define INV_GYR_CMPFM_FACTOR  (1.0f / (GYR_CMPFM_FACTOR + 1.0f))

  118. typedef struct fp_vector {               
  119.   float X,Y,Z;               
  120. } t_fp_vector_def;

  122. typedef union {               
  123.   float A[3];               
  124.   t_fp_vector_def V;               
  125. } t_fp_vector;

  127. typedef struct int32_t_vector {
  128.   int32_t X,Y,Z;
  129. } t_int32_t_vector_def;

  131. typedef union {
  132.   int32_t A[3];
  133.   t_int32_t_vector_def V;
  134. } t_int32_t_vector;

  136. int16_t _atan2(int32_t y, int32_t x){
  137.   float z = (float)y / x;
  138.   int16_t a;
  139.   if ( abs(y) < abs(x) ){
  140.      a = 573 * z / (1.0f + 0.28f * z * z);
  141.    if (x<0) {
  142.      if (y<0) a -= 1800;
  143.      else a += 1800;
  144.    }
  145.   } else {
  146.    a = 900 - 573 * z / (z * z + 0.28f);
  147.    if (y<0) a -= 1800;
  148.   }
  149.   return a;
  150. }

  152. float InvSqrt (float x){
  153.   union{  
  154.     int32_t i;  
  155.     float   f;
  156.   } conv;
  157.   conv.f = x;
  158.   conv.i = 0x5f3759df - (conv.i >> 1);
  159.   return 0.5f * conv.f * (3.0f - x * conv.f * conv.f);
  160. }

  162. // Rotate Estimated vector(s) with small angle approximation, according to the gyro data
  163. void rotateV(struct fp_vector *v,float* delta) {
  164.   fp_vector v_tmp = *v;
  165.   v->Z -= delta[ROLL]  * v_tmp.X + delta[PITCH] * v_tmp.Y;
  166.   v->X += delta[ROLL]  * v_tmp.Z - delta[YAW]   * v_tmp.Y;
  167.   v->Y += delta[PITCH] * v_tmp.Z + delta[YAW]   * v_tmp.X;
  168. }


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

  174. static t_fp_vector EstG;
  175. static t_int32_t_vector EstG32;
  176. #if MAG
  177.   static t_int32_t_vector EstM32;
  178.   static t_fp_vector EstM;
  179. #endif

  181. void getEstimatedAttitude(){
  182.   uint8_t axis;
  183.   int32_t accMag = 0;
  184.   float scale, deltaGyroAngle[3];
  185.   uint8_t validAcc;
  186.   static uint16_t previousT;
  187.   uint16_t currentT = micros();

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

  192.   // Initialization
  193.   for (axis = 0; axis < 3; axis++) {
  194.     deltaGyroAngle[axis] = imu.gyroADC[axis]  * scale; // radian

  196.     accLPF32[axis]    -= accLPF32[axis]>>ACC_LPF_FACTOR;
  197.     accLPF32[axis]    += imu.accADC[axis];
  198.     imu.accSmooth[axis]    = accLPF32[axis]>>ACC_LPF_FACTOR;

  200.     accMag += (int32_t)imu.accSmooth[axis]*imu.accSmooth[axis] ;
  201.   }

  203.   rotateV(&EstG.V,deltaGyroAngle);
  204.   #if MAG
  205.     rotateV(&EstM.V,deltaGyroAngle);
  206.   #endif

  208.   accMag = accMag*100/((int32_t)ACC_1G*ACC_1G);
  209.   validAcc = 72 < (uint16_t)accMag && (uint16_t)accMag < 133;
  210.   // Apply complimentary filter (Gyro drift correction)
  211.   // 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.
  212.   // To do that, we just skip filter, as EstV already rotated by Gyro
  213.   for (axis = 0; axis < 3; axis++) {
  214.     if ( validAcc )
  215.       EstG.A[axis] = (EstG.A[axis] * GYR_CMPF_FACTOR + imu.accSmooth[axis]) * INV_GYR_CMPF_FACTOR;
  216.     EstG32.A[axis] = EstG.A[axis]; //int32_t cross calculation is a little bit faster than float       
  217.     #if MAG
  218.       EstM.A[axis] = (EstM.A[axis] * GYR_CMPFM_FACTOR  + imu.magADC[axis]) * INV_GYR_CMPFM_FACTOR;
  219.       EstM32.A[axis] = EstM.A[axis];
  220.     #endif
  221.   }
  222.   
  223.   if ((int16_t)EstG32.A[2] > ACCZ_25deg)
  224.     f.SMALL_ANGLES_25 = 1;
  225.   else
  226.     f.SMALL_ANGLES_25 = 0;

  228.   // Attitude of the estimated vector
  229.   int32_t sqGX_sqGZ = sq(EstG32.V.X) + sq(EstG32.V.Z);
  230.   invG = InvSqrt(sqGX_sqGZ + sq(EstG32.V.Y));
  231.   att.angle[ROLL]  = _atan2(EstG32.V.X , EstG32.V.Z);
  232.   att.angle[PITCH] = _atan2(EstG32.V.Y , InvSqrt(sqGX_sqGZ)*sqGX_sqGZ);

  234.   #if MAG
  235.     att.heading = _atan2(
  236.       EstM32.V.Z * EstG32.V.X - EstM32.V.X * EstG32.V.Z,
  237.       (EstM.V.Y * sqGX_sqGZ  - (EstM32.V.X * EstG32.V.X + EstM32.V.Z * EstG32.V.Z) * EstG.V.Y)*invG );
  238.     att.heading += conf.mag_declination; // Set from GUI
  239.     att.heading /= 10;
  240.   #endif

  242.   #if defined(THROTTLE_ANGLE_CORRECTION)
  243.     cosZ = EstG.V.Z / ACC_1G * 100.0f;                                                        // cos(angleZ) * 100
  244.     throttleAngleCorrection = THROTTLE_ANGLE_CORRECTION * constrain(100 - cosZ, 0, 100) >>3;  // 16 bit ok: 200*150 = 30000  
  245.   #endif
  246. }

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

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


  254. #define applyDeadband(value, deadband)  \
  255.   if(abs(value) < deadband) {           \
  256.     value = 0;                          \
  257.   } else if(value > 0){                 \
  258.     value -= deadband;                  \
  259.   } else if(value < 0){                 \
  260.     value += deadband;                  \
  261.   }

  263. #if BARO
  264. uint8_t getEstimatedAltitude(){
  265.   int32_t  BaroAlt;
  266.   static float baroGroundTemperatureScale,logBaroGroundPressureSum;
  267.   static float vel = 0.0f;
  268.   static uint16_t previousT;
  269.   uint16_t currentT = micros();
  270.   uint16_t dTime;

  272.   dTime = currentT - previousT;
  273.   if (dTime < UPDATE_INTERVAL) return 0;
  274.   previousT = currentT;

  276.   if(calibratingB > 0) {
  277.     logBaroGroundPressureSum = log(baroPressureSum);
  278.     baroGroundTemperatureScale = (baroTemperature + 27315) *  29.271267f;
  279.     calibratingB--;
  280.   }

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

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

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

  294.     //I
  295.     errorAltitudeI += conf.pid[PIDALT].I8 * error16 >>6;
  296.     errorAltitudeI = constrain(errorAltitudeI,-30000,30000);
  297.     BaroPID += errorAltitudeI>>9; //I in range +/-60
  298. ……………………

  300. …………限于本文篇幅 余下代碼請從51黑下載附件…………
復制代碼

全部資料51hei下載地址：
MultiWii.zip (152.07 KB, 下載次數(shù): 62)
作者: ycb83523    時間: 2019-9-18 19:58
親，具體是怎么用呢？
作者: 騰飛的龍    時間: 2019-9-21 10:48
ycb83523 發(fā)表于 2019-9-18 19:58
親，具體是怎么用呢？

很簡單的飛控制作。
作者: 310139033    時間: 2019-11-9 17:16
謝謝分享，拿走了



歡迎光臨 (http://www.torrancerestoration.com/bbs/) Powered by Discuz! X3.1