nuclear@1: /************************************************************************************ nuclear@1: nuclear@1: Filename : OVR_SensorFusion.cpp nuclear@1: Content : Methods that determine head orientation from sensor data over time nuclear@1: Created : October 9, 2012 nuclear@1: Authors : Michael Antonov, Steve LaValle nuclear@1: nuclear@1: Copyright : Copyright 2012 Oculus VR, Inc. All Rights reserved. nuclear@1: nuclear@1: Use of this software is subject to the terms of the Oculus license nuclear@1: agreement provided at the time of installation or download, or which nuclear@1: otherwise accompanies this software in either electronic or hard copy form. nuclear@1: nuclear@1: *************************************************************************************/ nuclear@1: nuclear@1: #include "OVR_SensorFusion.h" nuclear@1: #include "Kernel/OVR_Log.h" nuclear@1: #include "Kernel/OVR_System.h" nuclear@1: #include "OVR_JSON.h" nuclear@1: #include "OVR_Profile.h" nuclear@1: nuclear@1: namespace OVR { nuclear@1: nuclear@1: //------------------------------------------------------------------------------------- nuclear@1: // ***** Sensor Fusion nuclear@1: nuclear@1: SensorFusion::SensorFusion(SensorDevice* sensor) nuclear@1: : Handler(getThis()), pDelegate(0), nuclear@1: Gain(0.05f), YawMult(1), EnableGravity(true), Stage(0), RunningTime(0), DeltaT(0.001f), nuclear@1: EnablePrediction(true), PredictionDT(0.03f), PredictionTimeIncrement(0.001f), nuclear@1: FRawMag(10), FAccW(20), FAngV(20), nuclear@1: TiltCondCount(0), TiltErrorAngle(0), nuclear@1: TiltErrorAxis(0,1,0), nuclear@1: MagCondCount(0), MagCalibrated(false), MagRefQ(0, 0, 0, 1), nuclear@1: MagRefM(0), MagRefYaw(0), YawErrorAngle(0), MagRefDistance(0.5f), nuclear@1: YawErrorCount(0), YawCorrectionActivated(false), YawCorrectionInProgress(false), nuclear@1: EnableYawCorrection(false), MagNumReferences(0), MagHasNearbyReference(false), nuclear@1: MotionTrackingEnabled(true) nuclear@1: { nuclear@1: if (sensor) nuclear@1: AttachToSensor(sensor); nuclear@1: MagCalibrationMatrix.SetIdentity(); nuclear@1: } nuclear@1: nuclear@1: SensorFusion::~SensorFusion() nuclear@1: { nuclear@1: } nuclear@1: nuclear@1: nuclear@1: bool SensorFusion::AttachToSensor(SensorDevice* sensor) nuclear@1: { nuclear@1: // clear the cached device information nuclear@1: CachedSensorInfo.SerialNumber[0] = 0; nuclear@1: CachedSensorInfo.VendorId = 0; nuclear@1: CachedSensorInfo.ProductId = 0; nuclear@1: nuclear@1: if (sensor != NULL) nuclear@1: { nuclear@1: // Cache the sensor device so we can access this information during nuclear@1: // mag saving and loading (avoid holding a reference to sensor to prevent nuclear@1: // deadlock on shutdown) nuclear@1: sensor->GetDeviceInfo(&CachedSensorInfo); // save the device information nuclear@1: MessageHandler* pCurrentHandler = sensor->GetMessageHandler(); nuclear@1: nuclear@1: if (pCurrentHandler == &Handler) nuclear@1: { nuclear@1: Reset(); nuclear@1: return true; nuclear@1: } nuclear@1: nuclear@1: if (pCurrentHandler != NULL) nuclear@1: { nuclear@1: OVR_DEBUG_LOG( nuclear@1: ("SensorFusion::AttachToSensor failed - sensor %p already has handler", sensor)); nuclear@1: return false; nuclear@1: } nuclear@1: nuclear@1: // Automatically load the default mag calibration for this sensor nuclear@1: LoadMagCalibration(); nuclear@1: } nuclear@1: nuclear@1: if (Handler.IsHandlerInstalled()) nuclear@1: { nuclear@1: Handler.RemoveHandlerFromDevices(); nuclear@1: } nuclear@1: nuclear@1: if (sensor != NULL) nuclear@1: { nuclear@1: sensor->SetMessageHandler(&Handler); nuclear@1: } nuclear@1: nuclear@1: Reset(); nuclear@1: return true; nuclear@1: } nuclear@1: nuclear@1: nuclear@1: // Resets the current orientation nuclear@1: void SensorFusion::Reset() nuclear@1: { nuclear@1: Lock::Locker lockScope(Handler.GetHandlerLock()); nuclear@1: Q = Quatf(); nuclear@1: QUncorrected = Quatf(); nuclear@1: Stage = 0; nuclear@1: RunningTime = 0; nuclear@1: MagNumReferences = 0; nuclear@1: MagHasNearbyReference = false; nuclear@1: } nuclear@1: nuclear@1: nuclear@1: void SensorFusion::handleMessage(const MessageBodyFrame& msg) nuclear@1: { nuclear@1: if (msg.Type != Message_BodyFrame || !IsMotionTrackingEnabled()) nuclear@1: return; nuclear@1: nuclear@1: // Put the sensor readings into convenient local variables nuclear@1: Vector3f angVel = msg.RotationRate; nuclear@1: Vector3f rawAccel = msg.Acceleration; nuclear@1: Vector3f mag = msg.MagneticField; nuclear@1: nuclear@1: // Set variables accessible through the class API nuclear@1: DeltaT = msg.TimeDelta; nuclear@1: AngV = msg.RotationRate; nuclear@1: AngV.y *= YawMult; // Warning: If YawMult != 1, then AngV is not true angular velocity nuclear@1: A = rawAccel; nuclear@1: nuclear@1: // Allow external access to uncalibrated magnetometer values nuclear@1: RawMag = mag; nuclear@1: nuclear@1: // Apply the calibration parameters to raw mag nuclear@1: if (HasMagCalibration()) nuclear@1: { nuclear@1: mag.x += MagCalibrationMatrix.M[0][3]; nuclear@1: mag.y += MagCalibrationMatrix.M[1][3]; nuclear@1: mag.z += MagCalibrationMatrix.M[2][3]; nuclear@1: } nuclear@1: nuclear@1: // Provide external access to calibrated mag values nuclear@1: // (if the mag is not calibrated, then the raw value is returned) nuclear@1: CalMag = mag; nuclear@1: nuclear@1: float angVelLength = angVel.Length(); nuclear@1: float accLength = rawAccel.Length(); nuclear@1: nuclear@1: nuclear@1: // Acceleration in the world frame (Q is current HMD orientation) nuclear@1: Vector3f accWorld = Q.Rotate(rawAccel); nuclear@1: nuclear@1: // Keep track of time nuclear@1: Stage++; nuclear@1: RunningTime += DeltaT; nuclear@1: nuclear@1: // Insert current sensor data into filter history nuclear@1: FRawMag.AddElement(RawMag); nuclear@1: FAccW.AddElement(accWorld); nuclear@1: FAngV.AddElement(angVel); nuclear@1: nuclear@1: // Update orientation Q based on gyro outputs. This technique is nuclear@1: // based on direct properties of the angular velocity vector: nuclear@1: // Its direction is the current rotation axis, and its magnitude nuclear@1: // is the rotation rate (rad/sec) about that axis. Our sensor nuclear@1: // sampling rate is so fast that we need not worry about integral nuclear@1: // approximation error (not yet, anyway). nuclear@1: if (angVelLength > 0.0f) nuclear@1: { nuclear@1: Vector3f rotAxis = angVel / angVelLength; nuclear@1: float halfRotAngle = angVelLength * DeltaT * 0.5f; nuclear@1: float sinHRA = sin(halfRotAngle); nuclear@1: Quatf deltaQ(rotAxis.x*sinHRA, rotAxis.y*sinHRA, rotAxis.z*sinHRA, cos(halfRotAngle)); nuclear@1: nuclear@1: Q = Q * deltaQ; nuclear@1: } nuclear@1: nuclear@1: // The quaternion magnitude may slowly drift due to numerical error, nuclear@1: // so it is periodically normalized. nuclear@1: if (Stage % 5000 == 0) nuclear@1: Q.Normalize(); nuclear@1: nuclear@1: // Maintain the uncorrected orientation for later use by predictive filtering nuclear@1: QUncorrected = Q; nuclear@1: nuclear@1: // Perform tilt correction using the accelerometer data. This enables nuclear@1: // drift errors in pitch and roll to be corrected. Note that yaw cannot be corrected nuclear@1: // because the rotation axis is parallel to the gravity vector. nuclear@1: if (EnableGravity) nuclear@1: { nuclear@1: // Correcting for tilt error by using accelerometer data nuclear@1: const float gravityEpsilon = 0.4f; nuclear@1: const float angVelEpsilon = 0.1f; // Relatively slow rotation nuclear@1: const int tiltPeriod = 50; // Required time steps of stability nuclear@1: const float maxTiltError = 0.05f; nuclear@1: const float minTiltError = 0.01f; nuclear@1: nuclear@1: // This condition estimates whether the only measured acceleration is due to gravity nuclear@1: // (the Rift is not linearly accelerating). It is often wrong, but tends to average nuclear@1: // out well over time. nuclear@1: if ((fabs(accLength - 9.81f) < gravityEpsilon) && nuclear@1: (angVelLength < angVelEpsilon)) nuclear@1: TiltCondCount++; nuclear@1: else nuclear@1: TiltCondCount = 0; nuclear@1: nuclear@1: // After stable measurements have been taken over a sufficiently long period, nuclear@1: // estimate the amount of tilt error and calculate the tilt axis for later correction. nuclear@1: if (TiltCondCount >= tiltPeriod) nuclear@1: { // Update TiltErrorEstimate nuclear@1: TiltCondCount = 0; nuclear@1: // Use an average value to reduce noise (could alternatively use an LPF) nuclear@1: Vector3f accWMean = FAccW.Mean(); nuclear@1: // Project the acceleration vector into the XZ plane nuclear@1: Vector3f xzAcc = Vector3f(accWMean.x, 0.0f, accWMean.z); nuclear@1: // The unit normal of xzAcc will be the rotation axis for tilt correction nuclear@1: Vector3f tiltAxis = Vector3f(xzAcc.z, 0.0f, -xzAcc.x).Normalized(); nuclear@1: Vector3f yUp = Vector3f(0.0f, 1.0f, 0.0f); nuclear@1: // This is the amount of rotation nuclear@1: float tiltAngle = yUp.Angle(accWMean); nuclear@1: // Record values if the tilt error is intolerable nuclear@1: if (tiltAngle > maxTiltError) nuclear@1: { nuclear@1: TiltErrorAngle = tiltAngle; nuclear@1: TiltErrorAxis = tiltAxis; nuclear@1: } nuclear@1: } nuclear@1: nuclear@1: // This part performs the actual tilt correction as needed nuclear@1: if (TiltErrorAngle > minTiltError) nuclear@1: { nuclear@1: if ((TiltErrorAngle > 0.4f)&&(RunningTime < 8.0f)) nuclear@1: { // Tilt completely to correct orientation nuclear@1: Q = Quatf(TiltErrorAxis, -TiltErrorAngle) * Q; nuclear@1: TiltErrorAngle = 0.0f; nuclear@1: } nuclear@1: else nuclear@1: { nuclear@1: //LogText("Performing tilt correction - Angle: %f Axis: %f %f %f\n", nuclear@1: // TiltErrorAngle,TiltErrorAxis.x,TiltErrorAxis.y,TiltErrorAxis.z); nuclear@1: //float deltaTiltAngle = -Gain*TiltErrorAngle*0.005f; nuclear@1: // This uses aggressive correction steps while your head is moving fast nuclear@1: float deltaTiltAngle = -Gain*TiltErrorAngle*0.005f*(5.0f*angVelLength+1.0f); nuclear@1: // Incrementally "un-tilt" by a small step size nuclear@1: Q = Quatf(TiltErrorAxis, deltaTiltAngle) * Q; nuclear@1: TiltErrorAngle += deltaTiltAngle; nuclear@1: } nuclear@1: } nuclear@1: } nuclear@1: nuclear@1: // Yaw drift correction based on magnetometer data. This corrects the part of the drift nuclear@1: // that the accelerometer cannot handle. nuclear@1: // This will only work if the magnetometer has been enabled, calibrated, and a reference nuclear@1: // point has been set. nuclear@1: const float maxAngVelLength = 3.0f; nuclear@1: const int magWindow = 5; nuclear@1: const float yawErrorMax = 0.1f; nuclear@1: const float yawErrorMin = 0.01f; nuclear@1: const int yawErrorCountLimit = 50; nuclear@1: const float yawRotationStep = 0.00002f; nuclear@1: nuclear@1: if (angVelLength < maxAngVelLength) nuclear@1: MagCondCount++; nuclear@1: else nuclear@1: MagCondCount = 0; nuclear@1: nuclear@1: // Find, create, and utilize reference points for the magnetometer nuclear@1: // Need to be careful not to set reference points while there is significant tilt error nuclear@1: if ((EnableYawCorrection && MagCalibrated)&&(RunningTime > 10.0f)&&(TiltErrorAngle < 0.2f)) nuclear@1: { nuclear@1: if (MagNumReferences == 0) nuclear@1: { nuclear@1: setMagReference(); // Use the current direction nuclear@1: } nuclear@1: else if (Q.Distance(MagRefQ) > MagRefDistance) nuclear@1: { nuclear@1: MagHasNearbyReference = false; nuclear@1: float bestDist = 100000.0f; nuclear@1: int bestNdx = 0; nuclear@1: float dist; nuclear@1: for (int i = 0; i < MagNumReferences; i++) nuclear@1: { nuclear@1: dist = Q.Distance(MagRefTableQ[i]); nuclear@1: if (dist < bestDist) nuclear@1: { nuclear@1: bestNdx = i; nuclear@1: bestDist = dist; nuclear@1: } nuclear@1: } nuclear@1: nuclear@1: if (bestDist < MagRefDistance) nuclear@1: { nuclear@1: MagHasNearbyReference = true; nuclear@1: MagRefQ = MagRefTableQ[bestNdx]; nuclear@1: MagRefM = MagRefTableM[bestNdx]; nuclear@1: MagRefYaw = MagRefTableYaw[bestNdx]; nuclear@1: //LogText("Using reference %d\n",bestNdx); nuclear@1: } nuclear@1: else if (MagNumReferences < MagMaxReferences) nuclear@1: setMagReference(); nuclear@1: } nuclear@1: } nuclear@1: nuclear@1: YawCorrectionInProgress = false; nuclear@1: if (EnableYawCorrection && MagCalibrated && (RunningTime > 2.0f) && (MagCondCount >= magWindow) && nuclear@1: MagHasNearbyReference) nuclear@1: { nuclear@1: // Use rotational invariance to bring reference mag value into global frame nuclear@1: Vector3f grefmag = MagRefQ.Rotate(GetCalibratedMagValue(MagRefM)); nuclear@1: // Bring current (averaged) mag reading into global frame nuclear@1: Vector3f gmag = Q.Rotate(GetCalibratedMagValue(FRawMag.Mean())); nuclear@1: // Calculate the reference yaw in the global frame nuclear@1: Anglef gryaw = Anglef(atan2(grefmag.x,grefmag.z)); nuclear@1: // Calculate the current yaw in the global frame nuclear@1: Anglef gyaw = Anglef(atan2(gmag.x,gmag.z)); nuclear@1: // The difference between reference and current yaws is the perceived error nuclear@1: Anglef YawErrorAngle = gyaw - gryaw; nuclear@1: nuclear@1: //LogText("Yaw error estimate: %f\n",YawErrorAngle.Get()); nuclear@1: // If the perceived error is large, keep count nuclear@1: if ((YawErrorAngle.Abs() > yawErrorMax) && (!YawCorrectionActivated)) nuclear@1: YawErrorCount++; nuclear@1: // After enough iterations of high perceived error, start the correction process nuclear@1: if (YawErrorCount > yawErrorCountLimit) nuclear@1: YawCorrectionActivated = true; nuclear@1: // If the perceived error becomes small, turn off the yaw correction nuclear@1: if ((YawErrorAngle.Abs() < yawErrorMin) && YawCorrectionActivated) nuclear@1: { nuclear@1: YawCorrectionActivated = false; nuclear@1: YawErrorCount = 0; nuclear@1: } nuclear@1: nuclear@1: // Perform the actual yaw correction, due to previously detected, large yaw error nuclear@1: if (YawCorrectionActivated) nuclear@1: { nuclear@1: YawCorrectionInProgress = true; nuclear@1: // Incrementally "unyaw" by a small step size nuclear@1: Q = Quatf(Vector3f(0.0f,1.0f,0.0f), -yawRotationStep * YawErrorAngle.Sign()) * Q; nuclear@1: } nuclear@1: } nuclear@1: } nuclear@1: nuclear@1: nuclear@1: // A predictive filter based on extrapolating the smoothed, current angular velocity nuclear@1: Quatf SensorFusion::GetPredictedOrientation(float pdt) nuclear@1: { nuclear@1: Lock::Locker lockScope(Handler.GetHandlerLock()); nuclear@1: Quatf qP = QUncorrected; nuclear@1: nuclear@1: if (EnablePrediction) nuclear@1: { nuclear@1: // This method assumes a constant angular velocity nuclear@1: Vector3f angVelF = FAngV.SavitzkyGolaySmooth8(); nuclear@1: float angVelFL = angVelF.Length(); nuclear@1: nuclear@1: // Force back to raw measurement nuclear@1: angVelF = AngV; nuclear@1: angVelFL = AngV.Length(); nuclear@1: nuclear@1: // Dynamic prediction interval: Based on angular velocity to reduce vibration nuclear@1: const float minPdt = 0.001f; nuclear@1: const float slopePdt = 0.1f; nuclear@1: float newpdt = pdt; nuclear@1: float tpdt = minPdt + slopePdt * angVelFL; nuclear@1: if (tpdt < pdt) nuclear@1: newpdt = tpdt; nuclear@1: //LogText("PredictonDTs: %d\n",(int)(newpdt / PredictionTimeIncrement + 0.5f)); nuclear@1: nuclear@1: if (angVelFL > 0.001f) nuclear@1: { nuclear@1: Vector3f rotAxisP = angVelF / angVelFL; nuclear@1: float halfRotAngleP = angVelFL * newpdt * 0.5f; nuclear@1: float sinaHRAP = sin(halfRotAngleP); nuclear@1: Quatf deltaQP(rotAxisP.x*sinaHRAP, rotAxisP.y*sinaHRAP, nuclear@1: rotAxisP.z*sinaHRAP, cos(halfRotAngleP)); nuclear@1: qP = QUncorrected * deltaQP; nuclear@1: } nuclear@1: } nuclear@1: return qP; nuclear@1: } nuclear@1: nuclear@1: nuclear@1: Vector3f SensorFusion::GetCalibratedMagValue(const Vector3f& rawMag) const nuclear@1: { nuclear@1: Vector3f mag = rawMag; nuclear@1: OVR_ASSERT(HasMagCalibration()); nuclear@1: mag.x += MagCalibrationMatrix.M[0][3]; nuclear@1: mag.y += MagCalibrationMatrix.M[1][3]; nuclear@1: mag.z += MagCalibrationMatrix.M[2][3]; nuclear@1: return mag; nuclear@1: } nuclear@1: nuclear@1: nuclear@1: void SensorFusion::setMagReference(const Quatf& q, const Vector3f& rawMag) nuclear@1: { nuclear@1: if (MagNumReferences < MagMaxReferences) nuclear@1: { nuclear@1: MagRefTableQ[MagNumReferences] = q; nuclear@1: MagRefTableM[MagNumReferences] = rawMag; //FRawMag.Mean(); nuclear@1: nuclear@1: //LogText("Inserting reference %d\n",MagNumReferences); nuclear@1: nuclear@1: MagRefQ = q; nuclear@1: MagRefM = rawMag; nuclear@1: nuclear@1: float pitch, roll, yaw; nuclear@1: Quatf q2 = q; nuclear@1: q2.GetEulerAngles(&pitch, &roll, &yaw); nuclear@1: MagRefTableYaw[MagNumReferences] = yaw; nuclear@1: MagRefYaw = yaw; nuclear@1: nuclear@1: MagNumReferences++; nuclear@1: nuclear@1: MagHasNearbyReference = true; nuclear@1: } nuclear@1: } nuclear@1: nuclear@1: nuclear@1: SensorFusion::BodyFrameHandler::~BodyFrameHandler() nuclear@1: { nuclear@1: RemoveHandlerFromDevices(); nuclear@1: } nuclear@1: nuclear@1: void SensorFusion::BodyFrameHandler::OnMessage(const Message& msg) nuclear@1: { nuclear@1: if (msg.Type == Message_BodyFrame) nuclear@1: pFusion->handleMessage(static_cast(msg)); nuclear@1: if (pFusion->pDelegate) nuclear@1: pFusion->pDelegate->OnMessage(msg); nuclear@1: } nuclear@1: nuclear@1: bool SensorFusion::BodyFrameHandler::SupportsMessageType(MessageType type) const nuclear@1: { nuclear@1: return (type == Message_BodyFrame); nuclear@1: } nuclear@1: nuclear@1: // Writes the current calibration for a particular device to a device profile file nuclear@1: // sensor - the sensor that was calibrated nuclear@1: // cal_name - an optional name for the calibration or default if cal_name == NULL nuclear@1: bool SensorFusion::SaveMagCalibration(const char* calibrationName) const nuclear@1: { nuclear@1: if (CachedSensorInfo.SerialNumber[0] == NULL || !HasMagCalibration()) nuclear@1: return false; nuclear@1: nuclear@1: // A named calibration may be specified for calibration in different nuclear@1: // environments, otherwise the default calibration is used nuclear@1: if (calibrationName == NULL) nuclear@1: calibrationName = "default"; nuclear@1: nuclear@1: // Generate a mag calibration event nuclear@1: JSON* calibration = JSON::CreateObject(); nuclear@1: // (hardcoded for now) the measurement and representation method nuclear@1: calibration->AddStringItem("Version", "1.0"); nuclear@1: calibration->AddStringItem("Name", "default"); nuclear@1: nuclear@1: // time stamp the calibration nuclear@1: char time_str[64]; nuclear@1: nuclear@1: #ifdef OVR_OS_WIN32 nuclear@1: struct tm caltime; nuclear@1: localtime_s(&caltime, &MagCalibrationTime); nuclear@1: strftime(time_str, 64, "%Y-%m-%d %H:%M:%S", &caltime); nuclear@1: #else nuclear@1: struct tm* caltime; nuclear@1: caltime = localtime(&MagCalibrationTime); nuclear@1: strftime(time_str, 64, "%Y-%m-%d %H:%M:%S", caltime); nuclear@1: #endif nuclear@1: nuclear@1: calibration->AddStringItem("Time", time_str); nuclear@1: nuclear@1: // write the full calibration matrix nuclear@1: Matrix4f calmat = GetMagCalibration(); nuclear@1: char matrix[128]; nuclear@1: int pos = 0; nuclear@1: for (int r=0; r<4; r++) nuclear@1: { nuclear@1: for (int c=0; c<4; c++) nuclear@1: { nuclear@1: pos += (int)OVR_sprintf(matrix+pos, 128, "%g ", calmat.M[r][c]); nuclear@1: } nuclear@1: } nuclear@1: calibration->AddStringItem("Calibration", matrix); nuclear@1: nuclear@1: nuclear@1: String path = GetBaseOVRPath(true); nuclear@1: path += "/Devices.json"; nuclear@1: nuclear@1: // Look for a prexisting device file to edit nuclear@1: Ptr root = *JSON::Load(path); nuclear@1: if (root) nuclear@1: { // Quick sanity check of the file type and format before we parse it nuclear@1: JSON* version = root->GetFirstItem(); nuclear@1: if (version && version->Name == "Oculus Device Profile Version") nuclear@1: { // In the future I may need to check versioning to determine parse method nuclear@1: } nuclear@1: else nuclear@1: { nuclear@1: root->Release(); nuclear@1: root = NULL; nuclear@1: } nuclear@1: } nuclear@1: nuclear@1: JSON* device = NULL; nuclear@1: if (root) nuclear@1: { nuclear@1: device = root->GetFirstItem(); // skip the header nuclear@1: device = root->GetNextItem(device); nuclear@1: while (device) nuclear@1: { // Search for a previous calibration with the same name for this device nuclear@1: // and remove it before adding the new one nuclear@1: if (device->Name == "Device") nuclear@1: { nuclear@1: JSON* item = device->GetItemByName("Serial"); nuclear@1: if (item && item->Value == CachedSensorInfo.SerialNumber) nuclear@1: { // found an entry for this device nuclear@1: item = device->GetNextItem(item); nuclear@1: while (item) nuclear@1: { nuclear@1: if (item->Name == "MagCalibration") nuclear@1: { nuclear@1: JSON* name = item->GetItemByName("Name"); nuclear@1: if (name && name->Value == calibrationName) nuclear@1: { // found a calibration of the same name nuclear@1: item->RemoveNode(); nuclear@1: item->Release(); nuclear@1: break; nuclear@1: } nuclear@1: } nuclear@1: item = device->GetNextItem(item); nuclear@1: } nuclear@1: nuclear@1: // update the auto-mag flag nuclear@1: item = device->GetItemByName("EnableYawCorrection"); nuclear@1: if (item) nuclear@1: item->dValue = (double)EnableYawCorrection; nuclear@1: else nuclear@1: device->AddBoolItem("EnableYawCorrection", EnableYawCorrection); nuclear@1: nuclear@1: break; nuclear@1: } nuclear@1: } nuclear@1: nuclear@1: device = root->GetNextItem(device); nuclear@1: } nuclear@1: } nuclear@1: else nuclear@1: { // Create a new device root nuclear@1: root = *JSON::CreateObject(); nuclear@1: root->AddStringItem("Oculus Device Profile Version", "1.0"); nuclear@1: } nuclear@1: nuclear@1: if (device == NULL) nuclear@1: { nuclear@1: device = JSON::CreateObject(); nuclear@1: device->AddStringItem("Product", CachedSensorInfo.ProductName); nuclear@1: device->AddNumberItem("ProductID", CachedSensorInfo.ProductId); nuclear@1: device->AddStringItem("Serial", CachedSensorInfo.SerialNumber); nuclear@1: device->AddBoolItem("EnableYawCorrection", EnableYawCorrection); nuclear@1: nuclear@1: root->AddItem("Device", device); nuclear@1: } nuclear@1: nuclear@1: // Create and the add the new calibration event to the device nuclear@1: device->AddItem("MagCalibration", calibration); nuclear@1: nuclear@1: return root->Save(path); nuclear@1: } nuclear@1: nuclear@1: // Loads a saved calibration for the specified device from the device profile file nuclear@1: // sensor - the sensor that the calibration was saved for nuclear@1: // cal_name - an optional name for the calibration or the default if cal_name == NULL nuclear@1: bool SensorFusion::LoadMagCalibration(const char* calibrationName) nuclear@1: { nuclear@1: if (CachedSensorInfo.SerialNumber[0] == NULL) nuclear@1: return false; nuclear@1: nuclear@1: // A named calibration may be specified for calibration in different nuclear@1: // environments, otherwise the default calibration is used nuclear@1: if (calibrationName == NULL) nuclear@1: calibrationName = "default"; nuclear@1: nuclear@1: String path = GetBaseOVRPath(true); nuclear@1: path += "/Devices.json"; nuclear@1: nuclear@1: // Load the device profiles nuclear@1: Ptr root = *JSON::Load(path); nuclear@1: if (root == NULL) nuclear@1: return false; nuclear@1: nuclear@1: // Quick sanity check of the file type and format before we parse it nuclear@1: JSON* version = root->GetFirstItem(); nuclear@1: if (version && version->Name == "Oculus Device Profile Version") nuclear@1: { // In the future I may need to check versioning to determine parse method nuclear@1: } nuclear@1: else nuclear@1: { nuclear@1: return false; nuclear@1: } nuclear@1: nuclear@1: bool autoEnableCorrection = false; nuclear@1: nuclear@1: JSON* device = root->GetNextItem(version); nuclear@1: while (device) nuclear@1: { // Search for a previous calibration with the same name for this device nuclear@1: // and remove it before adding the new one nuclear@1: if (device->Name == "Device") nuclear@1: { nuclear@1: JSON* item = device->GetItemByName("Serial"); nuclear@1: if (item && item->Value == CachedSensorInfo.SerialNumber) nuclear@1: { // found an entry for this device nuclear@1: nuclear@1: JSON* autoyaw = device->GetItemByName("EnableYawCorrection"); nuclear@1: if (autoyaw) nuclear@1: autoEnableCorrection = (autoyaw->dValue != 0); nuclear@1: nuclear@1: item = device->GetNextItem(item); nuclear@1: while (item) nuclear@1: { nuclear@1: if (item->Name == "MagCalibration") nuclear@1: { nuclear@1: JSON* calibration = item; nuclear@1: JSON* name = calibration->GetItemByName("Name"); nuclear@1: if (name && name->Value == calibrationName) nuclear@1: { // found a calibration with this name nuclear@1: nuclear@1: time_t now; nuclear@1: time(&now); nuclear@1: nuclear@1: // parse the calibration time nuclear@1: time_t calibration_time = now; nuclear@1: JSON* caltime = calibration->GetItemByName("Time"); nuclear@1: if (caltime) nuclear@1: { nuclear@1: const char* caltime_str = caltime->Value.ToCStr(); nuclear@1: nuclear@1: tm ct; nuclear@1: memset(&ct, 0, sizeof(tm)); nuclear@1: nuclear@1: #ifdef OVR_OS_WIN32 nuclear@1: struct tm nowtime; nuclear@1: localtime_s(&nowtime, &now); nuclear@1: ct.tm_isdst = nowtime.tm_isdst; nuclear@1: sscanf_s(caltime_str, "%d-%d-%d %d:%d:%d", nuclear@1: &ct.tm_year, &ct.tm_mon, &ct.tm_mday, nuclear@1: &ct.tm_hour, &ct.tm_min, &ct.tm_sec); nuclear@1: #else nuclear@1: struct tm* nowtime = localtime(&now); nuclear@1: ct.tm_isdst = nowtime->tm_isdst; nuclear@1: sscanf(caltime_str, "%d-%d-%d %d:%d:%d", nuclear@1: &ct.tm_year, &ct.tm_mon, &ct.tm_mday, nuclear@1: &ct.tm_hour, &ct.tm_min, &ct.tm_sec); nuclear@1: #endif nuclear@1: ct.tm_year -= 1900; nuclear@1: ct.tm_mon--; nuclear@1: calibration_time = mktime(&ct); nuclear@1: } nuclear@1: nuclear@1: // parse the calibration matrix nuclear@1: JSON* cal = calibration->GetItemByName("Calibration"); nuclear@1: if (cal) nuclear@1: { nuclear@1: const char* data_str = cal->Value.ToCStr(); nuclear@1: Matrix4f calmat; nuclear@1: for (int r=0; r<4; r++) nuclear@1: { nuclear@1: for (int c=0; c<4; c++) nuclear@1: { nuclear@1: calmat.M[r][c] = (float)atof(data_str); nuclear@1: while (data_str && *data_str != ' ') nuclear@1: data_str++; nuclear@1: nuclear@1: if (data_str) nuclear@1: data_str++; nuclear@1: } nuclear@1: } nuclear@1: nuclear@1: SetMagCalibration(calmat); nuclear@1: MagCalibrationTime = calibration_time; nuclear@1: EnableYawCorrection = autoEnableCorrection; nuclear@1: nuclear@1: return true; nuclear@1: } nuclear@1: } nuclear@1: } nuclear@1: item = device->GetNextItem(item); nuclear@1: } nuclear@1: nuclear@1: break; nuclear@1: } nuclear@1: } nuclear@1: nuclear@1: device = root->GetNextItem(device); nuclear@1: } nuclear@1: nuclear@1: return false; nuclear@1: } nuclear@1: nuclear@1: nuclear@1: nuclear@1: } // namespace OVR nuclear@1: