浅谈APM系列-----update_flight_mode（ModeStabilize）

update_flight_mode（ModeStabilize）

这里只看ModeStabilize。

位置：X:\ardupilot\ArduCopter\mode.cpp

// update_flight_mode - calls the appropriate attitude controllers based on flight mode（根据飞行模式调用适当的姿态控制器）
// called at 100hz or more
void Copter::update_flight_mode()
{
    // Update EKF speed limit - used to limit speed when we are using optical flow（当我们使用光流时，用来限制速度）
    ahrs.getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);//只是重新计算了一下这两值

    flightmode->run();//阅读  201808181419
	/*需要这里的flightmode，会随着飞行模式的切换调用不同的代码*/
}

自稳模式控制代码，最开始的地方。

位置：X:\ardupilot\ArduCopter\mode_stabilize.cpp

// stabilize_run - runs the main stabilize controller（运行主稳定控制器）
// should be called at 100hz or more
void Copter::ModeStabilize::run()
{
    float target_roll, target_pitch;
    float target_yaw_rate;
    float pilot_throttle_scaled;

    // if not armed set throttle to zero and exit immediately
    if (!motors->armed() || ap.throttle_zero || !motors->get_interlock()) {
        zero_throttle_and_relax_ac();//ac指姿态控制
        return;
    }

    // clear landing flag
    set_land_complete(false);

    motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);

    // apply SIMPLE mode transform to pilot inputs（将简单模式转换应用于试验输入）
    update_simple_mode();//根据简单模式，重新设置control_in

    AP_Vehicle::MultiCopter &aparm = copter.aparm;

    // convert pilot input to lean angles（将飞行员输入转换为倾斜角度）
    // To-Do: convert get_pilot_desired_lean_angles to return angles as floats
    get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max);

    // get pilot's desired yaw rate
    target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());//传入control_in，计算期望的偏航，并返回

    // get pilot's desired throttle
    pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->get_control_in());//油门处理

    // call attitude controller
    attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());

    // body-frame rate controller is run directly from 100hz loop

    // output pilot's throttle
    attitude_control->set_throttle_out(pilot_throttle_scaled, true, g.throttle_filt);
}

---

===========================================================================================

俯仰和横滚的期望值（根据遥控器的输入和最大倾斜角度参数计算得到）

===========================================================================================

 位置：X:\ardupilot\ArduCopter\mode_stabilize.cpp

// convert pilot input to lean angles（将飞行员输入转换为倾斜角度，也就是遥控器的输入）
// To-Do: convert get_pilot_desired_lean_angles to return angles as floats
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max);

其中的：

channel_roll->get_control_in()、channel_pitch->get_control_in()是获取遥控器的输入；

target_roll, target_pitch是限幅处理后，位于正负45度之间的值；

aparm.angle_max是一个角度限制的参数。

位置：X:\ardupilot\ArduCopter\mode.cpp

// pass-through functions to reduce code churn on conversion;
// these are candidates for moving into the Mode base
// class.
void Copter::Mode::get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out, float angle_max)
{
    copter.get_pilot_desired_lean_angles(roll_in, pitch_in, roll_out, pitch_out, angle_max);
}

这里只是做个一层简单的封装。

X:\ardupilot\ArduCopter\Attitude.cpp

// get_pilot_desired_angle - transform pilot's roll or pitch input into a desired lean angle
// returns desired angle in centi-degrees
void Copter::get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out, float angle_max)
{
    // sanity check angle max parameter（完备性检查角最大参数）
    aparm.angle_max = constrain_int16(aparm.angle_max,1000,8000);//限制最大角度参数在10到80度之间

    // limit max lean angle（限制最大倾斜角度）
    angle_max = constrain_float(angle_max, 1000, aparm.angle_max);//最大角度参数有效的情况下，判断参数的角度，而这里的angle_max = aparm.angle_max

    // scale roll_in, pitch_in to ANGLE_MAX parameter range
    float scaler = aparm.angle_max/(float)ROLL_PITCH_YAW_INPUT_MAX;//4500
    roll_in *= scaler;//根据最大角度参数的设置，放大和缩小遥控器横滚和俯仰的输入
    pitch_in *= scaler;

    // do circular limit（做圆形的限制）
    float total_in = norm(pitch_in, roll_in);
    if (total_in > angle_max) {
        float ratio = angle_max / total_in;//处理之后的角度，最终不能大于传参angle_max
        roll_in *= ratio;
        pitch_in *= ratio;
    }

    // do lateral tilt to euler roll conversion（横向倾斜到欧拉滚动转换）
    roll_in = (18000/M_PI) * atanf(cosf(pitch_in*(M_PI/18000))*tanf(roll_in*(M_PI/18000)));

    // return
    roll_out = roll_in;//对横滚做了一些处理
    pitch_out = pitch_in;//俯仰值不变
}

因此，调用函数所得到的target_roll、target_pitch，就是将遥控器的输入做了处理之后，得到的横滚、俯仰的期望值

---

===========================================================================================

偏航的期望值

===========================================================================================

 位置：X:\ardupilot\ArduCopter\mode_stabilize.cpp

// get pilot's desired yaw rate
    target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());

 位置：X:\ardupilot\ArduCopter\mode.cpp

float Copter::Mode::get_pilot_desired_yaw_rate(int16_t stick_angle)
{
    return copter.get_pilot_desired_yaw_rate(stick_angle);
}

这里只进行了简单的调用。

位置：X:\ardupilot\ArduCopter\Attitude.cpp

// 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
    if (g2.acro_y_expo <= 0) {
        yaw_request = stick_angle * g.acro_yaw_p;
    } else {
        // expo（exponent指数） 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;//4500
        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
    return yaw_request;
}

这里得到的是偏航的期望值。

===========================================================================================

油门的期望值

===========================================================================================

  位置：X:\ardupilot\ArduCopter\mode_stabilize.cpp

// get pilot's desired throttle
pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->get_control_in());//油门

X:\ardupilot\ArduCopter\mode.cpp

float Copter::Mode::get_pilot_desired_throttle(int16_t throttle_control, float thr_mid)
{
    return copter.get_pilot_desired_throttle(throttle_control, thr_mid);
}

 X:\ardupilot\ArduCopter\Attitude.cpp

// transform pilot's manual throttle input to make hover throttle mid stick
// used only for manual throttle modes
// thr_mid should be in the range 0 to 1
// returns throttle output 0 to 1
float Copter::get_pilot_desired_throttle(int16_t throttle_control, float thr_mid)
{
    if (thr_mid <= 0.0f) {
        thr_mid = motors->get_throttle_hover();//获取悬停油门
    }

    int16_t mid_stick = get_throttle_mid();//获取油门中值
    // protect against unlikely divide by zero
    if (mid_stick <= 0) {
        mid_stick = 500;
    }

    // ensure reasonable throttle values
    throttle_control = constrain_int16(throttle_control,0,1000);

    // calculate normalised throttle input
    float throttle_in;
    if (throttle_control < mid_stick) {
        // below the deadband（死区之内）
        throttle_in = ((float)throttle_control)*0.5f/(float)mid_stick;
    }else if(throttle_control > mid_stick) {
        // above the deadband（死区之外）
        throttle_in = 0.5f + ((float)(throttle_control-mid_stick)) * 0.5f / (float)(1000-mid_stick);
    }else{
        // must be in the deadband(死区临界值)
        throttle_in = 0.5f;
    }

    float expo = constrain_float(-(thr_mid-0.5)/0.375, -0.5f, 1.0f);
    // calculate the output throttle using the given expo function（使用指数函数计算输出节油门）
    float throttle_out = throttle_in*(1.0f-expo) + expo*throttle_in*throttle_in*throttle_in;
    return throttle_out;
}

到这里，横滚、俯仰、偏航、油门的期望值都已计算出来了。

 

---

---

---

 ===========================================================================================

姿态

 ===========================================================================================

  位置：X:\ardupilot\ArduCopter\mode_stabilize.cpp

// call attitude controller
    attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());

   位置：X:\ardupilot\libraries\AC_AttitudeControl\AC_AttitudeControl.cpp

// 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, float smoothing_gain)
{
    // Convert from centidegrees on public interface to radians
    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);

    // ensure smoothing gain can not cause overshoot
    smoothing_gain = constrain_float(smoothing_gain,1.0f,1/_dt);

    // Add roll trim to compensate tail rotor thrust in heli (will return zero on multirotors)
    euler_roll_angle += get_roll_trim_rad();//返回0

    if (_rate_bf_ff_enabled & _use_ff_and_input_shaping) {
        // translate the roll pitch and yaw acceleration limits to the euler axis（将滚动距和偏航加速度限制转换为欧拉轴）
        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.
        _attitude_target_euler_rate.x = input_shaping_angle(wrap_PI(euler_roll_angle-_attitude_target_euler_angle.x), smoothing_gain, 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), smoothing_gain, 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);

        // 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);
    } 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();//调用四元数姿态控制器
}

// translate the roll pitch and yaw acceleration limits to the euler axis
//将滚动距和偏航加速度限制转换为欧拉轴
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()));

X:\ardupilot\libraries\AC_AttitudeControl\AC_AttitudeControl.cpp

/ translates body frame acceleration limits to the euler axis（将机体框架加速度限制转换为欧拉轴）
Vector3f AC_AttitudeControl::euler_accel_limit(Vector3f euler_rad, Vector3f euler_accel)
{
    float sin_phi = constrain_float(fabsf(sinf(euler_rad.x)), 0.1f, 1.0f);
    float cos_phi = constrain_float(fabsf(cosf(euler_rad.x)), 0.1f, 1.0f);
    float sin_theta = constrain_float(fabsf(sinf(euler_rad.y)), 0.1f, 1.0f);

    Vector3f rot_accel;
    if(is_zero(euler_accel.x) || is_zero(euler_accel.y) || is_zero(euler_accel.z) || (euler_accel.x < 0.0f) || (euler_accel.y < 0.0f) || (euler_accel.z < 0.0f)) {
        rot_accel.x = euler_accel.x;
        rot_accel.y = euler_accel.y;
        rot_accel.z = euler_accel.z;
    } else {
        rot_accel.x = euler_accel.x;//先对齐X轴
        rot_accel.y = MIN(euler_accel.y/cos_phi, euler_accel.z/sin_phi);//再对齐Y轴
        rot_accel.z = MIN(MIN(euler_accel.x/sin_theta, euler_accel.y/sin_phi), euler_accel.z/cos_phi);//最后对齐Z轴
    }
    return rot_accel;
}

上面是对加速度限制的一种转换。

 ===========================================================================================

姿态控制---偏航

 ===========================================================================================

这行代码，用到了，期望的偏航角速率，继续追踪

// 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);

 

// 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)//第二个参数是期望的偏航角速率
{
    if (accel_max > 0.0f) {
        float delta_ang_vel = accel_max * _dt;//根据最大加速度限制，计算对应最大加、减速度值
        //这里计算了偏航角速率的偏差值，如果在加速度的范围类，设置为新的速度的期望值
        target_ang_vel += constrain_float(desired_ang_vel - target_ang_vel, -delta_ang_vel, delta_ang_vel);
    } else {
        target_ang_vel = desired_ang_vel;//重新设置期望的角速度
    }
    return target_ang_vel;
}

 

 ===========================================================================================

姿态控制---俯仰、横滚

 ===========================================================================================

 

 

稍后添加------------------------------------------------------------------------------------------------------

 

 

// 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);

这里传入三个参数：目前姿态的欧拉角、目标姿态的欧拉角速度、目标欧拉角速率

// Convert a 321-intrinsic euler angle derivative to an angular velocity vector
void AC_AttitudeControl::euler_rate_to_ang_vel(const Vector3f& euler_rad, const Vector3f& euler_rate_rads, Vector3f& ang_vel_rads)
{
    float sin_theta = sinf(euler_rad.y);
    float cos_theta = cosf(euler_rad.y);
    float sin_phi = sinf(euler_rad.x);
    float cos_phi = cosf(euler_rad.x);

    ang_vel_rads.x = euler_rate_rads.x - sin_theta * euler_rate_rads.z;
    ang_vel_rads.y = cos_phi  * euler_rate_rads.y + sin_phi * cos_theta * euler_rate_rads.z;
    ang_vel_rads.z = -sin_phi * euler_rate_rads.y + cos_theta * cos_phi * euler_rate_rads.z;
}

 

 

 

 

 

---

---

---

---

 

最后，调用四元数姿态控制器

// Call quaternion attitude controller
attitude_controller_run_quat();

// Calculates the body frame angular velocities to follow the target attitude
void AC_AttitudeControl::attitude_controller_run_quat()
{
    // Retrieve quaternion vehicle attitude
    // TODO add _ahrs.get_quaternion()
    Quaternion attitude_vehicle_quat;
    attitude_vehicle_quat.from_rotation_matrix(_ahrs.get_rotation_body_to_ned());//获取NED坐标系的四元数

    // Compute attitude error（计算姿态偏差）
    Vector3f attitude_error_vector;
    thrust_heading_rotation_angles(_attitude_target_quat, attitude_vehicle_quat, attitude_error_vector, _thrust_error_angle);

    // Compute the angular velocity target from the attitude error
    _rate_target_ang_vel = update_ang_vel_target_from_att_error(attitude_error_vector);

    // Add feedforward term that attempts to ensure that roll and pitch errors rotate with the body frame rather than the reference frame.
    _rate_target_ang_vel.x += attitude_error_vector.y * _ahrs.get_gyro().z;
    _rate_target_ang_vel.y += -attitude_error_vector.x * _ahrs.get_gyro().z;

    // Add the angular velocity feedforward, rotated into vehicle frame
    Quaternion attitude_target_ang_vel_quat = Quaternion(0.0f, _attitude_target_ang_vel.x, _attitude_target_ang_vel.y, _attitude_target_ang_vel.z);
    Quaternion attitude_error_quat = attitude_vehicle_quat.inverse() * _attitude_target_quat;
    Quaternion target_ang_vel_quat = attitude_error_quat.inverse()*attitude_target_ang_vel_quat*attitude_error_quat;

    // Correct the thrust vector and smoothly add feedforward and yaw input
    if(_thrust_error_angle > AC_ATTITUDE_THRUST_ERROR_ANGLE*2.0f){
        _rate_target_ang_vel.z = _ahrs.get_gyro().z;
    }else if(_thrust_error_angle > AC_ATTITUDE_THRUST_ERROR_ANGLE){
        float flip_scalar = (1.0f - (_thrust_error_angle-AC_ATTITUDE_THRUST_ERROR_ANGLE)/AC_ATTITUDE_THRUST_ERROR_ANGLE);
        _rate_target_ang_vel.x += target_ang_vel_quat.q2*flip_scalar;
        _rate_target_ang_vel.y += target_ang_vel_quat.q3*flip_scalar;
        _rate_target_ang_vel.z += target_ang_vel_quat.q4;
        _rate_target_ang_vel.z = _ahrs.get_gyro().z*(1.0-flip_scalar) + _rate_target_ang_vel.z*flip_scalar;
    } else {
        _rate_target_ang_vel.x += target_ang_vel_quat.q2;
        _rate_target_ang_vel.y += target_ang_vel_quat.q3;
        _rate_target_ang_vel.z += target_ang_vel_quat.q4;
    }

    if (_rate_bf_ff_enabled & _use_ff_and_input_shaping) {
        // rotate target and normalize
        Quaternion attitude_target_update_quat;
        attitude_target_update_quat.from_axis_angle(Vector3f(_attitude_target_ang_vel.x * _dt, _attitude_target_ang_vel.y * _dt, _attitude_target_ang_vel.z * _dt));
        _attitude_target_quat = _attitude_target_quat * attitude_target_update_quat;
        _attitude_target_quat.normalize();
    }
}

// Compute attitude error
Vector3f attitude_error_vector;
thrust_heading_rotation_angles(_attitude_target_quat, attitude_vehicle_quat, attitude_error_vector, _thrust_error_angle);

传入四个参数：

// thrust_heading_rotation_angles - calculates two ordered rotations to move the att_from_quat quaternion to the att_to_quat quaternion.
// The first rotation corrects the thrust vector and the second rotation corrects the heading vector.
void AC_AttitudeControl::thrust_heading_rotation_angles(Quaternion& att_to_quat, const Quaternion& att_from_quat, Vector3f& att_diff_angle, float& thrust_vec_dot)
{
    Matrix3f att_to_rot_matrix; // rotation from the target body frame to the inertial frame.（从目标身体框架旋转到惯性系）
    att_to_quat.rotation_matrix(att_to_rot_matrix);//当前姿态四元数旋转到惯性系，得到一个旋转矩阵
    Vector3f att_to_thrust_vec = att_to_rot_matrix*Vector3f(0.0f,0.0f,1.0f);//

    Matrix3f att_from_rot_matrix; // rotation from the current body frame to the inertial frame.
    att_from_quat.rotation_matrix(att_from_rot_matrix);
    Vector3f att_from_thrust_vec = att_from_rot_matrix*Vector3f(0.0f,0.0f,1.0f);//

    // the cross product of the desired and target thrust vector defines the rotation vector
    //所期望的和目标推力矢量的叉积定义了旋转矢量
    Vector3f thrust_vec_cross = att_from_thrust_vec % att_to_thrust_vec;

    // the dot product is used to calculate the angle between the target and desired thrust vectors
    //点积是用来计算目标和想要的推力矢量之间的夹角的
    thrust_vec_dot = acosf(constrain_float(att_from_thrust_vec * att_to_thrust_vec,-1.0f,1.0f));//

    // Normalize the thrust rotation vector（使推力旋转矢量正常化）
    float thrust_vector_length = thrust_vec_cross.length();//个人理解为推力偏差大小
    if(is_zero(thrust_vector_length) || is_zero(thrust_vec_dot)){
        thrust_vec_cross = Vector3f(0,0,1);
        thrust_vec_dot = 0.0f;
    }else{
        thrust_vec_cross /= thrust_vector_length;//规范化推力的偏差大小
    }
    Quaternion thrust_vec_correction_quat;
    thrust_vec_correction_quat.from_axis_angle(thrust_vec_cross, thrust_vec_dot);//推力矢量四元数
    thrust_vec_correction_quat = att_from_quat.inverse()*thrust_vec_correction_quat*att_from_quat;

    // calculate the remaining rotation required after thrust vector is rotated（计算推力矢量旋转后所需的剩余旋转）
    Quaternion heading_quat = thrust_vec_correction_quat.inverse()*att_from_quat.inverse()*att_to_quat;//

    Vector3f rotation;
    thrust_vec_correction_quat.to_axis_angle(rotation);
    att_diff_angle.x = rotation.x;
    att_diff_angle.y = rotation.y;

    heading_quat.to_axis_angle(rotation);//
    att_diff_angle.z = rotation.z;//
    if(!is_zero(_p_angle_yaw.kP()) && fabsf(att_diff_angle.z) > AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS/_p_angle_yaw.kP()){
        att_diff_angle.z = constrain_float(wrap_PI(att_diff_angle.z), -AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS/_p_angle_yaw.kP(), AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS/_p_angle_yaw.kP());
        heading_quat.from_axis_angle(Vector3f(0.0f,0.0f,att_diff_angle.z));
        att_to_quat = att_from_quat*thrust_vec_correction_quat*heading_quat;
    }
}