飞控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 static/image/common/back.gif
分享一下代码吧。也来看看
气压计定高代码在imu.c下面,望高手来指导, 大体的思路看明了了,主要是里面一些变量无法理解,个人觉得算法才是核心,所以想弄懂控制算法,mwc的算法还是比较好理解的,从简单的开始 MWC还有STM32版本的? 还是你自己转的? 我最近也在研究MWC的代码 ksh84222 发表于 2012-8-15 13:43 static/image/common/back.gif
我最近也在研究MWC的代码
您好您研究算法到哪进度了 红枫玉 发表于 2012-8-27 14:38 static/image/common/back.gif
您好您研究算法到哪进度了
楼主 以前成功飞过??? mark {:smile:} mark,mark,too
页:
[1]