mc_pos_control.cpp 之 control_position(dt)

• control_position(dt)

voidMulticopterPositionControl::control_position(float dt){    /* run position & altitude controllers,     * if enabled (otherwise use already computed velocity setpoints)      * 如果被使能，则运行位置和高度控制器（否则使用已经计算的速度设定值）*/    if (_run_pos_control) {        _vel_sp(0) = (_pos_sp(0) - _pos(0)) * _params.pos_p(0);        _vel_sp(1) = (_pos_sp(1) - _pos(1)) * _params.pos_p(1);    }    if (_run_alt_control) {        _vel_sp(2) = (_pos_sp(2) - _pos(2)) * _params.pos_p(2);    }    /* make sure velocity setpoint is saturated in xy      * 确保xy向速度设定值饱和 */    float vel_norm_xy = sqrtf(_vel_sp(0) * _vel_sp(0) +                  _vel_sp(1) * _vel_sp(1));    if (vel_norm_xy > _params.vel_max(0)) {        /* note assumes vel_max(0) == vel_max(1)          * 注意假设vel_max(0) == vel_max(1) */        _vel_sp(0) = _vel_sp(0) * _params.vel_max(0) / vel_norm_xy;        _vel_sp(1) = _vel_sp(1) * _params.vel_max(1) / vel_norm_xy;    }    /* make sure velocity setpoint is saturated in z     * 确保z向速度设定值饱和 */    if (_vel_sp(2) < -1.0f * _params.vel_max_up) {        _vel_sp(2) = -1.0f * _params.vel_max_up;    }    if (_vel_sp(2) >  _params.vel_max_down) {        _vel_sp(2) = _params.vel_max_down;    }    if (!_control_mode.flag_control_position_enabled) {        _reset_pos_sp = true;    }    if (!_control_mode.flag_control_altitude_enabled) {        _reset_alt_sp = true;    }    if (!_control_mode.flag_control_velocity_enabled) {        _vel_sp_prev(0) = _vel(0);        _vel_sp_prev(1) = _vel(1);        _vel_sp(0) = 0.0f;        _vel_sp(1) = 0.0f;    }    if (!_control_mode.flag_control_climb_rate_enabled) {        _vel_sp(2) = 0.0f;    }    /* TODO: remove this is a pathetic leftover, it's here just to make sure that     * _takeoff_jumped flags are reset      * TODO：删除。这是一个无价值的剩余物，它出现在这里只是确保_takeoff_jumped标志位被重置*/    if (_control_mode.flag_control_manual_enabled || !_pos_sp_triplet.current.valid        || _pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF        || !_control_mode.flag_armed) {        _takeoff_jumped = false;        _takeoff_thrust_sp = 0.0f;    }    limit_acceleration(dt);    _vel_sp_prev = _vel_sp;    _global_vel_sp.vx = _vel_sp(0);    _global_vel_sp.vy = _vel_sp(1);    _global_vel_sp.vz = _vel_sp(2);    /* publish velocity setpoint      * 发布速度设定值 */    if (_global_vel_sp_pub != nullptr) {        orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &_global_vel_sp);    } else {        _global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp);    }    if (_control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled ||        _control_mode.flag_control_acceleration_enabled) {        /* reset integrals if needed          * 如果需要，重置积分项 */        if (_control_mode.flag_control_climb_rate_enabled) {            if (_reset_int_z) {                _reset_int_z = false;                _thrust_int(2) = 0.0f;            }        } else {            _reset_int_z = true;        }        if (_control_mode.flag_control_velocity_enabled) {            if (_reset_int_xy) {                _reset_int_xy = false;                _thrust_int(0) = 0.0f;                _thrust_int(1) = 0.0f;            }        } else {            _reset_int_xy = true;        }        /* velocity error          * 速度误差 */        math::Vector<3> vel_err = _vel_sp - _vel;        /* thrust vector in NED frame          * 在北东地大地坐标系下的推力向量 */        math::Vector<3> thrust_sp;        if (_control_mode.flag_control_acceleration_enabled && _pos_sp_triplet.current.acceleration_valid) {            thrust_sp = math::Vector<3>(_pos_sp_triplet.current.a_x, _pos_sp_triplet.current.a_y, _pos_sp_triplet.current.a_z);        } else {            thrust_sp = vel_err.emult(_params.vel_p) + _vel_err_d.emult(_params.vel_d)                    + _thrust_int - math::Vector<3>(0.0f, 0.0f, _params.thr_hover);        }        if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF            && !_takeoff_jumped && !_control_mode.flag_control_manual_enabled) {            /* for jumped takeoffs use special thrust setpoint calculated above             * 对于跳跃起飞，使用上面计算的特殊推力设定值 */            thrust_sp.zero();            thrust_sp(2) = -_takeoff_thrust_sp;        }        if (!_control_mode.flag_control_velocity_enabled && !_control_mode.flag_control_acceleration_enabled) {            thrust_sp(0) = 0.0f;            thrust_sp(1) = 0.0f;        }        /* if still or already on ground command zero xy velcoity and          * zero xy thrust_sp in body frame to consider uneven ground          * 如果依然或者早已经在地面上，则把机体坐标系下的xy向的速度和推力设定值归零，以防止停在不平坦的地面上 */        if (_vehicle_land_detected.ground_contact) {            /* thrust setpoint in body frame              * 机体坐标系下的推力设定值 */            math::Vector<3> thrust_sp_body = _R.transposed() * thrust_sp;            /* we dont want to make any correction in body x and y              * 我们不想对机身的xy进行任何修正 */            thrust_sp_body(0) = 0.0f;            thrust_sp_body(1) = 0.0f;            /* make sure z component of thrust_sp_body is larger than 0              * (positive thrust is downward)              * 确保thrust_sp_body在Z向的分量大于0，主动的推力是向下的 */            thrust_sp_body(2) = thrust_sp(2) > 0.0f ? thrust_sp(2) : 0.0f;            /* convert back to local frame (NED)              * 转换回大地坐标系 */            thrust_sp = _R * thrust_sp_body;            /* set velocity setpoint to zero and reset position              * 设置速度设定值为0并重置位置 */            _vel_sp(0) = 0.0f;            _vel_sp(1) = 0.0f;            _pos_sp(0) = _pos(0);            _pos_sp(1) = _pos(1);        }        if (!_control_mode.flag_control_climb_rate_enabled && !_control_mode.flag_control_acceleration_enabled) {            thrust_sp(2) = 0.0f;        }        /* limit thrust vector and check for saturation          * 限制推力向量并检查饱和度 */        bool saturation_xy = false;        bool saturation_z = false;        /* limit min lift          * 限制最小升力 */        float thr_min = _params.thr_min;        if (!_control_mode.flag_control_velocity_enabled && thr_min < 0.0f) {            /* don't allow downside thrust direction in manual attitude mode              * 在手动控制模式，不允许向下的推力 */            thr_min = 0.0f;        }        float tilt_max = _params.tilt_max_air;        float thr_max = _params.thr_max;        /* filter vel_z over 1/8sec          * 每1/8s 对z向速度进行滤波 */        _vel_z_lp = _vel_z_lp * (1.0f - dt * 8.0f) + dt * 8.0f * _vel(2);        /* filter vel_z change over 1/8sec          * 每1/8秒 对z向速度变化进行滤波 */        float vel_z_change = (_vel(2) - _vel_prev(2)) / dt;        _acc_z_lp = _acc_z_lp * (1.0f - dt * 8.0f) + dt * 8.0f * vel_z_change;        /* We can only run the control if we're already in-air, have a takeoff setpoint,         * or if we're in offboard control.         * Otherwise, we should just bail out         * 如果我们早已在空中，我们只能运行控制器，除非我们在offboard控制模式下，否则我们需要一个起飞设定值，         * 否则，。。。*/        const bool got_takeoff_setpoint = (_pos_sp_triplet.current.valid &&                           _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) ||                          _control_mode.flag_control_offboard_enabled;        if (_vehicle_land_detected.landed && !got_takeoff_setpoint) {            /* Keep throttle low while still on ground.             *如果依然在地上，保持低油门 */            thr_max = 0.0f;        } else if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid &&               _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {            /* adjust limits for landing mode              * 调整着陆模式的限制 */            /* limit max tilt and min lift when landing              * 着陆时限制对打倾斜和最小升力 */            tilt_max = _params.tilt_max_land;            if (thr_min < 0.0f) {                thr_min = 0.0f;            }            /* descend stabilized, we're landing              * 下降稳定，我们正在着陆 */            if (!_in_landing && !_lnd_reached_ground                && (float)fabsf(_acc_z_lp) < 0.1f                && _vel_z_lp > 0.6f * _params.land_speed) {                _in_landing = true;            }            float land_z_threshold = 0.1f;            /* assume ground, cut thrust              * 到达地面，切断推力 */            if (_in_landing                && _vel_z_lp < land_z_threshold) {                thr_max = 0.0f;                _in_landing = false;                _lnd_reached_ground = true;            } else if (_in_landing                   && _vel_z_lp < math::min(0.3f * _params.land_speed, 2.5f * land_z_threshold)) {                /* not on ground but with ground contact, stop position and velocity control                  * 不在地面，但是又地面接触，停止位置和速度控制 */                thrust_sp(0) = 0.0f;                thrust_sp(1) = 0.0f;                _vel_sp(0) = _vel(0);                _vel_sp(1) = _vel(1);                _pos_sp(0) = _pos(0);                _pos_sp(1) = _pos(1);            }            /* once we assumed to have reached the ground always cut the thrust.             * Only free fall detection below can revoke this             * 一旦到达地面，总是切断推力，只有自由落体可以撤销该指令 */            if (!_in_landing && _lnd_reached_ground) {                thr_max = 0.0f;            }            /* if we suddenly fall, reset landing logic and remove thrust limit              * 一旦突然下落，重置着陆逻辑并取消推力限制 */            if (_lnd_reached_ground                /* XXX: magic value, assuming free fall above 4m/s2 acceleration                  * XXX：奇异值，我们假定自由落体加速度超过4m/s2 */                && (_acc_z_lp > 4.0f                || _vel_z_lp > 2.0f * _params.land_speed)) {                thr_max = _params.thr_max;                _in_landing = true;                _lnd_reached_ground = false;            }        } else {            _in_landing = false;            _lnd_reached_ground = false;        }        /* limit min lift          * 设置最小升力 */        if (-thrust_sp(2) < thr_min) {            thrust_sp(2) = -thr_min;            /* Don't freeze altitude integral if it wants to throttle up              * 如果要调大油门，就不要固定高度的积分项 */            saturation_z = vel_err(2) > 0.0f ? true : saturation_z;        }        if (_control_mode.flag_control_velocity_enabled || _control_mode.flag_control_acceleration_enabled) {            /* limit max tilt              * 限制最大倾斜角 */            if (thr_min >= 0.0f && tilt_max < M_PI_F / 2 - 0.05f) {                /* absolute horizontal thrust                  * 绝对的水平推力 */                float thrust_sp_xy_len = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();                if (thrust_sp_xy_len > 0.01f) {                    /* max horizontal thrust for given vertical thrust                     * 对于给定的竖直推力对应的最大水平推力 */                    float thrust_xy_max = -thrust_sp(2) * tanf(tilt_max);                    if (thrust_sp_xy_len > thrust_xy_max) {                        float k = thrust_xy_max / thrust_sp_xy_len;                        thrust_sp(0) *= k;                        thrust_sp(1) *= k;                        /* Don't freeze x,y integrals if they both want to throttle down                          * 如果要调小油门，就不要固定xy的积分项 */                        saturation_xy = ((vel_err(0) * _vel_sp(0) < 0.0f) && (vel_err(1) * _vel_sp(1) < 0.0f)) ? saturation_xy : true;                    }                }            }        }        if (_control_mode.flag_control_climb_rate_enabled && !_control_mode.flag_control_velocity_enabled) {            /* thrust compensation when vertical velocity but not horizontal velocity is controlled             * 竖直方向有推力补偿，而水平方向没有 */            float att_comp;            if (_R(2, 2) > TILT_COS_MAX) {                att_comp = 1.0f / _R(2, 2);            } else if (_R(2, 2) > 0.0f) {                att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _R(2, 2) + 1.0f;                saturation_z = true;            } else {                att_comp = 1.0f;                saturation_z = true;            }            thrust_sp(2) *= att_comp;        }        /* Calculate desired total thrust amount in body z direction.          * 计算机体坐标系z向的期望总推力 */        /* To compensate for excess thrust during attitude tracking errors we         * project the desired thrust force vector F onto the real vehicle's thrust axis in NED:         * body thrust axis [0,0,-1]' rotated by R is: R*[0,0,-1]' = -R_z          * 为了在姿态跟踪期间补偿过大的推力，我们将地理坐标系NED中的期望推力向量F投影到机体坐标系         * 机体推力轴[0,0,-1]'由R旋转得到：R*[0,0,-1]' = -R_z */        matrix::Vector3f R_z(_R(0, 2), _R(1, 2), _R(2, 2));        matrix::Vector3f F(thrust_sp.data);        /* recalculate because it might have changed          * 之所以重新计算，是因为它可能会被改写 */        float thrust_body_z = F.dot(-R_z);         /* limit max thrust          * 限制最大推力 */        if (fabsf(thrust_body_z) > thr_max) {            if (thrust_sp(2) < 0.0f) {                if (-thrust_sp(2) > thr_max) {                    /* thrust Z component is too large, limit it                      * 推力Z的比重太大，限制之 */                    thrust_sp(0) = 0.0f;                    thrust_sp(1) = 0.0f;                    thrust_sp(2) = -thr_max;                    saturation_xy = true;                    /* Don't freeze altitude integral if it wants to throttle down                      * 当要调小油门时，不要限制高度的积分项 */                    saturation_z = vel_err(2) < 0.0f ? true : saturation_z;                } else {                    /* preserve thrust Z component and lower XY,                      *keeping altitude is more important than position                      * 保持Z向推力的比重并降低xy向推力的比重，因为保持高度比保持位置更重要 */                    float thrust_xy_max = sqrtf(thr_max * thr_max - thrust_sp(2) * thrust_sp(2));                    float thrust_xy_abs = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();                    float k = thrust_xy_max / thrust_xy_abs;                    thrust_sp(0) *= k;                    thrust_sp(1) *= k;                    /* Don't freeze x,y integrals if they both want to throttle down                     * 当要降低xy向的油门时，不要限制积分项 */                    saturation_xy = ((vel_err(0) * _vel_sp(0) < 0.0f) && (vel_err(1) * _vel_sp(1) < 0.0f)) ? saturation_xy : true;                }            } else {                /* Z component is positive, going down (Z is positive down in NED),                  * simply limit thrust vector                  * z向分量为正则下降（在北东地坐标系中z的正向向下），知识限制推力向量 */                float k = thr_max / fabsf(thrust_body_z);                thrust_sp *= k;                saturation_xy = true;                saturation_z = true;            }            thrust_body_z = thr_max;        }        _att_sp.thrust = math::max(thrust_body_z, thr_min);        /* update integrals          * 更新积分项 */        if (_control_mode.flag_control_velocity_enabled && !saturation_xy) {            _thrust_int(0) += vel_err(0) * _params.vel_i(0) * dt;            _thrust_int(1) += vel_err(1) * _params.vel_i(1) * dt;        }        if (_control_mode.flag_control_climb_rate_enabled && !saturation_z) {            _thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt;        }        /* calculate attitude setpoint from thrust vector          * 由推力向量计算高度设定值 */        if (_control_mode.flag_control_velocity_enabled || _control_mode.flag_control_acceleration_enabled) {            /* desired body_z axis = -normalize(thrust_vector)              * 期望的body_z axis = -normalize(推力向量) */            math::Vector<3> body_x;            math::Vector<3> body_y;            math::Vector<3> body_z;            if (thrust_sp.length() > SIGMA) {                body_z = -thrust_sp.normalized();            } else {                /* no thrust, set Z axis to safe value                  * 没有推力则设定Z轴为安全值 */                body_z.zero();                body_z(2) = 1.0f;            }            /* vector of desired yaw direction in XY plane, rotated by PI/2              * 期望航向的方向矢量在xy平面内，旋转PI/2 */            math::Vector<3> y_C(-sinf(_att_sp.yaw_body), cosf(_att_sp.yaw_body), 0.0f);            if (fabsf(body_z(2)) > SIGMA) {                /* desired body_x axis, orthogonal to body_z                  * 期望的body_x轴正交于body_z */                body_x = y_C % body_z;                /* keep nose to front while inverted upside down                  * 当倒置倒立时，保持鼻子在前方（这是什么鬼） */                if (body_z(2) < 0.0f) {                    body_x = -body_x;                }                body_x.normalize();            } else {                /* desired thrust is in XY plane, set X downside to construct correct matrix,                 * but yaw component will not be used actually                  * 期望的推力在xy平面内，设定下面的X来构造正确的矩阵，但是航向部分实际上是不会被用的 */                body_x.zero();                body_x(2) = 1.0f;            }            /* desired body_y axis              * 期望的body_y轴 */            body_y = body_z % body_x;            /* fill rotation matrix              * 填写旋转矩阵 */            for (int i = 0; i < 3; i++) {                _R_setpoint(i, 0) = body_x(i);                _R_setpoint(i, 1) = body_y(i);                _R_setpoint(i, 2) = body_z(i);            }            /* copy quaternion setpoint to attitude setpoint topic              * 将四元数设定值复制到设定值框架中 */            matrix::Quatf q_sp = _R_setpoint;            memcpy(&_att_sp.q_d[0], q_sp.data(), sizeof(_att_sp.q_d));            _att_sp.q_d_valid = true;            /* calculate euler angles, for logging only, must not be used for control             * 计算欧拉角，仅用于记录，不能用于控制 */            matrix::Eulerf euler = _R_setpoint;            _att_sp.roll_body = euler(0);            _att_sp.pitch_body = euler(1);            /* yaw already used to construct rot matrix,              * but actual rotation matrix can have different yaw near singularity              * 航向已经用于构造旋转矩阵，但实际上旋转矩阵在奇异点附近有不同的航向 */        } else if (!_control_mode.flag_control_manual_enabled) {            /* autonomous altitude control without position control (failsafe landing),             * force level attitude, don't change yaw              * 无位置控制的自动高度控制（安全着陆失败）在不改变航向的前提下，强制控制水平姿态 */            _R_setpoint = matrix::Eulerf(0.0f, 0.0f, _att_sp.yaw_body);            /* copy quaternion setpoint to attitude setpoint topic              * 将四元数复制到姿态控制的框架中 */            matrix::Quatf q_sp = _R_setpoint;            memcpy(&_att_sp.q_d[0], q_sp.data(), sizeof(_att_sp.q_d));            _att_sp.q_d_valid = true;            _att_sp.roll_body = 0.0f;            _att_sp.pitch_body = 0.0f;        }        /* save thrust setpoint for logging          * 为记录日志保存推力设定值 */        _local_pos_sp.acc_x = thrust_sp(0) * ONE_G;        _local_pos_sp.acc_y = thrust_sp(1) * ONE_G;        _local_pos_sp.acc_z = thrust_sp(2) * ONE_G;        _att_sp.timestamp = hrt_absolute_time();    } else {        _reset_int_z = true;    }}