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) 源码

voidMulticopterPositionControl::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方向的速度设定值。