注册 登录  
 加关注
   显示下一条  |  关闭
温馨提示!由于新浪微博认证机制调整,您的新浪微博帐号绑定已过期,请重新绑定!立即重新绑定新浪微博》  |  关闭

FY

Johnson 's Blog

 
 
 

日志

 
 

APM源码分析之 油门跟踪  

2016-06-12 08:39:40|  分类: 飞控 |  标签: |举报 |字号 订阅

  下载LOFTER 我的照片书  |

http://blog.csdn.net/jxw_flyzero/article/details/47683975

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

代码结构

油门电机输出

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


0


  评论这张
 
阅读(67)| 评论(0)
推荐 转载

历史上的今天

在LOFTER的更多文章

评论

<#--最新日志,群博日志--> <#--推荐日志--> <#--引用记录--> <#--博主推荐--> <#--随机阅读--> <#--首页推荐--> <#--历史上的今天--> <#--被推荐日志--> <#--上一篇,下一篇--> <#-- 热度 --> <#-- 网易新闻广告 --> <#--右边模块结构--> <#--评论模块结构--> <#--引用模块结构--> <#--博主发起的投票-->
 
 
 
 
 
 
 
 
 
 
 
 
 
 

页脚

网易公司版权所有 ©1997-2017