飞控pid算法

红枫玉

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

void getEstimatedAltitude(void)
{
    uint32_t index;
    static uint32_t deadLine = INIT_DELAY;
    static int16_t BaroHistTab[BARO_TAB_SIZE];
    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[BaroHistIdx];
    BaroHistTab[BaroHistIdx] = BaroAlt / 10;
    BaroHigh += BaroHistTab[BaroHistIdx];
    index = (BaroHistIdx + (BARO_TAB_SIZE / 2)) % BARO_TAB_SIZE;
    BaroHigh -= BaroHistTab[index];
    BaroLow  += BaroHistTab[index];
    BaroLow  -= last;
    BaroHistIdx++;
    if (BaroHistIdx >= BARO_TAB_SIZE)
        BaroHistIdx = 0;

    BaroPID = 0;
    //D
    temp32 = cfg.D8[PIDALT] * (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[PIDALT] * constrain(temp32, (-2) * cfg.P8[PIDALT], 2 * cfg.P8[PIDALT]) / 100;
    BaroPID = constrain(BaroPID, -150, +150); // sum of P and D should be in range 150

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

Y-Edison

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

红枫玉

Y-Edison 发表于 2012-7-12 23:27
分享一下代码吧。也来看看

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

红枫玉

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

okARM

MWC还有STM32版本的？

okARM

还是你自己转的？

ksh84222

我最近也在研究MWC的代码

红枫玉

ksh84222 发表于 2012-8-15 13:43
我最近也在研究MWC的代码

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

260186221

红枫玉 发表于 2012-8-27 14:38
您好  您研究算法到哪进度了

楼主 以前成功飞过？？？

yfgww

mark

智波景严

mark,mark,too