oculus1

annotate libovr/Src/OVR_SensorFusion.cpp @ 24:8419d8a13cee

find changesets by author, revision, files, or words in the commit message
foo
author John Tsiombikas <nuclear@member.fsf.org>
date Fri, 04 Oct 2013 14:50:26 +0300
parents
children
rev   line source
nuclear@1 1 /************************************************************************************
nuclear@1 2
nuclear@1 3 Filename : OVR_SensorFusion.cpp
nuclear@1 4 Content : Methods that determine head orientation from sensor data over time
nuclear@1 5 Created : October 9, 2012
nuclear@1 6 Authors : Michael Antonov, Steve LaValle
nuclear@1 7
nuclear@1 8 Copyright : Copyright 2012 Oculus VR, Inc. All Rights reserved.
nuclear@1 9
nuclear@1 10 Use of this software is subject to the terms of the Oculus license
nuclear@1 11 agreement provided at the time of installation or download, or which
nuclear@1 12 otherwise accompanies this software in either electronic or hard copy form.
nuclear@1 13
nuclear@1 14 *************************************************************************************/
nuclear@1 15
nuclear@1 16 #include "OVR_SensorFusion.h"
nuclear@1 17 #include "Kernel/OVR_Log.h"
nuclear@1 18 #include "Kernel/OVR_System.h"
nuclear@1 19 #include "OVR_JSON.h"
nuclear@1 20 #include "OVR_Profile.h"
nuclear@1 21
nuclear@1 22 namespace OVR {
nuclear@1 23
nuclear@1 24 //-------------------------------------------------------------------------------------
nuclear@1 25 // ***** Sensor Fusion
nuclear@1 26
nuclear@1 27 SensorFusion::SensorFusion(SensorDevice* sensor)
nuclear@1 28 : Handler(getThis()), pDelegate(0),
nuclear@1 29 Gain(0.05f), YawMult(1), EnableGravity(true), Stage(0), RunningTime(0), DeltaT(0.001f),
nuclear@1 30 EnablePrediction(true), PredictionDT(0.03f), PredictionTimeIncrement(0.001f),
nuclear@1 31 FRawMag(10), FAccW(20), FAngV(20),
nuclear@1 32 TiltCondCount(0), TiltErrorAngle(0),
nuclear@1 33 TiltErrorAxis(0,1,0),
nuclear@1 34 MagCondCount(0), MagCalibrated(false), MagRefQ(0, 0, 0, 1),
nuclear@1 35 MagRefM(0), MagRefYaw(0), YawErrorAngle(0), MagRefDistance(0.5f),
nuclear@1 36 YawErrorCount(0), YawCorrectionActivated(false), YawCorrectionInProgress(false),
nuclear@1 37 EnableYawCorrection(false), MagNumReferences(0), MagHasNearbyReference(false),
nuclear@1 38 MotionTrackingEnabled(true)
nuclear@1 39 {
nuclear@1 40 if (sensor)
nuclear@1 41 AttachToSensor(sensor);
nuclear@1 42 MagCalibrationMatrix.SetIdentity();
nuclear@1 43 }
nuclear@1 44
nuclear@1 45 SensorFusion::~SensorFusion()
nuclear@1 46 {
nuclear@1 47 }
nuclear@1 48
nuclear@1 49
nuclear@1 50 bool SensorFusion::AttachToSensor(SensorDevice* sensor)
nuclear@1 51 {
nuclear@1 52 // clear the cached device information
nuclear@1 53 CachedSensorInfo.SerialNumber[0] = 0;
nuclear@1 54 CachedSensorInfo.VendorId = 0;
nuclear@1 55 CachedSensorInfo.ProductId = 0;
nuclear@1 56
nuclear@1 57 if (sensor != NULL)
nuclear@1 58 {
nuclear@1 59 // Cache the sensor device so we can access this information during
nuclear@1 60 // mag saving and loading (avoid holding a reference to sensor to prevent
nuclear@1 61 // deadlock on shutdown)
nuclear@1 62 sensor->GetDeviceInfo(&CachedSensorInfo); // save the device information
nuclear@1 63 MessageHandler* pCurrentHandler = sensor->GetMessageHandler();
nuclear@1 64
nuclear@1 65 if (pCurrentHandler == &Handler)
nuclear@1 66 {
nuclear@1 67 Reset();
nuclear@1 68 return true;
nuclear@1 69 }
nuclear@1 70
nuclear@1 71 if (pCurrentHandler != NULL)
nuclear@1 72 {
nuclear@1 73 OVR_DEBUG_LOG(
nuclear@1 74 ("SensorFusion::AttachToSensor failed - sensor %p already has handler", sensor));
nuclear@1 75 return false;
nuclear@1 76 }
nuclear@1 77
nuclear@1 78 // Automatically load the default mag calibration for this sensor
nuclear@1 79 LoadMagCalibration();
nuclear@1 80 }
nuclear@1 81
nuclear@1 82 if (Handler.IsHandlerInstalled())
nuclear@1 83 {
nuclear@1 84 Handler.RemoveHandlerFromDevices();
nuclear@1 85 }
nuclear@1 86
nuclear@1 87 if (sensor != NULL)
nuclear@1 88 {
nuclear@1 89 sensor->SetMessageHandler(&Handler);
nuclear@1 90 }
nuclear@1 91
nuclear@1 92 Reset();
nuclear@1 93 return true;
nuclear@1 94 }
nuclear@1 95
nuclear@1 96
nuclear@1 97 // Resets the current orientation
nuclear@1 98 void SensorFusion::Reset()
nuclear@1 99 {
nuclear@1 100 Lock::Locker lockScope(Handler.GetHandlerLock());
nuclear@1 101 Q = Quatf();
nuclear@1 102 QUncorrected = Quatf();
nuclear@1 103 Stage = 0;
nuclear@1 104 RunningTime = 0;
nuclear@1 105 MagNumReferences = 0;
nuclear@1 106 MagHasNearbyReference = false;
nuclear@1 107 }
nuclear@1 108
nuclear@1 109
nuclear@1 110 void SensorFusion::handleMessage(const MessageBodyFrame& msg)
nuclear@1 111 {
nuclear@1 112 if (msg.Type != Message_BodyFrame || !IsMotionTrackingEnabled())
nuclear@1 113 return;
nuclear@1 114
nuclear@1 115 // Put the sensor readings into convenient local variables
nuclear@1 116 Vector3f angVel = msg.RotationRate;
nuclear@1 117 Vector3f rawAccel = msg.Acceleration;
nuclear@1 118 Vector3f mag = msg.MagneticField;
nuclear@1 119
nuclear@1 120 // Set variables accessible through the class API
nuclear@1 121 DeltaT = msg.TimeDelta;
nuclear@1 122 AngV = msg.RotationRate;
nuclear@1 123 AngV.y *= YawMult; // Warning: If YawMult != 1, then AngV is not true angular velocity
nuclear@1 124 A = rawAccel;
nuclear@1 125
nuclear@1 126 // Allow external access to uncalibrated magnetometer values
nuclear@1 127 RawMag = mag;
nuclear@1 128
nuclear@1 129 // Apply the calibration parameters to raw mag
nuclear@1 130 if (HasMagCalibration())
nuclear@1 131 {
nuclear@1 132 mag.x += MagCalibrationMatrix.M[0][3];
nuclear@1 133 mag.y += MagCalibrationMatrix.M[1][3];
nuclear@1 134 mag.z += MagCalibrationMatrix.M[2][3];
nuclear@1 135 }
nuclear@1 136
nuclear@1 137 // Provide external access to calibrated mag values
nuclear@1 138 // (if the mag is not calibrated, then the raw value is returned)
nuclear@1 139 CalMag = mag;
nuclear@1 140
nuclear@1 141 float angVelLength = angVel.Length();
nuclear@1 142 float accLength = rawAccel.Length();
nuclear@1 143
nuclear@1 144
nuclear@1 145 // Acceleration in the world frame (Q is current HMD orientation)
nuclear@1 146 Vector3f accWorld = Q.Rotate(rawAccel);
nuclear@1 147
nuclear@1 148 // Keep track of time
nuclear@1 149 Stage++;
nuclear@1 150 RunningTime += DeltaT;
nuclear@1 151
nuclear@1 152 // Insert current sensor data into filter history
nuclear@1 153 FRawMag.AddElement(RawMag);
nuclear@1 154 FAccW.AddElement(accWorld);
nuclear@1 155 FAngV.AddElement(angVel);
nuclear@1 156
nuclear@1 157 // Update orientation Q based on gyro outputs. This technique is
nuclear@1 158 // based on direct properties of the angular velocity vector:
nuclear@1 159 // Its direction is the current rotation axis, and its magnitude
nuclear@1 160 // is the rotation rate (rad/sec) about that axis. Our sensor
nuclear@1 161 // sampling rate is so fast that we need not worry about integral
nuclear@1 162 // approximation error (not yet, anyway).
nuclear@1 163 if (angVelLength > 0.0f)
nuclear@1 164 {
nuclear@1 165 Vector3f rotAxis = angVel / angVelLength;
nuclear@1 166 float halfRotAngle = angVelLength * DeltaT * 0.5f;
nuclear@1 167 float sinHRA = sin(halfRotAngle);
nuclear@1 168 Quatf deltaQ(rotAxis.x*sinHRA, rotAxis.y*sinHRA, rotAxis.z*sinHRA, cos(halfRotAngle));
nuclear@1 169
nuclear@1 170 Q = Q * deltaQ;
nuclear@1 171 }
nuclear@1 172
nuclear@1 173 // The quaternion magnitude may slowly drift due to numerical error,
nuclear@1 174 // so it is periodically normalized.
nuclear@1 175 if (Stage % 5000 == 0)
nuclear@1 176 Q.Normalize();
nuclear@1 177
nuclear@1 178 // Maintain the uncorrected orientation for later use by predictive filtering
nuclear@1 179 QUncorrected = Q;
nuclear@1 180
nuclear@1 181 // Perform tilt correction using the accelerometer data. This enables
nuclear@1 182 // drift errors in pitch and roll to be corrected. Note that yaw cannot be corrected
nuclear@1 183 // because the rotation axis is parallel to the gravity vector.
nuclear@1 184 if (EnableGravity)
nuclear@1 185 {
nuclear@1 186 // Correcting for tilt error by using accelerometer data
nuclear@1 187 const float gravityEpsilon = 0.4f;
nuclear@1 188 const float angVelEpsilon = 0.1f; // Relatively slow rotation
nuclear@1 189 const int tiltPeriod = 50; // Required time steps of stability
nuclear@1 190 const float maxTiltError = 0.05f;
nuclear@1 191 const float minTiltError = 0.01f;
nuclear@1 192
nuclear@1 193 // This condition estimates whether the only measured acceleration is due to gravity
nuclear@1 194 // (the Rift is not linearly accelerating). It is often wrong, but tends to average
nuclear@1 195 // out well over time.
nuclear@1 196 if ((fabs(accLength - 9.81f) < gravityEpsilon) &&
nuclear@1 197 (angVelLength < angVelEpsilon))
nuclear@1 198 TiltCondCount++;
nuclear@1 199 else
nuclear@1 200 TiltCondCount = 0;
nuclear@1 201
nuclear@1 202 // After stable measurements have been taken over a sufficiently long period,
nuclear@1 203 // estimate the amount of tilt error and calculate the tilt axis for later correction.
nuclear@1 204 if (TiltCondCount >= tiltPeriod)
nuclear@1 205 { // Update TiltErrorEstimate
nuclear@1 206 TiltCondCount = 0;
nuclear@1 207 // Use an average value to reduce noise (could alternatively use an LPF)
nuclear@1 208 Vector3f accWMean = FAccW.Mean();
nuclear@1 209 // Project the acceleration vector into the XZ plane
nuclear@1 210 Vector3f xzAcc = Vector3f(accWMean.x, 0.0f, accWMean.z);
nuclear@1 211 // The unit normal of xzAcc will be the rotation axis for tilt correction
nuclear@1 212 Vector3f tiltAxis = Vector3f(xzAcc.z, 0.0f, -xzAcc.x).Normalized();
nuclear@1 213 Vector3f yUp = Vector3f(0.0f, 1.0f, 0.0f);
nuclear@1 214 // This is the amount of rotation
nuclear@1 215 float tiltAngle = yUp.Angle(accWMean);
nuclear@1 216 // Record values if the tilt error is intolerable
nuclear@1 217 if (tiltAngle > maxTiltError)
nuclear@1 218 {
nuclear@1 219 TiltErrorAngle = tiltAngle;
nuclear@1 220 TiltErrorAxis = tiltAxis;
nuclear@1 221 }
nuclear@1 222 }
nuclear@1 223
nuclear@1 224 // This part performs the actual tilt correction as needed
nuclear@1 225 if (TiltErrorAngle > minTiltError)
nuclear@1 226 {
nuclear@1 227 if ((TiltErrorAngle > 0.4f)&&(RunningTime < 8.0f))
nuclear@1 228 { // Tilt completely to correct orientation
nuclear@1 229 Q = Quatf(TiltErrorAxis, -TiltErrorAngle) * Q;
nuclear@1 230 TiltErrorAngle = 0.0f;
nuclear@1 231 }
nuclear@1 232 else
nuclear@1 233 {
nuclear@1 234 //LogText("Performing tilt correction - Angle: %f Axis: %f %f %f\n",
nuclear@1 235 // TiltErrorAngle,TiltErrorAxis.x,TiltErrorAxis.y,TiltErrorAxis.z);
nuclear@1 236 //float deltaTiltAngle = -Gain*TiltErrorAngle*0.005f;
nuclear@1 237 // This uses aggressive correction steps while your head is moving fast
nuclear@1 238 float deltaTiltAngle = -Gain*TiltErrorAngle*0.005f*(5.0f*angVelLength+1.0f);
nuclear@1 239 // Incrementally "un-tilt" by a small step size
nuclear@1 240 Q = Quatf(TiltErrorAxis, deltaTiltAngle) * Q;
nuclear@1 241 TiltErrorAngle += deltaTiltAngle;
nuclear@1 242 }
nuclear@1 243 }
nuclear@1 244 }
nuclear@1 245
nuclear@1 246 // Yaw drift correction based on magnetometer data. This corrects the part of the drift
nuclear@1 247 // that the accelerometer cannot handle.
nuclear@1 248 // This will only work if the magnetometer has been enabled, calibrated, and a reference
nuclear@1 249 // point has been set.
nuclear@1 250 const float maxAngVelLength = 3.0f;
nuclear@1 251 const int magWindow = 5;
nuclear@1 252 const float yawErrorMax = 0.1f;
nuclear@1 253 const float yawErrorMin = 0.01f;
nuclear@1 254 const int yawErrorCountLimit = 50;
nuclear@1 255 const float yawRotationStep = 0.00002f;
nuclear@1 256
nuclear@1 257 if (angVelLength < maxAngVelLength)
nuclear@1 258 MagCondCount++;
nuclear@1 259 else
nuclear@1 260 MagCondCount = 0;
nuclear@1 261
nuclear@1 262 // Find, create, and utilize reference points for the magnetometer
nuclear@1 263 // Need to be careful not to set reference points while there is significant tilt error
nuclear@1 264 if ((EnableYawCorrection && MagCalibrated)&&(RunningTime > 10.0f)&&(TiltErrorAngle < 0.2f))
nuclear@1 265 {
nuclear@1 266 if (MagNumReferences == 0)
nuclear@1 267 {
nuclear@1 268 setMagReference(); // Use the current direction
nuclear@1 269 }
nuclear@1 270 else if (Q.Distance(MagRefQ) > MagRefDistance)
nuclear@1 271 {
nuclear@1 272 MagHasNearbyReference = false;
nuclear@1 273 float bestDist = 100000.0f;
nuclear@1 274 int bestNdx = 0;
nuclear@1 275 float dist;
nuclear@1 276 for (int i = 0; i < MagNumReferences; i++)
nuclear@1 277 {
nuclear@1 278 dist = Q.Distance(MagRefTableQ[i]);
nuclear@1 279 if (dist < bestDist)
nuclear@1 280 {
nuclear@1 281 bestNdx = i;
nuclear@1 282 bestDist = dist;
nuclear@1 283 }
nuclear@1 284 }
nuclear@1 285
nuclear@1 286 if (bestDist < MagRefDistance)
nuclear@1 287 {
nuclear@1 288 MagHasNearbyReference = true;
nuclear@1 289 MagRefQ = MagRefTableQ[bestNdx];
nuclear@1 290 MagRefM = MagRefTableM[bestNdx];
nuclear@1 291 MagRefYaw = MagRefTableYaw[bestNdx];
nuclear@1 292 //LogText("Using reference %d\n",bestNdx);
nuclear@1 293 }
nuclear@1 294 else if (MagNumReferences < MagMaxReferences)
nuclear@1 295 setMagReference();
nuclear@1 296 }
nuclear@1 297 }
nuclear@1 298
nuclear@1 299 YawCorrectionInProgress = false;
nuclear@1 300 if (EnableYawCorrection && MagCalibrated && (RunningTime > 2.0f) && (MagCondCount >= magWindow) &&
nuclear@1 301 MagHasNearbyReference)
nuclear@1 302 {
nuclear@1 303 // Use rotational invariance to bring reference mag value into global frame
nuclear@1 304 Vector3f grefmag = MagRefQ.Rotate(GetCalibratedMagValue(MagRefM));
nuclear@1 305 // Bring current (averaged) mag reading into global frame
nuclear@1 306 Vector3f gmag = Q.Rotate(GetCalibratedMagValue(FRawMag.Mean()));
nuclear@1 307 // Calculate the reference yaw in the global frame
nuclear@1 308 Anglef gryaw = Anglef(atan2(grefmag.x,grefmag.z));
nuclear@1 309 // Calculate the current yaw in the global frame
nuclear@1 310 Anglef gyaw = Anglef(atan2(gmag.x,gmag.z));
nuclear@1 311 // The difference between reference and current yaws is the perceived error
nuclear@1 312 Anglef YawErrorAngle = gyaw - gryaw;
nuclear@1 313
nuclear@1 314 //LogText("Yaw error estimate: %f\n",YawErrorAngle.Get());
nuclear@1 315 // If the perceived error is large, keep count
nuclear@1 316 if ((YawErrorAngle.Abs() > yawErrorMax) && (!YawCorrectionActivated))
nuclear@1 317 YawErrorCount++;
nuclear@1 318 // After enough iterations of high perceived error, start the correction process
nuclear@1 319 if (YawErrorCount > yawErrorCountLimit)
nuclear@1 320 YawCorrectionActivated = true;
nuclear@1 321 // If the perceived error becomes small, turn off the yaw correction
nuclear@1 322 if ((YawErrorAngle.Abs() < yawErrorMin) && YawCorrectionActivated)
nuclear@1 323 {
nuclear@1 324 YawCorrectionActivated = false;
nuclear@1 325 YawErrorCount = 0;
nuclear@1 326 }
nuclear@1 327
nuclear@1 328 // Perform the actual yaw correction, due to previously detected, large yaw error
nuclear@1 329 if (YawCorrectionActivated)
nuclear@1 330 {
nuclear@1 331 YawCorrectionInProgress = true;
nuclear@1 332 // Incrementally "unyaw" by a small step size
nuclear@1 333 Q = Quatf(Vector3f(0.0f,1.0f,0.0f), -yawRotationStep * YawErrorAngle.Sign()) * Q;
nuclear@1 334 }
nuclear@1 335 }
nuclear@1 336 }
nuclear@1 337
nuclear@1 338
nuclear@1 339 // A predictive filter based on extrapolating the smoothed, current angular velocity
nuclear@1 340 Quatf SensorFusion::GetPredictedOrientation(float pdt)
nuclear@1 341 {
nuclear@1 342 Lock::Locker lockScope(Handler.GetHandlerLock());
nuclear@1 343 Quatf qP = QUncorrected;
nuclear@1 344
nuclear@1 345 if (EnablePrediction)
nuclear@1 346 {
nuclear@1 347 // This method assumes a constant angular velocity
nuclear@1 348 Vector3f angVelF = FAngV.SavitzkyGolaySmooth8();
nuclear@1 349 float angVelFL = angVelF.Length();
nuclear@1 350
nuclear@1 351 // Force back to raw measurement
nuclear@1 352 angVelF = AngV;
nuclear@1 353 angVelFL = AngV.Length();
nuclear@1 354
nuclear@1 355 // Dynamic prediction interval: Based on angular velocity to reduce vibration
nuclear@1 356 const float minPdt = 0.001f;
nuclear@1 357 const float slopePdt = 0.1f;
nuclear@1 358 float newpdt = pdt;
nuclear@1 359 float tpdt = minPdt + slopePdt * angVelFL;
nuclear@1 360 if (tpdt < pdt)
nuclear@1 361 newpdt = tpdt;
nuclear@1 362 //LogText("PredictonDTs: %d\n",(int)(newpdt / PredictionTimeIncrement + 0.5f));
nuclear@1 363
nuclear@1 364 if (angVelFL > 0.001f)
nuclear@1 365 {
nuclear@1 366 Vector3f rotAxisP = angVelF / angVelFL;
nuclear@1 367 float halfRotAngleP = angVelFL * newpdt * 0.5f;
nuclear@1 368 float sinaHRAP = sin(halfRotAngleP);
nuclear@1 369 Quatf deltaQP(rotAxisP.x*sinaHRAP, rotAxisP.y*sinaHRAP,
nuclear@1 370 rotAxisP.z*sinaHRAP, cos(halfRotAngleP));
nuclear@1 371 qP = QUncorrected * deltaQP;
nuclear@1 372 }
nuclear@1 373 }
nuclear@1 374 return qP;
nuclear@1 375 }
nuclear@1 376
nuclear@1 377
nuclear@1 378 Vector3f SensorFusion::GetCalibratedMagValue(const Vector3f& rawMag) const
nuclear@1 379 {
nuclear@1 380 Vector3f mag = rawMag;
nuclear@1 381 OVR_ASSERT(HasMagCalibration());
nuclear@1 382 mag.x += MagCalibrationMatrix.M[0][3];
nuclear@1 383 mag.y += MagCalibrationMatrix.M[1][3];
nuclear@1 384 mag.z += MagCalibrationMatrix.M[2][3];
nuclear@1 385 return mag;
nuclear@1 386 }
nuclear@1 387
nuclear@1 388
nuclear@1 389 void SensorFusion::setMagReference(const Quatf& q, const Vector3f& rawMag)
nuclear@1 390 {
nuclear@1 391 if (MagNumReferences < MagMaxReferences)
nuclear@1 392 {
nuclear@1 393 MagRefTableQ[MagNumReferences] = q;
nuclear@1 394 MagRefTableM[MagNumReferences] = rawMag; //FRawMag.Mean();
nuclear@1 395
nuclear@1 396 //LogText("Inserting reference %d\n",MagNumReferences);
nuclear@1 397
nuclear@1 398 MagRefQ = q;
nuclear@1 399 MagRefM = rawMag;
nuclear@1 400
nuclear@1 401 float pitch, roll, yaw;
nuclear@1 402 Quatf q2 = q;
nuclear@1 403 q2.GetEulerAngles<Axis_X, Axis_Z, Axis_Y>(&pitch, &roll, &yaw);
nuclear@1 404 MagRefTableYaw[MagNumReferences] = yaw;
nuclear@1 405 MagRefYaw = yaw;
nuclear@1 406
nuclear@1 407 MagNumReferences++;
nuclear@1 408
nuclear@1 409 MagHasNearbyReference = true;
nuclear@1 410 }
nuclear@1 411 }
nuclear@1 412
nuclear@1 413
nuclear@1 414 SensorFusion::BodyFrameHandler::~BodyFrameHandler()
nuclear@1 415 {
nuclear@1 416 RemoveHandlerFromDevices();
nuclear@1 417 }
nuclear@1 418
nuclear@1 419 void SensorFusion::BodyFrameHandler::OnMessage(const Message& msg)
nuclear@1 420 {
nuclear@1 421 if (msg.Type == Message_BodyFrame)
nuclear@1 422 pFusion->handleMessage(static_cast<const MessageBodyFrame&>(msg));
nuclear@1 423 if (pFusion->pDelegate)
nuclear@1 424 pFusion->pDelegate->OnMessage(msg);
nuclear@1 425 }
nuclear@1 426
nuclear@1 427 bool SensorFusion::BodyFrameHandler::SupportsMessageType(MessageType type) const
nuclear@1 428 {
nuclear@1 429 return (type == Message_BodyFrame);
nuclear@1 430 }
nuclear@1 431
nuclear@1 432 // Writes the current calibration for a particular device to a device profile file
nuclear@1 433 // sensor - the sensor that was calibrated
nuclear@1 434 // cal_name - an optional name for the calibration or default if cal_name == NULL
nuclear@1 435 bool SensorFusion::SaveMagCalibration(const char* calibrationName) const
nuclear@1 436 {
nuclear@1 437 if (CachedSensorInfo.SerialNumber[0] == NULL || !HasMagCalibration())
nuclear@1 438 return false;
nuclear@1 439
nuclear@1 440 // A named calibration may be specified for calibration in different
nuclear@1 441 // environments, otherwise the default calibration is used
nuclear@1 442 if (calibrationName == NULL)
nuclear@1 443 calibrationName = "default";
nuclear@1 444
nuclear@1 445 // Generate a mag calibration event
nuclear@1 446 JSON* calibration = JSON::CreateObject();
nuclear@1 447 // (hardcoded for now) the measurement and representation method
nuclear@1 448 calibration->AddStringItem("Version", "1.0");
nuclear@1 449 calibration->AddStringItem("Name", "default");
nuclear@1 450
nuclear@1 451 // time stamp the calibration
nuclear@1 452 char time_str[64];
nuclear@1 453
nuclear@1 454 #ifdef OVR_OS_WIN32
nuclear@1 455 struct tm caltime;
nuclear@1 456 localtime_s(&caltime, &MagCalibrationTime);
nuclear@1 457 strftime(time_str, 64, "%Y-%m-%d %H:%M:%S", &caltime);
nuclear@1 458 #else
nuclear@1 459 struct tm* caltime;
nuclear@1 460 caltime = localtime(&MagCalibrationTime);
nuclear@1 461 strftime(time_str, 64, "%Y-%m-%d %H:%M:%S", caltime);
nuclear@1 462 #endif
nuclear@1 463
nuclear@1 464 calibration->AddStringItem("Time", time_str);
nuclear@1 465
nuclear@1 466 // write the full calibration matrix
nuclear@1 467 Matrix4f calmat = GetMagCalibration();
nuclear@1 468 char matrix[128];
nuclear@1 469 int pos = 0;
nuclear@1 470 for (int r=0; r<4; r++)
nuclear@1 471 {
nuclear@1 472 for (int c=0; c<4; c++)
nuclear@1 473 {
nuclear@1 474 pos += (int)OVR_sprintf(matrix+pos, 128, "%g ", calmat.M[r][c]);
nuclear@1 475 }
nuclear@1 476 }
nuclear@1 477 calibration->AddStringItem("Calibration", matrix);
nuclear@1 478
nuclear@1 479
nuclear@1 480 String path = GetBaseOVRPath(true);
nuclear@1 481 path += "/Devices.json";
nuclear@1 482
nuclear@1 483 // Look for a prexisting device file to edit
nuclear@1 484 Ptr<JSON> root = *JSON::Load(path);
nuclear@1 485 if (root)
nuclear@1 486 { // Quick sanity check of the file type and format before we parse it
nuclear@1 487 JSON* version = root->GetFirstItem();
nuclear@1 488 if (version && version->Name == "Oculus Device Profile Version")
nuclear@1 489 { // In the future I may need to check versioning to determine parse method
nuclear@1 490 }
nuclear@1 491 else
nuclear@1 492 {
nuclear@1 493 root->Release();
nuclear@1 494 root = NULL;
nuclear@1 495 }
nuclear@1 496 }
nuclear@1 497
nuclear@1 498 JSON* device = NULL;
nuclear@1 499 if (root)
nuclear@1 500 {
nuclear@1 501 device = root->GetFirstItem(); // skip the header
nuclear@1 502 device = root->GetNextItem(device);
nuclear@1 503 while (device)
nuclear@1 504 { // Search for a previous calibration with the same name for this device
nuclear@1 505 // and remove it before adding the new one
nuclear@1 506 if (device->Name == "Device")
nuclear@1 507 {
nuclear@1 508 JSON* item = device->GetItemByName("Serial");
nuclear@1 509 if (item && item->Value == CachedSensorInfo.SerialNumber)
nuclear@1 510 { // found an entry for this device
nuclear@1 511 item = device->GetNextItem(item);
nuclear@1 512 while (item)
nuclear@1 513 {
nuclear@1 514 if (item->Name == "MagCalibration")
nuclear@1 515 {
nuclear@1 516 JSON* name = item->GetItemByName("Name");
nuclear@1 517 if (name && name->Value == calibrationName)
nuclear@1 518 { // found a calibration of the same name
nuclear@1 519 item->RemoveNode();
nuclear@1 520 item->Release();
nuclear@1 521 break;
nuclear@1 522 }
nuclear@1 523 }
nuclear@1 524 item = device->GetNextItem(item);
nuclear@1 525 }
nuclear@1 526
nuclear@1 527 // update the auto-mag flag
nuclear@1 528 item = device->GetItemByName("EnableYawCorrection");
nuclear@1 529 if (item)
nuclear@1 530 item->dValue = (double)EnableYawCorrection;
nuclear@1 531 else
nuclear@1 532 device->AddBoolItem("EnableYawCorrection", EnableYawCorrection);
nuclear@1 533
nuclear@1 534 break;
nuclear@1 535 }
nuclear@1 536 }
nuclear@1 537
nuclear@1 538 device = root->GetNextItem(device);
nuclear@1 539 }
nuclear@1 540 }
nuclear@1 541 else
nuclear@1 542 { // Create a new device root
nuclear@1 543 root = *JSON::CreateObject();
nuclear@1 544 root->AddStringItem("Oculus Device Profile Version", "1.0");
nuclear@1 545 }
nuclear@1 546
nuclear@1 547 if (device == NULL)
nuclear@1 548 {
nuclear@1 549 device = JSON::CreateObject();
nuclear@1 550 device->AddStringItem("Product", CachedSensorInfo.ProductName);
nuclear@1 551 device->AddNumberItem("ProductID", CachedSensorInfo.ProductId);
nuclear@1 552 device->AddStringItem("Serial", CachedSensorInfo.SerialNumber);
nuclear@1 553 device->AddBoolItem("EnableYawCorrection", EnableYawCorrection);
nuclear@1 554
nuclear@1 555 root->AddItem("Device", device);
nuclear@1 556 }
nuclear@1 557
nuclear@1 558 // Create and the add the new calibration event to the device
nuclear@1 559 device->AddItem("MagCalibration", calibration);
nuclear@1 560
nuclear@1 561 return root->Save(path);
nuclear@1 562 }
nuclear@1 563
nuclear@1 564 // Loads a saved calibration for the specified device from the device profile file
nuclear@1 565 // sensor - the sensor that the calibration was saved for
nuclear@1 566 // cal_name - an optional name for the calibration or the default if cal_name == NULL
nuclear@1 567 bool SensorFusion::LoadMagCalibration(const char* calibrationName)
nuclear@1 568 {
nuclear@1 569 if (CachedSensorInfo.SerialNumber[0] == NULL)
nuclear@1 570 return false;
nuclear@1 571
nuclear@1 572 // A named calibration may be specified for calibration in different
nuclear@1 573 // environments, otherwise the default calibration is used
nuclear@1 574 if (calibrationName == NULL)
nuclear@1 575 calibrationName = "default";
nuclear@1 576
nuclear@1 577 String path = GetBaseOVRPath(true);
nuclear@1 578 path += "/Devices.json";
nuclear@1 579
nuclear@1 580 // Load the device profiles
nuclear@1 581 Ptr<JSON> root = *JSON::Load(path);
nuclear@1 582 if (root == NULL)
nuclear@1 583 return false;
nuclear@1 584
nuclear@1 585 // Quick sanity check of the file type and format before we parse it
nuclear@1 586 JSON* version = root->GetFirstItem();
nuclear@1 587 if (version && version->Name == "Oculus Device Profile Version")
nuclear@1 588 { // In the future I may need to check versioning to determine parse method
nuclear@1 589 }
nuclear@1 590 else
nuclear@1 591 {
nuclear@1 592 return false;
nuclear@1 593 }
nuclear@1 594
nuclear@1 595 bool autoEnableCorrection = false;
nuclear@1 596
nuclear@1 597 JSON* device = root->GetNextItem(version);
nuclear@1 598 while (device)
nuclear@1 599 { // Search for a previous calibration with the same name for this device
nuclear@1 600 // and remove it before adding the new one
nuclear@1 601 if (device->Name == "Device")
nuclear@1 602 {
nuclear@1 603 JSON* item = device->GetItemByName("Serial");
nuclear@1 604 if (item && item->Value == CachedSensorInfo.SerialNumber)
nuclear@1 605 { // found an entry for this device
nuclear@1 606
nuclear@1 607 JSON* autoyaw = device->GetItemByName("EnableYawCorrection");
nuclear@1 608 if (autoyaw)
nuclear@1 609 autoEnableCorrection = (autoyaw->dValue != 0);
nuclear@1 610
nuclear@1 611 item = device->GetNextItem(item);
nuclear@1 612 while (item)
nuclear@1 613 {
nuclear@1 614 if (item->Name == "MagCalibration")
nuclear@1 615 {
nuclear@1 616 JSON* calibration = item;
nuclear@1 617 JSON* name = calibration->GetItemByName("Name");
nuclear@1 618 if (name && name->Value == calibrationName)
nuclear@1 619 { // found a calibration with this name
nuclear@1 620
nuclear@1 621 time_t now;
nuclear@1 622 time(&now);
nuclear@1 623
nuclear@1 624 // parse the calibration time
nuclear@1 625 time_t calibration_time = now;
nuclear@1 626 JSON* caltime = calibration->GetItemByName("Time");
nuclear@1 627 if (caltime)
nuclear@1 628 {
nuclear@1 629 const char* caltime_str = caltime->Value.ToCStr();
nuclear@1 630
nuclear@1 631 tm ct;
nuclear@1 632 memset(&ct, 0, sizeof(tm));
nuclear@1 633
nuclear@1 634 #ifdef OVR_OS_WIN32
nuclear@1 635 struct tm nowtime;
nuclear@1 636 localtime_s(&nowtime, &now);
nuclear@1 637 ct.tm_isdst = nowtime.tm_isdst;
nuclear@1 638 sscanf_s(caltime_str, "%d-%d-%d %d:%d:%d",
nuclear@1 639 &ct.tm_year, &ct.tm_mon, &ct.tm_mday,
nuclear@1 640 &ct.tm_hour, &ct.tm_min, &ct.tm_sec);
nuclear@1 641 #else
nuclear@1 642 struct tm* nowtime = localtime(&now);
nuclear@1 643 ct.tm_isdst = nowtime->tm_isdst;
nuclear@1 644 sscanf(caltime_str, "%d-%d-%d %d:%d:%d",
nuclear@1 645 &ct.tm_year, &ct.tm_mon, &ct.tm_mday,
nuclear@1 646 &ct.tm_hour, &ct.tm_min, &ct.tm_sec);
nuclear@1 647 #endif
nuclear@1 648 ct.tm_year -= 1900;
nuclear@1 649 ct.tm_mon--;
nuclear@1 650 calibration_time = mktime(&ct);
nuclear@1 651 }
nuclear@1 652
nuclear@1 653 // parse the calibration matrix
nuclear@1 654 JSON* cal = calibration->GetItemByName("Calibration");
nuclear@1 655 if (cal)
nuclear@1 656 {
nuclear@1 657 const char* data_str = cal->Value.ToCStr();
nuclear@1 658 Matrix4f calmat;
nuclear@1 659 for (int r=0; r<4; r++)
nuclear@1 660 {
nuclear@1 661 for (int c=0; c<4; c++)
nuclear@1 662 {
nuclear@1 663 calmat.M[r][c] = (float)atof(data_str);
nuclear@1 664 while (data_str && *data_str != ' ')
nuclear@1 665 data_str++;
nuclear@1 666
nuclear@1 667 if (data_str)
nuclear@1 668 data_str++;
nuclear@1 669 }
nuclear@1 670 }
nuclear@1 671
nuclear@1 672 SetMagCalibration(calmat);
nuclear@1 673 MagCalibrationTime = calibration_time;
nuclear@1 674 EnableYawCorrection = autoEnableCorrection;
nuclear@1 675
nuclear@1 676 return true;
nuclear@1 677 }
nuclear@1 678 }
nuclear@1 679 }
nuclear@1 680 item = device->GetNextItem(item);
nuclear@1 681 }
nuclear@1 682
nuclear@1 683 break;
nuclear@1 684 }
nuclear@1 685 }
nuclear@1 686
nuclear@1 687 device = root->GetNextItem(device);
nuclear@1 688 }
nuclear@1 689
nuclear@1 690 return false;
nuclear@1 691 }
nuclear@1 692
nuclear@1 693
nuclear@1 694
nuclear@1 695 } // namespace OVR
nuclear@1 696