px4源码学习五--固定翼位置控制模块

fw_pos_control模块

class landingslope

为固定翼着陆的角度变化模块

• calulateSlopeValues() void private
  更新H1,H0,d1,根据log（H0/H1）的比例调整 d1 / d1+ delta d的比例，更新其他参数
• getLandingSlopeRelativeAltitude(wp_landing_distance) float
  返回在距离落航点的着陆坡上点的相对高度，调用多参数的同名函数
• getLandingSlopeRelativeAltitudeSave(wp_landing_distance,bearing_lastwp_currwp,bearing_airplane_currwp) float public
  返回在距离落航点的着陆坡上点的相对高度，检查飞行器是否在航点上来避免爬升
• getLandingSlopeAbsoluteAltitude(wp_landing_distance,wp_altitude) float
  返回在距离落航点的着陆坡上点的绝对高度

基本上示例都是这么一个结构

• getLandingSlopeAbsoluteAltitudeSave(wp_landing_distance,bearing_last_wp_currwp,bearing_airplane_currwp) float
• 检查飞行器是否在航点上来避免爬升
• getLandingSlopeRelativeAltitude(wp_landing_distance,horizontal_slope_displacement,landing_slope_angle_rad) float static
  返回h_flare.rel的高度，返回在距离落航点的着陆坡上点的相对高度
• getLandingSlopeAbsoluteAltitude(wp_landing_distance,wp_landing_altitude,horizontal_slope_displacement,landing_slope_angle_rad) float static
  返回h_flare.rel + H1的高度，返回在距离落航点的着陆坡上点的绝对高度
• getLandigSlopeWPDistance(slope_altitude,wp_landing_altitude,horizontal_slope_displacement,landing_slope_angle_rad) float static
  给定降落高度，返回距离预定降落点的距离
• getFlareCurveRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp) float
• getFlareCurveRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, wp_altitude) float
  获取Flare曲线的相对高度
• updata(landing_slope_angle_Rad_new, flare_relative_alt_new, mmotor_lim_relative_alt_new, H1_virt_new) void
  将值重新赋值，并且重新调用calcuateSlopeValues()

class lib/extern_lgpl/tecs

这个类负责控制俯仰和油门来控制速度和高度，在后面会用到，不做深入讲究，只是为了简绍这个类的作用

/*
 *  Written by Paul Riseborough 2013 to provide:
 *  - Combined control of speed and height using throttle to control
 *    total energy and pitch angle to control exchange of energy between
 *    potential and kinetic.
 *    通过控制油门来控制能量，控制俯仰控制势能，使得速度高度结合控制，在势能和运动结合
 *    Selectable speed or height priority modes when calculating pitch angle
 *    当计算俯仰角度时优先选择速度/高度模式
 *  - Fallback mode when no airspeed measurement is available that
 *    sets throttle based on height rate demand and switches pitch angle control to
 *    height priority
 *    当空速测量不管用时，切换俯仰角度优先控制到高度优先控制，并设置油门基于高度速率需求
 *  - Underspeed protection that demands maximum throttle switches pitch angle control
 *    to speed priority mode
 *    需求最大油门的低速保护下，切换俯仰控制优先到速度优先模式
 *  - Relative ease of tuning through use of intuitive time constant, trim rate and damping parameters and the use
 *    of easy to measure aircraft performance data
 *    通过使用直观的时间常数，微调速率和阻尼参数以及使用易于测量飞机性能数据的相对易于调谐
 *   
 */

class FixedwingPositionControl

FixedwingPositionControl:

使用param_find()函数定位所有的参数,赋初值

start() int static public

开始控制

task_running() bool

任务的状态

更新控制函数：

• parameters_update() int private
  更新所有的类内参数，使用param_get()方法
• control_update() void
  更新控制输出
• vehicle_control_mode_poll()
  飞行器模式更新，通过orb_check来检测是否更新
• vehicle_status_poll()
  飞行器状态更新，通过orb_check
• vehicle_manual_control_setpoint_poll()
  飞行器手动控制设定点更新，同上
• control_state_poll()
  控制状态更新
  如果空速1s内没有更新，认定是非法的。
  将四元组的控制信息转化为欧拉角度
  更新TECS的状态
• vehicle_sensor_combined_poll()
  飞行器传感器状态更新
• vehicle_setpoint_poll()
  设置点更新
• navigation_capabilities_publish()
  导航能力输出

get_demanded_airspeed() float

返回需求的空速

实现：

根据油门输入(_manual.z)大于0.5与否，决定速度应该变大还是变小（按照比例）

// 如果油门输入<0.5，输出空速应该是 最小速度+(可缩减速度)*(2×输入值)  

    // 可缩减速度为当前速度到最小速度的差，以0.5*2为1，中值，不会变化

calculate_target_airspeed(airspeed_demand) float

返回目标速度，输出需求速度

实现：

一般只对需求速度限制在最大和最小的范围内

在目的空速加上最小下冲速度(_groundspeed_undershoot)，只有在有强风时候这个最小下冲速度不为0

calculate_gnd_speed_undershoot(&current_pos