初学四旋翼之定高

本项目使用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;
   

猜你喜欢

转载自blog.csdn.net/dhaduce/article/details/75195552