void ahrs::fuse_default(quaternion& rotation, float dt, const vector& angular_velocity,
const vector& acceleration, const vector& magnetic_field)
{
vector correction = vector(0, 0, 0);
if (abs(acceleration.norm() - 1) <= 0.3)
{
// The magnitude of acceleration is close to 1 g, so
// it might be pointing up and we can do drift correction.
const float correction_strength = 1;
matrix rotationCompass = rotationFromCompass(acceleration, magnetic_field);
matrix rotationMatrix = rotation.toRotationMatrix();
correction = (
rotationCompass.row(0).cross(rotationMatrix.row(0)) +
rotationCompass.row(1).cross(rotationMatrix.row(1)) +
rotationCompass.row(2).cross(rotationMatrix.row(2))
) * correction_strength;
}
rotate(rotation, angular_velocity + correction, dt);
}
void ahrs::output_matrix(quaternion & rotation)
{
std::cout << rotation.toRotationMatrix();
}
void ahrs::output_euler(quaternion & rotation)
{
std::cout << (vector)(rotation.toRotationMatrix().eulerAngles(2, 1, 0) * (180 / M_PI));
}
