无人机项目跟踪记录五十二---高度悬停模块

高度悬停模块函数是void CtrlAlti(void)：

函数基本注释如下：

//函数名：CtrlAlti()
//输入：无
//输出: 最终结果输出到全局变量thrustZSp
//描述：控制高度，也就是高度悬停控制函数
//only in climb rate mode and landind mode. now we don't work on manual mode
void CtrlAlti(void)
{
    float manThr=0,alt=0,velZ=0;  //手动油门  高度  z轴速度
    float altSp=0;                //期望高度
    float posZVelSp=0;
    float altSpOffset,altSpOffsetMax=0;  //期望高度偏差 期望高度偏差最大值
    float dt=0,t=0;                          //时间间隔，时间
	  static float tPrev=0,velZPrev=0;         //上次调用的时间   上一怕的z轴速度
    float posZErr=0,velZErr=0,valZErrD=0;    //z轴的位置误差  z轴的速度误差   z轴的速度误差
    float thrustXYSpLen=0,thrustSpLen=0;    //X、Y平面油门期望长度   油门期望长度
    float thrustXYMax=0;                     //x、y平面油门最大长度
    float minThrust;                         //最小油门值

    //get dt
    //保证dt运算不能被打断，保持更新，否则dt过大，积分爆满。
    if(tPrev==0) {
        tPrev=micros();              
        return;
    } else {
        t=micros();
        dt=(t-tPrev) /1000000.0f;
        tPrev=t;
    }

    //only in climb rate mode and landind mode. now we don't work on manual mode
    //手动模式不使用该高度控制算法
    if(MANUAL == altCtrlMode || !FLY_ENABLE) {
        return;
    }

    //--------------pos z ctrol---------------//
    //get current alt
    alt=-nav.z;                                 //当前高度值
    //get desired move rate from stick
    manThr=RC_DATA.THROTTLE / 1000.0f;          //当前油门数据归一化
    spZMoveRate= -dbScaleLinear(manThr-0.5f,0.5f,ALT_CTRL_Z_DB);	// scale to -1~1 . NED frame//北天地，所以爬升为负值
    spZMoveRate = spZMoveRate * ALT_VEL_MAX;	// scale to vel min max  /Z轴移动的速度

    //get alt setpoint in CLIMB rate mode
    altSp 	=-nav.z;						//only alt is not in ned frame.  //期望的高度
    altSp  -= spZMoveRate * dt;   //本周期期望移动的高度
    //limit alt setpoint
    altSpOffsetMax=ALT_VEL_MAX / alt_PID.P * 2.0f;    //期望高度偏差最大值
    altSpOffset = altSp-alt;                          //期望与实际的偏差
    if( altSpOffset > altSpOffsetMax) {
        altSp=alt +  altSpOffsetMax;                   //限制高度最大值
    } else if( altSpOffset < -altSpOffsetMax) {
        altSp=alt - altSpOffsetMax;                  //限制高度下限
    }

    //限高
    if(isAltLimit)
    {
        if(altSp - altLand > ALT_LIMIT)
        {
            altSp=altLand+ALT_LIMIT;
            spZMoveRate=0;
        }
    }

    // pid and feedforward control . in ned frame
    posZErr= -(altSp - alt);
    posZVelSp = posZErr * alt_PID.P + spZMoveRate * ALT_FEED_FORWARD;   //z轴期望的速度
    //consider landing mode
    if(altCtrlMode==LANDING)           
        posZVelSp = LAND_SPEED;   //如果是着陆模式，z轴期望的速度等于着陆的速度

    //获取一个预估的Z轴悬停基准值，相关因素有电池电压
    //get hold throttle. give it a estimated value
    if(zIntReset) {
        thrustZInt = estimateHoverThru();   //根据电压值来判断悬停的油门值
        zIntReset = 0;
    }

    velZ=nav.vz;                    //期望的z轴速度
    velZErr = posZVelSp - velZ;     //误差
    valZErrD = (spZMoveRate - velZ) * alt_PID.P - (velZ - velZPrev) / dt;	//spZMoveRate is from manual stick vel control  微分误差值
    velZPrev=velZ;

    thrustZSp= velZErr * alt_vel_PID.P + valZErrD * alt_vel_PID.D + thrustZInt;	//in ned frame. thrustZInt contains hover thrust PID算期望油门值

    //限制最小下降油门
    minThrust = estimateMinThru();
    if(altCtrlMode!=LANDING) {
        if (-thrustZSp < minThrust) {
            thrustZSp = -minThrust;
        }
    }

    //与动力分配相关	testing
    satXY=0;
    satZ=0;
    thrustXYSp[0]= sinf(RC_DATA.ROOL * M_PI_F /180.0f) ;//目标角度转加速度
    thrustXYSp[1]= sinf(RC_DATA.PITCH * M_PI_F /180.0f) ; 	//归一化
    thrustXYSpLen= sqrtf(thrustXYSp[0] * thrustXYSp[0] + thrustXYSp[1] * thrustXYSp[1]);   //xy平面合力大小
    //limit tilt max
    if(thrustXYSpLen >0.01f )
    {
        thrustXYMax=-thrustZSp * tanf(TILT_MAX);   //xy平面油门的最大值
        if(thrustXYSpLen > thrustXYMax)
        {
            float k=thrustXYMax / thrustXYSpLen;    //获得比例值
            thrustXYSp[1] *= k;
            thrustXYSp[0] *= k;
            satXY=1;   //过饱和
            thrustXYSpLen= sqrtf(thrustXYSp[0] * thrustXYSp[0] + thrustXYSp[1] * thrustXYSp[1]);    //从新计算最大油门值大小
        }

    }
    //limit max thrust!!
    thrustSpLen=sqrtf(thrustXYSpLen * thrustXYSpLen + thrustZSp * thrustZSp);    //计算出油门值大小
    if(thrustSpLen > THR_MAX)
    {
        if(thrustZSp < 0.0f)	//going up   //往上飞
        {
            if (-thrustZSp > THR_MAX) 
            {
                /* thrust Z component is too large, limit it */              //z轴的值太大了，限制它
                thrustXYSp[0] = 0.0f;
                thrustXYSp[1] = 0.0f;
                thrustZSp = -THR_MAX;
                satXY = 1;
                satZ = 1;

            }
            else {                   //保留最大油门和z轴方向的力，不去考虑xy方向的位置
                float k = 0;
                /* preserve thrust Z component and lower XY, keeping altitude is more important than position */
                thrustXYMax = sqrtf(THR_MAX * THR_MAX- thrustZSp * thrustZSp);
                k=thrustXYMax / thrustXYSpLen;   //等比例缩放
                thrustXYSp[1] *=k;
                thrustXYSp[0] *= k;
                satXY=1;  //过饱和
            }
        }
        else {		//going down
            /* Z component is negative, going down, simply limit thrust vector */
            float k = THR_MAX / thrustSpLen;
            thrustZSp *= k;              //等比例缩放
            thrustXYSp[1] *=k;
            thrustXYSp[0] *= k;
            satXY = 1;
            satZ = 1;
        }

    }
    rollSp= asinf(thrustXYSp[0]) * 180.0f /M_PI_F;      //期望的横滚，力的分解
    pitchSp = asinf(thrustXYSp[1]) * 180.0f /M_PI_F;     //期望的俯仰，力的分解


    // if saturation ,don't integral
    if(!satZ )//&& fabs(thrustZSp)<THR_MAX           //z轴非饱和的情况下
    {
        thrustZInt += velZErr * alt_vel_PID.I * dt;   //对油门值进行pid，求得z方向的油门值
        if (thrustZInt > 0.0f)
            thrustZInt = 0.0f;
    }
}