STM32超声波定高返回的高度数据如何用PID算法处理？

计算高度控制的设定值。注：NED系，高度方向，向下是正，所以是- z。
Alt_sp = - z;
Alt_sp -= spZMoveRate * dt;

根据题主的问题，估且就认为是在有高度信息（无论是超声波、气压计还是多传感器融合获取的）的情况下，如何用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函数主体部分如下所示
ALT_CTRL_Z_DB为定高模式的死区大小。

进行位置控制器的运算，是P加前馈控制，加上前馈控制，响应会快些。
Z_pos_err= -( Alt_sp - alt);
Z_pos_VelSp = Z_pos_err * alt_PID.P + ZRate_sp * ALT_FEED_FORWARD;
位置控制器的输出是Z轴的速度设定值。所以接下来，就是速度控制器,主要是PID控制。