From d46694c91c2bec4eb1e282c0c0101e6dab26e082 Mon Sep 17 00:00:00 2001 From: Brad Davis Date: Wed, 3 Jul 2013 09:16:03 -0700 Subject: SDK 0.2.3 --- LibOVR/Src/OVR_SensorFusion.cpp | 813 +++++++++++++++++++++------------------- 1 file changed, 435 insertions(+), 378 deletions(-) (limited to 'LibOVR/Src/OVR_SensorFusion.cpp') diff --git a/LibOVR/Src/OVR_SensorFusion.cpp b/LibOVR/Src/OVR_SensorFusion.cpp index 78dd128..a4c5809 100644 --- a/LibOVR/Src/OVR_SensorFusion.cpp +++ b/LibOVR/Src/OVR_SensorFusion.cpp @@ -1,378 +1,435 @@ -/************************************************************************************ - -Filename : OVR_SensorFusion.cpp -Content : Methods that determine head orientation from sensor data over time -Created : October 9, 2012 -Authors : Michael Antonov, Steve LaValle - -Copyright : Copyright 2012 Oculus VR, Inc. All Rights reserved. - -Use of this software is subject to the terms of the Oculus license -agreement provided at the time of installation or download, or which -otherwise accompanies this software in either electronic or hard copy form. - -*************************************************************************************/ - -#include "OVR_SensorFusion.h" -#include "Kernel/OVR_Log.h" -#include "Kernel/OVR_System.h" - -namespace OVR { - -//------------------------------------------------------------------------------------- -// ***** Sensor Fusion - -SensorFusion::SensorFusion(SensorDevice* sensor) - : Handler(getThis()), pDelegate(0), - Gain(0.05f), YawMult(1), EnableGravity(true), Stage(0), DeltaT(0.001f), - EnablePrediction(false), PredictionDT(0.03f), - FRawMag(10), FAccW(20), FAngV(20), - TiltCondCount(0), TiltErrorAngle(0), - TiltErrorAxis(0,1,0), - MagCondCount(0), MagReady(false), MagCalibrated(false), MagReferenced(false), - MagRefQ(0, 0, 0, 1), MagRefM(0), MagRefYaw(0), YawErrorAngle(0), MagRefDistance(0.15f), - YawErrorCount(0), YawCorrectionActivated(false), YawCorrectionInProgress(false), - EnableYawCorrection(false) -{ - if (sensor) - AttachToSensor(sensor); - MagCalibrationMatrix.SetIdentity(); -} - -SensorFusion::~SensorFusion() -{ -} - - -bool SensorFusion::AttachToSensor(SensorDevice* sensor) -{ - - if (sensor != NULL) - { - MessageHandler* pCurrentHandler = sensor->GetMessageHandler(); - - if (pCurrentHandler == &Handler) - { - Reset(); - return true; - } - - if (pCurrentHandler != NULL) - { - OVR_DEBUG_LOG( - ("SensorFusion::AttachToSensor failed - sensor %p already has handler", sensor)); - return false; - } - } - - if (Handler.IsHandlerInstalled()) - { - Handler.RemoveHandlerFromDevices(); - } - - if (sensor != NULL) - { - sensor->SetMessageHandler(&Handler); - } - - Reset(); - return true; -} - - - - -void SensorFusion::handleMessage(const MessageBodyFrame& msg) -{ - if (msg.Type != Message_BodyFrame) - return; - - // Put the sensor readings into convenient local variables - Vector3f angVel = msg.RotationRate; - Vector3f rawAccel = msg.Acceleration; - Vector3f mag = msg.MagneticField; - - // Set variables accessible through the class API - DeltaT = msg.TimeDelta; - AngV = msg.RotationRate; - AngV.y *= YawMult; // Warning: If YawMult != 1, then AngV is not true angular velocity - A = rawAccel; - - // Allow external access to uncalibrated magnetometer values - RawMag = mag; - - // Apply the calibration parameters to raw mag - if (HasMagCalibration()) - { - mag.x += MagCalibrationMatrix.M[0][3]; - mag.y += MagCalibrationMatrix.M[1][3]; - mag.z += MagCalibrationMatrix.M[2][3]; - } - - // Provide external access to calibrated mag values - // (if the mag is not calibrated, then the raw value is returned) - CalMag = mag; - - float angVelLength = angVel.Length(); - float accLength = rawAccel.Length(); - - - // Acceleration in the world frame (Q is current HMD orientation) - Vector3f accWorld = Q.Rotate(rawAccel); - - // Keep track of time - Stage++; - float currentTime = Stage * DeltaT; // Assumes uniform time spacing - - // Insert current sensor data into filter history - FRawMag.AddElement(RawMag); - FAccW.AddElement(accWorld); - FAngV.AddElement(angVel); - - // Update orientation Q based on gyro outputs. This technique is - // based on direct properties of the angular velocity vector: - // Its direction is the current rotation axis, and its magnitude - // is the rotation rate (rad/sec) about that axis. Our sensor - // sampling rate is so fast that we need not worry about integral - // approximation error (not yet, anyway). - if (angVelLength > 0.0f) - { - Vector3f rotAxis = angVel / angVelLength; - float halfRotAngle = angVelLength * DeltaT * 0.5f; - float sinHRA = sin(halfRotAngle); - Quatf deltaQ(rotAxis.x*sinHRA, rotAxis.y*sinHRA, rotAxis.z*sinHRA, cos(halfRotAngle)); - - Q = Q * deltaQ; - } - - // The quaternion magnitude may slowly drift due to numerical error, - // so it is periodically normalized. - if (Stage % 5000 == 0) - Q.Normalize(); - - // Maintain the uncorrected orientation for later use by predictive filtering - QUncorrected = Q; - - // Perform tilt correction using the accelerometer data. This enables - // drift errors in pitch and roll to be corrected. Note that yaw cannot be corrected - // because the rotation axis is parallel to the gravity vector. - if (EnableGravity) - { - // Correcting for tilt error by using accelerometer data - const float gravityEpsilon = 0.4f; - const float angVelEpsilon = 0.1f; // Relatively slow rotation - const int tiltPeriod = 50; // Req'd time steps of stability - const float maxTiltError = 0.05f; - const float minTiltError = 0.01f; - - // This condition estimates whether the only measured acceleration is due to gravity - // (the Rift is not linearly accelerating). It is often wrong, but tends to average - // out well over time. - if ((fabs(accLength - 9.81f) < gravityEpsilon) && - (angVelLength < angVelEpsilon)) - TiltCondCount++; - else - TiltCondCount = 0; - - // After stable measurements have been taken over a sufficiently long period, - // estimate the amount of tilt error and calculate the tilt axis for later correction. - if (TiltCondCount >= tiltPeriod) - { // Update TiltErrorEstimate - TiltCondCount = 0; - // Use an average value to reduce noice (could alternatively use an LPF) - Vector3f accWMean = FAccW.Mean(); - // Project the acceleration vector into the XZ plane - Vector3f xzAcc = Vector3f(accWMean.x, 0.0f, accWMean.z); - // The unit normal of xzAcc will be the rotation axis for tilt correction - Vector3f tiltAxis = Vector3f(xzAcc.z, 0.0f, -xzAcc.x).Normalized(); - Vector3f yUp = Vector3f(0.0f, 1.0f, 0.0f); - // This is the amount of rotation - float tiltAngle = yUp.Angle(accWMean); - // Record values if the tilt error is intolerable - if (tiltAngle > maxTiltError) - { - TiltErrorAngle = tiltAngle; - TiltErrorAxis = tiltAxis; - } - } - - // This part performs the actual tilt correction as needed - if (TiltErrorAngle > minTiltError) - { - if ((TiltErrorAngle > 0.4f)&&(Stage < 8000)) - { // Tilt completely to correct orientation - Q = Quatf(TiltErrorAxis, -TiltErrorAngle) * Q; - TiltErrorAngle = 0.0f; - } - else - { - //LogText("Performing tilt correction - Angle: %f Axis: %f %f %f\n", - // TiltErrorAngle,TiltErrorAxis.x,TiltErrorAxis.y,TiltErrorAxis.z); - //float deltaTiltAngle = -Gain*TiltErrorAngle*0.005f; - // This uses agressive correction steps while your head is moving fast - float deltaTiltAngle = -Gain*TiltErrorAngle*0.005f*(5.0f*angVelLength+1.0f); - // Incrementally "untilt" by a small step size - Q = Quatf(TiltErrorAxis, deltaTiltAngle) * Q; - TiltErrorAngle += deltaTiltAngle; - } - } - } - - // Yaw drift correction based on magnetometer data. This corrects the part of the drift - // that the accelerometer cannot handle. - // This will only work if the magnetometer has been enabled, calibrated, and a reference - // point has been set. - const float maxAngVelLength = 3.0f; - const int magWindow = 5; - const float yawErrorMax = 0.1f; - const float yawErrorMin = 0.01f; - const int yawErrorCountLimit = 50; - const float yawRotationStep = 0.00002f; - - if (angVelLength < maxAngVelLength) - MagCondCount++; - else - MagCondCount = 0; - - YawCorrectionInProgress = false; - if (EnableYawCorrection && MagReady && (currentTime > 2.0f) && (MagCondCount >= magWindow) && - (Q.Distance(MagRefQ) < MagRefDistance)) - { - // Use rotational invariance to bring reference mag value into global frame - Vector3f grefmag = MagRefQ.Rotate(GetCalibratedMagValue(MagRefM)); - // Bring current (averaged) mag reading into global frame - Vector3f gmag = Q.Rotate(GetCalibratedMagValue(FRawMag.Mean())); - // Calculate the reference yaw in the global frame - float gryaw = atan2(grefmag.x,grefmag.z); - // Calculate the current yaw in the global frame - float gyaw = atan2(gmag.x,gmag.z); - //LogText("Yaw error estimate: %f\n",YawErrorAngle); - // The difference between reference and current yaws is the perceived error - YawErrorAngle = AngleDifference(gyaw,gryaw); - // If the perceived error is large, keep count - if ((fabs(YawErrorAngle) > yawErrorMax) && (!YawCorrectionActivated)) - YawErrorCount++; - // After enough iterations of high perceived error, start the correction process - if (YawErrorCount > yawErrorCountLimit) - YawCorrectionActivated = true; - // If the perceived error becomes small, turn off the yaw correction - if ((fabs(YawErrorAngle) < yawErrorMin) && YawCorrectionActivated) - { - YawCorrectionActivated = false; - YawErrorCount = 0; - } - // Perform the actual yaw correction, due to previously detected, large yaw error - if (YawCorrectionActivated) - { - YawCorrectionInProgress = true; - int sign = (YawErrorAngle > 0.0f) ? 1 : -1; - // Incrementally "unyaw" by a small step size - Q = Quatf(Vector3f(0.0f,1.0f,0.0f), -yawRotationStep * sign) * Q; - } - } -} - - - // This is a simple predictive filter based only on extrapolating the smoothed, current angular velocity. - // Note that both QP (the predicted future orientation) and Q (the current orientation) are both maintained. -Quatf SensorFusion::GetPredictedOrientation() -{ - Lock::Locker lockScope(Handler.GetHandlerLock()); - Quatf qP = QUncorrected; - if (EnablePrediction) { -#if 1 - Vector3f angVelF = FAngV.SavitzkyGolaySmooth8(); - float angVelFL = angVelF.Length(); - - if (angVelFL > 0.001f) - { - Vector3f rotAxisP = angVelF / angVelFL; - float halfRotAngleP = angVelFL * PredictionDT * 0.5f; - float sinaHRAP = sin(halfRotAngleP); - Quatf deltaQP(rotAxisP.x*sinaHRAP, rotAxisP.y*sinaHRAP, - rotAxisP.z*sinaHRAP, cos(halfRotAngleP)); - qP = QUncorrected * deltaQP; - } -#else - Quatd qpd = Quatd(Q.x,Q.y,Q.z,Q.w); - int predictionStages = (int)(PredictionDT / DeltaT); - Vector3f aa = FAngV.SavitzkyGolayDerivative12(); - Vector3d aad = Vector3d(aa.x,aa.y,aa.z); - Vector3f angVelF = FAngV.SavitzkyGolaySmooth8(); - Vector3d avkd = Vector3d(angVelF.x,angVelF.y,angVelF.z); - for (int i = 0; i < predictionStages; i++) - { - double angVelLengthd = avkd.Length(); - Vector3d rotAxisd = avkd / angVelLengthd; - double halfRotAngled = angVelLengthd * DeltaT * 0.5; - double sinHRAd = sin(halfRotAngled); - Quatd deltaQd = Quatd(rotAxisd.x*sinHRAd, rotAxisd.y*sinHRAd, rotAxisd.z*sinHRAd, - cos(halfRotAngled)); - qpd = qpd * deltaQd; - // Update vel - avkd += aad; - } - qP = Quatf((float)qpd.x,(float)qpd.y,(float)qpd.z,(float)qpd.w); -#endif - } - return qP; -} - - -Vector3f SensorFusion::GetCalibratedMagValue(const Vector3f& rawMag) const -{ - Vector3f mag = rawMag; - OVR_ASSERT(HasMagCalibration()); - mag.x += MagCalibrationMatrix.M[0][3]; - mag.y += MagCalibrationMatrix.M[1][3]; - mag.z += MagCalibrationMatrix.M[2][3]; - return mag; -} - - -void SensorFusion::SetMagReference(const Quatf& q, const Vector3f& rawMag) -{ - MagRefQ = q; - MagRefM = rawMag; - - float pitch, roll, yaw; - Q.GetEulerAngles(&pitch, &roll, &yaw); - MagRefYaw = yaw; - MagReferenced = true; - if (MagCalibrated) - MagReady = true; -} - - -float SensorFusion::AngleDifference(float theta1, float theta2) -{ - float x = theta1 - theta2; - if (x > Math::Pi) - return x - Math::TwoPi; - if (x < -Math::Pi) - return x + Math::TwoPi; - return x; -} - - -SensorFusion::BodyFrameHandler::~BodyFrameHandler() -{ - RemoveHandlerFromDevices(); -} - -void SensorFusion::BodyFrameHandler::OnMessage(const Message& msg) -{ - if (msg.Type == Message_BodyFrame) - pFusion->handleMessage(static_cast(msg)); - if (pFusion->pDelegate) - pFusion->pDelegate->OnMessage(msg); -} - -bool SensorFusion::BodyFrameHandler::SupportsMessageType(MessageType type) const -{ - return (type == Message_BodyFrame); -} - - -} // namespace OVR - +/************************************************************************************ + +Filename : OVR_SensorFusion.cpp +Content : Methods that determine head orientation from sensor data over time +Created : October 9, 2012 +Authors : Michael Antonov, Steve LaValle + +Copyright : Copyright 2012 Oculus VR, Inc. All Rights reserved. + +Use of this software is subject to the terms of the Oculus license +agreement provided at the time of installation or download, or which +otherwise accompanies this software in either electronic or hard copy form. + +*************************************************************************************/ + +#include "OVR_SensorFusion.h" +#include "Kernel/OVR_Log.h" +#include "Kernel/OVR_System.h" + +namespace OVR { + +//------------------------------------------------------------------------------------- +// ***** Sensor Fusion + +SensorFusion::SensorFusion(SensorDevice* sensor) + : Handler(getThis()), pDelegate(0), + Gain(0.05f), YawMult(1), EnableGravity(true), Stage(0), RunningTime(0), DeltaT(0.001f), + EnablePrediction(true), PredictionDT(0.03f), PredictionTimeIncrement(0.001f), + FRawMag(10), FAccW(20), FAngV(20), + TiltCondCount(0), TiltErrorAngle(0), + TiltErrorAxis(0,1,0), + MagCondCount(0), MagCalibrated(false), MagRefQ(0, 0, 0, 1), + MagRefM(0), MagRefYaw(0), YawErrorAngle(0), MagRefDistance(0.5f), + YawErrorCount(0), YawCorrectionActivated(false), YawCorrectionInProgress(false), + EnableYawCorrection(false), MagNumReferences(0), MagHasNearbyReference(false) +{ + if (sensor) + AttachToSensor(sensor); + MagCalibrationMatrix.SetIdentity(); +} + +SensorFusion::~SensorFusion() +{ +} + + +bool SensorFusion::AttachToSensor(SensorDevice* sensor) +{ + + if (sensor != NULL) + { + MessageHandler* pCurrentHandler = sensor->GetMessageHandler(); + + if (pCurrentHandler == &Handler) + { + Reset(); + return true; + } + + if (pCurrentHandler != NULL) + { + OVR_DEBUG_LOG( + ("SensorFusion::AttachToSensor failed - sensor %p already has handler", sensor)); + return false; + } + } + + if (Handler.IsHandlerInstalled()) + { + Handler.RemoveHandlerFromDevices(); + } + + if (sensor != NULL) + { + sensor->SetMessageHandler(&Handler); + } + + Reset(); + return true; +} + + + // Resets the current orientation +void SensorFusion::Reset() +{ + Lock::Locker lockScope(Handler.GetHandlerLock()); + Q = Quatf(); + QUncorrected = Quatf(); + Stage = 0; + RunningTime = 0; + MagNumReferences = 0; + MagHasNearbyReference = false; +} + + +void SensorFusion::handleMessage(const MessageBodyFrame& msg) +{ + if (msg.Type != Message_BodyFrame) + return; + + // Put the sensor readings into convenient local variables + Vector3f angVel = msg.RotationRate; + Vector3f rawAccel = msg.Acceleration; + Vector3f mag = msg.MagneticField; + + // Set variables accessible through the class API + DeltaT = msg.TimeDelta; + AngV = msg.RotationRate; + AngV.y *= YawMult; // Warning: If YawMult != 1, then AngV is not true angular velocity + A = rawAccel; + + // Allow external access to uncalibrated magnetometer values + RawMag = mag; + + // Apply the calibration parameters to raw mag + if (HasMagCalibration()) + { + mag.x += MagCalibrationMatrix.M[0][3]; + mag.y += MagCalibrationMatrix.M[1][3]; + mag.z += MagCalibrationMatrix.M[2][3]; + } + + // Provide external access to calibrated mag values + // (if the mag is not calibrated, then the raw value is returned) + CalMag = mag; + + float angVelLength = angVel.Length(); + float accLength = rawAccel.Length(); + + + // Acceleration in the world frame (Q is current HMD orientation) + Vector3f accWorld = Q.Rotate(rawAccel); + + // Keep track of time + Stage++; + RunningTime += DeltaT; + + // Insert current sensor data into filter history + FRawMag.AddElement(RawMag); + FAccW.AddElement(accWorld); + FAngV.AddElement(angVel); + + // Update orientation Q based on gyro outputs. This technique is + // based on direct properties of the angular velocity vector: + // Its direction is the current rotation axis, and its magnitude + // is the rotation rate (rad/sec) about that axis. Our sensor + // sampling rate is so fast that we need not worry about integral + // approximation error (not yet, anyway). + if (angVelLength > 0.0f) + { + Vector3f rotAxis = angVel / angVelLength; + float halfRotAngle = angVelLength * DeltaT * 0.5f; + float sinHRA = sin(halfRotAngle); + Quatf deltaQ(rotAxis.x*sinHRA, rotAxis.y*sinHRA, rotAxis.z*sinHRA, cos(halfRotAngle)); + + Q = Q * deltaQ; + } + + // The quaternion magnitude may slowly drift due to numerical error, + // so it is periodically normalized. + if (Stage % 5000 == 0) + Q.Normalize(); + + // Maintain the uncorrected orientation for later use by predictive filtering + QUncorrected = Q; + + // Perform tilt correction using the accelerometer data. This enables + // drift errors in pitch and roll to be corrected. Note that yaw cannot be corrected + // because the rotation axis is parallel to the gravity vector. + if (EnableGravity) + { + // Correcting for tilt error by using accelerometer data + const float gravityEpsilon = 0.4f; + const float angVelEpsilon = 0.1f; // Relatively slow rotation + const int tiltPeriod = 50; // Required time steps of stability + const float maxTiltError = 0.05f; + const float minTiltError = 0.01f; + + // This condition estimates whether the only measured acceleration is due to gravity + // (the Rift is not linearly accelerating). It is often wrong, but tends to average + // out well over time. + if ((fabs(accLength - 9.81f) < gravityEpsilon) && + (angVelLength < angVelEpsilon)) + TiltCondCount++; + else + TiltCondCount = 0; + + // After stable measurements have been taken over a sufficiently long period, + // estimate the amount of tilt error and calculate the tilt axis for later correction. + if (TiltCondCount >= tiltPeriod) + { // Update TiltErrorEstimate + TiltCondCount = 0; + // Use an average value to reduce noise (could alternatively use an LPF) + Vector3f accWMean = FAccW.Mean(); + // Project the acceleration vector into the XZ plane + Vector3f xzAcc = Vector3f(accWMean.x, 0.0f, accWMean.z); + // The unit normal of xzAcc will be the rotation axis for tilt correction + Vector3f tiltAxis = Vector3f(xzAcc.z, 0.0f, -xzAcc.x).Normalized(); + Vector3f yUp = Vector3f(0.0f, 1.0f, 0.0f); + // This is the amount of rotation + float tiltAngle = yUp.Angle(accWMean); + // Record values if the tilt error is intolerable + if (tiltAngle > maxTiltError) + { + TiltErrorAngle = tiltAngle; + TiltErrorAxis = tiltAxis; + } + } + + // This part performs the actual tilt correction as needed + if (TiltErrorAngle > minTiltError) + { + if ((TiltErrorAngle > 0.4f)&&(RunningTime < 8.0f)) + { // Tilt completely to correct orientation + Q = Quatf(TiltErrorAxis, -TiltErrorAngle) * Q; + TiltErrorAngle = 0.0f; + } + else + { + //LogText("Performing tilt correction - Angle: %f Axis: %f %f %f\n", + // TiltErrorAngle,TiltErrorAxis.x,TiltErrorAxis.y,TiltErrorAxis.z); + //float deltaTiltAngle = -Gain*TiltErrorAngle*0.005f; + // This uses aggressive correction steps while your head is moving fast + float deltaTiltAngle = -Gain*TiltErrorAngle*0.005f*(5.0f*angVelLength+1.0f); + // Incrementally "un-tilt" by a small step size + Q = Quatf(TiltErrorAxis, deltaTiltAngle) * Q; + TiltErrorAngle += deltaTiltAngle; + } + } + } + + // Yaw drift correction based on magnetometer data. This corrects the part of the drift + // that the accelerometer cannot handle. + // This will only work if the magnetometer has been enabled, calibrated, and a reference + // point has been set. + const float maxAngVelLength = 3.0f; + const int magWindow = 5; + const float yawErrorMax = 0.1f; + const float yawErrorMin = 0.01f; + const int yawErrorCountLimit = 50; + const float yawRotationStep = 0.00002f; + + if (angVelLength < maxAngVelLength) + MagCondCount++; + else + MagCondCount = 0; + + // Find, create, and utilize reference points for the magnetometer + // Need to be careful not to set reference points while there is significant tilt error + if ((EnableYawCorrection && MagCalibrated)&&(RunningTime > 10.0f)&&(TiltErrorAngle < 0.2f)) + { + if (MagNumReferences == 0) + { + SetMagReference(); // Use the current direction + } + else if (Q.Distance(MagRefQ) > MagRefDistance) + { + MagHasNearbyReference = false; + float bestDist = 100000.0f; + int bestNdx = 0; + float dist; + for (int i = 0; i < MagNumReferences; i++) + { + dist = Q.Distance(MagRefTableQ[i]); + if (dist < bestDist) + { + bestNdx = i; + bestDist = dist; + } + } + + if (bestDist < MagRefDistance) + { + MagHasNearbyReference = true; + MagRefQ = MagRefTableQ[bestNdx]; + MagRefM = MagRefTableM[bestNdx]; + MagRefYaw = MagRefTableYaw[bestNdx]; + //LogText("Using reference %d\n",bestNdx); + } + else if (MagNumReferences < MagMaxReferences) + SetMagReference(); + } + } + + YawCorrectionInProgress = false; + if (EnableYawCorrection && MagCalibrated && (RunningTime > 2.0f) && (MagCondCount >= magWindow) && + MagHasNearbyReference) + { + // Use rotational invariance to bring reference mag value into global frame + Vector3f grefmag = MagRefQ.Rotate(GetCalibratedMagValue(MagRefM)); + // Bring current (averaged) mag reading into global frame + Vector3f gmag = Q.Rotate(GetCalibratedMagValue(FRawMag.Mean())); + // Calculate the reference yaw in the global frame + Anglef gryaw = Anglef(atan2(grefmag.x,grefmag.z)); + // Calculate the current yaw in the global frame + Anglef gyaw = Anglef(atan2(gmag.x,gmag.z)); + // The difference between reference and current yaws is the perceived error + Anglef YawErrorAngle = gyaw - gryaw; + + //LogText("Yaw error estimate: %f\n",YawErrorAngle.Get()); + // If the perceived error is large, keep count + if ((YawErrorAngle.Abs() > yawErrorMax) && (!YawCorrectionActivated)) + YawErrorCount++; + // After enough iterations of high perceived error, start the correction process + if (YawErrorCount > yawErrorCountLimit) + YawCorrectionActivated = true; + // If the perceived error becomes small, turn off the yaw correction + if ((YawErrorAngle.Abs() < yawErrorMin) && YawCorrectionActivated) + { + YawCorrectionActivated = false; + YawErrorCount = 0; + } + + // Perform the actual yaw correction, due to previously detected, large yaw error + if (YawCorrectionActivated) + { + YawCorrectionInProgress = true; + // Incrementally "unyaw" by a small step size + Q = Quatf(Vector3f(0.0f,1.0f,0.0f), -yawRotationStep * YawErrorAngle.Sign()) * Q; + } + } +} + + +// Simple predictive filters based on extrapolating the smoothed, current angular velocity +// or using smooth time derivative information. The argument is the amount of time into +// the future to predict. +Quatf SensorFusion::GetPredictedOrientation(float pdt) +{ + Lock::Locker lockScope(Handler.GetHandlerLock()); + Quatf qP = QUncorrected; + + if (EnablePrediction) + { +#if 1 + // This method assumes a constant angular velocity + Vector3f angVelF = FAngV.SavitzkyGolaySmooth8(); + float angVelFL = angVelF.Length(); + + if (angVelFL > 0.001f) + { + Vector3f rotAxisP = angVelF / angVelFL; + float halfRotAngleP = angVelFL * pdt * 0.5f; + float sinaHRAP = sin(halfRotAngleP); + Quatf deltaQP(rotAxisP.x*sinaHRAP, rotAxisP.y*sinaHRAP, + rotAxisP.z*sinaHRAP, cos(halfRotAngleP)); + qP = QUncorrected * deltaQP; + } +#else + // This method estimates angular acceleration, conservatively + OVR_ASSERT(pdt >= 0); + int predictionStages = (int)(pdt / PredictionTimeIncrement + 0.5f); + Quatd qpd = Quatd(Q.x,Q.y,Q.z,Q.w); + Vector3f aa = FAngV.SavitzkyGolayDerivative12(); + Vector3d aad = Vector3d(aa.x,aa.y,aa.z); + Vector3f angVelF = FAngV.SavitzkyGolaySmooth8(); + Vector3d avkd = Vector3d(angVelF.x,angVelF.y,angVelF.z); + Vector3d rotAxisd = Vector3d(0,1,0); + for (int i = 0; i < predictionStages; i++) + { + double angVelLengthd = avkd.Length(); + if (angVelLengthd > 0) + rotAxisd = avkd / angVelLengthd; + double halfRotAngled = angVelLengthd * PredictionTimeIncrement * 0.5; + double sinHRAd = sin(halfRotAngled); + Quatd deltaQd = Quatd(rotAxisd.x*sinHRAd, rotAxisd.y*sinHRAd, rotAxisd.z*sinHRAd, + cos(halfRotAngled)); + qpd = qpd * deltaQd; + // Update angular velocity by using the angular acceleration estimate + avkd += aad; + } + qP = Quatf((float)qpd.x,(float)qpd.y,(float)qpd.z,(float)qpd.w); +#endif + } + return qP; +} + + +Vector3f SensorFusion::GetCalibratedMagValue(const Vector3f& rawMag) const +{ + Vector3f mag = rawMag; + OVR_ASSERT(HasMagCalibration()); + mag.x += MagCalibrationMatrix.M[0][3]; + mag.y += MagCalibrationMatrix.M[1][3]; + mag.z += MagCalibrationMatrix.M[2][3]; + return mag; +} + + +void SensorFusion::SetMagReference(const Quatf& q, const Vector3f& rawMag) +{ + if (MagNumReferences < MagMaxReferences) + { + MagRefTableQ[MagNumReferences] = q; + MagRefTableM[MagNumReferences] = rawMag; //FRawMag.Mean(); + + //LogText("Inserting reference %d\n",MagNumReferences); + + MagRefQ = q; + MagRefM = rawMag; + + float pitch, roll, yaw; + Quatf q2 = q; + q2.GetEulerAngles(&pitch, &roll, &yaw); + MagRefTableYaw[MagNumReferences] = yaw; + MagRefYaw = yaw; + + MagNumReferences++; + + MagHasNearbyReference = true; + } +} + + +SensorFusion::BodyFrameHandler::~BodyFrameHandler() +{ + RemoveHandlerFromDevices(); +} + +void SensorFusion::BodyFrameHandler::OnMessage(const Message& msg) +{ + if (msg.Type == Message_BodyFrame) + pFusion->handleMessage(static_cast(msg)); + if (pFusion->pDelegate) + pFusion->pDelegate->OnMessage(msg); +} + +bool SensorFusion::BodyFrameHandler::SupportsMessageType(MessageType type) const +{ + return (type == Message_BodyFrame); +} + + +} // namespace OVR + -- cgit v1.2.3