{ LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning), "CTUN", "Qhhfffecchh", "TimeUS,ThrIn,AngBst,ThrOut,DAlt,Alt,BarAlt,DSAlt,SAlt,DCRt,CRt" },
void Copter::Log_Write_Control_Tuning()
{
struct log_Control_Tuning pkt = {
LOG_PACKET_HEADER_INIT(LOG_CONTROL_TUNING_MSG),
time_us : AP_HAL::micros64(),
throttle_in : channel_throttle->control_in,
angle_boost : attitude_control.angle_boost(),
throttle_out : motors.get_throttle(),
desired_alt : pos_control.get_alt_target() / 100.0f,
inav_alt : inertial_nav.get_altitude() / 100.0f,
baro_alt : baro_alt,
desired_sonar_alt : (int16_t)target_sonar_alt,
sonar_alt : sonar_alt,
desired_climb_rate : (int16_t)pos_control.get_vel_target_z(),
climb_rate : climb_rate
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
上面的结构体就是我们要分析定高效果时需要查看的部分主要信息，
time_us为运行时间，
throttle_in为油门输入，0到1000的整数值。
throttle_out油门输出，0到1000的float数值。
desired_alt目标高度， _pos_target.z的值除以100，单位为米。
inav_alt 飞机目前高度，_relpos_cm.z的值除以100，单位为米。
baro_alt 气压计的高度，单位为厘米。此值在sensors.cpp的read_barometer的函数中更新，read_barometer通过ArduCopter.cpp中的update_altitude调用，update_altitude为定义的10hz的schedule.
desired_climb_rate为需要的爬升速率，_vel_target.z的值，单位cm/s。
climb_rate为当前的爬升速率，此值在inertia.cpp中的read_inertial_altitude函数中更新，实际上就是读取的inertial_nav的_velocity_cm.z的值，read_inertial_altitude在ArduCopter.cpp中的throttle_loop函数中被调用，throttle_loop为定义的50hz的schedule.
当切换到定高模式时，althold_init先被运行，其代码如下：
bool Copter::althold_init(bool ignore_checks)
{
// initialize vertical speeds and leash lengths
//(-250,250),设置最大下降速率_speed_down_cms，最大上升速率_speed_up_cms，并重新计算了_leash_up_z，_leash_down_z。
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
//250,设置最大加速度_accel_z_cms，并重新计 算了_leash_up_z，_leash_down_z。
pos_control.set_accel_z(g.pilot_accel_z);
// initialise position and desired velocity
//将当前EKF的高度值（_relpos_cm.z）赋值给_pos_target.z。
pos_control.set_alt_target(inertial_nav.get_altitude());
//将当前EKF的爬升速率（_velocity_cm.z）赋值给_vel_desired.z。从这里可以看到，如果我们切到定高模式之前，飞机仍在爬升或下降，切换到定高模式后，飞机 还是会继续爬升或下降的
pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
// stop takeoff if running
takeoff_stop();
return true;
}
高度控制函数在精简掉姿态控制相关内容后，代码如下:
void Copter::althold_run()
{
AltHoldModeState althold_state;
float takeoff_climb_rate = 0.0f;
// initialize vertical speeds and acceleration
// 设置z轴最小速度和最大速度
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
// 设置z轴加速度
pos_control.set_accel_z(g.pilot_accel_z);
// get pilot desired climb rate
// 根据遥控器输入，算出爬升速度
float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->control_in);
target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max, g.pilot_velocity_z_max);
// call position controller
pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
pos_control.update_z_controller();
}
在get_pilot_desired_climb_rate中需要获取mid_stick，注意由于THR_MIN和RC3_DZ的存在，这个值并不是500(千分之)，而是548(futuba t8fg遥控器)，所以deadband是448到648，要想保持定要，油门输入必须保持在448到648之间。
set_alt_target_from_climb_rate_ff是根据target_climb_rate的速度去计算目标高度，在这个函数内部用了一个取平方根的方式，使速度变化由快变慢，可以由下面matlab生成的图片看出来，x轴为此函数运行次数，y轴为_vel_desired.z，_vel_desired.z初始值为0，target_climb_rate设置100的时候画出的图，这样做的目的估计是使飞行器可以先快后慢的变化，保持最后的平稳。
