红枫玉 发表于 2012-7-12 23:04:56

飞控pid算法

最近在研究mwc飞控的气压计定高算法，不知道有没有看过mwc代码的高手来分析一下此段代码

void getEstimatedAltitude(void)
{
    uint32_t index;
    static uint32_t deadLine = INIT_DELAY;
    static int16_t BaroHistTab;
    static uint32_t BaroHistIdx;
    static int32_t BaroHigh = 0;
    static int32_t BaroLow = 0;
    int32_t temp32;
    int16_t last;

    if (currentTime < deadLine)
      return;
    deadLine = currentTime + UPDATE_INTERVAL;

    //**** Alt. Set Point stabilization PID ****
    //calculate speed for D calculation
    last = BaroHistTab;
    BaroHistTab = BaroAlt / 10;
    BaroHigh += BaroHistTab;
    index = (BaroHistIdx + (BARO_TAB_SIZE / 2)) % BARO_TAB_SIZE;
    BaroHigh -= BaroHistTab;
    BaroLow+= BaroHistTab;
    BaroLow-= last;
    BaroHistIdx++;
    if (BaroHistIdx >= BARO_TAB_SIZE)
      BaroHistIdx = 0;

    BaroPID = 0;
    //D
    temp32 = cfg.D8 * (BaroHigh - BaroLow) / 40;
    BaroPID -= temp32;

    EstAlt = BaroHigh * 10 / (BARO_TAB_SIZE / 2);

    temp32 = AltHold - EstAlt;
    if (abs(temp32) < 10 && abs(BaroPID) < 10)
      BaroPID = 0;// remove small D parameter to reduce noise near zero position
    // P
    BaroPID += cfg.P8 * constrain(temp32, (-2) * cfg.P8, 2 * cfg.P8) / 100;
    BaroPID = constrain(BaroPID, -150, +150); // sum of P and D should be in range 150

    // I
    errorAltitudeI += temp32 * cfg.I8 / 50;
    errorAltitudeI = constrain(errorAltitudeI, -30000, 30000);
    temp32 = errorAltitudeI / 500; // I in range +/-60
    BaroPID += temp32;

Y-Edison 发表于 2012-7-12 23:27:10

分享一下代码吧。也来看看

红枫玉 发表于 2012-7-13 01:07:19

Y-Edison 发表于 2012-7-12 23:27 static/image/common/back.gif
分享一下代码吧。也来看看

气压计定高代码在imu.c下面，望高手来指导，

红枫玉 发表于 2012-7-13 01:11:06

大体的思路看明了了，主要是里面一些变量无法理解，个人觉得算法才是核心，所以想弄懂控制算法，mwc的算法还是比较好理解的，从简单的开始

okARM 发表于 2012-7-13 12:10:27

MWC还有STM32版本的？

okARM 发表于 2012-7-13 12:10:44

还是你自己转的？

ksh84222 发表于 2012-8-15 13:43:35

我最近也在研究MWC的代码

红枫玉 发表于 2012-8-27 14:38:33

ksh84222 发表于 2012-8-15 13:43 static/image/common/back.gif
我最近也在研究MWC的代码

您好您研究算法到哪进度了

260186221 发表于 2012-8-28 15:10:15

红枫玉 发表于 2012-8-27 14:38 static/image/common/back.gif
您好您研究算法到哪进度了

楼主 以前成功飞过？？？

yfgww 发表于 2012-8-28 16:15:20

mark {:smile:}

智波景严 发表于 2013-11-6 11:24:39

mark,mark,too
页: [1]
查看完整版本: 飞控pid算法