设为首页收藏本站
开启辅助访问 切换到宽版

 找回密码
 立即注册

QQ登录

只需一步，快速开始

热门： 51单片机 | 24小时必答区 | 单片机教程 | 单片机DIY制作 | STM32 | Cortex M3 | 模数电子 | 电子DIY制作 | 音响/功放 | 拆机乐园 | Arduino | 嵌入式OS | 程序设计

搜索
»论坛 › 电子技术分类讨论区 › 四轴/航模/飞行器DIY › 自制MWC飞控2.3固件Arduino源码
返回列表 发新帖
查看: 6572|回复: 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"

  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, 下载次数: 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 | 显示全部楼层
谢谢分享，拿走了
回复

使用道具 举报

返回列表 发新帖
高级模式
B Color Image Link Quote Code Smilies
您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

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

Powered by 单片机教程网

快速回复 返回顶部 返回列表