oculus1
diff libovr/Src/OVR_SensorFusion.cpp @ 1:e2f9e4603129
added LibOVR and started a simple vr wrapper.
author | John Tsiombikas <nuclear@member.fsf.org> |
---|---|
date | Sat, 14 Sep 2013 16:14:59 +0300 |
parents | |
children |
line diff
1.1 --- /dev/null Thu Jan 01 00:00:00 1970 +0000 1.2 +++ b/libovr/Src/OVR_SensorFusion.cpp Sat Sep 14 16:14:59 2013 +0300 1.3 @@ -0,0 +1,696 @@ 1.4 +/************************************************************************************ 1.5 + 1.6 +Filename : OVR_SensorFusion.cpp 1.7 +Content : Methods that determine head orientation from sensor data over time 1.8 +Created : October 9, 2012 1.9 +Authors : Michael Antonov, Steve LaValle 1.10 + 1.11 +Copyright : Copyright 2012 Oculus VR, Inc. All Rights reserved. 1.12 + 1.13 +Use of this software is subject to the terms of the Oculus license 1.14 +agreement provided at the time of installation or download, or which 1.15 +otherwise accompanies this software in either electronic or hard copy form. 1.16 + 1.17 +*************************************************************************************/ 1.18 + 1.19 +#include "OVR_SensorFusion.h" 1.20 +#include "Kernel/OVR_Log.h" 1.21 +#include "Kernel/OVR_System.h" 1.22 +#include "OVR_JSON.h" 1.23 +#include "OVR_Profile.h" 1.24 + 1.25 +namespace OVR { 1.26 + 1.27 +//------------------------------------------------------------------------------------- 1.28 +// ***** Sensor Fusion 1.29 + 1.30 +SensorFusion::SensorFusion(SensorDevice* sensor) 1.31 + : Handler(getThis()), pDelegate(0), 1.32 + Gain(0.05f), YawMult(1), EnableGravity(true), Stage(0), RunningTime(0), DeltaT(0.001f), 1.33 + EnablePrediction(true), PredictionDT(0.03f), PredictionTimeIncrement(0.001f), 1.34 + FRawMag(10), FAccW(20), FAngV(20), 1.35 + TiltCondCount(0), TiltErrorAngle(0), 1.36 + TiltErrorAxis(0,1,0), 1.37 + MagCondCount(0), MagCalibrated(false), MagRefQ(0, 0, 0, 1), 1.38 + MagRefM(0), MagRefYaw(0), YawErrorAngle(0), MagRefDistance(0.5f), 1.39 + YawErrorCount(0), YawCorrectionActivated(false), YawCorrectionInProgress(false), 1.40 + EnableYawCorrection(false), MagNumReferences(0), MagHasNearbyReference(false), 1.41 + MotionTrackingEnabled(true) 1.42 +{ 1.43 + if (sensor) 1.44 + AttachToSensor(sensor); 1.45 + MagCalibrationMatrix.SetIdentity(); 1.46 +} 1.47 + 1.48 +SensorFusion::~SensorFusion() 1.49 +{ 1.50 +} 1.51 + 1.52 + 1.53 +bool SensorFusion::AttachToSensor(SensorDevice* sensor) 1.54 +{ 1.55 + // clear the cached device information 1.56 + CachedSensorInfo.SerialNumber[0] = 0; 1.57 + CachedSensorInfo.VendorId = 0; 1.58 + CachedSensorInfo.ProductId = 0; 1.59 + 1.60 + if (sensor != NULL) 1.61 + { 1.62 + // Cache the sensor device so we can access this information during 1.63 + // mag saving and loading (avoid holding a reference to sensor to prevent 1.64 + // deadlock on shutdown) 1.65 + sensor->GetDeviceInfo(&CachedSensorInfo); // save the device information 1.66 + MessageHandler* pCurrentHandler = sensor->GetMessageHandler(); 1.67 + 1.68 + if (pCurrentHandler == &Handler) 1.69 + { 1.70 + Reset(); 1.71 + return true; 1.72 + } 1.73 + 1.74 + if (pCurrentHandler != NULL) 1.75 + { 1.76 + OVR_DEBUG_LOG( 1.77 + ("SensorFusion::AttachToSensor failed - sensor %p already has handler", sensor)); 1.78 + return false; 1.79 + } 1.80 + 1.81 + // Automatically load the default mag calibration for this sensor 1.82 + LoadMagCalibration(); 1.83 + } 1.84 + 1.85 + if (Handler.IsHandlerInstalled()) 1.86 + { 1.87 + Handler.RemoveHandlerFromDevices(); 1.88 + } 1.89 + 1.90 + if (sensor != NULL) 1.91 + { 1.92 + sensor->SetMessageHandler(&Handler); 1.93 + } 1.94 + 1.95 + Reset(); 1.96 + return true; 1.97 +} 1.98 + 1.99 + 1.100 + // Resets the current orientation 1.101 +void SensorFusion::Reset() 1.102 +{ 1.103 + Lock::Locker lockScope(Handler.GetHandlerLock()); 1.104 + Q = Quatf(); 1.105 + QUncorrected = Quatf(); 1.106 + Stage = 0; 1.107 + RunningTime = 0; 1.108 + MagNumReferences = 0; 1.109 + MagHasNearbyReference = false; 1.110 +} 1.111 + 1.112 + 1.113 +void SensorFusion::handleMessage(const MessageBodyFrame& msg) 1.114 +{ 1.115 + if (msg.Type != Message_BodyFrame || !IsMotionTrackingEnabled()) 1.116 + return; 1.117 + 1.118 + // Put the sensor readings into convenient local variables 1.119 + Vector3f angVel = msg.RotationRate; 1.120 + Vector3f rawAccel = msg.Acceleration; 1.121 + Vector3f mag = msg.MagneticField; 1.122 + 1.123 + // Set variables accessible through the class API 1.124 + DeltaT = msg.TimeDelta; 1.125 + AngV = msg.RotationRate; 1.126 + AngV.y *= YawMult; // Warning: If YawMult != 1, then AngV is not true angular velocity 1.127 + A = rawAccel; 1.128 + 1.129 + // Allow external access to uncalibrated magnetometer values 1.130 + RawMag = mag; 1.131 + 1.132 + // Apply the calibration parameters to raw mag 1.133 + if (HasMagCalibration()) 1.134 + { 1.135 + mag.x += MagCalibrationMatrix.M[0][3]; 1.136 + mag.y += MagCalibrationMatrix.M[1][3]; 1.137 + mag.z += MagCalibrationMatrix.M[2][3]; 1.138 + } 1.139 + 1.140 + // Provide external access to calibrated mag values 1.141 + // (if the mag is not calibrated, then the raw value is returned) 1.142 + CalMag = mag; 1.143 + 1.144 + float angVelLength = angVel.Length(); 1.145 + float accLength = rawAccel.Length(); 1.146 + 1.147 + 1.148 + // Acceleration in the world frame (Q is current HMD orientation) 1.149 + Vector3f accWorld = Q.Rotate(rawAccel); 1.150 + 1.151 + // Keep track of time 1.152 + Stage++; 1.153 + RunningTime += DeltaT; 1.154 + 1.155 + // Insert current sensor data into filter history 1.156 + FRawMag.AddElement(RawMag); 1.157 + FAccW.AddElement(accWorld); 1.158 + FAngV.AddElement(angVel); 1.159 + 1.160 + // Update orientation Q based on gyro outputs. This technique is 1.161 + // based on direct properties of the angular velocity vector: 1.162 + // Its direction is the current rotation axis, and its magnitude 1.163 + // is the rotation rate (rad/sec) about that axis. Our sensor 1.164 + // sampling rate is so fast that we need not worry about integral 1.165 + // approximation error (not yet, anyway). 1.166 + if (angVelLength > 0.0f) 1.167 + { 1.168 + Vector3f rotAxis = angVel / angVelLength; 1.169 + float halfRotAngle = angVelLength * DeltaT * 0.5f; 1.170 + float sinHRA = sin(halfRotAngle); 1.171 + Quatf deltaQ(rotAxis.x*sinHRA, rotAxis.y*sinHRA, rotAxis.z*sinHRA, cos(halfRotAngle)); 1.172 + 1.173 + Q = Q * deltaQ; 1.174 + } 1.175 + 1.176 + // The quaternion magnitude may slowly drift due to numerical error, 1.177 + // so it is periodically normalized. 1.178 + if (Stage % 5000 == 0) 1.179 + Q.Normalize(); 1.180 + 1.181 + // Maintain the uncorrected orientation for later use by predictive filtering 1.182 + QUncorrected = Q; 1.183 + 1.184 + // Perform tilt correction using the accelerometer data. This enables 1.185 + // drift errors in pitch and roll to be corrected. Note that yaw cannot be corrected 1.186 + // because the rotation axis is parallel to the gravity vector. 1.187 + if (EnableGravity) 1.188 + { 1.189 + // Correcting for tilt error by using accelerometer data 1.190 + const float gravityEpsilon = 0.4f; 1.191 + const float angVelEpsilon = 0.1f; // Relatively slow rotation 1.192 + const int tiltPeriod = 50; // Required time steps of stability 1.193 + const float maxTiltError = 0.05f; 1.194 + const float minTiltError = 0.01f; 1.195 + 1.196 + // This condition estimates whether the only measured acceleration is due to gravity 1.197 + // (the Rift is not linearly accelerating). It is often wrong, but tends to average 1.198 + // out well over time. 1.199 + if ((fabs(accLength - 9.81f) < gravityEpsilon) && 1.200 + (angVelLength < angVelEpsilon)) 1.201 + TiltCondCount++; 1.202 + else 1.203 + TiltCondCount = 0; 1.204 + 1.205 + // After stable measurements have been taken over a sufficiently long period, 1.206 + // estimate the amount of tilt error and calculate the tilt axis for later correction. 1.207 + if (TiltCondCount >= tiltPeriod) 1.208 + { // Update TiltErrorEstimate 1.209 + TiltCondCount = 0; 1.210 + // Use an average value to reduce noise (could alternatively use an LPF) 1.211 + Vector3f accWMean = FAccW.Mean(); 1.212 + // Project the acceleration vector into the XZ plane 1.213 + Vector3f xzAcc = Vector3f(accWMean.x, 0.0f, accWMean.z); 1.214 + // The unit normal of xzAcc will be the rotation axis for tilt correction 1.215 + Vector3f tiltAxis = Vector3f(xzAcc.z, 0.0f, -xzAcc.x).Normalized(); 1.216 + Vector3f yUp = Vector3f(0.0f, 1.0f, 0.0f); 1.217 + // This is the amount of rotation 1.218 + float tiltAngle = yUp.Angle(accWMean); 1.219 + // Record values if the tilt error is intolerable 1.220 + if (tiltAngle > maxTiltError) 1.221 + { 1.222 + TiltErrorAngle = tiltAngle; 1.223 + TiltErrorAxis = tiltAxis; 1.224 + } 1.225 + } 1.226 + 1.227 + // This part performs the actual tilt correction as needed 1.228 + if (TiltErrorAngle > minTiltError) 1.229 + { 1.230 + if ((TiltErrorAngle > 0.4f)&&(RunningTime < 8.0f)) 1.231 + { // Tilt completely to correct orientation 1.232 + Q = Quatf(TiltErrorAxis, -TiltErrorAngle) * Q; 1.233 + TiltErrorAngle = 0.0f; 1.234 + } 1.235 + else 1.236 + { 1.237 + //LogText("Performing tilt correction - Angle: %f Axis: %f %f %f\n", 1.238 + // TiltErrorAngle,TiltErrorAxis.x,TiltErrorAxis.y,TiltErrorAxis.z); 1.239 + //float deltaTiltAngle = -Gain*TiltErrorAngle*0.005f; 1.240 + // This uses aggressive correction steps while your head is moving fast 1.241 + float deltaTiltAngle = -Gain*TiltErrorAngle*0.005f*(5.0f*angVelLength+1.0f); 1.242 + // Incrementally "un-tilt" by a small step size 1.243 + Q = Quatf(TiltErrorAxis, deltaTiltAngle) * Q; 1.244 + TiltErrorAngle += deltaTiltAngle; 1.245 + } 1.246 + } 1.247 + } 1.248 + 1.249 + // Yaw drift correction based on magnetometer data. This corrects the part of the drift 1.250 + // that the accelerometer cannot handle. 1.251 + // This will only work if the magnetometer has been enabled, calibrated, and a reference 1.252 + // point has been set. 1.253 + const float maxAngVelLength = 3.0f; 1.254 + const int magWindow = 5; 1.255 + const float yawErrorMax = 0.1f; 1.256 + const float yawErrorMin = 0.01f; 1.257 + const int yawErrorCountLimit = 50; 1.258 + const float yawRotationStep = 0.00002f; 1.259 + 1.260 + if (angVelLength < maxAngVelLength) 1.261 + MagCondCount++; 1.262 + else 1.263 + MagCondCount = 0; 1.264 + 1.265 + // Find, create, and utilize reference points for the magnetometer 1.266 + // Need to be careful not to set reference points while there is significant tilt error 1.267 + if ((EnableYawCorrection && MagCalibrated)&&(RunningTime > 10.0f)&&(TiltErrorAngle < 0.2f)) 1.268 + { 1.269 + if (MagNumReferences == 0) 1.270 + { 1.271 + setMagReference(); // Use the current direction 1.272 + } 1.273 + else if (Q.Distance(MagRefQ) > MagRefDistance) 1.274 + { 1.275 + MagHasNearbyReference = false; 1.276 + float bestDist = 100000.0f; 1.277 + int bestNdx = 0; 1.278 + float dist; 1.279 + for (int i = 0; i < MagNumReferences; i++) 1.280 + { 1.281 + dist = Q.Distance(MagRefTableQ[i]); 1.282 + if (dist < bestDist) 1.283 + { 1.284 + bestNdx = i; 1.285 + bestDist = dist; 1.286 + } 1.287 + } 1.288 + 1.289 + if (bestDist < MagRefDistance) 1.290 + { 1.291 + MagHasNearbyReference = true; 1.292 + MagRefQ = MagRefTableQ[bestNdx]; 1.293 + MagRefM = MagRefTableM[bestNdx]; 1.294 + MagRefYaw = MagRefTableYaw[bestNdx]; 1.295 + //LogText("Using reference %d\n",bestNdx); 1.296 + } 1.297 + else if (MagNumReferences < MagMaxReferences) 1.298 + setMagReference(); 1.299 + } 1.300 + } 1.301 + 1.302 + YawCorrectionInProgress = false; 1.303 + if (EnableYawCorrection && MagCalibrated && (RunningTime > 2.0f) && (MagCondCount >= magWindow) && 1.304 + MagHasNearbyReference) 1.305 + { 1.306 + // Use rotational invariance to bring reference mag value into global frame 1.307 + Vector3f grefmag = MagRefQ.Rotate(GetCalibratedMagValue(MagRefM)); 1.308 + // Bring current (averaged) mag reading into global frame 1.309 + Vector3f gmag = Q.Rotate(GetCalibratedMagValue(FRawMag.Mean())); 1.310 + // Calculate the reference yaw in the global frame 1.311 + Anglef gryaw = Anglef(atan2(grefmag.x,grefmag.z)); 1.312 + // Calculate the current yaw in the global frame 1.313 + Anglef gyaw = Anglef(atan2(gmag.x,gmag.z)); 1.314 + // The difference between reference and current yaws is the perceived error 1.315 + Anglef YawErrorAngle = gyaw - gryaw; 1.316 + 1.317 + //LogText("Yaw error estimate: %f\n",YawErrorAngle.Get()); 1.318 + // If the perceived error is large, keep count 1.319 + if ((YawErrorAngle.Abs() > yawErrorMax) && (!YawCorrectionActivated)) 1.320 + YawErrorCount++; 1.321 + // After enough iterations of high perceived error, start the correction process 1.322 + if (YawErrorCount > yawErrorCountLimit) 1.323 + YawCorrectionActivated = true; 1.324 + // If the perceived error becomes small, turn off the yaw correction 1.325 + if ((YawErrorAngle.Abs() < yawErrorMin) && YawCorrectionActivated) 1.326 + { 1.327 + YawCorrectionActivated = false; 1.328 + YawErrorCount = 0; 1.329 + } 1.330 + 1.331 + // Perform the actual yaw correction, due to previously detected, large yaw error 1.332 + if (YawCorrectionActivated) 1.333 + { 1.334 + YawCorrectionInProgress = true; 1.335 + // Incrementally "unyaw" by a small step size 1.336 + Q = Quatf(Vector3f(0.0f,1.0f,0.0f), -yawRotationStep * YawErrorAngle.Sign()) * Q; 1.337 + } 1.338 + } 1.339 +} 1.340 + 1.341 + 1.342 +// A predictive filter based on extrapolating the smoothed, current angular velocity 1.343 +Quatf SensorFusion::GetPredictedOrientation(float pdt) 1.344 +{ 1.345 + Lock::Locker lockScope(Handler.GetHandlerLock()); 1.346 + Quatf qP = QUncorrected; 1.347 + 1.348 + if (EnablePrediction) 1.349 + { 1.350 + // This method assumes a constant angular velocity 1.351 + Vector3f angVelF = FAngV.SavitzkyGolaySmooth8(); 1.352 + float angVelFL = angVelF.Length(); 1.353 + 1.354 + // Force back to raw measurement 1.355 + angVelF = AngV; 1.356 + angVelFL = AngV.Length(); 1.357 + 1.358 + // Dynamic prediction interval: Based on angular velocity to reduce vibration 1.359 + const float minPdt = 0.001f; 1.360 + const float slopePdt = 0.1f; 1.361 + float newpdt = pdt; 1.362 + float tpdt = minPdt + slopePdt * angVelFL; 1.363 + if (tpdt < pdt) 1.364 + newpdt = tpdt; 1.365 + //LogText("PredictonDTs: %d\n",(int)(newpdt / PredictionTimeIncrement + 0.5f)); 1.366 + 1.367 + if (angVelFL > 0.001f) 1.368 + { 1.369 + Vector3f rotAxisP = angVelF / angVelFL; 1.370 + float halfRotAngleP = angVelFL * newpdt * 0.5f; 1.371 + float sinaHRAP = sin(halfRotAngleP); 1.372 + Quatf deltaQP(rotAxisP.x*sinaHRAP, rotAxisP.y*sinaHRAP, 1.373 + rotAxisP.z*sinaHRAP, cos(halfRotAngleP)); 1.374 + qP = QUncorrected * deltaQP; 1.375 + } 1.376 + } 1.377 + return qP; 1.378 +} 1.379 + 1.380 + 1.381 +Vector3f SensorFusion::GetCalibratedMagValue(const Vector3f& rawMag) const 1.382 +{ 1.383 + Vector3f mag = rawMag; 1.384 + OVR_ASSERT(HasMagCalibration()); 1.385 + mag.x += MagCalibrationMatrix.M[0][3]; 1.386 + mag.y += MagCalibrationMatrix.M[1][3]; 1.387 + mag.z += MagCalibrationMatrix.M[2][3]; 1.388 + return mag; 1.389 +} 1.390 + 1.391 + 1.392 +void SensorFusion::setMagReference(const Quatf& q, const Vector3f& rawMag) 1.393 +{ 1.394 + if (MagNumReferences < MagMaxReferences) 1.395 + { 1.396 + MagRefTableQ[MagNumReferences] = q; 1.397 + MagRefTableM[MagNumReferences] = rawMag; //FRawMag.Mean(); 1.398 + 1.399 + //LogText("Inserting reference %d\n",MagNumReferences); 1.400 + 1.401 + MagRefQ = q; 1.402 + MagRefM = rawMag; 1.403 + 1.404 + float pitch, roll, yaw; 1.405 + Quatf q2 = q; 1.406 + q2.GetEulerAngles<Axis_X, Axis_Z, Axis_Y>(&pitch, &roll, &yaw); 1.407 + MagRefTableYaw[MagNumReferences] = yaw; 1.408 + MagRefYaw = yaw; 1.409 + 1.410 + MagNumReferences++; 1.411 + 1.412 + MagHasNearbyReference = true; 1.413 + } 1.414 +} 1.415 + 1.416 + 1.417 +SensorFusion::BodyFrameHandler::~BodyFrameHandler() 1.418 +{ 1.419 + RemoveHandlerFromDevices(); 1.420 +} 1.421 + 1.422 +void SensorFusion::BodyFrameHandler::OnMessage(const Message& msg) 1.423 +{ 1.424 + if (msg.Type == Message_BodyFrame) 1.425 + pFusion->handleMessage(static_cast<const MessageBodyFrame&>(msg)); 1.426 + if (pFusion->pDelegate) 1.427 + pFusion->pDelegate->OnMessage(msg); 1.428 +} 1.429 + 1.430 +bool SensorFusion::BodyFrameHandler::SupportsMessageType(MessageType type) const 1.431 +{ 1.432 + return (type == Message_BodyFrame); 1.433 +} 1.434 + 1.435 +// Writes the current calibration for a particular device to a device profile file 1.436 +// sensor - the sensor that was calibrated 1.437 +// cal_name - an optional name for the calibration or default if cal_name == NULL 1.438 +bool SensorFusion::SaveMagCalibration(const char* calibrationName) const 1.439 +{ 1.440 + if (CachedSensorInfo.SerialNumber[0] == NULL || !HasMagCalibration()) 1.441 + return false; 1.442 + 1.443 + // A named calibration may be specified for calibration in different 1.444 + // environments, otherwise the default calibration is used 1.445 + if (calibrationName == NULL) 1.446 + calibrationName = "default"; 1.447 + 1.448 + // Generate a mag calibration event 1.449 + JSON* calibration = JSON::CreateObject(); 1.450 + // (hardcoded for now) the measurement and representation method 1.451 + calibration->AddStringItem("Version", "1.0"); 1.452 + calibration->AddStringItem("Name", "default"); 1.453 + 1.454 + // time stamp the calibration 1.455 + char time_str[64]; 1.456 + 1.457 +#ifdef OVR_OS_WIN32 1.458 + struct tm caltime; 1.459 + localtime_s(&caltime, &MagCalibrationTime); 1.460 + strftime(time_str, 64, "%Y-%m-%d %H:%M:%S", &caltime); 1.461 +#else 1.462 + struct tm* caltime; 1.463 + caltime = localtime(&MagCalibrationTime); 1.464 + strftime(time_str, 64, "%Y-%m-%d %H:%M:%S", caltime); 1.465 +#endif 1.466 + 1.467 + calibration->AddStringItem("Time", time_str); 1.468 + 1.469 + // write the full calibration matrix 1.470 + Matrix4f calmat = GetMagCalibration(); 1.471 + char matrix[128]; 1.472 + int pos = 0; 1.473 + for (int r=0; r<4; r++) 1.474 + { 1.475 + for (int c=0; c<4; c++) 1.476 + { 1.477 + pos += (int)OVR_sprintf(matrix+pos, 128, "%g ", calmat.M[r][c]); 1.478 + } 1.479 + } 1.480 + calibration->AddStringItem("Calibration", matrix); 1.481 + 1.482 + 1.483 + String path = GetBaseOVRPath(true); 1.484 + path += "/Devices.json"; 1.485 + 1.486 + // Look for a prexisting device file to edit 1.487 + Ptr<JSON> root = *JSON::Load(path); 1.488 + if (root) 1.489 + { // Quick sanity check of the file type and format before we parse it 1.490 + JSON* version = root->GetFirstItem(); 1.491 + if (version && version->Name == "Oculus Device Profile Version") 1.492 + { // In the future I may need to check versioning to determine parse method 1.493 + } 1.494 + else 1.495 + { 1.496 + root->Release(); 1.497 + root = NULL; 1.498 + } 1.499 + } 1.500 + 1.501 + JSON* device = NULL; 1.502 + if (root) 1.503 + { 1.504 + device = root->GetFirstItem(); // skip the header 1.505 + device = root->GetNextItem(device); 1.506 + while (device) 1.507 + { // Search for a previous calibration with the same name for this device 1.508 + // and remove it before adding the new one 1.509 + if (device->Name == "Device") 1.510 + { 1.511 + JSON* item = device->GetItemByName("Serial"); 1.512 + if (item && item->Value == CachedSensorInfo.SerialNumber) 1.513 + { // found an entry for this device 1.514 + item = device->GetNextItem(item); 1.515 + while (item) 1.516 + { 1.517 + if (item->Name == "MagCalibration") 1.518 + { 1.519 + JSON* name = item->GetItemByName("Name"); 1.520 + if (name && name->Value == calibrationName) 1.521 + { // found a calibration of the same name 1.522 + item->RemoveNode(); 1.523 + item->Release(); 1.524 + break; 1.525 + } 1.526 + } 1.527 + item = device->GetNextItem(item); 1.528 + } 1.529 + 1.530 + // update the auto-mag flag 1.531 + item = device->GetItemByName("EnableYawCorrection"); 1.532 + if (item) 1.533 + item->dValue = (double)EnableYawCorrection; 1.534 + else 1.535 + device->AddBoolItem("EnableYawCorrection", EnableYawCorrection); 1.536 + 1.537 + break; 1.538 + } 1.539 + } 1.540 + 1.541 + device = root->GetNextItem(device); 1.542 + } 1.543 + } 1.544 + else 1.545 + { // Create a new device root 1.546 + root = *JSON::CreateObject(); 1.547 + root->AddStringItem("Oculus Device Profile Version", "1.0"); 1.548 + } 1.549 + 1.550 + if (device == NULL) 1.551 + { 1.552 + device = JSON::CreateObject(); 1.553 + device->AddStringItem("Product", CachedSensorInfo.ProductName); 1.554 + device->AddNumberItem("ProductID", CachedSensorInfo.ProductId); 1.555 + device->AddStringItem("Serial", CachedSensorInfo.SerialNumber); 1.556 + device->AddBoolItem("EnableYawCorrection", EnableYawCorrection); 1.557 + 1.558 + root->AddItem("Device", device); 1.559 + } 1.560 + 1.561 + // Create and the add the new calibration event to the device 1.562 + device->AddItem("MagCalibration", calibration); 1.563 + 1.564 + return root->Save(path); 1.565 +} 1.566 + 1.567 +// Loads a saved calibration for the specified device from the device profile file 1.568 +// sensor - the sensor that the calibration was saved for 1.569 +// cal_name - an optional name for the calibration or the default if cal_name == NULL 1.570 +bool SensorFusion::LoadMagCalibration(const char* calibrationName) 1.571 +{ 1.572 + if (CachedSensorInfo.SerialNumber[0] == NULL) 1.573 + return false; 1.574 + 1.575 + // A named calibration may be specified for calibration in different 1.576 + // environments, otherwise the default calibration is used 1.577 + if (calibrationName == NULL) 1.578 + calibrationName = "default"; 1.579 + 1.580 + String path = GetBaseOVRPath(true); 1.581 + path += "/Devices.json"; 1.582 + 1.583 + // Load the device profiles 1.584 + Ptr<JSON> root = *JSON::Load(path); 1.585 + if (root == NULL) 1.586 + return false; 1.587 + 1.588 + // Quick sanity check of the file type and format before we parse it 1.589 + JSON* version = root->GetFirstItem(); 1.590 + if (version && version->Name == "Oculus Device Profile Version") 1.591 + { // In the future I may need to check versioning to determine parse method 1.592 + } 1.593 + else 1.594 + { 1.595 + return false; 1.596 + } 1.597 + 1.598 + bool autoEnableCorrection = false; 1.599 + 1.600 + JSON* device = root->GetNextItem(version); 1.601 + while (device) 1.602 + { // Search for a previous calibration with the same name for this device 1.603 + // and remove it before adding the new one 1.604 + if (device->Name == "Device") 1.605 + { 1.606 + JSON* item = device->GetItemByName("Serial"); 1.607 + if (item && item->Value == CachedSensorInfo.SerialNumber) 1.608 + { // found an entry for this device 1.609 + 1.610 + JSON* autoyaw = device->GetItemByName("EnableYawCorrection"); 1.611 + if (autoyaw) 1.612 + autoEnableCorrection = (autoyaw->dValue != 0); 1.613 + 1.614 + item = device->GetNextItem(item); 1.615 + while (item) 1.616 + { 1.617 + if (item->Name == "MagCalibration") 1.618 + { 1.619 + JSON* calibration = item; 1.620 + JSON* name = calibration->GetItemByName("Name"); 1.621 + if (name && name->Value == calibrationName) 1.622 + { // found a calibration with this name 1.623 + 1.624 + time_t now; 1.625 + time(&now); 1.626 + 1.627 + // parse the calibration time 1.628 + time_t calibration_time = now; 1.629 + JSON* caltime = calibration->GetItemByName("Time"); 1.630 + if (caltime) 1.631 + { 1.632 + const char* caltime_str = caltime->Value.ToCStr(); 1.633 + 1.634 + tm ct; 1.635 + memset(&ct, 0, sizeof(tm)); 1.636 + 1.637 +#ifdef OVR_OS_WIN32 1.638 + struct tm nowtime; 1.639 + localtime_s(&nowtime, &now); 1.640 + ct.tm_isdst = nowtime.tm_isdst; 1.641 + sscanf_s(caltime_str, "%d-%d-%d %d:%d:%d", 1.642 + &ct.tm_year, &ct.tm_mon, &ct.tm_mday, 1.643 + &ct.tm_hour, &ct.tm_min, &ct.tm_sec); 1.644 +#else 1.645 + struct tm* nowtime = localtime(&now); 1.646 + ct.tm_isdst = nowtime->tm_isdst; 1.647 + sscanf(caltime_str, "%d-%d-%d %d:%d:%d", 1.648 + &ct.tm_year, &ct.tm_mon, &ct.tm_mday, 1.649 + &ct.tm_hour, &ct.tm_min, &ct.tm_sec); 1.650 +#endif 1.651 + ct.tm_year -= 1900; 1.652 + ct.tm_mon--; 1.653 + calibration_time = mktime(&ct); 1.654 + } 1.655 + 1.656 + // parse the calibration matrix 1.657 + JSON* cal = calibration->GetItemByName("Calibration"); 1.658 + if (cal) 1.659 + { 1.660 + const char* data_str = cal->Value.ToCStr(); 1.661 + Matrix4f calmat; 1.662 + for (int r=0; r<4; r++) 1.663 + { 1.664 + for (int c=0; c<4; c++) 1.665 + { 1.666 + calmat.M[r][c] = (float)atof(data_str); 1.667 + while (data_str && *data_str != ' ') 1.668 + data_str++; 1.669 + 1.670 + if (data_str) 1.671 + data_str++; 1.672 + } 1.673 + } 1.674 + 1.675 + SetMagCalibration(calmat); 1.676 + MagCalibrationTime = calibration_time; 1.677 + EnableYawCorrection = autoEnableCorrection; 1.678 + 1.679 + return true; 1.680 + } 1.681 + } 1.682 + } 1.683 + item = device->GetNextItem(item); 1.684 + } 1.685 + 1.686 + break; 1.687 + } 1.688 + } 1.689 + 1.690 + device = root->GetNextItem(device); 1.691 + } 1.692 + 1.693 + return false; 1.694 +} 1.695 + 1.696 + 1.697 + 1.698 +} // namespace OVR 1.699 +