void AC_AttitudeControl::attitude_control_quat(Quaternion& attitude_target, const Quaternion& attitude_body, Vector3f& attitude_error, float& thrust_error_angle)
{
const Vector3f thrust_vector_up{0.0f, 0.0f, -1.0f};
Vector3f att_target_thrust_vec = attitude_target * thrust_vector_up;
Vector3f att_body_thrust_vec = attitude_body * thrust_vector_up;
Quaternion qd_red(att_body_thrust_vec, att_target_thrust_vec);
thrust_error_angle = 2.0f * acosf(qd_red.q1);
if(fabsf(qd_red.q2) > (1.f - 1e-5f) || fabsf(qd_red.q3) > (1.f - 1e-5f))
{
qd_red = attitude_target;
}
else
{
/* transform rotation from current to desired thrust vector into a world frame reduced desired attitude */
qd_red *= attitude_body;
}
/* mix full and reduced desired attitude */
Quaternion q_mix = qd_red.inverse() * attitude_target;
if(q_mix.q1 < 0)
{
q_mix = q_mix * -1.0f;
}
/* catch numerical problems with the domain of acosf and asinf */
q_mix.q1 = constrain_float(q_mix.q1, -1.f, 1.f);
q_mix.q4 = constrain_float(q_mix.q4, -1.f, 1.f);
// qd = qd_red * Quaternion(cosf(0.4 * acosf(q_mix.q1)), 0, 0, sinf(0.4 * asinf(q_mix.q4)));
Quaternion qd = qd_red * Quaternion(cosf(0.4 * acosf(q_mix.q1)), 0, 0, sinf(0.4 * asinf(q_mix.q4)));
/* quaternion attitude control law, qe is rotation from q to qd */
Quaternion qe = attitude_body.inverse() * qd;
attitude_error.x = 2.0f * qe.q2;
attitude_error.y = 2.0f * qe.q3;
attitude_error.z = 2.0f * qe.q4;
if(qe.q1 < 0)
{
attitude_error = attitude_error * -1.0f;
}
}
A github pull request explaining why you want to change it would be a lot better.