无人机高度控制

这个第一次看就完全没看懂，现在再看还是没看懂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;
        }
    }
}

看不懂，我服了。。。这玩意就先到这吧，定高这部分查了查资料也没结果，好像新的无人机有啥光流传感，接下来看看那个吧。。