mc_pos_control.cpp 之 generate_attitude_setpoint(dt)

void
MulticopterPositionControl::generate_attitude_setpoint(float dt)
{
    /* reset yaw setpoint to current position if needed */
    if (_reset_yaw_sp) {
        _reset_yaw_sp = false;
        _att_sp.yaw_body = _yaw;
    }

    /* do not move yaw while sitting on the ground */
    else if (!_vehicle_land_detected.landed &&
         !(!_control_mode.flag_control_altitude_enabled && _manual.z < 0.1f)) {

        /* we want to know the real constraint, and global overrides manual */
        const float yaw_rate_max = (_params.man_yaw_max < _params.global_yaw_max) ? _params.man_yaw_max :
                       _params.global_yaw_max;
        const float yaw_offset_max = yaw_rate_max / _params.mc_att_yaw_p;

        _att_sp.yaw_sp_move_rate = _manual.r * yaw_rate_max;
        float yaw_target = _wrap_pi(_att_sp.yaw_body + _att_sp.yaw_sp_move_rate * dt);
        float yaw_offs = _wrap_pi(yaw_target - _yaw);

        // If the yaw offset became too big for the system to track stop
        // shifting it, only allow if it would make the offset smaller again.
        if (fabsf(yaw_offs) < yaw_offset_max ||
            (_att_sp.yaw_sp_move_rate > 0 && yaw_offs < 0) ||
            (_att_sp.yaw_sp_move_rate < 0 && yaw_offs > 0)) {
            _att_sp.yaw_body = yaw_target;
        }
    }

    /* control throttle directly if no climb rate controller is active */
    if (!_control_mode.flag_control_climb_rate_enabled) {
        float thr_val = throttle_curve(_manual.z, _params.thr_hover);
        _att_sp.thrust = math::min(thr_val, _manual_thr_max.get());

        /* enforce minimum throttle if not landed */
        if (!_vehicle_land_detected.landed) {
            _att_sp.thrust = math::max(_att_sp.thrust, _manual_thr_min.get());
        }
    }

    /* control roll and pitch directly if no aiding velocity controller is active */
    if (!_control_mode.flag_control_velocity_enabled) {
        _att_sp.roll_body = _manual.y * _params.man_roll_max;
        _att_sp.pitch_body = -_manual.x * _params.man_pitch_max;

        /* only if optimal recovery is not used, modify roll/pitch */
        if (_params.opt_recover <= 0) {
            // construct attitude setpoint rotation matrix. modify the setpoints for roll
            // and pitch such that they reflect the user's intention even if a yaw error
            // (yaw_sp - yaw) is present. In the presence of a yaw error constructing a rotation matrix
            // from the pure euler angle setpoints will lead to unexpected attitude behaviour from
            // the user's view as the euler angle sequence uses the  yaw setpoint and not the current
            // heading of the vehicle.

            // calculate our current yaw error
            float yaw_error = _wrap_pi(_att_sp.yaw_body - _yaw);

            // compute the vector obtained by rotating a z unit vector by the rotation
            // given by the roll and pitch commands of the user
            math::Vector<3> zB = {0, 0, 1};
            math::Matrix<3, 3> R_sp_roll_pitch;
            R_sp_roll_pitch.from_euler(_att_sp.roll_body, _att_sp.pitch_body, 0);
            math::Vector<3> z_roll_pitch_sp = R_sp_roll_pitch * zB;


            // transform the vector into a new frame which is rotated around the z axis
            // by the current yaw error. this vector defines the desired tilt when we look
            // into the direction of the desired heading
            math::Matrix<3, 3> R_yaw_correction;
            R_yaw_correction.from_euler(0.0f, 0.0f, -yaw_error);
            z_roll_pitch_sp = R_yaw_correction * z_roll_pitch_sp;

            // use the formula z_roll_pitch_sp = R_tilt * [0;0;1]
            // R_tilt is computed from_euler; only true if cos(roll) not equal zero
            // -> valid if roll is not +-pi/2;
            _att_sp.roll_body = -asinf(z_roll_pitch_sp(1));
            _att_sp.pitch_body = atan2f(z_roll_pitch_sp(0), z_roll_pitch_sp(2));
        }

        /* copy quaternion setpoint to attitude setpoint topic */
        matrix::Quatf q_sp = matrix::Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body);
        memcpy(&_att_sp.q_d[0], q_sp.data(), sizeof(_att_sp.q_d));
        _att_sp.q_d_valid = true;
    }

    if (_manual.gear_switch == manual_control_setpoint_s::SWITCH_POS_ON &&
        !_vehicle_land_detected.landed) {
        _att_sp.landing_gear = 1.0f;

    } else if (_manual.gear_switch == manual_control_setpoint_s::SWITCH_POS_OFF) {
        _att_sp.landing_gear = -1.0f;
    }

    _att_sp.timestamp = hrt_absolute_time();
}