PixHawk学习笔记 之 源码浅析—mc_pos_control.cpp—do_control(dt)—control_non_manual(dt)

开头说两句

非手动控制在此

介绍两个词汇

vaild：有效的。
generation：产生。

再整一个框架

• 第一个 if：选择机外控制还是自动控制
  if（flag_control_offboard_enabled）
  { 机外控制 }
  else
  { 自动控制 }；
• 第二个if
• 第三个if：速度追踪
• 第四个if：着陆
• 第五个if：起飞
  if（）
  { }
  else
  { 起飞失败 }；
• 第六个if：空闲状态

再看 control_non_manual(dt) 源码

voidMulticopterPositionControl::control_non_manual(float dt){/* ==========================================   第一个if   ========================================== */    /* select control source     * 选择控制源 */    /* 不是机外控制就是自动控制，二选一 */    if (_control_mode.flag_control_offboard_enabled) {        /* offboard control         * 机外控制 */        control_offboard(dt);        _mode_auto = false;    } else {        _hold_offboard_xy = false;        _hold_offboard_z = false;        /* AUTO         * 自动控制 */        control_auto(dt);    }/* ==========================================   第二个if   ========================================== */    /* weather-vane mode for vtol: disable yaw control     * 垂直起降的“weather-vane”模式：禁用航向控制 */    if (_pos_sp_triplet.current.disable_mc_yaw_control == true) {        _att_sp.disable_mc_yaw_control = true;    } else {        /* reset in case of setpoint updates         * 重置一下，以防设定值更新 */        _att_sp.disable_mc_yaw_control = false;    }/* ==========================================   第三个if   ========================================== */    /* guard against any bad velocity values     * 防止任何错误的速度值 */    /* vaild：有效的。velocity_valid 是用来判定速度有效的一个bool型变量 */    bool velocity_valid = PX4_ISFINITE(_pos_sp_triplet.current.vx) &&                  PX4_ISFINITE(_pos_sp_triplet.current.vy) &&                  _pos_sp_triplet.current.velocity_valid;    /* do not go slower than the follow target velocity when position tracking is active (set to valid)      * 当位置追踪处于激活状态（设置为有效）时，不要慢于被追踪目标的速度 */    if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET &&        velocity_valid &&        _pos_sp_triplet.current.position_valid) {        math::Vector<3> ft_vel(_pos_sp_triplet.current.vx, _pos_sp_triplet.current.vy, 0);        float cos_ratio = (ft_vel * _vel_sp) / (ft_vel.length() * _vel_sp.length());        /* only override velocity set points when uav is traveling in same direction as target          * and vector component is greater than calculated position set point velocity component         * 当无人机飞行方向与我们目标方向一致并且矢量分量大于计算出的位置设定值的速度分量时，覆盖速度设定值 */        if (cos_ratio > 0) {            ft_vel *= (cos_ratio);            /* min speed a little faster than target vel              * 最小速度比目标速度值稍大 */            ft_vel += ft_vel.normalized() * 1.5f;        } else {            ft_vel.zero();        }        _vel_sp(0) = fabsf(ft_vel(0)) > fabsf(_vel_sp(0)) ? ft_vel(0) : _vel_sp(0);        _vel_sp(1) = fabsf(ft_vel(1)) > fabsf(_vel_sp(1)) ? ft_vel(1) : _vel_sp(1);        /* track target using velocity only         * 仅使用速度跟踪目标 */    } else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET &&           velocity_valid) {        _vel_sp(0) = _pos_sp_triplet.current.vx;        _vel_sp(1) = _pos_sp_triplet.current.vy;    }/* ==========================================   第四个if   ========================================== */    /* use constant descend rate when landing, ignore altitude setpoint      * 着陆时使用恒定下降速率，同时忽略高度设定值 */    if (_pos_sp_triplet.current.valid        && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {        _vel_sp(2) = _params.land_speed;        _run_alt_control = false;    }/* ==========================================   第五个if   ========================================== */    /* special thrust setpoint generation for takeoff from ground      * 从地面起飞时，产生特殊的推力设定值 */    if (_pos_sp_triplet.current.valid        && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF        && _control_mode.flag_armed) {        /* check if we are not already in air.         * if yes then we don't need a jumped takeoff anymore         * 检查我们是否已经不在地面了，如果是，我们就不再需要起飞了（这里英文注释写错了） */        if (!_takeoff_jumped && !_vehicle_land_detected.landed && fabsf(_takeoff_thrust_sp) < FLT_EPSILON) {            _takeoff_jumped = true;        }        if (!_takeoff_jumped) {            /* ramp thrust setpoint up              * 增大推力设定值 */            if (_vel(2) > -(_params.tko_speed / 2.0f)) {                _takeoff_thrust_sp += 0.5f * dt;                _vel_sp.zero();                _vel_prev.zero();            } else {                /* copter has reached our takeoff speed. split the thrust setpoint up                 * into an integral part and into a P part                 * 直升机已达到起飞速度。将向上的推力分成一个整体部分，并作为P部分 */                _thrust_int(2) = _takeoff_thrust_sp - _params.vel_p(2) * fabsf(_vel(2));                _thrust_int(2) = -math::constrain(_thrust_int(2), _params.thr_min, _params.thr_max);                _vel_sp_prev(2) = -_params.tko_speed;                _takeoff_jumped = true;                _reset_int_z = false;            }        }        if (_takeoff_jumped) {            _vel_sp(2) = -_params.tko_speed;        }    } else {        _takeoff_jumped = false;        _takeoff_thrust_sp = 0.0f;    }/* ==========================================   第六个if   ========================================== */    if (_pos_sp_triplet.current.valid        && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {        /* idle state, don't run controller and set zero thrust         * 空闲状态，不运行控制器并且设定推力值为0 */        _R_setpoint.identity();        matrix::Quatf qd = _R_setpoint;        memcpy(&_att_sp.q_d[0], qd.data(), sizeof(_att_sp.q_d));        _att_sp.q_d_valid = true;        _att_sp.roll_body = 0.0f;        _att_sp.pitch_body = 0.0f;        _att_sp.yaw_body = _yaw;        _att_sp.thrust = 0.0f;        _att_sp.timestamp = hrt_absolute_time();    } else {        /* 位置控制，该子程序有源码解读，如需查看，请至文后链接 */        control_position(dt);    }}

几个子程序

control_position(dt)

写在最后

本程序的输出是：
_vel_sp(0)
_vel_sp(1)
_vel_sp(2)
分别对应XYZ的速度设定值。