//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); }