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

开头说两句

手动控制的核心在此。

再说两个名词

local frame：相当于b系吧（我感觉是，不确定）

再整一个框架

• 前期准备开始
• 前期准备结束
• 由推杆获取b系下的速度设定值
• 根据公式：v_n = C_n_b * v_b获取n系下的速度设定值
• 第一个if：水平轴
  if（flag_control_position_enabled） { }
  else { }；
• 第二个if：垂直轴
  if（flag_control_altitude_enabled） { }
  else { }；
• 第三个if：着陆后
  if（_vehicle_land_detected）{ }
  else { }；

再看 control_manual(dt) 源码

void
MulticopterPositionControl::control_manual(float dt)
{
/* =========================================   前期准备开始   ======================================== */
    /* Entering manual control from non-manual control mode,
     * reset alt/pos setpoints
     * 由非手动控制切换到手动控制，重置高度和位置设定值 */
    if (_mode_auto) {
        _mode_auto = false;

        /* Reset alt pos flags if resetting is enabled
         * 如果可以的话，复位高度和位置标志位*/
        if (_do_reset_alt_pos_flag) {
            _reset_pos_sp = true;
            _reset_alt_sp = true;
        }
    }

/* ========================================   前期准备结束   ========================================= */
    /* 由推杆获取b系下的速度设定值 */
    /* X,Y in local frame and Z in global (D), in [-1,1] normalized range 
     * X，Y在机体坐标系，Z在大地坐标系，均被量化在[-1，1] */
    /* req_vel_sp 是期望速度设定值 */
    math::Vector<3> req_vel_sp; 
    /* 将其初始化为0向量  */
    req_vel_sp.zero();

    /* 以下两个if为 req_vel_sp 赋值 */
    if (_control_mode.flag_control_altitude_enabled) {
        /* set vertical velocity setpoint with throttle stick
         * 通过油门杆设定垂直速度设定值*/
        req_vel_sp(2) = -scale_control(_manual.z - 0.5f, 0.5f, _params.alt_ctl_dz, _params.alt_ctl_dy);
    }

    if (_control_mode.flag_control_position_enabled) {
        /* set horizontal velocity setpoint with roll/pitch stick
         * 通过横滚航线推杆设定XY向速度设定值*/
        req_vel_sp(0) = _manual.x;
        req_vel_sp(1) = _manual.y;
    }
    /* req_vel_sp 赋值完成 */

    /* 推杆给 req_vel_sp 赋值完成后，重置高度和位置设定值为当前状态下的值 */
    if (_control_mode.flag_control_altitude_enabled) {
        /* reset alt setpoint to current altitude if needed
         * 将高度设定值重置为当前高度 */
        reset_alt_sp();
    }
    if (_control_mode.flag_control_position_enabled) {
        /* reset position setpoint to current position if needed
         * 将位置设定值重置为当前位置 */
        reset_pos_sp();
    }

    /* limit velocity setpoint
     * 限制速度设定值在[-1.0 , 1.0] */
    float req_vel_sp_norm = req_vel_sp.length();
    if (req_vel_sp_norm > 1.0f) {
        req_vel_sp /= req_vel_sp_norm;
    }
    /* 至此，b系下的速度设定值已经计算完毕，为 req_vel_sp，它是一个3×1的矩阵 */

    /*  根据公式：v_n = C_n_b * v_b获取n系下的速度设定值 */
    /* _req_vel_sp scaled to 0..1, scale it to max speed and rotate around yaw
     *将_req_vel_sp量化为0-1，将其缩放到最大速度并绕航向旋转,这是什么鬼 */
    /* R_yaw_sp为旋转矩阵，即C_n_b */
    math::Matrix<3, 3> R_yaw_sp;
    /* 通过欧拉角得到旋转矩阵 */
    R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
    /* n系下的速度 = C_n_b * b系下的速度
     * req_vel_sp_scaled 为在n系下的速度，即我们观测到的速度 
     * 以下为计算n系下的速度 */
    math::Vector<3> req_vel_sp_scaled = R_yaw_sp * req_vel_sp.emult(
            _params.vel_cruise); // in NED and scaled to actual velocity
            /* 至此，n系下的速度设定值已经计算完毕，它是一个3×1的矩阵 */

    /* assisted velocity mode: user controls velocity, but if velocity is small enough, position
     * hold is activated for the corresponding axis
     * 辅助速度模式：用户控制速度，但是如果速度足够小，则相应轴的位置保持被激活 */

/* ==========================================   第一个if   ========================================== */
    /* horizontal axes
     * 水平轴 */
    if (_control_mode.flag_control_position_enabled) {
        /* check for pos. hold
         * 检查pos.hold */
        if (fabsf(req_vel_sp(0)) < _params.hold_xy_dz && fabsf(req_vel_sp(1)) < _params.hold_xy_dz) {
            if (!_pos_hold_engaged) {

                float vel_xy_mag = sqrtf(_vel(0) * _vel(0) + _vel(1) * _vel(1));

                if (_params.hold_max_xy < FLT_EPSILON || vel_xy_mag < _params.hold_max_xy) {
                    /* reset position setpoint to have smooth transition
                     * from velocity control to position control
                     * 重置位置设定值，以便于能够平稳地由速度控制模式转换到位置控制模式 */
                    _pos_hold_engaged = true;
                    _pos_sp(0) = _pos(0);
                    _pos_sp(1) = _pos(1);

                } else {
                    _pos_hold_engaged = false;
                }
            }

        } else {
            _pos_hold_engaged = false;
        }

        /* set requested velocity setpoint
         * 设置要求的速度设定值 */
        if (!_pos_hold_engaged) {
            _pos_sp(0) = _pos(0);
            _pos_sp(1) = _pos(1);
            /* request velocity setpoint to be used, instead of position setpoint
             * “要求的速度设定值”将被使用，而非位置设定值 */
            _run_pos_control = false; 
            _vel_sp(0) = req_vel_sp_scaled(0);
            _vel_sp(1) = req_vel_sp_scaled(1);
        }
    }

/* ==========================================   第二个if   ========================================== */
    /* vertical axis
     * 垂直轴 */
    if (_control_mode.flag_control_altitude_enabled) {
        /* check for pos. hold
         * 检查pos. hold */
        if (fabsf(req_vel_sp(2)) < FLT_EPSILON) {
            if (!_alt_hold_engaged) {
                if (_params.hold_max_z < FLT_EPSILON || fabsf(_vel(2)) < _params.hold_max_z) {
                    /* reset position setpoint to have smooth transition 
                     *from velocity control to position control
                     * 重置位置设定值，以便于能够平稳地由速度控制模式转换为位置控制模式 */
                    _alt_hold_engaged = true;
                    _pos_sp(2) = _pos(2);

                } else {
                    _alt_hold_engaged = false;
                }
            }

        } else {
            _alt_hold_engaged = false;
            _pos_sp(2) = _pos(2);
        }

        /* set requested velocity setpoint
         * 设置需要的速度设定值 */
        if (!_alt_hold_engaged) {
            /* request velocity setpoint to be used, instead of altitude setpoint
             * “要求的速度设定值”将被使用，而非高度设定值 */
            _run_alt_control = false; 
            _vel_sp(2) = req_vel_sp_scaled(2);
        }
    }

/* ==========================================   第三个if   ========================================== */
    if (_vehicle_land_detected.landed) {
        /* don't run controller when landed
         * 着陆后不要运行控制器 */
        _reset_pos_sp = true;
        _reset_alt_sp = true;
        _mode_auto = false;
        _reset_int_z = true;
        _reset_int_xy = true;

        _R_setpoint.identity();

        _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);
    }

}

写在最后

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