//---------------------------------------------------------------------------------------------------
bool mahony_update(State &s, Sensor g, Sensor a, Sensor m)
{
if (!g.is_good())
return false;
// Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
if (!m.is_good() && a.is_good())
{
normalize(a);
// Estimated direction of gravity and vector perpendicular to magnetic flux
const float halfvx = s.orient.q1 * s.orient.q3 - s.orient.q0 * s.orient.q2;
const float halfvy = s.orient.q0 * s.orient.q1 + s.orient.q2 * s.orient.q3;
const float halfvz = s.orient.q0 * s.orient.q0 - 0.5f + s.orient.q3 * s.orient.q3;
// Error is sum of cross product between estimated and measured direction of gravity
const float halfex = (a.y * halfvz - a.z * halfvy);
const float halfey = (a.z * halfvx - a.x * halfvz);
const float halfez = (a.x * halfvy - a.y * halfvx);
apply_error(halfex, halfey, halfez, s, g);
}
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
else if (a.is_good())
{
normalize(a);
normalize(m);
// Auxiliary variables to avoid repeated arithmetic
const float q0q0 = s.orient.q0 * s.orient.q0;
const float q0q1 = s.orient.q0 * s.orient.q1;
const float q0q2 = s.orient.q0 * s.orient.q2;
const float q0q3 = s.orient.q0 * s.orient.q3;
const float q1q1 = s.orient.q1 * s.orient.q1;
const float q1q2 = s.orient.q1 * s.orient.q2;
const float q1q3 = s.orient.q1 * s.orient.q3;
const float q2q2 = s.orient.q2 * s.orient.q2;
const float q2q3 = s.orient.q2 * s.orient.q3;
const float q3q3 = s.orient.q3 * s.orient.q3;
// Reference direction of Earth's magnetic field
const float hx = 2.0f * (m.x * (0.5f - q2q2 - q3q3) + m.y * (q1q2 - q0q3) + m.z * (q1q3 + q0q2));
const float hy = 2.0f * (m.x * (q1q2 + q0q3) + m.y * (0.5f - q1q1 - q3q3) + m.z * (q2q3 - q0q1));
const float bx = sqrt(hx * hx + hy * hy);
const float bz = 2.0f * (m.x * (q1q3 - q0q2) + m.y * (q2q3 + q0q1) + m.z * (0.5f - q1q1 - q2q2));
// Estimated direction of gravity and magnetic field
const float halfvx = q1q3 - q0q2;
const float halfvy = q0q1 + q2q3;
const float halfvz = q0q0 - 0.5f + q3q3;
const float halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
const float halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
const float halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);
// Error is sum of cross product between estimated direction and measured direction of field vectors
const float halfex = (a.y * halfvz - a.z * halfvy) + (m.y * halfwz - m.z * halfwy);
const float halfey = (a.z * halfvx - a.x * halfvz) + (m.z * halfwx - m.x * halfwz);
const float halfez = (a.x * halfvy - a.y * halfvx) + (m.x * halfwy - m.y * halfwx);
apply_error(halfex, halfey, halfez, s, g);
}
integrate_gyro(s, g);
return true;
}
