diff options
Diffstat (limited to 'LibOVR/Src/OVR_SensorFusion.cpp')
| -rw-r--r-- | LibOVR/Src/OVR_SensorFusion.cpp | 580 |
1 files changed, 244 insertions, 336 deletions
diff --git a/LibOVR/Src/OVR_SensorFusion.cpp b/LibOVR/Src/OVR_SensorFusion.cpp index ba913c1..f396775 100644 --- a/LibOVR/Src/OVR_SensorFusion.cpp +++ b/LibOVR/Src/OVR_SensorFusion.cpp @@ -3,7 +3,7 @@ Filename : OVR_SensorFusion.cpp Content : Methods that determine head orientation from sensor data over time Created : October 9, 2012 -Authors : Michael Antonov, Steve LaValle +Authors : Michael Antonov, Steve LaValle, Max Katsev Copyright : Copyright 2012 Oculus VR, Inc. All Rights reserved. @@ -19,22 +19,21 @@ otherwise accompanies this software in either electronic or hard copy form. #include "OVR_JSON.h" #include "OVR_Profile.h" +#define MAX_DEVICE_PROFILE_MAJOR_VERSION 1 + 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), + : Stage(0), RunningTime(0), DeltaT(0.001f), + Handler(getThis()), pDelegate(0), + Gain(0.05f), EnableGravity(true), + EnablePrediction(true), PredictionDT(0.03f), PredictionTimeIncrement(0.001f), + FRawMag(10), FAngV(20), + GyroOffset(), TiltAngleFilter(1000), + EnableYawCorrection(false), MagCalibrated(false), MagNumReferences(0), MagRefIdx(-1), MagRefScore(0), MotionTrackingEnabled(true) { if (sensor) @@ -49,10 +48,17 @@ SensorFusion::~SensorFusion() bool SensorFusion::AttachToSensor(SensorDevice* sensor) { - - pSensor = sensor; + // clear the cached device information + CachedSensorInfo.SerialNumber[0] = 0; + CachedSensorInfo.VendorId = 0; + CachedSensorInfo.ProductId = 0; + if (sensor != NULL) { + // Cache the sensor device so we can access this information during + // mag saving and loading (avoid holding a reference to sensor to prevent + // deadlock on shutdown) + sensor->GetDeviceInfo(&CachedSensorInfo); // save the device information MessageHandler* pCurrentHandler = sensor->GetMessageHandler(); if (pCurrentHandler == &Handler) @@ -94,315 +100,214 @@ void SensorFusion::Reset() Q = Quatf(); QUncorrected = Quatf(); Stage = 0; - RunningTime = 0; - MagNumReferences = 0; - MagHasNearbyReference = false; + RunningTime = 0; + MagNumReferences = 0; + MagRefIdx = -1; + GyroOffset = Vector3f(); } +// Compute a rotation required to transform "estimated" into "measured" +// Returns an approximation of the goal rotation in the Simultaneous Orthogonal Rotations Angle representation +// (vector direction is the axis of rotation, norm is the angle) +Vector3f SensorFusion_ComputeCorrection(Vector3f measured, Vector3f estimated) +{ + measured.Normalize(); + estimated.Normalize(); + Vector3f correction = measured.Cross(estimated); + float cosError = measured.Dot(estimated); + // from the def. of cross product, correction.Length() = sin(error) + // therefore sin(error) * sqrt(2 / (1 + cos(error))) = 2 * sin(error / 2) ~= error in [-pi, pi] + // Mathf::Tolerance is used to avoid div by 0 if cos(error) = -1 + return correction * sqrt(2 / (1 + cosError + Mathf::Tolerance)); +} void SensorFusion::handleMessage(const MessageBodyFrame& msg) { if (msg.Type != Message_BodyFrame || !IsMotionTrackingEnabled()) 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; + // Put the sensor readings into convenient local variables + Vector3f gyro = msg.RotationRate; + Vector3f accel = msg.Acceleration; + Vector3f mag = msg.MagneticField; - // Allow external access to uncalibrated magnetometer values - RawMag = mag; + // Insert current sensor data into filter history + FRawMag.AddElement(mag); + FAngV.AddElement(gyro); // 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]; - } + Vector3f calMag = MagCalibrated ? GetCalibratedMagValue(FRawMag.Mean()) : FRawMag.Mean(); - // 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); + // 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; - // 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) + // Small preprocessing + Quatf Qinv = Q.Inverted(); + Vector3f up = Qinv.Rotate(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) { - 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)); + 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); - Q = Q * deltaQ; + 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 (fabs(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); } - - // 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) + if (EnableYawCorrection && MagCalibrated && RunningTime > 2.0f) { - // 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) + 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) { - TiltErrorAngle = tiltAngle; - TiltErrorAxis = tiltAxis; + 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++; } } - // This part performs the actual tilt correction as needed - if (TiltErrorAngle > minTiltError) + if (MagRefIdx >= 0) { - if ((TiltErrorAngle > 0.4f)&&(RunningTime < 8.0f)) - { // Tilt completely to correct orientation - Q = Quatf(TiltErrorAxis, -TiltErrorAngle) * Q; - TiltErrorAngle = 0.0f; + 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 (fabs(up.Dot(magEstimated - magMeasured)) < maxTiltError) + { + MagRefScore += 2; } - else + else // If the vertical angle is wrong, decrease the score { - //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; + MagRefScore -= 1; + proportionalGain = integralGain = 0; } + gyroCorrected += (yawCorrection * proportionalGain); + GyroOffset -= (yawCorrection * integralGain * DeltaT); } } - // 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; - } - } + // Update the orientation quaternion based on the corrected angular velocity vector + Q = Q * Quatf(gyroCorrected, gyroCorrected.Length() * DeltaT); + + // The quaternion magnitude may slowly drift due to numerical error, + // so it is periodically normalized. + if (Stage % 500 == 0) + Q.Normalize(); } - // A predictive filter based on extrapolating the smoothed, current angular velocity Quatf SensorFusion::GetPredictedOrientation(float pdt) { - Lock::Locker lockScope(Handler.GetHandlerLock()); - Quatf qP = QUncorrected; - + Lock::Locker lockScope(Handler.GetHandlerLock()); + Quatf qP = Q; + if (EnablePrediction) { - // This method assumes a constant angular velocity - Vector3f angVelF = FAngV.SavitzkyGolaySmooth8(); + // This method assumes a constant angular velocity + Vector3f angVelF = FAngV.SavitzkyGolaySmooth8(); float angVelFL = angVelF.Length(); - // Force back to raw measurement + // Force back to raw measurement angVelF = AngV; - angVelFL = AngV.Length(); + angVelFL = AngV.Length(); - // Dynamic prediction interval: Based on angular velocity to reduce vibration - const float minPdt = 0.001f; - const float slopePdt = 0.1f; - float newpdt = pdt; - float tpdt = minPdt + slopePdt * angVelFL; - if (tpdt < pdt) - newpdt = tpdt; - //LogText("PredictonDTs: %d\n",(int)(newpdt / PredictionTimeIncrement + 0.5f)); + // Dynamic prediction interval: Based on angular velocity to reduce vibration + const float minPdt = 0.001f; + const float slopePdt = 0.1f; + float newpdt = pdt; + float tpdt = minPdt + slopePdt * angVelFL; + if (tpdt < pdt) + newpdt = tpdt; + //LogText("PredictonDTs: %d\n",(int)(newpdt / PredictionTimeIncrement + 0.5f)); if (angVelFL > 0.001f) { Vector3f rotAxisP = angVelF / angVelFL; float halfRotAngleP = angVelFL * newpdt * 0.5f; float sinaHRAP = sin(halfRotAngleP); - Quatf deltaQP(rotAxisP.x*sinaHRAP, rotAxisP.y*sinaHRAP, + Quatf deltaQP(rotAxisP.x*sinaHRAP, rotAxisP.y*sinaHRAP, rotAxisP.z*sinaHRAP, cos(halfRotAngleP)); - qP = QUncorrected * deltaQP; - } - } + qP = Q * deltaQP; + } + } 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; + return MagCalibrationMatrix.Transform(rawMag); } -} - SensorFusion::BodyFrameHandler::~BodyFrameHandler() { @@ -427,21 +332,18 @@ bool SensorFusion::BodyFrameHandler::SupportsMessageType(MessageType type) const // cal_name - an optional name for the calibration or default if cal_name == NULL bool SensorFusion::SaveMagCalibration(const char* calibrationName) const { - if (pSensor == NULL || !HasMagCalibration()) + if (CachedSensorInfo.SerialNumber[0] == 0 || !HasMagCalibration()) return false; // A named calibration may be specified for calibration in different // environments, otherwise the default calibration is used if (calibrationName == NULL) calibrationName = "default"; - - SensorInfo sinfo; - pSensor->GetDeviceInfo(&sinfo); // Generate a mag calibration event JSON* calibration = JSON::CreateObject(); // (hardcoded for now) the measurement and representation method - calibration->AddStringItem("Version", "1.0"); + calibration->AddStringItem("Version", "2.0"); calibration->AddStringItem("Name", "default"); // time stamp the calibration @@ -460,19 +362,20 @@ bool SensorFusion::SaveMagCalibration(const char* calibrationName) const calibration->AddStringItem("Time", time_str); // write the full calibration matrix + char matrix[256]; Matrix4f calmat = GetMagCalibration(); - char matrix[128]; - int pos = 0; - for (int r=0; r<4; r++) - { - for (int c=0; c<4; c++) - { - pos += (int)OVR_sprintf(matrix+pos, 128, "%g ", calmat.M[r][c]); - } - } + calmat.ToString(matrix, 256); + calibration->AddStringItem("CalibrationMatrix", matrix); + // save just the offset, for backwards compatibility + // this can be removed when we don't want to support 0.2.4 anymore + Vector3f center(calmat.M[0][3], calmat.M[1][3], calmat.M[2][3]); + Matrix4f tmp = calmat; tmp.M[0][3] = tmp.M[1][3] = tmp.M[2][3] = 0; tmp.M[3][3] = 1; + center = tmp.Inverted().Transform(center); + Matrix4f oldcalmat; oldcalmat.M[0][3] = center.x; oldcalmat.M[1][3] = center.y; oldcalmat.M[2][3] = center.z; + oldcalmat.ToString(matrix, 256); calibration->AddStringItem("Calibration", matrix); - + String path = GetBaseOVRPath(true); path += "/Devices.json"; @@ -482,7 +385,14 @@ bool SensorFusion::SaveMagCalibration(const char* calibrationName) const { // Quick sanity check of the file type and format before we parse it JSON* version = root->GetFirstItem(); if (version && version->Name == "Oculus Device Profile Version") - { // In the future I may need to check versioning to determine parse method + { + int major = atoi(version->Value.ToCStr()); + if (major > MAX_DEVICE_PROFILE_MAJOR_VERSION) + { + // don't use the file on unsupported major version number + root->Release(); + root = NULL; + } } else { @@ -502,7 +412,7 @@ bool SensorFusion::SaveMagCalibration(const char* calibrationName) const if (device->Name == "Device") { JSON* item = device->GetItemByName("Serial"); - if (item && item->Value == sinfo.SerialNumber) + if (item && item->Value == CachedSensorInfo.SerialNumber) { // found an entry for this device item = device->GetNextItem(item); while (item) @@ -543,9 +453,9 @@ bool SensorFusion::SaveMagCalibration(const char* calibrationName) const if (device == NULL) { device = JSON::CreateObject(); - device->AddStringItem("Product", sinfo.ProductName); - device->AddNumberItem("ProductID", sinfo.ProductId); - device->AddStringItem("Serial", sinfo.SerialNumber); + device->AddStringItem("Product", CachedSensorInfo.ProductName); + device->AddNumberItem("ProductID", CachedSensorInfo.ProductId); + device->AddStringItem("Serial", CachedSensorInfo.SerialNumber); device->AddBoolItem("EnableYawCorrection", EnableYawCorrection); root->AddItem("Device", device); @@ -562,16 +472,13 @@ bool SensorFusion::SaveMagCalibration(const char* calibrationName) const // cal_name - an optional name for the calibration or the default if cal_name == NULL bool SensorFusion::LoadMagCalibration(const char* calibrationName) { - if (pSensor == NULL) + if (CachedSensorInfo.SerialNumber[0] == 0) return false; // A named calibration may be specified for calibration in different // environments, otherwise the default calibration is used if (calibrationName == NULL) calibrationName = "default"; - - SensorInfo sinfo; - pSensor->GetDeviceInfo(&sinfo); String path = GetBaseOVRPath(true); path += "/Devices.json"; @@ -584,7 +491,10 @@ bool SensorFusion::LoadMagCalibration(const char* calibrationName) // Quick sanity check of the file type and format before we parse it JSON* version = root->GetFirstItem(); if (version && version->Name == "Oculus Device Profile Version") - { // In the future I may need to check versioning to determine parse method + { + int major = atoi(version->Value.ToCStr()); + if (major > MAX_DEVICE_PROFILE_MAJOR_VERSION) + return false; // don't parse the file on unsupported major version number } else { @@ -600,13 +510,14 @@ bool SensorFusion::LoadMagCalibration(const char* calibrationName) if (device->Name == "Device") { JSON* item = device->GetItemByName("Serial"); - if (item && item->Value == sinfo.SerialNumber) + if (item && item->Value == CachedSensorInfo.SerialNumber) { // found an entry for this device JSON* autoyaw = device->GetItemByName("EnableYawCorrection"); if (autoyaw) autoEnableCorrection = (autoyaw->dValue != 0); + int maxCalibrationVersion = 0; item = device->GetNextItem(item); while (item) { @@ -617,69 +528,66 @@ bool SensorFusion::LoadMagCalibration(const char* calibrationName) if (name && name->Value == calibrationName) { // found a calibration with this name - time_t now; - time(&now); + int major = 0; + JSON* version = calibration->GetItemByName("Version"); + if (version) + major = atoi(version->Value.ToCStr()); - // parse the calibration time - time_t calibration_time = now; - JSON* caltime = calibration->GetItemByName("Time"); - if (caltime) + if (major > maxCalibrationVersion && major <= 2) { - const char* caltime_str = caltime->Value.ToCStr(); + time_t now; + time(&now); - tm ct; - memset(&ct, 0, sizeof(tm)); + // parse the calibration time + time_t calibration_time = now; + JSON* caltime = calibration->GetItemByName("Time"); + if (caltime) + { + const char* caltime_str = caltime->Value.ToCStr(); + + tm ct; + memset(&ct, 0, sizeof(tm)); #ifdef OVR_OS_WIN32 - struct tm nowtime; - localtime_s(&nowtime, &now); - ct.tm_isdst = nowtime.tm_isdst; - sscanf_s(caltime_str, "%d-%d-%d %d:%d:%d", - &ct.tm_year, &ct.tm_mon, &ct.tm_mday, - &ct.tm_hour, &ct.tm_min, &ct.tm_sec); + struct tm nowtime; + localtime_s(&nowtime, &now); + ct.tm_isdst = nowtime.tm_isdst; + sscanf_s(caltime_str, "%d-%d-%d %d:%d:%d", + &ct.tm_year, &ct.tm_mon, &ct.tm_mday, + &ct.tm_hour, &ct.tm_min, &ct.tm_sec); #else - struct tm* nowtime = localtime(&now); - ct.tm_isdst = nowtime->tm_isdst; - sscanf(caltime_str, "%d-%d-%d %d:%d:%d", - &ct.tm_year, &ct.tm_mon, &ct.tm_mday, - &ct.tm_hour, &ct.tm_min, &ct.tm_sec); + struct tm* nowtime = localtime(&now); + ct.tm_isdst = nowtime->tm_isdst; + sscanf(caltime_str, "%d-%d-%d %d:%d:%d", + &ct.tm_year, &ct.tm_mon, &ct.tm_mday, + &ct.tm_hour, &ct.tm_min, &ct.tm_sec); #endif - ct.tm_year -= 1900; - ct.tm_mon--; - calibration_time = mktime(&ct); - } + ct.tm_year -= 1900; + ct.tm_mon--; + calibration_time = mktime(&ct); + } - // parse the calibration matrix - JSON* cal = calibration->GetItemByName("Calibration"); - if (cal) - { - const char* data_str = cal->Value.ToCStr(); - Matrix4f calmat; - for (int r=0; r<4; r++) + // parse the calibration matrix + JSON* cal = calibration->GetItemByName("CalibrationMatrix"); + if (cal == NULL) + cal = calibration->GetItemByName("Calibration"); + + if (cal) { - for (int c=0; c<4; c++) - { - calmat.M[r][c] = (float)atof(data_str); - while (data_str && *data_str != ' ') - data_str++; - - if (data_str) - data_str++; - } - } + Matrix4f calmat = Matrix4f::FromString(cal->Value.ToCStr()); + SetMagCalibration(calmat); + MagCalibrationTime = calibration_time; + EnableYawCorrection = autoEnableCorrection; - SetMagCalibration(calmat); - MagCalibrationTime = calibration_time; - EnableYawCorrection = autoEnableCorrection; - - return true; + maxCalibrationVersion = major; + } } } } item = device->GetNextItem(item); } - break; + return (maxCalibrationVersion > 0); } } |