static void test_euler(float roll, float pitch, float yaw)
{
Matrix3f m;
float roll2, pitch2, yaw2;
m.from_euler(roll, pitch, yaw);
m.to_euler(&roll2, &pitch2, &yaw2);
check_result("test_euler", roll, pitch, yaw, roll2, pitch2, yaw2);
}
static void test_quaternion(float roll, float pitch, float yaw)
{
Quaternion q;
Matrix3f m;
float roll2, pitch2, yaw2;
q.from_euler(roll, pitch, yaw);
q.to_euler(roll2, pitch2, yaw2);
check_result("test_quaternion1", roll, pitch, yaw, roll2, pitch2, yaw2);
m.from_euler(roll, pitch, yaw);
m.to_euler(&roll2, &pitch2, &yaw2);
check_result("test_quaternion2", roll, pitch, yaw, roll2, pitch2, yaw2);
m.from_euler(roll, pitch, yaw);
q.from_rotation_matrix(m);
q.to_euler(roll2, pitch2, yaw2);
check_result("test_quaternion3", roll, pitch, yaw, roll2, pitch2, yaw2);
q.rotation_matrix(m);
m.to_euler(&roll2, &pitch2, &yaw2);
check_result("test_quaternion4", roll, pitch, yaw, roll2, pitch2, yaw2);
}
/*
convert angular velocities from body frame to
earth frame.
all inputs and outputs are in radians/s
*/
Vector3f SITL::convert_earth_frame(const Matrix3f &dcm, const Vector3f &gyro)
{
float p = gyro.x;
float q = gyro.y;
float r = gyro.z;
float phi, theta, psi;
dcm.to_euler(&phi, &theta, &psi);
float phiDot = p + tanf(theta)*(q*sinf(phi) + r*cosf(phi));
float thetaDot = q*cosf(phi) - r*sinf(phi);
if (fabsf(cosf(theta)) < 1.0e-20f) {
theta += 1.0e-10f;
}
float psiDot = (q*sinf(phi) + r*cosf(phi))/cosf(theta);
return Vector3f(phiDot, thetaDot, psiDot);
}
/*
calculate sin and cos of roll/pitch/yaw from a body_to_ned rotation matrix
*/
void AP_AHRS::calc_trig(const Matrix3f &rot,
float &cr, float &cp, float &cy,
float &sr, float &sp, float &sy) const
{
Vector2f yaw_vector(rot.a.x, rot.b.x);
if (fabsf(yaw_vector.x) > 0 ||
fabsf(yaw_vector.y) > 0) {
yaw_vector.normalize();
}
sy = constrain_float(yaw_vector.y, -1.0f, 1.0f);
cy = constrain_float(yaw_vector.x, -1.0f, 1.0f);
// sanity checks
if (yaw_vector.is_inf() || yaw_vector.is_nan()) {
sy = 0.0f;
cy = 1.0f;
}
const float cx2 = rot.c.x * rot.c.x;
if (cx2 >= 1.0f) {
cp = 0;
cr = 1.0f;
} else {
cp = safe_sqrt(1 - cx2);
cr = rot.c.z / cp;
}
cp = constrain_float(cp, 0.0f, 1.0f);
cr = constrain_float(cr, -1.0f, 1.0f); // this relies on constrain_float() of infinity doing the right thing
sp = -rot.c.x;
if (!is_zero(cp)) {
sr = rot.c.y / cp;
}
if (is_zero(cp) || isinf(cr) || isnan(cr) || isinf(sr) || isnan(sr)) {
float r, p, y;
rot.to_euler(&r, &p, &y);
cr = cosf(r);
sr = sinf(r);
}
}