mc_pos_control.cpp 之 control_offboard(dt)

• control_offboard(float dt)

void
MulticopterPositionControl::control_offboard(float dt)
{
    if (_pos_sp_triplet.current.valid) {

        if (_control_mode.flag_control_position_enabled && _pos_sp_triplet.current.position_valid) {
            /* control position
             * 控制位置 */
            _pos_sp(0) = _pos_sp_triplet.current.x;
            _pos_sp(1) = _pos_sp_triplet.current.y;
            _run_pos_control = true;

            _hold_offboard_xy = false;

        } else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) {
            /* control velocity
             * 控制速度 */

            /* reset position setpoint to current position if needed
             * 如果需要，将位置设定值重置为当前位置 */
            reset_pos_sp();

            if (fabsf(_pos_sp_triplet.current.vx) <= FLT_EPSILON &&
                fabsf(_pos_sp_triplet.current.vy) <= FLT_EPSILON &&
                _local_pos.xy_valid) {

                if (!_hold_offboard_xy) {
                    _pos_sp(0) = _pos(0);
                    _pos_sp(1) = _pos(1);
                    _hold_offboard_xy = true;
                }

                _run_pos_control = true;

            } else {

                if (_pos_sp_triplet.current.velocity_frame == position_setpoint_s::VELOCITY_FRAME_LOCAL_NED) {
                    /* set position setpoint move rate
                     * 设定位置设定值的变化速率 */
                    _vel_sp(0) = _pos_sp_triplet.current.vx;
                    _vel_sp(1) = _pos_sp_triplet.current.vy;

                } else if (_pos_sp_triplet.current.velocity_frame == position_setpoint_s::VELOCITY_FRAME_BODY_NED) {
                    /* Transform velocity command from body frame to NED frame
                     * 速度转换指令：将机体坐标系下的速度转换到北东地大地坐标系 */
                    _vel_sp(0) = cosf(_yaw) * _pos_sp_triplet.current.vx - sinf(_yaw) * _pos_sp_triplet.current.vy;
                    _vel_sp(1) = sinf(_yaw) * _pos_sp_triplet.current.vx + cosf(_yaw) * _pos_sp_triplet.current.vy;

                } else {
                    PX4_WARN("Unknown velocity offboard coordinate frame");
                }

                _run_pos_control = false;

                _hold_offboard_xy = false;
            }

        }

        if (_control_mode.flag_control_altitude_enabled && _pos_sp_triplet.current.alt_valid) {
            /* control altitude as it is enabled
             * 入股使能，则控制高度 */
            _pos_sp(2) = _pos_sp_triplet.current.z;
            _run_alt_control = true;

            _hold_offboard_z = false;

        } else if (_control_mode.flag_control_climb_rate_enabled && _pos_sp_triplet.current.velocity_valid) {

            /* reset alt setpoint to current altitude if needed
             * 如果需要，将高度设定值重置为当前高度 */
            reset_alt_sp();

            if (fabsf(_pos_sp_triplet.current.vz) <= FLT_EPSILON &&
                _local_pos.z_valid) {

                if (!_hold_offboard_z) {
                    _pos_sp(2) = _pos(2);
                    _hold_offboard_z = true;
                }

                _run_alt_control = true;

            } else {
                /* set position setpoint move rate
                 * 设定位置设定值变化速率 */
                _vel_sp(2) = _pos_sp_triplet.current.vz;
                _run_alt_control = false;

                _hold_offboard_z = false;
            }
        }

        if (_pos_sp_triplet.current.yaw_valid) {
            _att_sp.yaw_body = _pos_sp_triplet.current.yaw;

        } else if (_pos_sp_triplet.current.yawspeed_valid) {
            _att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt;
        }

    } else {
        _hold_offboard_xy = false;
        _hold_offboard_z = false;
        reset_pos_sp();
        reset_alt_sp();
    }
}