本项目使用US-100超声波模块测高,与飞控的通讯方式为UART。
硬件连接应注意:通常飞控的发送管脚连超声波的接收管脚,飞控的接收管脚连超声波的发送管脚(即tx—rx;rx—tx);但有时为了方便连接,开发者在飞控板上直接标注管脚的连接示意(即tx—tx;rx-rx)。
(一) 首先,为什么要做PID?
1. 由于外界原因四旋翼的高度有时不稳定;
2. 有时需要让四旋翼高度稳定在目标值;
3. 高度控制系统是闭环,才能满足整个系统的稳定要求。
(二)PID思路
1. 计算高度误差
2. 累计误差(用于积分),累计误差需要限幅(误差无限累计后会造成溢出,导致积分环节出错。比如有符号数溢出,输出突变)
3. 计算pid alt.output =alt.p*alt.error+alt.d*(alt.error-alt.error_old)+alt.i*alt.error_sum
4. 输出限幅(防止输出超过pwm的调节范围)
5. 保存当前误差
alt.error_old = alt.error
(三)代码
/*计算高度误差*/
alt.error = alt.target-us100_alt;
if(abs(alt.error)<=80)
{
alt.error_sum+=alt.error;
}
else
{
alt.error_sum=0;
}
alt.error_sum = constrain_float(alt.error_sum,-800,800);
/*计算高度PID输出*/
alt.output = alt.p*alt.error+alt.d*(alt.error-alt.error_old)+alt.i*alt.error_sum;
/*输出限幅*/
alt.output = constrain_float(alt.output,-100,100);
/*误差随时间传递*/
alt.error_old_old = alt.error_old;
alt.error_old = alt.error;