在有高度信息（无论是超声波、气压计还是多传感器融合获取的）的情况下，如何用PID算法做PID控制。

以下所描述的参考开源pixhawk飞控的思路，以伪代码方式大概简单描述。

1）程序中自然是判断是否已经进入定高模式。进入定高后，先根据油门杆量，算出高度方向的速度设定值（爬升率/下降率）。注：定高模式下，油门杆不再直接控制电机转速，而是Z轴方向的速度，杆量在中位，自然不再有速度，因此高度方向达到悬停。

这三行代码主要是进行归一化，并进行限速。

Thr=THROTTLE / 1000.0f;

ZRate_sp= -Scale (Thr-0.5f,0.5f,ALT_CTRL_Z_DB);

ZRate_sp = ZRate_sp * ALT_VEL_MAX;

其中scale函数主体部分如下所示

https://pic4.zhimg.com/af9a204937cc0625cfb9e66f3be054cf_b.pngALT_CTRL_Z_DB为定高模式的死区大小。

2）计算高度控制的设定值。注：NED系，高度方向，向下是正，所以是- z。

Alt_sp = - z;

Alt_sp -= spZMoveRate * dt;

3）进行位置控制器的运算，是P加前馈控制，加上前馈控制，响应会快些。

Z_pos_err= -( Alt_sp - alt);

Z_pos_VelSp = Z_pos_err * alt_PID.P + ZRate_sp * ALT_FEED_FORWARD;

位置控制器的输出是Z轴的速度设定值。所以接下来，就是速度控制器,主要是PID控制。

4）速度控制器

velZerr = Z_pos_VelSp - velZ;

thrustZInt += velZerr * alt_vel_PID.I * dt;

valZerrD = (ZRate_sp - velZ) * alt_PID.P - (velZ - velZPrev) / dt;

velZPrev=velZ;

Z_Sp= velZerr * alt_vel_PID.P + valZErrD * alt_vel_PID.D + thrustZInt;

5）将油门的设定值，转换成电机输出。

Throttle=(- Z_Sp) * 1000;

本文只是大概描述了下思路，所有PID的参数都是要慢慢调的，具体还有部分细节，比如需要对设定值进行限幅啊、积分什么时候起作用，什么时候需要重置等等，详情参考pixhawk源码中的mc_pos_control。整个控制回路包含了两层，分别是位置、速度控制，需要Z轴方向的高度、速度信息（参见位置估计，传感器的数据融合、组合导航获得NED系下的位置、速度）。