MWC气压计定高

最新推荐文章于 2023-04-03 21:49:19 发布

bbzz2

最新推荐文章于 2023-04-03 21:49:19 发布

阅读量5.2k

点赞数

分类专栏： Drone

Drone 专栏收录该内容

42 篇文章 4 订阅

订阅专栏

写在前面... 1

1. 修正油门... 1

2. BaroPID.. 2

写在前面

之前分析过MWC源码，也写过一些文章对MWC进行简单的分析。但有时候觉得那些文章写的不够详细，即没有针对一个特定的功能进行深入分析。那么今天就来详细分析下MWC气压计定高，也免得自己遗忘。

1. 修正油门

在MWC中，气压计直接对油门进行操作：

#if BARO && (!defined(SUPPRESS_BARO_ALTHOLD)) //?????

/* Smooth alt change routine , for slow auto and aerophoto modes (in general solution from alexmos). It's slowly increase/decrease

* altitude proportional to stick movement (+/-100 throttle gives about +/-50 cm in 1 second with cycle time about 3-4ms)

if (f.BARO_MODE) { //????????,????????

static uint8_t isAltHoldChanged = 0;

static int16_t AltHoldCorr = 0;

if (abs(rcCommand[THROTTLE]-initialThrottleHold)>ALT_HOLD_THROTTLE_NEUTRAL_ZONE) {

// Slowly increase/decrease AltHold proportional to stick movement ( +100 throttle gives ~ +50 cm in 1 second with cycle time about 3-4ms)

AltHoldCorr+= rcCommand[THROTTLE] - initialThrottleHold;

if(abs(AltHoldCorr) > 512) {

AltHold += AltHoldCorr/512; //????????.

AltHoldCorr %= 512;

}

isAltHoldChanged = 1;

} else if (isAltHoldChanged) {

AltHold = alt.EstAlt; //?????????,?????????

isAltHoldChanged = 0;

}

rcCommand[THROTTLE] = initialThrottleHold + BaroPID;

}

#endif

从代码中我们看到，油门量是直接被修改了的，换句话说油门量不直接参与电机控制。而是由initialThrottleHold和BaroPID两个量接替。BaroPID我们很容易知道是气压计经PID运算而得到的，那么initialThrottleHold又是什么？从字面上看这是一个油门的初始值，那么这个初始有指的是什么？这个我们稍后再讨论，还是回到上面的代码。理解这段代码我们得首先搞懂一个变量：AltHold即锁高值。比如其值为200cm，那么飞控将被锁定在200cm的高度。于是这段代码就很好理解了，其实就是用来控制锁搞高度的。当油门量与initialThrottleHold的差值超过一个阈值时改变高度随着油门的改变而改变，即油门是用来控高的。当插值小于阈值的瞬间，当前实际高度即被锁定。而中间那段代码就是高度怎样随着油门的改变而改变。单纯从代码看有点像是控制升降速度。

#if (!defined(SUPPRESS_BARO_ALTHOLD))

if (rcOptions[BOXBARO]) {

if (!f.BARO_MODE) {

f.BARO_MODE = 1; // ????????

AltHold = alt.EstAlt; //??????

#if defined(ALT_HOLD_THROTTLE_MIDPOINT)

initialThrottleHold = ALT_HOLD_THROTTLE_MIDPOINT;

#else

initialThrottleHold = rcCommand[THROTTLE]; //???????.

#endif

errorAltitudeI = 0;

BaroPID=0;

}

} else {

f.BARO_MODE = 0;

}

#endif

这段代码就是进入气压计模式。从这段代码中我们可以很清楚看到initialThrottleHold就是进入气压计模式那一刻的油门量。后面通过油门改变高度都是以这个油门量为基准。当然MWC同时也支持设定这个油门量。

2. BaroPID

从前面的分析中我们知道气压计控高中对我们最重要的应该是BaroPID变量了。毕竟initialThrottleHold相对来说是一个固定的值没那么多变化，比较好考虑。

IMU.cpp (multiwii): BaroPID = constrain((conf.pid[PIDALT].P8 * error16 >>7), -150, +150);

IMU.cpp (multiwii): BaroPID += errorAltitudeI>>9; //I in range +/-60

IMU.cpp (multiwii): BaroPID -= constrain(conf.pid[PIDALT].D8 * alt.vario >>4, -150, 150);

这个就是BaroPID的PID计算。相应的代码如下：

//P

int16_t error16 = constrain(AltHold - alt.EstAlt, -300, 300);

applyDeadband(error16, 10); //remove small P parametr to reduce noise near zero position

BaroPID = constrain((conf.pid[PIDALT].P8 * error16 >>7), -150, +150);

//I

errorAltitudeI += conf.pid[PIDALT].I8 * error16 >>6;

errorAltitudeI = constrain(errorAltitudeI,-30000,30000);

BaroPID += errorAltitudeI>>9; //I in range +/-60

//D

alt.vario = vel;

applyDeadband(alt.vario, 5);

BaroPID -= constrain(conf.pid[PIDALT].D8 * alt.vario >>4, -150, 150);

即便没有注释我们也很容易看出这就是PID。PI其实都没有什么花样，关键在于vel：

IMU.cpp (multiwii): static float vel = 0.0f;

IMU.cpp (multiwii): vel += accZ * ACC_VelScale * dTime;

IMU.cpp (multiwii): vel = vel * 0.985f + baroVel * 0.015f;

vel的计算关系到另外两个值：baroVel和accZ。前者是气压计算得的升降速度：

static int32_t lastBaroAlt;

//int16_t baroVel = (alt.EstAlt - lastBaroAlt) * 1000000.0f / dTime;

int16_t baroVel = (alt.EstAlt - lastBaroAlt) * (1000000 / UPDATE_INTERVAL);

lastBaroAlt = alt.EstAlt;

baroVel = constrain(baroVel, -300, 300); // constrain baro velocity +/- 300cm/s

而后者是跟加计相关的：

// Integrator - velocity, cm/sec

vel += accZ * ACC_VelScale * dTime;

也就是说对Z轴的加计值积分得到的速度。“对Z轴的加计值积分”这么说似乎有点奇怪，实际上也确实很奇怪。因为这里边还有名堂！

// projection of ACC vector to global Z, with 1G subtructed

// Math: accZ = A * G / |G| - 1G

int16_t accZ = (imu.accSmooth[ROLL] * EstG32.V.X + imu.accSmooth[PITCH] * EstG32.V.Y + imu.accSmooth[YAW] * EstG32.V.Z) * invG;

static int16_t accZoffset = 0; //???????????6G

if (!f.ARMED) {

accZoffset -= accZoffset>>3;

accZoffset += accZ;

}

accZ -= accZoffset>>3;

代码没几行，但最关键的在于Math那行注释。于是理解这段就变成了理解这个公式。那么这个公式又是怎么来的？accZ具体的含义又是什么？可以确定的是绝不是字面理解的那样：Z轴的加计值！

理解这个公式我们还得从向量叉乘说起：

向量a·向量b=|a||b|cos<a,b>

如果我们引入坐标系，那么：

　　若向量a=(a1,b1,c1)，向量b=(a2,b2,c2)，

　　则

　　向量a×向量b=

　　| i j k |

　　|a1 b1 c1|

　　|a2 b2 c2|

　　=(b1c2-b2c1,c1a2-a1c2,a1b2-a2b1)

　　（i、j、k分别为空间中相互垂直的三条坐标轴的单位向量）。

这是向量叉乘的两种形式。从前一个形式中我们可以得出下面的结果：

(向量a·向量b)/ |a|=|b|cos<a,b>

我们需要知道，这里|a|和|b|不是向量而是标量，即分别是各自向量的模。向量的模就是向量的长度。这样公式右边的意义就很明确了，即向量b在向量a的投影。

现在我们再回到前面的注释我们就很清楚” A * G / |G|”实际上就是向量A在向量G上的投影。投影完了之后两者的模做差这时候得到的就是超重(失重)量，进而可以通过积分计算出升降速度。于是对照向量叉乘公式，这段代码其实不难理解。唯一不好理解的可能就是“- 1G”，在代码中对应的值是：“-= accZoffset>>3”。也就是说单位重力的计算其实就是对加计在重力方向上的投影做线性插值(某些时候线性插值与均值滤波是一样的)得到的。而且这个计算是在解锁前。由于解锁之前认为四轴是静止的，那么这个值也就刚好是重力了。

从前面的分析中我们看到MWC气压计控高依然是以气压计为主，而加计只在微分项其作用，也就是说加计只会影响其动态响应。而最后究竟能否定在自己希望的高度则完全由气压计决定。但是单纯从理论上分析，由于加计的引入我始终认为如果震动过滤不理想将给控高带来麻烦。只是目前我还没有从MWC源码中看出MWC是如何规避这一问题的。至于有人说气压计定高效果不是很好，这个由于我没有亲自测试过，不方便发表言论。只是从MWC的定高来看，不管是气压计还是加计任何一方出现纰漏效果肯定都不会好的。