Merge branch 'master' into stable

12 files changed, 521 insertions, 162 deletions

diff --git a/LibOVR/Src/Kernel/OVR_List.h b/LibOVR/Src/Kernel/OVR_List.h
index 4b32f49..6ad471d 100644
--- a/LibOVR/Src/Kernel/OVR_List.h
+++ b/LibOVR/Src/Kernel/OVR_List.h
@@ -115,7 +115,7 @@ struct ListNode
 //    }
 //
 
-// List<> represents a doubly-linked list if T, where each T must derive
+// List<> represents a doubly-linked list of T, where each T must derive
 // from ListNode<B>. B specifies the base class that was directly
 // derived from ListNode, and is only necessary if there is an intermediate
 // inheritance chain.
diff --git a/LibOVR/Src/Kernel/OVR_Math.h b/LibOVR/Src/Kernel/OVR_Math.h
index 2b255c7..567ea9c 100644
--- a/LibOVR/Src/Kernel/OVR_Math.h
+++ b/LibOVR/Src/Kernel/OVR_Math.h
@@ -879,7 +879,7 @@ public:
     }
 
     // Sets this quaternion to the one rotates in the opposite direction.
-    void        Invert() const
+    void        Invert()
     {
         *this = Quat(-x, -y, -z, w);
     }
diff --git a/LibOVR/Src/OVR_DeviceImpl.h b/LibOVR/Src/OVR_DeviceImpl.h
index 2273d3d..e61e37b 100644
--- a/LibOVR/Src/OVR_DeviceImpl.h
+++ b/LibOVR/Src/OVR_DeviceImpl.h
@@ -350,7 +350,7 @@ public:
     virtual void Shutdown();
 
 
-    // Every DeviceManager has an associated profile manager, which us used to store
+    // Every DeviceManager has an associated profile manager, which is used to store
     // user settings that may affect device behavior. 
     virtual ProfileManager* GetProfileManager() const { return pProfileManager.GetPtr(); }
 
diff --git a/LibOVR/Src/OVR_Linux_HIDDevice.cpp b/LibOVR/Src/OVR_Linux_HIDDevice.cpp
index 6656e9a..062d566 100644
--- a/LibOVR/Src/OVR_Linux_HIDDevice.cpp
+++ b/LibOVR/Src/OVR_Linux_HIDDevice.cpp
@@ -258,7 +258,7 @@ bool HIDDeviceManager::Enumerate(HIDEnumerateVisitor* enumVisitor)
             udev_device_unref(hid);
             entry = udev_list_entry_get_next(entry);
         }
-	}
+    }
 
 	// Free the enumerator and udev objects
     udev_enumerate_unref(devices);
diff --git a/LibOVR/Src/OVR_Linux_HMDDevice.cpp b/LibOVR/Src/OVR_Linux_HMDDevice.cpp
index 7b26479..dc94851 100644
--- a/LibOVR/Src/OVR_Linux_HMDDevice.cpp
+++ b/LibOVR/Src/OVR_Linux_HMDDevice.cpp
@@ -176,7 +176,7 @@ void HMDDeviceFactory::EnumerateDevices(EnumerateVisitor& visitor)
     bool foundHMD = false;
 
     Display* display = XOpenDisplay(NULL);
-    if (display)
+    if (display && XineramaIsActive(display))
     {
         int numberOfScreens;
         XineramaScreenInfo* screens = XineramaQueryScreens(display, &numberOfScreens);
diff --git a/LibOVR/Src/OVR_Profile.cpp b/LibOVR/Src/OVR_Profile.cpp
index 7554a28..83eade3 100644
--- a/LibOVR/Src/OVR_Profile.cpp
+++ b/LibOVR/Src/OVR_Profile.cpp
@@ -46,16 +46,15 @@ namespace OVR {
 
 //-----------------------------------------------------------------------------
 // Returns the pathname of the JSON file containing the stored profiles
-String GetProfilePath(bool create_dir)
+String GetBaseOVRPath(bool create_dir)
 {
     String path;
 
 #if defined(OVR_OS_WIN32)
-    
-    PWSTR data_path = NULL;
-    SHGetKnownFolderPath(FOLDERID_LocalAppData, 0, NULL, &data_path);
+
+    TCHAR data_path[MAX_PATH];
+    SHGetFolderPath(0, CSIDL_LOCAL_APPDATA, NULL, 0, data_path);
     path = String(data_path);
-    CoTaskMemFree(data_path);
     
     path += "/Oculus";
 
@@ -113,10 +112,15 @@ String GetProfilePath(bool create_dir)
 
 #endif
 
-    path += "/Profiles.json";
     return path;
 }
 
+String GetProfilePath(bool create_dir)
+{
+    String path = GetBaseOVRPath(create_dir);
+    path += "/Profiles.json";
+    return path;
+}
 
 //-----------------------------------------------------------------------------
 // ***** ProfileManager
@@ -232,22 +236,25 @@ void ProfileManager::LoadCache(ProfileType device)
         Ptr<Profile>  profile     = *CreateProfileObject(profileName, device, &deviceName);
 
         // Read the base profile fields.
-        while (item = profileItem->GetNextItem(item), item)
+        if (profile)
         {
-            if (item->Type != JSON_Object)
+            while (item = profileItem->GetNextItem(item), item)
             {
-                profile->ParseProperty(item->Name, item->Value);
-            }
-            else
-            {   // Search for the matching device to get device specific fields
-                if (!deviceFound && OVR_strcmp(item->Name, deviceName) == 0)
+                if (item->Type != JSON_Object)
                 {
-                    deviceFound = true;
-
-                    for (JSON* deviceItem = item->GetFirstItem(); deviceItem;
-                         deviceItem = item->GetNextItem(deviceItem))
+                    profile->ParseProperty(item->Name, item->Value);
+                }
+                else
+                {   // Search for the matching device to get device specific fields
+                    if (!deviceFound && OVR_strcmp(item->Name, deviceName) == 0)
                     {
-                        profile->ParseProperty(deviceItem->Name, deviceItem->Value);
+                        deviceFound = true;
+
+                        for (JSON* deviceItem = item->GetFirstItem(); deviceItem;
+                             deviceItem = item->GetNextItem(deviceItem))
+                        {
+                            profile->ParseProperty(deviceItem->Name, deviceItem->Value);
+                        }
                     }
                 }
             }
diff --git a/LibOVR/Src/OVR_Profile.h b/LibOVR/Src/OVR_Profile.h
index bfd8ea9..7184aae 100644
--- a/LibOVR/Src/OVR_Profile.h
+++ b/LibOVR/Src/OVR_Profile.h
@@ -181,6 +181,9 @@ protected:
     friend class ProfileManager;
 };
 
+
+String GetBaseOVRPath(bool create_dir);
+
 }
 
 #endif // OVR_Profile_h
\ No newline at end of file
diff --git a/LibOVR/Src/OVR_SensorFusion.cpp b/LibOVR/Src/OVR_SensorFusion.cpp
index a4c5809..ba913c1 100644
--- a/LibOVR/Src/OVR_SensorFusion.cpp
+++ b/LibOVR/Src/OVR_SensorFusion.cpp
@@ -16,6 +16,8 @@ otherwise accompanies this software in either electronic or hard copy form.
 #include "OVR_SensorFusion.h"
 #include "Kernel/OVR_Log.h"
 #include "Kernel/OVR_System.h"
+#include "OVR_JSON.h"
+#include "OVR_Profile.h"
 
 namespace OVR {
 
@@ -32,7 +34,8 @@ SensorFusion::SensorFusion(SensorDevice* sensor)
     MagCondCount(0), MagCalibrated(false), MagRefQ(0, 0, 0, 1), 
 	MagRefM(0), MagRefYaw(0), YawErrorAngle(0), MagRefDistance(0.5f),
     YawErrorCount(0), YawCorrectionActivated(false), YawCorrectionInProgress(false), 
-	EnableYawCorrection(false), MagNumReferences(0), MagHasNearbyReference(false)
+	EnableYawCorrection(false), MagNumReferences(0), MagHasNearbyReference(false),
+    MotionTrackingEnabled(true)
 {
    if (sensor)
        AttachToSensor(sensor);
@@ -47,6 +50,7 @@ SensorFusion::~SensorFusion()
 bool SensorFusion::AttachToSensor(SensorDevice* sensor)
 {
     
+    pSensor = sensor;
     if (sensor != NULL)
     {
         MessageHandler* pCurrentHandler = sensor->GetMessageHandler();
@@ -63,6 +67,9 @@ bool SensorFusion::AttachToSensor(SensorDevice* sensor)
                 ("SensorFusion::AttachToSensor failed - sensor %p already has handler", sensor));
             return false;
         }
+
+        // Automatically load the default mag calibration for this sensor
+        LoadMagCalibration();        
     }
 
     if (Handler.IsHandlerInstalled())
@@ -95,7 +102,7 @@ void SensorFusion::Reset()
 
 void SensorFusion::handleMessage(const MessageBodyFrame& msg)
 {
-    if (msg.Type != Message_BodyFrame)
+    if (msg.Type != Message_BodyFrame || !IsMotionTrackingEnabled())
         return;
   
     // Put the sensor readings into convenient local variables
@@ -251,7 +258,7 @@ void SensorFusion::handleMessage(const MessageBodyFrame& msg)
 	{
 	  if (MagNumReferences == 0)
       {
-		  SetMagReference(); // Use the current direction
+		  setMagReference(); // Use the current direction
       }
 	  else if (Q.Distance(MagRefQ) > MagRefDistance) 
       {
@@ -278,7 +285,7 @@ void SensorFusion::handleMessage(const MessageBodyFrame& msg)
               //LogText("Using reference %d\n",bestNdx);
           }
           else if (MagNumReferences < MagMaxReferences)
-              SetMagReference();
+              setMagReference();
 	  }
 	}
 
@@ -322,9 +329,7 @@ void SensorFusion::handleMessage(const MessageBodyFrame& msg)
 }
 
  
-//  Simple predictive filters based on extrapolating the smoothed, current angular velocity
-// or using smooth time derivative information.  The argument is the amount of time into
-// the future to predict.
+//  A predictive filter based on extrapolating the smoothed, current angular velocity
 Quatf SensorFusion::GetPredictedOrientation(float pdt)
 {		
 	Lock::Locker lockScope(Handler.GetHandlerLock());
@@ -332,45 +337,32 @@ Quatf SensorFusion::GetPredictedOrientation(float pdt)
 	
     if (EnablePrediction)
     {
-#if 1
 		// This method assumes a constant angular velocity
 	    Vector3f angVelF  = FAngV.SavitzkyGolaySmooth8();
         float    angVelFL = angVelF.Length();
-            
+
+		// Force back to raw measurement
+        angVelF  = AngV;
+		angVelFL = AngV.Length();
+
+		// Dynamic prediction interval: Based on angular velocity to reduce vibration
+		const float minPdt   = 0.001f;
+		const float slopePdt = 0.1f;
+		float       newpdt   = pdt;
+		float       tpdt     = minPdt + slopePdt * angVelFL;
+		if (tpdt < pdt)
+			newpdt = tpdt;
+		//LogText("PredictonDTs: %d\n",(int)(newpdt / PredictionTimeIncrement + 0.5f));
+
         if (angVelFL > 0.001f)
         {
             Vector3f    rotAxisP      = angVelF / angVelFL;  
-            float       halfRotAngleP = angVelFL * pdt * 0.5f;
+            float       halfRotAngleP = angVelFL * newpdt * 0.5f;
             float       sinaHRAP      = sin(halfRotAngleP);
 		    Quatf       deltaQP(rotAxisP.x*sinaHRAP, rotAxisP.y*sinaHRAP,
                                 rotAxisP.z*sinaHRAP, cos(halfRotAngleP));
             qP = QUncorrected * deltaQP;
-        }
-#else
-		// This method estimates angular acceleration, conservatively
-		OVR_ASSERT(pdt >= 0);
-        int       predictionStages = (int)(pdt / PredictionTimeIncrement + 0.5f);
-        Quatd     qpd        = Quatd(Q.x,Q.y,Q.z,Q.w);
-        Vector3f  aa         = FAngV.SavitzkyGolayDerivative12();
-        Vector3d  aad        = Vector3d(aa.x,aa.y,aa.z);
-        Vector3f  angVelF    = FAngV.SavitzkyGolaySmooth8();
-        Vector3d  avkd       = Vector3d(angVelF.x,angVelF.y,angVelF.z);
-		Vector3d  rotAxisd   = Vector3d(0,1,0);
-        for (int i = 0; i < predictionStages; i++)
-        {
-            double   angVelLengthd = avkd.Length();
-			if (angVelLengthd > 0)
-                rotAxisd = avkd / angVelLengthd;
-            double   halfRotAngled = angVelLengthd * PredictionTimeIncrement * 0.5;
-            double   sinHRAd       = sin(halfRotAngled);
-            Quatd    deltaQd       = Quatd(rotAxisd.x*sinHRAd, rotAxisd.y*sinHRAd, rotAxisd.z*sinHRAd,
-                                           cos(halfRotAngled));
-            qpd = qpd * deltaQd;
-            // Update angular velocity by using the angular acceleration estimate
-            avkd += aad;
-        }
-        qP = Quatf((float)qpd.x,(float)qpd.y,(float)qpd.z,(float)qpd.w);
-#endif
+		}
 	}
     return qP;
 }    
@@ -387,7 +379,7 @@ Vector3f SensorFusion::GetCalibratedMagValue(const Vector3f& rawMag) const
 }
 
 
-void SensorFusion::SetMagReference(const Quatf& q, const Vector3f& rawMag) 
+void SensorFusion::setMagReference(const Quatf& q, const Vector3f& rawMag) 
 {
     if (MagNumReferences < MagMaxReferences)
     {
@@ -430,6 +422,274 @@ bool SensorFusion::BodyFrameHandler::SupportsMessageType(MessageType type) const
     return (type == Message_BodyFrame);
 }
 
+// Writes the current calibration for a particular device to a device profile file
+// sensor - the sensor that was calibrated
+// cal_name - an optional name for the calibration or default if cal_name == NULL
+bool SensorFusion::SaveMagCalibration(const char* calibrationName) const
+{
+    if (pSensor == NULL || !HasMagCalibration())
+        return false;
+    
+    // A named calibration may be specified for calibration in different
+    // environments, otherwise the default calibration is used
+    if (calibrationName == NULL)
+        calibrationName = "default";
+    
+    SensorInfo sinfo;
+    pSensor->GetDeviceInfo(&sinfo);
+
+    // Generate a mag calibration event
+    JSON* calibration = JSON::CreateObject();
+    // (hardcoded for now) the measurement and representation method 
+    calibration->AddStringItem("Version", "1.0");   
+    calibration->AddStringItem("Name", "default");
+
+    // time stamp the calibration
+    char time_str[64];
+   
+#ifdef OVR_OS_WIN32
+    struct tm caltime;
+    localtime_s(&caltime, &MagCalibrationTime);
+    strftime(time_str, 64, "%Y-%m-%d %H:%M:%S", &caltime);
+#else
+    struct tm* caltime;
+    caltime = localtime(&MagCalibrationTime);
+    strftime(time_str, 64, "%Y-%m-%d %H:%M:%S", caltime);
+#endif
+   
+    calibration->AddStringItem("Time", time_str);
+
+    // write the full calibration matrix
+    Matrix4f calmat = GetMagCalibration();
+    char matrix[128];
+    int pos = 0;
+    for (int r=0; r<4; r++)
+    {
+        for (int c=0; c<4; c++)
+        {
+            pos += (int)OVR_sprintf(matrix+pos, 128, "%g ", calmat.M[r][c]);
+        }
+    }
+    calibration->AddStringItem("Calibration", matrix);
+
+    
+    String path = GetBaseOVRPath(true);
+    path += "/Devices.json";
+
+    // Look for a prexisting device file to edit
+    Ptr<JSON> root = *JSON::Load(path);
+    if (root)
+    {   // Quick sanity check of the file type and format before we parse it
+        JSON* version = root->GetFirstItem();
+        if (version && version->Name == "Oculus Device Profile Version")
+        {   // In the future I may need to check versioning to determine parse method
+        }
+        else
+        {
+            root->Release();
+            root = NULL;
+        }
+    }
+
+    JSON* device = NULL;
+    if (root)
+    {
+        device = root->GetFirstItem();   // skip the header
+        device = root->GetNextItem(device);
+        while (device)
+        {   // Search for a previous calibration with the same name for this device
+            // and remove it before adding the new one
+            if (device->Name == "Device")
+            {   
+                JSON* item = device->GetItemByName("Serial");
+                if (item && item->Value == sinfo.SerialNumber)
+                {   // found an entry for this device
+                    item = device->GetNextItem(item);
+                    while (item)
+                    {
+                        if (item->Name == "MagCalibration")
+                        {   
+                            JSON* name = item->GetItemByName("Name");
+                            if (name && name->Value == calibrationName)
+                            {   // found a calibration of the same name
+                                item->RemoveNode();
+                                item->Release();
+                                break;
+                            } 
+                        }
+                        item = device->GetNextItem(item);
+                    }
+
+                    // update the auto-mag flag
+                    item = device->GetItemByName("EnableYawCorrection");
+                    if (item)
+                        item->dValue = (double)EnableYawCorrection;
+                    else
+                        device->AddBoolItem("EnableYawCorrection", EnableYawCorrection);
+
+                    break;
+                }
+            }
+
+            device = root->GetNextItem(device);
+        }
+    }
+    else
+    {   // Create a new device root
+        root = *JSON::CreateObject();
+        root->AddStringItem("Oculus Device Profile Version", "1.0");
+    }
+
+    if (device == NULL)
+    {
+        device = JSON::CreateObject();
+        device->AddStringItem("Product", sinfo.ProductName);
+        device->AddNumberItem("ProductID", sinfo.ProductId);
+        device->AddStringItem("Serial", sinfo.SerialNumber);
+        device->AddBoolItem("EnableYawCorrection", EnableYawCorrection);
+
+        root->AddItem("Device", device);
+    }
+
+    // Create and the add the new calibration event to the device
+    device->AddItem("MagCalibration", calibration);
+
+    return root->Save(path);
+}
+
+// Loads a saved calibration for the specified device from the device profile file
+// sensor - the sensor that the calibration was saved for
+// cal_name - an optional name for the calibration or the default if cal_name == NULL
+bool SensorFusion::LoadMagCalibration(const char* calibrationName)
+{
+    if (pSensor == NULL)
+        return false;
+
+    // A named calibration may be specified for calibration in different
+    // environments, otherwise the default calibration is used
+    if (calibrationName == NULL)
+        calibrationName = "default";
+    
+    SensorInfo sinfo;
+    pSensor->GetDeviceInfo(&sinfo);
+
+    String path = GetBaseOVRPath(true);
+    path += "/Devices.json";
+
+    // Load the device profiles
+    Ptr<JSON> root = *JSON::Load(path);
+    if (root == NULL)
+        return false;
+
+    // Quick sanity check of the file type and format before we parse it
+    JSON* version = root->GetFirstItem();
+    if (version && version->Name == "Oculus Device Profile Version")
+    {   // In the future I may need to check versioning to determine parse method
+    }
+    else
+    {
+        return false;
+    }
+
+    bool autoEnableCorrection = false;    
+
+    JSON* device = root->GetNextItem(version);
+    while (device)
+    {   // Search for a previous calibration with the same name for this device
+        // and remove it before adding the new one
+        if (device->Name == "Device")
+        {   
+            JSON* item = device->GetItemByName("Serial");
+            if (item && item->Value == sinfo.SerialNumber)
+            {   // found an entry for this device
+
+                JSON* autoyaw = device->GetItemByName("EnableYawCorrection");
+                if (autoyaw)
+                    autoEnableCorrection = (autoyaw->dValue != 0);
+
+                item = device->GetNextItem(item);
+                while (item)
+                {
+                    if (item->Name == "MagCalibration")
+                    {   
+                        JSON* calibration = item;
+                        JSON* name = calibration->GetItemByName("Name");
+                        if (name && name->Value == calibrationName)
+                        {   // found a calibration with this name
+                            
+                            time_t now;
+                            time(&now);
+
+                            // parse the calibration time
+                            time_t calibration_time = now;
+                            JSON* caltime = calibration->GetItemByName("Time");
+                            if (caltime)
+                            {
+                                const char* caltime_str = caltime->Value.ToCStr();
+
+                                tm ct;
+                                memset(&ct, 0, sizeof(tm));
+                            
+#ifdef OVR_OS_WIN32
+                                struct tm nowtime;
+                                localtime_s(&nowtime, &now);
+                                ct.tm_isdst = nowtime.tm_isdst;
+                                sscanf_s(caltime_str, "%d-%d-%d %d:%d:%d", 
+                                    &ct.tm_year, &ct.tm_mon, &ct.tm_mday,
+                                    &ct.tm_hour, &ct.tm_min, &ct.tm_sec);
+#else
+                                struct tm* nowtime = localtime(&now);
+                                ct.tm_isdst = nowtime->tm_isdst;
+                                sscanf(caltime_str, "%d-%d-%d %d:%d:%d", 
+                                    &ct.tm_year, &ct.tm_mon, &ct.tm_mday,
+                                    &ct.tm_hour, &ct.tm_min, &ct.tm_sec);
+#endif
+                                ct.tm_year -= 1900;
+                                ct.tm_mon--;
+                                calibration_time = mktime(&ct);
+                            }
+                                                        
+                            // parse the calibration matrix
+                            JSON* cal = calibration->GetItemByName("Calibration");
+                            if (cal)
+                            {
+                                const char* data_str = cal->Value.ToCStr();
+                                Matrix4f calmat;
+                                for (int r=0; r<4; r++)
+                                {
+                                    for (int c=0; c<4; c++)
+                                    {
+                                        calmat.M[r][c] = (float)atof(data_str);
+                                        while (data_str && *data_str != ' ')
+                                            data_str++;
+
+                                        if (data_str)
+                                            data_str++;
+                                    }
+                                }
+
+                                SetMagCalibration(calmat);
+                                MagCalibrationTime  = calibration_time;
+                                EnableYawCorrection = autoEnableCorrection;
+
+                                return true;
+                            }
+                        } 
+                    }
+                    item = device->GetNextItem(item);
+                }
+
+                break;
+            }
+        }
+
+        device = root->GetNextItem(device);
+    }
+    
+    return false;
+}
+
+
 
 } // namespace OVR
 
diff --git a/LibOVR/Src/OVR_SensorFusion.h b/LibOVR/Src/OVR_SensorFusion.h
index adfe780..c660a86 100644
--- a/LibOVR/Src/OVR_SensorFusion.h
+++ b/LibOVR/Src/OVR_SensorFusion.h
@@ -19,6 +19,7 @@ otherwise accompanies this software in either electronic or hard copy form.
 
 #include "OVR_Device.h"
 #include "OVR_SensorFilter.h"
+#include <time.h>
 
 namespace OVR {
 
@@ -27,6 +28,8 @@ namespace OVR {
 
 // SensorFusion class accumulates Sensor notification messages to keep track of
 // orientation, which involves integrating the gyro and doing correction with gravity.
+// Magnetometer based yaw drift correction is also supported; it is usually enabled
+// automatically based on loaded magnetometer configuration.
 // Orientation is reported as a quaternion, from which users can obtain either the
 // rotation matrix or Euler angles.
 //
@@ -35,16 +38,20 @@ namespace OVR {
 //  - By attaching SensorFusion to a SensorDevice, in which case it will
 //    automatically handle notifications from that device.
 
+
 class SensorFusion : public NewOverrideBase
 {
     enum
     {
         MagMaxReferences = 80
-    };
+    };        
 
 public:
     SensorFusion(SensorDevice* sensor = 0);
     ~SensorFusion();
+
+
+    // *** Setup
     
     // Attaches this SensorFusion to a sensor device, from which it will receive
     // notification messages. If a sensor is attached, manual message notification
@@ -52,15 +59,85 @@ public:
     bool        AttachToSensor(SensorDevice* sensor);
 
     // Returns true if this Sensor fusion object is attached to a sensor.
-    bool        IsAttachedToSensor() const              { return Handler.IsHandlerInstalled(); }
+    bool        IsAttachedToSensor() const  { return Handler.IsHandlerInstalled(); }
+
+
+
+    // *** State Query
+
+    // Obtain the current accumulated orientation. Many apps will want to use GetPredictedOrientation
+    // instead to reduce latency.
+    Quatf       GetOrientation() const      { return lockedGet(&Q); }
+
+    // Get predicted orientaion in the near future; predictDt is lookahead amount in seconds.
+    Quatf       GetPredictedOrientation(float predictDt);
+    Quatf       GetPredictedOrientation()   { return GetPredictedOrientation(PredictionDT); }
+
+    // Obtain the last absolute acceleration reading, in m/s^2.
+    Vector3f    GetAcceleration() const     { return lockedGet(&A); }
+    // Obtain the last angular velocity reading, in rad/s.
+    Vector3f    GetAngularVelocity() const  { return lockedGet(&AngV); }
+
+    // Obtain the last raw magnetometer reading, in Gauss
+    Vector3f    GetMagnetometer() const     { return lockedGet(&RawMag); }   
+    // Obtain the calibrated magnetometer reading (direction and field strength)
+    Vector3f    GetCalibratedMagnetometer() const  { OVR_ASSERT(MagCalibrated); return lockedGet(&CalMag); }
+
+
+    // Resets the current orientation.
+    void        Reset();
+
+
+
+    // *** Configuration
+
+    void        EnableMotionTracking(bool enable = true)    { MotionTrackingEnabled = enable; }
+    bool        IsMotionTrackingEnabled() const             { return MotionTrackingEnabled;   }
+
+    // Multiplier for yaw rotation (turning); setting this higher than 1 (the default) can allow the game
+    // to be played without auxillary rotation controls, possibly making it more immersive.
+    // Whether this is more or less likely to cause motion sickness is unknown.
+    float       GetYawMultiplier() const  { return YawMult; }
+    void        SetYawMultiplier(float y) { YawMult = y; }
+
 
-    void        SetGravityEnabled(bool enableGravity)   { EnableGravity = enableGravity; }
-   
-    bool        IsGravityEnabled() const                { return EnableGravity;}
+    // *** Prediction Control
 
-	void        SetYawCorrectionEnabled(bool enableYawCorrection) { EnableYawCorrection = enableYawCorrection; }
-   
-    // Yaw correction is set up to work
+    // Prediction functions.
+    // Prediction delta specifes how much prediction should be applied in seconds; it should in
+    // general be under the average rendering latency. Call GetPredictedOrientation() to get
+    // predicted orientation.
+    float       GetPredictionDelta() const                  { return PredictionDT; }
+    void        SetPrediction(float dt, bool enable = true) { PredictionDT = dt; EnablePrediction = enable; }
+    void		SetPredictionEnabled(bool enable = true)    { EnablePrediction = enable; }    
+    bool		IsPredictionEnabled()                       { return EnablePrediction; }
+
+
+    // *** Accelerometer/Gravity Correction Control
+
+    // Enables/disables gravity correction (on by default).
+    void        SetGravityEnabled(bool enableGravity)       { EnableGravity = enableGravity; }   
+    bool        IsGravityEnabled() const                    { return EnableGravity;}
+
+    // Gain used to correct gyro with accel. Default value is appropriate for typical use.
+    float       GetAccelGain() const                        { return Gain; }
+    void        SetAccelGain(float ag)                      { Gain = ag; }
+
+
+
+    // *** Magnetometer and Yaw Drift Correction Control
+
+    // Methods to load and save a mag calibration.  Calibrations can optionally
+    // be specified by name to differentiate multiple calibrations under different conditions
+    // If LoadMagCalibration succeeds, it will override YawCorrectionEnabled based on
+    // saved calibration setting.
+    bool        SaveMagCalibration(const char* calibrationName = NULL) const;
+    bool        LoadMagCalibration(const char* calibrationName = NULL);
+
+    // Enables/disables magnetometer based yaw drift correction. Must also have mag calibration
+    // data for this correction to work.
+	void        SetYawCorrectionEnabled(bool enable)    { EnableYawCorrection = enable; }
+    // Determines if yaw correction is enabled.
     bool        IsYawCorrectionEnabled() const          { return EnableYawCorrection;}
 
     // Yaw correction is currently working (forcing a corrective yaw rotation)
@@ -70,12 +147,17 @@ public:
     void        SetMagCalibration(const Matrix4f& m)
     {
         MagCalibrationMatrix = m;
+        time(&MagCalibrationTime);   // time stamp the calibration
         MagCalibrated = true;
     }
 
+    // Retrieves the magnetometer calibration matrix
+    Matrix4f    GetMagCalibration() const        { return MagCalibrationMatrix; }
+    // Retrieve the time of the calibration
+    time_t      GetMagCalibrationTime() const    { return MagCalibrationTime; }
+
     // True only if the mag has calibration values stored
-    bool        HasMagCalibration() const        { return MagCalibrated;}
-  
+    bool        HasMagCalibration() const        { return MagCalibrated;}  
     // Force the mag into the uncalibrated state
     void        ClearMagCalibration()            { MagCalibrated = false; }
 
@@ -83,110 +165,49 @@ public:
 	void        ClearMagReferences()             { MagNumReferences = 0; }
     void        SetMagRefDistance(const float d) { MagRefDistance = d; }
 
-    // Notifies SensorFusion object about a new BodyFrame message from a sensor.
-    // Should be called by user if not attaching to a sensor.
-    void        OnMessage(const MessageBodyFrame& msg)
-    {
-        OVR_ASSERT(!IsAttachedToSensor());
-        handleMessage(msg);
-    }
-
-    // Obtain the current accumulated orientation.
-    Quatf       GetOrientation() const
-    {
-        Lock::Locker lockScope(Handler.GetHandlerLock());
-        return Q;
-    }    
-
-    // Use a predictive filter to estimate the future orientation
-	Quatf       GetPredictedOrientation(float pdt); // Specify lookahead time in ms
-	Quatf       GetPredictedOrientation() { return GetPredictedOrientation(PredictionDT); }
-
-    // Obtain the last absolute acceleration reading, in m/s^2.
-    Vector3f    GetAcceleration() const
-    {
-        Lock::Locker lockScope(Handler.GetHandlerLock());
-        return A;
-    }
-    
-    // Obtain the last angular velocity reading, in rad/s.
-    Vector3f    GetAngularVelocity() const
-    {
-        Lock::Locker lockScope(Handler.GetHandlerLock());
-        return AngV;
-    }
-    // Obtain the last magnetometer reading, in Gauss
-    Vector3f    GetMagnetometer() const
-    {
-        Lock::Locker lockScope(Handler.GetHandlerLock());
-        return RawMag;
-    }
-    // Obtain the filtered magnetometer reading, in Gauss
-    Vector3f    GetFilteredMagnetometer() const
-    {
-        Lock::Locker lockScope(Handler.GetHandlerLock());
-        return FRawMag.Mean();
-    }
-    // Obtain the calibrated magnetometer reading (direction and field strength)
-    Vector3f    GetCalibratedMagnetometer() const
-    {
-        OVR_ASSERT(MagCalibrated);
-        Lock::Locker lockScope(Handler.GetHandlerLock());
-        return CalMag;
-    }
 
     Vector3f    GetCalibratedMagValue(const Vector3f& rawMag) const;
 
-    float       GetMagRefYaw() const
-    {
-        return MagRefYaw;
-    }
+    float       GetMagRefYaw() const             { return MagRefYaw; }
+    float       GetYawErrorAngle() const         { return YawErrorAngle; }
 
-	float       GetYawErrorAngle() const
-	{
-		return YawErrorAngle;
-	}
-    // For later
-    //Vector3f    GetGravity() const;
 
-    // Resets the current orientation
-    void        Reset();
 
-    // Configuration
+    // *** Message Handler Logic
 
-    // Gain used to correct gyro with accel. Default value is appropriate for typical use.
-    float       GetAccelGain() const   { return Gain; }
-    void        SetAccelGain(float ag) { Gain = ag; }
-
-    // Multiplier for yaw rotation (turning); setting this higher than 1 (the default) can allow the game
-    // to be played without auxillary rotation controls, possibly making it more immersive. Whether this is more
-    // or less likely to cause motion sickness is unknown.
-    float       GetYawMultiplier() const  { return YawMult; }
-    void        SetYawMultiplier(float y) { YawMult = y; }
+    // Notifies SensorFusion object about a new BodyFrame message from a sensor.
+    // Should be called by user if not attaching to a sensor.
+    void        OnMessage(const MessageBodyFrame& msg)
+    {
+        OVR_ASSERT(!IsAttachedToSensor());
+        handleMessage(msg);
+    }
 
     void        SetDelegateMessageHandler(MessageHandler* handler)
     { pDelegate = handler; }
 
-	// Prediction functions.
-    // Prediction delta specifes how much prediction should be applied in seconds; it should in
-    // general be under the average rendering latency. Call GetPredictedOrientation() to get
-    // predicted orientation.
-    float       GetPredictionDelta() const                  { return PredictionDT; }
-    void        SetPrediction(float dt, bool enable = true) { PredictionDT = dt; EnablePrediction = enable; }
-	void		SetPredictionEnabled(bool enable = true)    { EnablePrediction = enable; }    
-	bool		IsPredictionEnabled()                       { return EnablePrediction; }
+
 
 private:
+
     SensorFusion* getThis()  { return this; }
 
+    // Helper used to read and return value within a Lock.
+    template<class C>
+    C lockedGet(const C* p) const
+    {
+        Lock::Locker lockScope(Handler.GetHandlerLock());
+        return *p;
+    }
+
     // Internal handler for messages; bypasses error checking.
-    void handleMessage(const MessageBodyFrame& msg);
+    void        handleMessage(const MessageBodyFrame& msg);
 
     // Set the magnetometer's reference orientation for use in yaw correction
     // The supplied mag is an uncalibrated value
-    void        SetMagReference(const Quatf& q, const Vector3f& rawMag);
+    void        setMagReference(const Quatf& q, const Vector3f& rawMag);
     // Default to current HMD orientation
-    void        SetMagReference()                { SetMagReference(Q, RawMag); }
+    void        setMagReference()  { setMagReference(Q, RawMag); }
 
 	class BodyFrameHandler : public MessageHandler
     {
@@ -199,6 +220,8 @@ private:
         virtual bool SupportsMessageType(MessageType type) const;
     };   
 
+    Ptr<SensorDevice> pSensor;
+    
     Quatf             Q;
 	Quatf			  QUncorrected;
     Vector3f          A;    
@@ -228,6 +251,7 @@ private:
 
     bool              EnableYawCorrection;
     Matrix4f          MagCalibrationMatrix;
+    time_t            MagCalibrationTime;    
     bool              MagCalibrated;
     int               MagCondCount;
     float             MagRefDistance;
@@ -244,6 +268,7 @@ private:
     bool              YawCorrectionInProgress;
 	bool			  YawCorrectionActivated;
 
+    bool              MotionTrackingEnabled;
 };
 
 
diff --git a/LibOVR/Src/Util/Util_MagCalibration.cpp b/LibOVR/Src/Util/Util_MagCalibration.cpp
index c537154..58b8c45 100644
--- a/LibOVR/Src/Util/Util_MagCalibration.cpp
+++ b/LibOVR/Src/Util/Util_MagCalibration.cpp
@@ -23,6 +23,12 @@ void MagCalibration::BeginAutoCalibration(SensorFusion& sf)
     // This is a "hard" reset of the mag, so need to clear stored values
     sf.ClearMagCalibration();
     SampleCount = 0;
+
+    // reset the statistics
+    MinMagValues = Vector3f(10000.0f,10000.0f,10000.0f);
+    MaxMagValues = Vector3f(-10000.0f,-10000.0f,-10000.0f);
+	MinQuatValues = Quatf(1.0f,1.0f,1.0f,1.0f);
+	MaxQuatValues = Quatf(0.0f,0.0f,0.0f,0.0f);
 }
 
 unsigned MagCalibration::UpdateAutoCalibration(SensorFusion& sf)
@@ -36,7 +42,11 @@ unsigned MagCalibration::UpdateAutoCalibration(SensorFusion& sf)
     InsertIfAcceptable(q, m);
 
     if ((SampleCount == 4) && (Stat == Mag_AutoCalibrating))
+	{
+		//LogText("Magnetometer Output Spread: %f %f %f\n",MagSpread.x,MagSpread.y,MagSpread.z);
+		//LogText("Quaternion Spread: %f %f %f %f\n",QuatSpread.x,QuatSpread.y,QuatSpread.z,QuatSpread.w);
         SetCalibration(sf);
+	}
 
     return Stat;
 
@@ -83,7 +93,39 @@ bool MagCalibration::IsAcceptableSample(const Quatf& q, const Vector3f& m)
 
 bool MagCalibration::InsertIfAcceptable(const Quatf& q, const Vector3f& m)
 {
-    if (IsAcceptableSample(q, m))
+	// Update some statistics
+	if (m.x < MinMagValues.x)
+		MinMagValues.x = m.x;
+	if (m.y < MinMagValues.y)
+		MinMagValues.y = m.y;
+	if (m.z < MinMagValues.z)
+		MinMagValues.z = m.z;
+	if (m.x > MaxMagValues.x)
+		MaxMagValues.x = m.x;
+	if (m.y > MaxMagValues.y)
+		MaxMagValues.y = m.y;
+	if (m.z > MaxMagValues.z)
+		MaxMagValues.z = m.z;
+	if (q.x < MinQuatValues.x)
+		MinQuatValues.x = q.x;
+	if (q.y < MinQuatValues.y)
+		MinQuatValues.y = q.y;
+	if (q.z < MinQuatValues.z)
+		MinQuatValues.z = q.z;
+	if (q.w < MinQuatValues.w)
+		MinQuatValues.w = q.w;
+	if (q.x > MaxQuatValues.x)
+		MaxQuatValues.x = q.x;
+	if (q.y > MaxQuatValues.y)
+		MaxQuatValues.y = q.y;
+	if (q.z > MaxQuatValues.z)
+		MaxQuatValues.z = q.z;
+	if (q.w > MaxQuatValues.w)
+		MaxQuatValues.w = q.w;
+	MagSpread = MaxMagValues - MinMagValues;
+	QuatSpread = MaxQuatValues - MinQuatValues;
+
+	if (IsAcceptableSample(q, m))
     {
         MagSamples[SampleCount] = m;
         QuatSamples[SampleCount] = q;
@@ -94,6 +136,14 @@ bool MagCalibration::InsertIfAcceptable(const Quatf& q, const Vector3f& m)
     return false;
 }
 
+Matrix4f MagCalibration::GetMagCalibration() const
+{
+    Matrix4f calMat = Matrix4f();
+    calMat.M[0][3] = -MagCenter.x;
+    calMat.M[1][3] = -MagCenter.y;
+    calMat.M[2][3] = -MagCenter.z;
+    return calMat;
+}
 
 bool MagCalibration::SetCalibration(SensorFusion& sf)
 {
@@ -101,10 +151,7 @@ bool MagCalibration::SetCalibration(SensorFusion& sf)
         return false;
 
     MagCenter = CalculateSphereCenter(MagSamples[0],MagSamples[1],MagSamples[2],MagSamples[3]);
-    Matrix4f calMat = Matrix4f();
-    calMat.M[0][3] = -MagCenter.x;
-    calMat.M[1][3] = -MagCenter.y;
-    calMat.M[2][3] = -MagCenter.z;
+    Matrix4f calMat = GetMagCalibration();
     sf.SetMagCalibration(calMat);
     Stat = Mag_Calibrated;
     //LogText("MagCenter: %f %f %f\n",MagCenter.x,MagCenter.y,MagCenter.z);
diff --git a/LibOVR/Src/Util/Util_MagCalibration.h b/LibOVR/Src/Util/Util_MagCalibration.h
index fd26d22..1f8e8cb 100644
--- a/LibOVR/Src/Util/Util_MagCalibration.h
+++ b/LibOVR/Src/Util/Util_MagCalibration.h
@@ -41,7 +41,11 @@ public:
     {
         MinMagDistanceSq = MinMagDistance * MinMagDistance;
         MinQuatDistanceSq = MinQuatDistance * MinQuatDistance;
-    }
+        MinMagValues = Vector3f(10000.0f,10000.0f,10000.0f);
+        MaxMagValues = Vector3f(-10000.0f,-10000.0f,-10000.0f);
+		MinQuatValues = Quatf(1.0f,1.0f,1.0f,1.0f);
+		MaxQuatValues = Quatf(0.0f,0.0f,0.0f,0.0f);
+		}
 
     // Methods that are useful for either auto or manual calibration
     bool     IsUnitialized() const       { return Stat == Mag_Uninitialized; }
@@ -93,6 +97,12 @@ public:
     // A result of the calibration, which is the center of a sphere that 
     // roughly approximates the magnetometer data.
     Vector3f GetMagCenter() const { return MagCenter; }
+    // Retrieves the full magnetometer calibration matrix
+    Matrix4f GetMagCalibration() const;
+    // Retrieves the range of each quaternion term during calibration
+    Quatf GetCalibrationQuatSpread() const { return QuatSpread; }
+    // Retrieves the range of each magnetometer term during calibration
+    Vector3f GetCalibrationMagSpread() const { return MagSpread; }
 
 private:
     // Determine the unique sphere through 4 non-coplanar points
@@ -109,6 +119,13 @@ private:
     float    MinQuatDistance;
     float    MinMagDistanceSq;
     float    MinQuatDistanceSq;
+	// For gathering statistics during calibration
+	Vector3f    MinMagValues;
+	Vector3f    MaxMagValues;
+	Vector3f    MagSpread;
+	Quatf		MinQuatValues;
+	Quatf		MaxQuatValues;
+	Quatf       QuatSpread;
 
     unsigned SampleCount;
     Vector3f MagSamples[4];
diff --git a/LibOVR/Src/Util/Util_Render_Stereo.cpp b/LibOVR/Src/Util/Util_Render_Stereo.cpp
index b16cfb5..e2600a9 100644
--- a/LibOVR/Src/Util/Util_Render_Stereo.cpp
+++ b/LibOVR/Src/Util/Util_Render_Stereo.cpp
@@ -247,7 +247,7 @@ void StereoConfig::update2D()
     // eye, where hmd screen projection surface is at 0.05m distance.
     // This introduces an extra off-center pixel projection shift based on eye distance.
     // This offCenterShift is the pixel offset of the other camera's center
-    // in your reference camera based on surface distance.
+    // in your reference camera based on surface distance.    
     float metersToPixels          = (HMD.HResolution / HMD.HScreenSize);
     float lensDistanceScreenPixels= metersToPixels * HMD.LensSeparationDistance;
     float eyeDistanceScreenPixels = metersToPixels * InterpupillaryDistance;
@@ -278,7 +278,7 @@ void StereoConfig::update2D()
 void StereoConfig::updateEyeParams()
 {
     // Projection matrix for the center eye, which the left/right matrices are based on.
-    Matrix4f projCenter = Matrix4f::PerspectiveRH(YFov, Aspect, 0.01f, 1000.0f);
+    Matrix4f projCenter = Matrix4f::PerspectiveRH(YFov, Aspect, 0.01f, 2000.0f);
    
     switch(Mode)
     {