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(悬停值,这个值时刻在计算修正)

APM源码分析之 油门跟踪_第1张图片

你可能感兴趣的:(飞行控制,APM,APM,飞控,pid,无人机)