diff options
Diffstat (limited to 'LibOVR/Src/OVR_SensorFusion.cpp')
| -rw-r--r-- | LibOVR/Src/OVR_SensorFusion.cpp | 813 |
1 files changed, 435 insertions, 378 deletions
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<Axis_X, Axis_Z, Axis_Y>(&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<float>::Pi)
- return x - Math<float>::TwoPi;
- if (x < -Math<float>::Pi)
- return x + Math<float>::TwoPi;
- return x;
-}
-
-
-SensorFusion::BodyFrameHandler::~BodyFrameHandler()
-{
- RemoveHandlerFromDevices();
-}
-
-void SensorFusion::BodyFrameHandler::OnMessage(const Message& msg)
-{
- if (msg.Type == Message_BodyFrame)
- pFusion->handleMessage(static_cast<const MessageBodyFrame&>(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<Axis_X, Axis_Z, Axis_Y>(&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<const MessageBodyFrame&>(msg)); + if (pFusion->pDelegate) + pFusion->pDelegate->OnMessage(msg); +} + +bool SensorFusion::BodyFrameHandler::SupportsMessageType(MessageType type) const +{ + return (type == Message_BodyFrame); +} + + +} // namespace OVR + |