//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();
}
private void OnTrackerMessage(RiftInputReport report)
{
const float timeUnit = (1.0f / 1000.0f);
if (SequenceValid) {
uint timestampDelta;
if (report.Timestamp < LastTimestamp)
timestampDelta = (uint)((((int)report.Timestamp) + 0x10000) - (int)LastTimestamp);
else
timestampDelta = (uint)(report.Timestamp - LastTimestamp);
// If we missed a small number of samples, replicate the last sample.
if ((timestampDelta > LastSampleCount) && (timestampDelta <= 254)) {
MessageBodyFrame sensors = new MessageBodyFrame(this);
sensors.TimeDelta = (timestampDelta - LastSampleCount) * timeUnit;
sensors.Acceleration = LastAcceleration;
sensors.RotationRate = LastRotationRate;
sensors.MagneticField = LastMagneticField;
sensors.Temperature = LastTemperature;
Sensor.OnMessage(sensors);
}
} else {
LastAcceleration = new Vector3f();
LastRotationRate = new Vector3f();
LastMagneticField = new Vector3f();
LastTemperature = 0;
SequenceValid = true;
}
LastSampleCount = report.SampleCount;
LastTimestamp = report.Timestamp;
bool convertHMDToSensor = (Coordinates == CoordinateFrame.Sensor) && (HWCoordinates == CoordinateFrame.HMD);
//if (HandlerRef.GetHandler())
{
MessageBodyFrame sensors = new MessageBodyFrame(this);
Byte iterations = report.SampleCount;
if (report.SampleCount > 3) {
iterations = 3;
sensors.TimeDelta = (report.SampleCount - 2) * timeUnit;
} else {
sensors.TimeDelta = timeUnit;
}
for (Byte i = 0; i < iterations; i++) {
sensors.Acceleration = AccelFromBodyFrameUpdate(report, i, convertHMDToSensor);
sensors.RotationRate = EulerFromBodyFrameUpdate(report, i, convertHMDToSensor);
sensors.MagneticField = MagFromBodyFrameUpdate(report, convertHMDToSensor);
sensors.Temperature = report.Temperature * 0.01f;
Sensor.OnMessage(sensors);
// TimeDelta for the last two sample is always fixed.
sensors.TimeDelta = timeUnit;
}
LastAcceleration = sensors.Acceleration;
LastRotationRate = sensors.RotationRate;
LastMagneticField = sensors.MagneticField;
LastTemperature = sensors.Temperature;
}
//else
//{
// UByte i = (report.SampleCount > 3) ? 2 : (report.SampleCount - 1);
// LastAcceleration = AccelFromBodyFrameUpdate(report, i, convertHMDToSensor);
// LastRotationRate = EulerFromBodyFrameUpdate(report, i, convertHMDToSensor);
// LastMagneticField = MagFromBodyFrameUpdate(report, convertHMDToSensor);
// LastTemperature = report.Temperature * 0.01f;
//}
}
// *** Message Handler Logic
// Notifies SensorFusion object about a new BodyFrame message from a sensor.
// Should be called by user if not attaching to a sensor.
public void OnMessage(MessageBodyFrame msg)
{
handleMessage(msg);
}
