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 +