//public void SetDelegateMessageHandler(MessageHandler* handler)
//{ pDelegate = handler; }
// Internal handler for messages; bypasses error checking.
private void handleMessage(MessageBodyFrame msg)
{
// Put the sensor readings into convenient local variables
Vector3f gyro = msg.RotationRate;
Vector3f accel = msg.Acceleration;
Vector3f mag = msg.MagneticField;
// Insert current sensor data into filter history
FRawMag.AddElement(mag);
FAngV.AddElement(gyro);
// Apply the calibration parameters to raw mag
Vector3f calMag = MagCalibrated ? GetCalibratedMagValue(FRawMag.Mean()) : FRawMag.Mean();
// Set variables accessible through the class API
DeltaT = msg.TimeDelta;
AngV = gyro;
A = accel;
RawMag = mag;
CalMag = calMag;
// Keep track of time
Stage++;
RunningTime += DeltaT;
// Small preprocessing
Quatf Qinv = Q.Inverted();
Vector3f up = Qinv.Rotate(new Vector3f(0, 1, 0));
Vector3f gyroCorrected = gyro;
// Apply integral term
// All the corrections are stored in the Simultaneous Orthogonal Rotations Angle representation,
// which allows to combine and scale them by just addition and multiplication
if (EnableGravity || EnableYawCorrection)
gyroCorrected -= GyroOffset;
if (EnableGravity) {
const float spikeThreshold = 0.01f;
const float gravityThreshold = 0.1f;
float proportionalGain = 5 * Gain; // Gain parameter should be removed in a future release
float integralGain = 0.0125f;
Vector3f tiltCorrection = SensorFusion_ComputeCorrection(accel, up);
if (Stage > 5) {
// Spike detection
float tiltAngle = up.Angle(accel);
TiltAngleFilter.AddElement(tiltAngle);
if (tiltAngle > TiltAngleFilter.Mean() + spikeThreshold)
proportionalGain = integralGain = 0;
// Acceleration detection
const float gravity = 9.8f;
if (System.Math.Abs(accel.Length() / gravity - 1) > gravityThreshold)
integralGain = 0;
} else // Apply full correction at the startup
{
proportionalGain = 1 / DeltaT;
integralGain = 0;
}
gyroCorrected += (tiltCorrection * proportionalGain);
GyroOffset -= (tiltCorrection * integralGain * DeltaT);
}
if (EnableYawCorrection && MagCalibrated && RunningTime > 2.0f) {
const float maxMagRefDist = 0.1f;
const float maxTiltError = 0.05f;
float proportionalGain = 0.01f;
float integralGain = 0.0005f;
// Update the reference point if needed
if (MagRefIdx < 0 || calMag.Distance(MagRefsInBodyFrame[MagRefIdx]) > maxMagRefDist) {
// Delete a bad point
if (MagRefIdx >= 0 && MagRefScore < 0) {
MagNumReferences--;
MagRefsInBodyFrame[MagRefIdx] = MagRefsInBodyFrame[MagNumReferences];
MagRefsInWorldFrame[MagRefIdx] = MagRefsInWorldFrame[MagNumReferences];
}
// Find a new one
MagRefIdx = -1;
MagRefScore = 1000;
float bestDist = maxMagRefDist;
for (int i = 0; i < MagNumReferences; i++) {
float dist = calMag.Distance(MagRefsInBodyFrame[i]);
if (bestDist > dist) {
bestDist = dist;
MagRefIdx = i;
}
}
// Create one if needed
if (MagRefIdx < 0 && MagNumReferences < MagMaxReferences) {
MagRefIdx = MagNumReferences;
MagRefsInBodyFrame[MagRefIdx] = calMag;
MagRefsInWorldFrame[MagRefIdx] = Q.Rotate(calMag).Normalized();
MagNumReferences++;
}
}
if (MagRefIdx >= 0) {
Vector3f magEstimated = Qinv.Rotate(MagRefsInWorldFrame[MagRefIdx]);
Vector3f magMeasured = calMag.Normalized();
// Correction is computed in the horizontal plane (in the world frame)
Vector3f yawCorrection = SensorFusion_ComputeCorrection(magMeasured.ProjectToPlane(up),
magEstimated.ProjectToPlane(up));
if (System.Math.Abs(up.Dot(magEstimated - magMeasured)) < maxTiltError) {
MagRefScore += 2;
} else // If the vertical angle is wrong, decrease the score
{
MagRefScore -= 1;
proportionalGain = integralGain = 0;
}
gyroCorrected += (yawCorrection * proportionalGain);
GyroOffset -= (yawCorrection * integralGain * DeltaT);
}
}
// Update the orientation quaternion based on the corrected angular velocity vector
float angle = gyroCorrected.Length() * DeltaT;
if (angle > 0.0f)
Q = Q * new Quatf(gyroCorrected, angle);
// The quaternion magnitude may slowly drift due to numerical error,
// so it is periodically normalized.
if (Stage % 500 == 0)
Q.Normalize();
}
public float DistanceSq(Quatf q)
{
float d1 = (this - q).LengthSq();
float d2 = (this + q).LengthSq(); // Antipodal point check
return (d1 < d2) ? d1 : d2;
}
// Resets the current orientation.
public void Reset()
{
Q = new Quatf();
QUncorrected = new Quatf();
Stage = 0;
RunningTime = 0;
MagNumReferences = 0;
MagRefIdx = -1;
GyroOffset = new Vector3f();
}
