Arducopter Yaw角分析

Arducopter Yaw

现梳理一遍Poshold模式下的yaw的情况：

---

• 首先从 Copter::fast_loop() –> update_flight_mode()–> Copter::ModePosHold::run()

// get_pilot_desired_heading - transform pilot's yaw input into a desired yaw rate returns desired yaw rate in centi-degrees per second
// 偏航的控制，之间进行比例控制，然后把输出作为内环的控制量，由于偏航通道可以控制无人机顺时针旋转和逆时针旋转，
// 所以这里怎么区分出来的呢？肯定是摇杆中立点是分界线，处理后，往左打是负值，往右是正，区分两个方向。
// 最后得到偏航角速度目标控制量，偏航打杆控制速率，跟横滚，俯仰有所不同。
float Copter::get_pilot_desired_yaw_rate(int16_t stick_angle)
{
    float yaw_request;

    // calculate yaw rate request
    // acro_y_expo 这个值为0-0.5 用来设置当打杆向一边时，设置旋转速度的快慢
    // acro_yaw_p 这个值为把遥控yaw输入转化为期望旋转速率，越大意味着旋转速率会更大
    if (g2.acro_y_expo <= 0) {
        yaw_request = stick_angle * g.acro_yaw_p;
    } else {
        // expo variables
        float y_in, y_in3, y_out;

        // range check expo
        if (g2.acro_y_expo > 1.0f || g2.acro_y_expo < 0.5f) {
            g2.acro_y_expo = 1.0f;
        }

        // yaw expo
        y_in = float(stick_angle)/ROLL_PITCH_YAW_INPUT_MAX;
        y_in3 = y_in*y_in*y_in;
        y_out = (g2.acro_y_expo * y_in3) + ((1.0f - g2.acro_y_expo) * y_in);
        yaw_request = ROLL_PITCH_YAW_INPUT_MAX * y_out * g.acro_yaw_p;
    }
    // convert pilot input to the desired yaw rate
    // 转化pilot 的输入为期望的yaw速率
    return yaw_request;
}

---

• 当无人机在地面ap.land_complete时attitude_control->set_yaw_target_to_current_heading();
  —>shift_ef_yaw_target(degrees(_ahrs.yaw - _attitude_target_euler_angle.z)100.0f);

//改变地球坐标系下的目标yaw角度 ，最终算入四元数中
//shift_ef_yaw_target(degrees(_ahrs.yaw - _attitude_target_euler_angle.z)*100.0f)
void AC_AttitudeControl::shift_ef_yaw_target(float yaw_shift_cd)
{
    float yaw_shift = radians(yaw_shift_cd*0.01f);
    Quaternion _attitude_target_update_quat;
    _attitude_target_update_quat.from_axis_angle(Vector3f(0.0f, 0.0f, yaw_shift));
    _attitude_target_quat = _attitude_target_update_quat*_attitude_target_quat;
}

---

• 接下来就是姿态更新器attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0);

// Command an euler roll and pitch angle and an euler yaw rate with angular velocity feedforward and smoothing
void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds)
{
    // Convert from centidegrees on public interface to radians
    // 转换为弧度且缩小100
    float euler_roll_angle = radians(euler_roll_angle_cd*0.01f);
    float euler_pitch_angle = radians(euler_pitch_angle_cd*0.01f);
    float euler_yaw_rate = radians(euler_yaw_rate_cds*0.01f);

    // calculate the attitude target euler angles
    // 计算所需要的目标姿态欧拉角度
    _attitude_target_quat.to_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);

    // Add roll trim to compensate tail rotor thrust in heli (will return zero on multirotors)
    // 在四轴上 get_roll_trim_rad() 为0
    euler_roll_angle += get_roll_trim_rad();
    // 机体坐标系下的前馈 _rate_bf_ff_enabled 开启或者关闭
    if (_rate_bf_ff_enabled) {
        // translate the roll pitch and yaw acceleration limits to the euler axis
        // 将横滚，俯仰和偏航加速度极限转换为欧拉轴  get_accel_yaw_max_radss()为yaw角的最大加速度设置为 27000 cdeg/s/s
        // 总的来说，就是为了限制角度变化的加速度
        Vector3f euler_accel = euler_accel_limit(_attitude_target_euler_angle, Vector3f(get_accel_roll_max_radss(), get_accel_pitch_max_radss(), get_accel_yaw_max_radss()));

        // When acceleration limiting and feedforward are enabled, the sqrt controller is used to compute an euler
        // angular velocity that will cause the euler angle to smoothly stop at the input angle with limited deceleration
        // and an exponential decay specified by smoothing_gain at the end.
        //当启用加速限制和前馈时，使用Sqt控制器计算欧拉。
        //角速度将使欧拉角在有限减速下的输入角平稳停止。
        //最后由平滑增益指定的指数衰减。

        _attitude_target_euler_rate.x = input_shaping_angle(wrap_PI(euler_roll_angle-_attitude_target_euler_angle.x), _input_tc, euler_accel.x, _attitude_target_euler_rate.x, _dt);
        _attitude_target_euler_rate.y = input_shaping_angle(wrap_PI(euler_pitch_angle-_attitude_target_euler_angle.y), _input_tc, euler_accel.y, _attitude_target_euler_rate.y, _dt);

        // When yaw acceleration limiting is enabled, the yaw input shaper constrains angular acceleration about the yaw axis, slewing
        // the output rate towards the input rate.
        // 缓慢让输出速率朝输入速率变化
        _attitude_target_euler_rate.z = input_shaping_ang_vel(_attitude_target_euler_rate.z, euler_yaw_rate, euler_accel.z, _dt);

        // Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward
        // 将实际的姿态的欧拉角导数，转化到机体坐标系中速度矢量，作为前馈速度矢量
        euler_rate_to_ang_vel(_attitude_target_euler_angle, _attitude_target_euler_rate, _attitude_target_ang_vel);
        // Limit the angular velocity
        ang_vel_limit(_attitude_target_ang_vel, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), radians(_ang_vel_yaw_max));
        // Convert body-frame angular velocity into euler angle derivative of desired attitude
        // 转化机体坐标系角度速度到期望姿态欧垃角倒数
        ang_vel_to_euler_rate(_attitude_target_euler_angle, _attitude_target_ang_vel, _attitude_target_euler_rate);
    } else {
        // When feedforward is not enabled, the target euler angle is input into the target and the feedforward rate is zeroed.
        _attitude_target_euler_angle.x = euler_roll_angle;
        _attitude_target_euler_angle.y = euler_pitch_angle;
        _attitude_target_euler_angle.z += euler_yaw_rate*_dt;
        // Compute quaternion target attitude
        _attitude_target_quat.from_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);

        // Set rate feedforward requests to zero
        _attitude_target_euler_rate = Vector3f(0.0f, 0.0f, 0.0f);
        _attitude_target_ang_vel = Vector3f(0.0f, 0.0f, 0.0f);
    }

    // Call quaternion attitude controller
    attitude_controller_run_quat();
}

// limits the acceleration and deceleration of a velocity request
float AC_AttitudeControl::input_shaping_ang_vel(float target_ang_vel, float desired_ang_vel, float accel_max, float dt)
{
    // Acceleration is limited directly to smooth the beginning of the curve.
    if (is_positive(accel_max)) {
        float delta_ang_vel = accel_max * dt;
        return constrain_float(desired_ang_vel, target_ang_vel-delta_ang_vel, target_ang_vel+delta_ang_vel);
    } else {
        return desired_ang_vel;
    }
}

---

• 当无人机起飞了，takeoff

// update attitude controller targets
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(poshold.roll, poshold.pitch, target_yaw_rate);