這個第一次看就完全沒看懂，現在再看還是沒看懂orz，而且定高還是重要的功能和考點，詳細分析一下程序吧。。。。

首先是表示高度信息的結構體：

//數據結構聲明
typedef struct
{
    float Z_Speed;
    float Z_Acc;
    float Z_Postion;

    float Alt;
    uint16_t Thr;
}HeightInfo_t;


//Extern引用
extern HeightInfo_t HeightInfo;

分別是速度、加速度和位置（也就是高度？），下面兩個是高度（？）和油門（throttle）

然后就是代碼了，長的一批，頭文件省略了：

#define THROTTLE_BASE 600

//Extern引用
extern SPL06Manager_t g_SPL06Manager;

spl06的信息結構體，定義是這樣的，數據有點多：

typedef struct {
SPL06Param_t0 Param;
uint8_t u8Chip_id;
long i32RawPressure;
long i32RawTemperature;
long i32KP;
long i32KT;

long fGround_Alt;
long fALT; //height above sea level
float fRelative_Alt;

float fTemperature;
float fPressure;
float fLast_Pressure;

float fOffset;
bool Check; }SPL06Manager_t;

//私有函數區

//私有變量區
HeightInfo_t HeightInfo;
float dt2 = 0;
bool Acc_Enable_Flag = false;

#include "gcs.h"

#define MAX_EXP_WZ_VEL_UP 200
#define MAX_EXP_WZ_VEL_DW 150
#define MAX_EXP_WZ_ACC    500

exp表示期望值，所以上面三個宏定義意思大概是：最大的期望上升速度，最大的期望下降速度，以及最大的期望加速度。WZ大概是指Z軸方向？不確定

//wz_ctrl struct
static float exp_vel_transition[4];
static float exp_vel_d;

應該是期望速度和期望速度增量，都是靜態變量。

static float exp_acc;
static float fb_acc;
static float exp_vel;
static float fb_vel;
float exp_hei;
static float fb_hei;

期望加速度、反饋加速度、期望速度、反饋速度、期望高度、反饋高度？
不知道反饋在這里指啥。。。

//pid struct
static float hei_err,vel_err,acc_err,acc_err_i,acc_out,wz_out;

#define H_KP 1.5f
#define V_KP 5.0f
#define V_KD 0.05f
#define A_KP 0.4f
#define A_KI 0.6f

上面這些與pid有關，定義了靜態變量：高度偏差、速度偏差、加速度偏差、加速度偏差i（？不知道是啥）、加速度輸出（？）、wz輸出（？）。。。一半都看不懂。。。
宏定義的話可能比較好理解，是高度控制的p系數、速度控制的PD系數、加速度控制的PI系數。

//fc的意思是飛控
//extern uint8_t fc_state
uint8_t fc_state_take_off = 0;

void ALT_Ctrl(float dT_s)
{
    //==input calculate
    //fb = feedback 更新反饋信息
    fb_vel = HeightInfo.Z_Speed;
    fb_hei = HeightInfo.Z_Postion;
    fb_acc = HeightInfo.Z_Acc;

反饋的高度、速度、加速度信息就是當前狀態結構體中的三個值。

    //期望速度由遙控器數據轉換而來
    exp_vel_transition[0] = (Remote.thr - 1500)*0.0008f;
    
    //死區控制SSZQxaQxzz
    if(exp_vel_transition[0]<0.3f && exp_vel_transition[0]>-0.3f)
    {
        exp_vel_transition[0] = 0;
    }
    
    //超過死區范圍，則計算期望速度
    if(exp_vel_transition[0] > 0)
    {
        exp_vel_transition[1] = exp_vel_transition[0] * MAX_EXP_WZ_VEL_UP;
    }
    else
    {
        exp_vel_transition[1] = exp_vel_transition[0] * MAX_EXP_WZ_VEL_DW;
    }

這個tran[4]數組作用是。。。沒看懂，大膽猜測，應該是速度的期望值需要逐步變換求得？
其中tran[0]直接表示油門轉換來的數據，然后乘以一個最大期望速度，得到tran[1]，猜想應該0代表了比例，1代表有意義的期望值？

    //計算最大的期望速度增量
    float tmp = MAX_EXP_WZ_ACC * dT_s;
    
    //計算期望速度增量
    exp_vel_d = (exp_vel_transition[1] - exp_vel_transition[2]);
    
    //期望速度增量限幅
    if(exp_vel_d > tmp)
    {
        exp_vel_d = tmp;
    }
    else if(exp_vel_d < -tmp)
    {
        exp_vel_d = -tmp;
    }

可以看出tran[1]-tran[2]是期望速度增量，而tran[1]是期望速度，那么tran[2]也是期望速度，但是是上一次的。

    //期望速度為疊加速度增量，相當于加速度積分
    exp_vel_transition[2] += exp_vel_d;
    
    //期望速度LPF
    exp_vel_transition[3] += 0.2f *(exp_vel_transition[2] - exp_vel_transition[3]);

嗯，tran[2]就是上次的期望速度，加上期望速度增量，就是本次的期望速度。
然后經過濾波得到tran[3]，tran[3]是最后得到的期望速度。

    //==exp_val state
    //
    if(g_UAVinfo.UAV_Mode >= Altitude_Hold && fc_state_take_off != 0 )
    {
        //最終的期望速度為exp_vel
        exp_vel = exp_vel_transition[3];
        
        //期望高度為期望速度的積分
        exp_hei += exp_vel * dT_s;
        
        //期望高度限幅
        if(exp_hei > fb_hei+150)
        {
            exp_hei = fb_hei+150;
        }
        else if(exp_hei < fb_hei-150)
        {
            exp_hei = fb_hei-150;
        }
    }
    else
    {
        exp_vel = 0;
        exp_hei = fb_hei;
 
    }

然后得到的期望速度tran[3]，乘以單位時間，加在當前高度上，就得到了期望高度，然后限幅，就得到了最終的期望高度。

    //==ctrl
    //高度誤差 = 期望高度誤差 - 反饋誤差
    hei_err = (exp_hei - fb_hei);
    
    
    //速度誤差 = (Kp * 高度誤差 + 期望速度) - (反饋速度 + 反饋加速度 * kd)
    //為何此處速度誤差不為：vel_err = exp_vel - fb_vel;
    //PD控制，快速收斂到期望速度
    vel_err = ((H_KP * hei_err + exp_vel) - (fb_vel + V_KD *fb_acc));

高度偏差等于期望高度—反饋高度
速度偏差等于這個公式。。。不知道啥意思，為何不是這個我也想問。。。
反正先這樣了。。。

    //期望加速度 = Kp * 速度誤差，純P控制，快速收斂到期望加速度
    exp_acc = (V_KP * vel_err);
    
    //加速度誤差 = 期望加速度 - 反饋加速度
    //對加速度進行PI控制
    acc_err = exp_acc - fb_acc;
    acc_err_i += A_KI * acc_err * dT_s;
    acc_err_i = (acc_err_i > 600)?600:((acc_err_i<0)?0:acc_err_i);
    
    //最終輸出為 Kp * 加速度期望值 + 加速度積分值（PI控制）
    acc_out = A_KP * exp_acc;
    wz_out = acc_out + acc_err_i;
    
    //輸出限幅
    wz_out = (wz_out > 1000)?1000:((wz_out < 0)?0:wz_out);
    HeightInfo.Thr = (uint16_t)wz_out;

反正不管咋樣得到了油門的值。。。。

    //unlock state
    //如果飛機未解鎖，則將加速度的積分量置零，高度期望值與反饋值相同
    if(g_FMUflg.unlock == 0)
    {
        acc_err_i = 0;
        exp_hei = fb_hei;
        fc_state_take_off = 0;
         for(int i = 0;i<4;i++)
        {
          exp_vel_transition[i] = 0;
        }
    }
    else
    {
        if(g_UAVinfo.UAV_Mode >= Altitude_Hold)
        {
            //超過中位，狀態切換為起飛
            if(exp_vel_transition[0]>0)
            {
                fc_state_take_off = 1;
            }
        }
        else//g_UAVinfo.UAV_Mode < Altitude_Hold
        {
            fc_state_take_off = 1;
        }
    }
}

看不懂，我服了。。。這玩意就先到這吧，定高這部分查了查資料也沒結果，好像新的無人機有啥光流傳感，接下來看看那個吧。。