mc_pos_control.cpp 之 control_offboard(dt)

• control_offboard(float dt)

voidMulticopterPositionControl::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();    }}