APM源码分析之 油门跟踪

这篇文章的目的是跟踪从遥控到电机输出的过程，已定高模式为例。

代码结构

油门电机输出

AP_Motors:: set_throttle: _rc_throttle.servo_out

AP_Motors::output()->output_armed()->_rc_throttle.calc_pwm():_rc_throttle.radio_out

油门量计算（定高模式）

target_climb_rate =get_pilot_desired_climb_rate(g.rc_3.control_in) 获取爬升速度(期望)

AC_PosControl::set_alt_target_from_climb_rate:_pos_target.z 计算目标高度

AC_PosControl::update_z_controller()

     if上次被调用的时间大于200ms

          _flags.reset_rate_to_accel_z= true(导致:_vel_last.z = _vel_target.z)

         _flags.reset_accel_to_throttle = true(导致: _accel_error.z=0)

     calc_leash_length_z()

         _leash_up_z= calc_leash_length(_speed_up_cms, _accel_z_cms, _p_alt_pos.kP()

            _leash_down_z =calc_leash_length(-_speed_down_cms, _accel_z_cms, _p_alt_pos.kP())

                 x = POILOT_VELZ_MAX/THR_ALT_MAX

     pos_to_rate_z()

         _pos_error.z= _pos_target.z - curr_alt

         调整 _pos_target.z 和 _pos_error.z,防止与当前高度差太大

         计算_vel_target.z 目标速度

         rate_to_accel_z()

              最大速度限制（_speed_down_cms /_speed_up_cms ：PILOT_VELZ_MAX）

              _accel_feedforward.z= (_vel_target.z - _vel_last.z)/_dt 设置加速度前馈值

              desired_accel=_accel_feedforward.z+ THROTTLE_RATE_P * _vel_error.z

              accel_to_throttle(desired_accel)加速度转换为油门值

                   _attitude_control.set_throttle_out(_throttle_hover+ pid值,true)

遥控器油门到rc_3.control_in

read_radio()

     rc_3.set_pwm(hal.rcin->read(CH_3))

         control_in= pwm_to_range()

              pwm_to_range_dz(_dead_zone)把pwm 值转换为阀门值（按比例转换）
详细分析

step1：获取飞机的期望爬升率，根据遥控输入 g.rc_3.control_in (0-1000),计算期望的爬山速度，此时影响到结果的除了遥控输入就是参数PILOT_VELZ_MAX。

step2：设置位置控制的期望爬升速度，且计算目标高度：_pos_target.z（原高度+期望速度*时间），剩下的工作就交给位置控制器去做了。

step3：计算目标垂直速度:_vel_target.z,首先计算高度Error :_pos_error.z,，然后计算目标爬升速度（注：如果error小的时候直接为THR_ALT_P*error,大的时候，根据PILOT_VELZ_MAX值采用线性接近的方式）

step4：计算期望垂直加速度, 首先更加当前的垂直速度（受反弹空气影响起飞阶段肯定不准确）和目标目标垂直速度计算出垂直速度Error, 然后期望加速度=THROTTLE_RATE_P*error+加速度前馈

step5：根据当前垂直加速度和期望垂直加速度计算出姿态油门输出，首先计算垂直加速度Error(经过低通滤波后)，最终姿态油门输出=p+i+d + _throttle_hover(悬停值，这个值时刻在计算修正)