Add OculusSDK 0.3.2 Linux Source Code w/o Samples, docs or binaries (libs or tools)

1 files changed, 1165 insertions, 0 deletions

diff --git a/LibOVR/Src/OVR_SensorImpl.cpp b/LibOVR/Src/OVR_SensorImpl.cpp
new file mode 100644
index 0000000..91ae7e0
--- /dev/null
+++ b/LibOVR/Src/OVR_SensorImpl.cpp
@@ -0,0 +1,1165 @@
+/************************************************************************************
+
+Filename    :   OVR_SensorImpl.cpp
+Content     :   Oculus Sensor device implementation.
+Created     :   March 7, 2013
+Authors     :   Lee Cooper, Dov Katz
+
+Copyright   :   Copyright 2014 Oculus VR, Inc. All Rights reserved.
+
+Licensed under the Oculus VR Rift SDK License Version 3.1 (the "License"); 
+you may not use the Oculus VR Rift SDK except in compliance with the License, 
+which is provided at the time of installation or download, or which 
+otherwise accompanies this software in either electronic or hard copy form.
+
+You may obtain a copy of the License at
+
+http://www.oculusvr.com/licenses/LICENSE-3.1 
+
+Unless required by applicable law or agreed to in writing, the Oculus VR SDK 
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.
+
+*************************************************************************************/
+
+#include "OVR_SensorImpl.h"
+#include "OVR_Sensor2Impl.h"
+#include "OVR_SensorImpl_Common.h"
+#include "OVR_JSON.h"
+#include "OVR_Profile.h"
+#include "Kernel/OVR_Alg.h"
+#include <time.h>
+
+// HMDDeviceDesc can be created/updated through Sensor carrying DisplayInfo.
+
+#include "Kernel/OVR_Timer.h"
+
+//extern FILE *SF_LOG_fp;
+
+namespace OVR {
+
+using namespace Alg;
+
+//-------------------------------------------------------------------------------------
+// ***** Oculus Sensor-specific packet data structures
+
+enum {    
+    Sensor_VendorId  = Oculus_VendorId,
+    Sensor_Tracker_ProductId = Device_Tracker_ProductId,
+    Sensor_Tracker2_ProductId = Device_Tracker2_ProductId,
+    Sensor_KTracker_ProductId = Device_KTracker_ProductId,
+
+    Sensor_BootLoader   = 0x1001,
+
+    Sensor_DefaultReportRate = 500, // Hz
+    Sensor_MaxReportRate     = 1000 // Hz
+};
+
+
+// Messages we care for
+enum TrackerMessageType
+{
+    TrackerMessage_None              = 0,
+    TrackerMessage_Sensors           = 1,
+    TrackerMessage_Unknown           = 0x100,
+    TrackerMessage_SizeError         = 0x101,
+};
+
+
+struct TrackerSensors
+{
+    UByte	SampleCount;
+    UInt16	Timestamp;
+    UInt16	LastCommandID;
+    SInt16	Temperature;
+
+    TrackerSample Samples[3];
+
+    SInt16	MagX, MagY, MagZ;
+
+    TrackerMessageType Decode(const UByte* buffer, int size)
+    {
+        if (size < 62)
+            return TrackerMessage_SizeError;
+
+        SampleCount		= buffer[1];
+        Timestamp		= DecodeUInt16(buffer + 2);
+        LastCommandID	= DecodeUInt16(buffer + 4);
+        Temperature		= DecodeSInt16(buffer + 6);
+        
+        //if (SampleCount > 2)        
+        //    OVR_DEBUG_LOG_TEXT(("TackerSensor::Decode SampleCount=%d\n", SampleCount));        
+
+        // Only unpack as many samples as there actually are
+        int iterationCount = (SampleCount > 2) ? 3 : SampleCount;
+
+        for (int i = 0; i < iterationCount; i++)
+        {
+            UnpackSensor(buffer + 8 + 16 * i,  &Samples[i].AccelX, &Samples[i].AccelY, &Samples[i].AccelZ);
+            UnpackSensor(buffer + 16 + 16 * i, &Samples[i].GyroX,  &Samples[i].GyroY,  &Samples[i].GyroZ);
+        }
+
+        MagX = DecodeSInt16(buffer + 56);
+        MagY = DecodeSInt16(buffer + 58);
+        MagZ = DecodeSInt16(buffer + 60);
+
+        return TrackerMessage_Sensors;
+    }
+};
+
+struct TrackerMessage
+{
+    TrackerMessageType Type;
+    TrackerSensors     Sensors;
+};
+
+
+//-------------------------------------------------------------------------------------
+// ***** SensorDisplayInfoImpl
+SensorDisplayInfoImpl::SensorDisplayInfoImpl()
+ :  CommandId(0), DistortionType(Base_None)
+{
+    memset(Buffer, 0, PacketSize);
+    Buffer[0] = 9;
+}
+
+void SensorDisplayInfoImpl::Unpack()
+{
+    CommandId                       = Buffer[1] | (UInt16(Buffer[2]) << 8);
+    DistortionType                  = Buffer[3];
+    HResolution                     = DecodeUInt16(Buffer+4);
+    VResolution                     = DecodeUInt16(Buffer+6);
+    HScreenSize                     = DecodeUInt32(Buffer+8) *  (1/1000000.f);
+    VScreenSize                     = DecodeUInt32(Buffer+12) * (1/1000000.f);
+    VCenter                         = DecodeUInt32(Buffer+16) * (1/1000000.f);
+    LensSeparation                  = DecodeUInt32(Buffer+20) * (1/1000000.f);
+
+#if 0
+    // These are not well-measured on most devices - probably best to ignore them.
+    OutsideLensSurfaceToScreen[0]   = DecodeUInt32(Buffer+24) * (1/1000000.f);
+    OutsideLensSurfaceToScreen[1]   = DecodeUInt32(Buffer+28) * (1/1000000.f);
+    // TODO: add spline-based distortion.
+    // TODO: currently these values are all zeros in the HMD itself.
+    DistortionK[0]                  = DecodeFloat(Buffer+32);
+    DistortionK[1]                  = DecodeFloat(Buffer+36);
+    DistortionK[2]                  = DecodeFloat(Buffer+40);
+    DistortionK[3]                  = DecodeFloat(Buffer+44);
+    DistortionK[4]                  = DecodeFloat(Buffer+48);
+    DistortionK[5]                  = DecodeFloat(Buffer+52);
+#else
+    // The above are either measured poorly, or don't have values at all.
+    // To remove the temptation to use them, set them to junk.
+    OutsideLensSurfaceToScreen[0]   = -1.0f;
+    OutsideLensSurfaceToScreen[1]   = -1.0f;
+    DistortionK[0]                  = -1.0f;
+    DistortionK[1]                  = -1.0f;
+    DistortionK[2]                  = -1.0f;
+    DistortionK[3]                  = -1.0f;
+    DistortionK[4]                  = -1.0f;
+    DistortionK[5]                  = -1.0f;
+#endif
+}
+
+
+//-------------------------------------------------------------------------------------
+// ***** SensorDeviceFactory
+
+SensorDeviceFactory &SensorDeviceFactory::GetInstance()
+{
+		static SensorDeviceFactory instance;
+		return instance;
+}
+
+void SensorDeviceFactory::EnumerateDevices(EnumerateVisitor& visitor)
+{
+
+    class SensorEnumerator : public HIDEnumerateVisitor
+    {
+        // Assign not supported; suppress MSVC warning.
+        void operator = (const SensorEnumerator&) { }
+
+        DeviceFactory*     pFactory;
+        EnumerateVisitor&  ExternalVisitor;   
+    public:
+        SensorEnumerator(DeviceFactory* factory, EnumerateVisitor& externalVisitor)
+            : pFactory(factory), ExternalVisitor(externalVisitor) { }
+
+        virtual bool MatchVendorProduct(UInt16 vendorId, UInt16 productId)
+        {
+            return pFactory->MatchVendorProduct(vendorId, productId);
+        }
+
+        virtual void Visit(HIDDevice& device, const HIDDeviceDesc& desc)
+        {
+            
+            if (desc.ProductId == Sensor_BootLoader)
+            {   // If we find a sensor in boot loader mode then notify the app
+                // about the existence of the device, but don't allow the app
+                // to create or access the device
+                BootLoaderDeviceCreateDesc createDesc(pFactory, desc);
+                ExternalVisitor.Visit(createDesc);
+                return;
+            }
+            
+            SensorDeviceCreateDesc createDesc(pFactory, desc);
+            ExternalVisitor.Visit(createDesc);
+
+            // Check if the sensor returns DisplayInfo. If so, try to use it to override potentially
+            // mismatching monitor information (in case wrong EDID is reported by splitter),
+            // or to create a new "virtualized" HMD Device.
+            
+            SensorDisplayInfoImpl displayInfo;
+
+            if (device.GetFeatureReport(displayInfo.Buffer, SensorDisplayInfoImpl::PacketSize))
+            {
+                displayInfo.Unpack();
+
+                // If we got display info, try to match / create HMDDevice as well
+                // so that sensor settings give preference.
+                if (displayInfo.DistortionType & SensorDisplayInfoImpl::Mask_BaseFmt)
+                {
+                    SensorDeviceImpl::EnumerateHMDFromSensorDisplayInfo(displayInfo, ExternalVisitor);
+                }
+            }
+        }
+    };
+
+    //double start = Timer::GetProfileSeconds();
+
+    SensorEnumerator sensorEnumerator(this, visitor);
+    GetManagerImpl()->GetHIDDeviceManager()->Enumerate(&sensorEnumerator);
+
+    //double totalSeconds = Timer::GetProfileSeconds() - start; 
+}
+
+bool SensorDeviceFactory::MatchVendorProduct(UInt16 vendorId, UInt16 productId) const
+{
+    return 	((vendorId == Sensor_VendorId) && (productId == Sensor_Tracker_ProductId)) ||
+    		((vendorId == Sensor_VendorId) && (productId == Sensor_Tracker2_ProductId)) ||
+    		((vendorId == Sensor_VendorId) && (productId == Sensor_KTracker_ProductId));
+}
+
+bool SensorDeviceFactory::DetectHIDDevice(DeviceManager* pdevMgr, const HIDDeviceDesc& desc)
+{
+    if (MatchVendorProduct(desc.VendorId, desc.ProductId))
+    {
+        if (desc.ProductId == Sensor_BootLoader)
+        {   // If we find a sensor in boot loader mode then notify the app
+            // about the existence of the device, but don't allow them
+            // to create or access the device
+            BootLoaderDeviceCreateDesc createDesc(this, desc);
+            pdevMgr->AddDevice_NeedsLock(createDesc);
+            return false;  // return false to allow upstream boot loader factories to catch the device
+        }
+        else
+        {
+            SensorDeviceCreateDesc createDesc(this, desc);
+            return pdevMgr->AddDevice_NeedsLock(createDesc).GetPtr() != NULL;
+        }
+    }
+    return false;
+}
+
+//-------------------------------------------------------------------------------------
+// ***** SensorDeviceCreateDesc
+
+DeviceBase* SensorDeviceCreateDesc::NewDeviceInstance()
+{
+    if (HIDDesc.ProductId == Sensor_Tracker2_ProductId)
+    {
+        return new Sensor2DeviceImpl(this);
+    }
+
+    return new SensorDeviceImpl(this);
+}
+
+bool SensorDeviceCreateDesc::GetDeviceInfo(DeviceInfo* info) const
+{
+    if ((info->InfoClassType != Device_Sensor) &&
+        (info->InfoClassType != Device_None))
+        return false;
+
+    info->Type =            Device_Sensor;
+    info->ProductName =     HIDDesc.Product;
+    info->Manufacturer =    HIDDesc.Manufacturer;
+    info->Version =         HIDDesc.VersionNumber;
+
+    if (info->InfoClassType == Device_Sensor)
+    {
+        SensorInfo* sinfo = (SensorInfo*)info;
+        sinfo->VendorId  = HIDDesc.VendorId;
+        sinfo->ProductId = HIDDesc.ProductId;
+        sinfo->MaxRanges = SensorRangeImpl::GetMaxSensorRange();
+        sinfo->SerialNumber = HIDDesc.SerialNumber;
+    }
+    return true;
+}
+
+//-------------------------------------------------------------------------------------
+// ***** SensorDevice
+
+SensorDeviceImpl::SensorDeviceImpl(SensorDeviceCreateDesc* createDesc)
+    : OVR::HIDDeviceImpl<OVR::SensorDevice>(createDesc, 0),
+      Coordinates(SensorDevice::Coord_Sensor),
+      HWCoordinates(SensorDevice::Coord_HMD), // HW reports HMD coordinates by default.
+      NextKeepAliveTickSeconds(0),
+      FullTimestamp(0),      
+      MaxValidRange(SensorRangeImpl::GetMaxSensorRange()),
+      magCalibrated(false)
+{
+    SequenceValid   = false;
+    LastSampleCount = 0;
+    LastTimestamp   = 0;
+
+    OldCommandId = 0;
+
+    PrevAbsoluteTime = 0.0;
+
+#ifdef OVR_OS_ANDROID
+    pPhoneSensors = PhoneSensors::Create();
+#endif
+}
+
+SensorDeviceImpl::~SensorDeviceImpl()
+{
+    // Check that Shutdown() was called.
+    OVR_ASSERT(!pCreateDesc->pDevice);    
+}
+
+
+// Internal creation APIs.
+bool SensorDeviceImpl::Initialize(DeviceBase* parent)
+{
+    if (HIDDeviceImpl<OVR::SensorDevice>::Initialize(parent))
+    {
+        openDevice();
+        return true;
+    }
+
+    return false;
+}
+
+void SensorDeviceImpl::openDevice()
+{
+
+    // Read the currently configured range from sensor.
+    SensorRangeImpl sr(SensorRange(), 0);
+
+    if (GetInternalDevice()->GetFeatureReport(sr.Buffer, SensorRangeImpl::PacketSize))
+    {
+        sr.Unpack();
+        sr.GetSensorRange(&CurrentRange);
+        // Increase the magnetometer range, since the default value is not enough in practice
+        CurrentRange.MaxMagneticField = 2.5f;
+        setRange(CurrentRange);
+    }
+
+    // Read the currently configured calibration from sensor.
+    SensorFactoryCalibrationImpl sc;
+    if (GetInternalDevice()->GetFeatureReport(sc.Buffer, SensorFactoryCalibrationImpl::PacketSize))
+    {
+        sc.Unpack();
+        AccelCalibrationOffset = sc.AccelOffset;
+        GyroCalibrationOffset  = sc.GyroOffset;
+        AccelCalibrationMatrix = sc.AccelMatrix;
+        GyroCalibrationMatrix  = sc.GyroMatrix;
+        CalibrationTemperature = sc.Temperature;
+    }
+
+    // If the sensor has "DisplayInfo" data, use HMD coordinate frame by default.
+    SensorDisplayInfoImpl displayInfo;
+    if (GetInternalDevice()->GetFeatureReport(displayInfo.Buffer, SensorDisplayInfoImpl::PacketSize))
+    {
+        displayInfo.Unpack();
+        Coordinates = (displayInfo.DistortionType & SensorDisplayInfoImpl::Mask_BaseFmt) ?
+                      Coord_HMD : Coord_Sensor;
+    }
+
+    // Read/Apply sensor config.
+    setCoordinateFrame(Coordinates);
+    setReportRate(Sensor_DefaultReportRate);
+
+    // Set Keep-alive at 10 seconds.
+    SensorKeepAliveImpl skeepAlive(10 * 1000);
+    GetInternalDevice()->SetFeatureReport(skeepAlive.Buffer, SensorKeepAliveImpl::PacketSize);
+
+    // Load mag calibration
+    MagCalibrationReport report;
+    bool res = GetMagCalibrationReport(&report);
+    if (res && report.Version > 0)
+    {
+        magCalibration = report.Calibration;
+        magCalibrated = true;
+    }
+}
+
+void SensorDeviceImpl::closeDeviceOnError()
+{
+    LogText("OVR::SensorDevice - Lost connection to '%s'\n", getHIDDesc()->Path.ToCStr());
+    NextKeepAliveTickSeconds = 0;
+}
+
+void SensorDeviceImpl::Shutdown()
+{   
+    HIDDeviceImpl<OVR::SensorDevice>::Shutdown();
+
+    LogText("OVR::SensorDevice - Closed '%s'\n", getHIDDesc()->Path.ToCStr());
+}
+
+void SensorDeviceImpl::OnInputReport(UByte* pData, UInt32 length)
+{
+
+	bool processed = false;
+    if (!processed)
+    {
+        TrackerMessage message;
+        if (decodeTrackerMessage(&message, pData, length))
+        {
+            processed = true;
+            onTrackerMessage(&message);
+        }
+    }
+}
+
+double SensorDeviceImpl::OnTicks(double tickSeconds)
+{
+    if (tickSeconds >= NextKeepAliveTickSeconds)
+    {
+        // Use 3-seconds keep alive by default.
+        double keepAliveDelta = 3.0;
+
+        // Set Keep-alive at 10 seconds.
+        SensorKeepAliveImpl skeepAlive(10 * 1000);
+        // OnTicks is called from background thread so we don't need to add this to the command queue.
+        GetInternalDevice()->SetFeatureReport(skeepAlive.Buffer, SensorKeepAliveImpl::PacketSize);
+
+		// Emit keep-alive every few seconds.
+        NextKeepAliveTickSeconds = tickSeconds + keepAliveDelta;
+    }
+    return NextKeepAliveTickSeconds - tickSeconds;
+}
+
+bool SensorDeviceImpl::SetRange(const SensorRange& range, bool waitFlag)
+{
+    bool                 result = 0;
+    ThreadCommandQueue * threadQueue = GetManagerImpl()->GetThreadQueue();
+
+    if (!waitFlag)
+    {
+        return threadQueue->PushCall(this, &SensorDeviceImpl::setRange, range);
+    }
+    
+    if (!threadQueue->PushCallAndWaitResult(this, 
+                                            &SensorDeviceImpl::setRange,
+                                            &result, 
+                                            range))
+    {
+        return false;
+    }
+
+    return result;
+}
+
+void SensorDeviceImpl::GetRange(SensorRange* range) const
+{
+    Lock::Locker lockScope(GetLock());
+    *range = CurrentRange;
+}
+
+bool SensorDeviceImpl::setRange(const SensorRange& range)
+{
+    SensorRangeImpl sr(range);
+    
+    if (GetInternalDevice()->SetFeatureReport(sr.Buffer, SensorRangeImpl::PacketSize))
+    {
+        Lock::Locker lockScope(GetLock());
+        sr.GetSensorRange(&CurrentRange);
+        return true;
+    }
+    
+    return false;
+}
+
+void SensorDeviceImpl::SetCoordinateFrame(CoordinateFrame coordframe)
+{ 
+    // Push call with wait.
+    GetManagerImpl()->GetThreadQueue()->
+        PushCall(this, &SensorDeviceImpl::setCoordinateFrame, coordframe, true);
+}
+
+SensorDevice::CoordinateFrame SensorDeviceImpl::GetCoordinateFrame() const
+{
+    return Coordinates;
+}
+
+Void SensorDeviceImpl::setCoordinateFrame(CoordinateFrame coordframe)
+{
+
+    Coordinates = coordframe;
+
+    // Read the original coordinate frame, then try to change it.
+    SensorConfigImpl scfg;
+    if (GetInternalDevice()->GetFeatureReport(scfg.Buffer, SensorConfigImpl::PacketSize))
+    {
+        scfg.Unpack();
+    }
+
+    scfg.SetSensorCoordinates(coordframe == Coord_Sensor);
+    scfg.Pack();
+
+    GetInternalDevice()->SetFeatureReport(scfg.Buffer, SensorConfigImpl::PacketSize);
+    
+    // Re-read the state, in case of older firmware that doesn't support Sensor coordinates.
+    if (GetInternalDevice()->GetFeatureReport(scfg.Buffer, SensorConfigImpl::PacketSize))
+    {
+        scfg.Unpack();
+        HWCoordinates = scfg.IsUsingSensorCoordinates() ? Coord_Sensor : Coord_HMD;
+    }
+    else
+    {
+        HWCoordinates = Coord_HMD;
+    }
+    return 0;
+}
+
+void SensorDeviceImpl::SetReportRate(unsigned rateHz)
+{ 
+    // Push call with wait.
+    GetManagerImpl()->GetThreadQueue()->
+        PushCall(this, &SensorDeviceImpl::setReportRate, rateHz, true);
+}
+
+unsigned SensorDeviceImpl::GetReportRate() const
+{
+    // Read the original configuration
+    SensorConfigImpl scfg;
+    if (GetInternalDevice()->GetFeatureReport(scfg.Buffer, SensorConfigImpl::PacketSize))
+    {
+        scfg.Unpack();
+        return Sensor_MaxReportRate / (scfg.PacketInterval + 1);
+    }
+    return 0; // error
+}
+
+Void SensorDeviceImpl::setReportRate(unsigned rateHz)
+{
+    // Read the original configuration
+    SensorConfigImpl scfg;
+    if (GetInternalDevice()->GetFeatureReport(scfg.Buffer, SensorConfigImpl::PacketSize))
+    {
+        scfg.Unpack();
+    }
+
+    if (rateHz > Sensor_MaxReportRate)
+        rateHz = Sensor_MaxReportRate;
+    else if (rateHz == 0)
+        rateHz = Sensor_DefaultReportRate;
+
+    scfg.PacketInterval = UInt16((Sensor_MaxReportRate / rateHz) - 1);
+
+    scfg.Pack();
+
+    GetInternalDevice()->SetFeatureReport(scfg.Buffer, SensorConfigImpl::PacketSize);
+    return 0;
+}
+
+void SensorDeviceImpl::GetFactoryCalibration(Vector3f* AccelOffset, Vector3f* GyroOffset,
+                                             Matrix4f* AccelMatrix, Matrix4f* GyroMatrix, 
+                                             float* Temperature)
+{
+    *AccelOffset = AccelCalibrationOffset;
+    *GyroOffset  = GyroCalibrationOffset;
+    *AccelMatrix = AccelCalibrationMatrix;
+    *GyroMatrix  = GyroCalibrationMatrix;
+    *Temperature = CalibrationTemperature;
+}
+
+bool SensorDeviceImpl::IsMagCalibrated()
+{
+    return magCalibrated;
+}
+
+void SensorDeviceImpl::SetOnboardCalibrationEnabled(bool enabled)
+{
+    // Push call with wait.
+    GetManagerImpl()->GetThreadQueue()->
+        PushCall(this, &SensorDeviceImpl::setOnboardCalibrationEnabled, enabled, true);
+}
+
+Void SensorDeviceImpl::setOnboardCalibrationEnabled(bool enabled)
+{
+    // Read the original configuration
+    SensorConfigImpl scfg;
+    if (GetInternalDevice()->GetFeatureReport(scfg.Buffer, SensorConfigImpl::PacketSize))
+    {
+        scfg.Unpack();
+    }
+
+    if (enabled)
+        scfg.Flags |= (SensorConfigImpl::Flag_AutoCalibration | SensorConfigImpl::Flag_UseCalibration);
+    else
+        scfg.Flags &= ~(SensorConfigImpl::Flag_AutoCalibration | SensorConfigImpl::Flag_UseCalibration);
+
+    scfg.Pack();
+
+    GetInternalDevice()->SetFeatureReport(scfg.Buffer, SensorConfigImpl::PacketSize);
+    return 0;
+}
+
+void SensorDeviceImpl::AddMessageHandler(MessageHandler* handler)
+{
+    if (handler)
+        SequenceValid = false;
+    DeviceBase::AddMessageHandler(handler);
+}
+
+// Sensor reports data in the following coordinate system:
+// Accelerometer: 10^-4 m/s^2; X forward, Y right, Z Down.
+// Gyro:          10^-4 rad/s; X positive roll right, Y positive pitch up; Z positive yaw right.
+
+
+// We need to convert it to the following RHS coordinate system:
+// X right, Y Up, Z Back (out of screen)
+//
+Vector3f AccelFromBodyFrameUpdate(const TrackerSensors& update, UByte sampleNumber,
+                                  bool convertHMDToSensor = false)
+{
+    const TrackerSample& sample = update.Samples[sampleNumber];
+    float                ax = (float)sample.AccelX;
+    float                ay = (float)sample.AccelY;
+    float                az = (float)sample.AccelZ;
+
+    Vector3f val = convertHMDToSensor ? Vector3f(ax, az, -ay) :  Vector3f(ax, ay, az);
+    return val * 0.0001f;
+}
+
+
+Vector3f MagFromBodyFrameUpdate(const TrackerSensors& update,
+                                Matrix4f magCalibration,
+                                bool convertHMDToSensor = false)
+{   
+    float mx = (float)update.MagX;
+    float my = (float)update.MagY;
+    float mz = (float)update.MagZ;
+    // Note: Y and Z are swapped in comparison to the Accel.  
+    // This accounts for DK1 sensor firmware axis swap, which should be undone in future releases.
+    Vector3f mag = convertHMDToSensor ? Vector3f(mx, my, -mz) : Vector3f(mx, mz, my);
+    mag *= 0.0001f;
+    // Apply calibration
+    return magCalibration.Transform(mag);
+}
+
+Vector3f EulerFromBodyFrameUpdate(const TrackerSensors& update, UByte sampleNumber,
+                                  bool convertHMDToSensor = false)
+{
+    const TrackerSample& sample = update.Samples[sampleNumber];
+    float                gx = (float)sample.GyroX;
+    float                gy = (float)sample.GyroY;
+    float                gz = (float)sample.GyroZ;
+
+    Vector3f val = convertHMDToSensor ? Vector3f(gx, gz, -gy) :  Vector3f(gx, gy, gz);
+    return val * 0.0001f;
+}
+
+bool  SensorDeviceImpl::decodeTrackerMessage(TrackerMessage* message, UByte* buffer, int size)
+{
+    memset(message, 0, sizeof(TrackerMessage));
+
+    if (size < 4)
+    {
+        message->Type = TrackerMessage_SizeError;
+        return false;
+    }
+
+    switch (buffer[0])
+    {
+    case TrackerMessage_Sensors:
+        message->Type = message->Sensors.Decode(buffer, size);
+        break;
+
+    default:
+        message->Type = TrackerMessage_Unknown;
+        break;
+    }
+
+    return (message->Type < TrackerMessage_Unknown) && (message->Type != TrackerMessage_None);
+}
+
+void SensorDeviceImpl::onTrackerMessage(TrackerMessage* message)
+{
+    if (message->Type != TrackerMessage_Sensors)
+        return;
+    
+    const double    timeUnit        = (1.0 / 1000.0);
+    double          scaledTimeUnit  = timeUnit;
+    TrackerSensors& s               = message->Sensors;
+    // DK1 timestamps the first sample, so the actual device time will be later
+    // by the time we get the message if there are multiple samples.
+    int             timestampAdjust = (s.SampleCount > 0) ? s.SampleCount-1 : 0;
+
+    const double now                 = Timer::GetSeconds();
+    double       absoluteTimeSeconds = 0.0;
+    
+
+    if (SequenceValid)
+    {
+        unsigned timestampDelta;
+
+        if (s.Timestamp < LastTimestamp)
+        {
+        	// The timestamp rolled around the 16 bit counter, so FullTimeStamp
+        	// needs a high word increment.
+        	FullTimestamp += 0x10000;
+            timestampDelta = ((((int)s.Timestamp) + 0x10000) - (int)LastTimestamp);
+        }
+        else
+        {
+            timestampDelta = (s.Timestamp - LastTimestamp);
+        }
+        // Update the low word of FullTimeStamp
+        FullTimestamp = ( FullTimestamp & ~0xffff ) | s.Timestamp;       
+
+        double deviceTime   = (FullTimestamp + timestampAdjust) * timeUnit;
+        absoluteTimeSeconds = TimeFilter.SampleToSystemTime(deviceTime, now, PrevAbsoluteTime);
+        scaledTimeUnit      = TimeFilter.ScaleTimeUnit(timeUnit);
+        PrevAbsoluteTime    = absoluteTimeSeconds;
+        
+        // If we missed a small number of samples, generate the sample that would have immediately
+        // proceeded the current one. Re-use the IMU values from the last processed sample.
+        if ((timestampDelta > LastSampleCount) && (timestampDelta <= 254))
+        {
+            if (HandlerRef.HasHandlers())
+            {
+                MessageBodyFrame sensors(this);
+
+                sensors.AbsoluteTimeSeconds = absoluteTimeSeconds - s.SampleCount * scaledTimeUnit;
+                sensors.TimeDelta           = (float)((timestampDelta - LastSampleCount) * scaledTimeUnit);
+                sensors.Acceleration        = LastAcceleration;
+                sensors.RotationRate        = LastRotationRate;
+                sensors.MagneticField       = LastMagneticField;
+                sensors.Temperature         = LastTemperature;
+
+                HandlerRef.Call(sensors);
+            }
+        }
+    }
+    else
+    {
+        LastAcceleration = Vector3f(0);
+        LastRotationRate = Vector3f(0);
+        LastMagneticField= Vector3f(0);
+        LastTemperature  = 0;
+        SequenceValid    = true;
+
+        // This is our baseline sensor to host time delta,
+        // it will be adjusted with each new message.
+        FullTimestamp = s.Timestamp;
+
+        double deviceTime   = (FullTimestamp + timestampAdjust) * timeUnit;
+        absoluteTimeSeconds = TimeFilter.SampleToSystemTime(deviceTime, now, PrevAbsoluteTime);
+        scaledTimeUnit      = TimeFilter.ScaleTimeUnit(timeUnit);
+        PrevAbsoluteTime    = absoluteTimeSeconds;
+    }
+
+    LastSampleCount = s.SampleCount;
+    LastTimestamp   = s.Timestamp;
+
+    bool convertHMDToSensor = (Coordinates == Coord_Sensor) && (HWCoordinates == Coord_HMD);
+	
+#ifdef OVR_OS_ANDROID
+    // LDC - Normally we get the coordinate system from the tracker.
+    // Since KTracker doesn't store it we'll always assume HMD coordinate system.
+    convertHMDToSensor = false;
+#endif
+
+    if (HandlerRef.HasHandlers())
+    {
+        MessageBodyFrame sensors(this);
+        UByte            iterations = s.SampleCount;
+
+        if (s.SampleCount > 3)
+        {
+            iterations        = 3;
+            sensors.TimeDelta = (float)((s.SampleCount - 2) * scaledTimeUnit);
+        }
+        else
+        {
+            sensors.TimeDelta = (float)scaledTimeUnit;
+        }
+
+        for (UByte i = 0; i < iterations; i++)
+        {     
+            sensors.AbsoluteTimeSeconds = absoluteTimeSeconds - ( iterations - 1 - i ) * scaledTimeUnit;
+            sensors.Acceleration        = AccelFromBodyFrameUpdate(s, i, convertHMDToSensor);
+            sensors.RotationRate        = EulerFromBodyFrameUpdate(s, i, convertHMDToSensor);
+            sensors.MagneticField       = MagFromBodyFrameUpdate(s, magCalibration, convertHMDToSensor);
+
+#ifdef OVR_OS_ANDROID
+            replaceWithPhoneMag(&(sensors.MagneticField));
+#endif
+            sensors.Temperature   = s.Temperature * 0.01f;
+            HandlerRef.Call(sensors);
+            // TimeDelta for the last two sample is always fixed.
+            sensors.TimeDelta = (float)scaledTimeUnit;
+        }
+
+        LastAcceleration = sensors.Acceleration;
+        LastRotationRate = sensors.RotationRate;
+        LastMagneticField= sensors.MagneticField;
+        LastTemperature  = sensors.Temperature;
+    }
+    else
+    {
+        UByte i = (s.SampleCount > 3) ? 2 : (s.SampleCount - 1);
+        LastAcceleration  = AccelFromBodyFrameUpdate(s, i, convertHMDToSensor);
+        LastRotationRate  = EulerFromBodyFrameUpdate(s, i, convertHMDToSensor);
+        LastMagneticField = MagFromBodyFrameUpdate(s, magCalibration, convertHMDToSensor);
+
+#ifdef OVR_OS_ANDROID
+        replaceWithPhoneMag(&LastMagneticField);
+#endif
+        LastTemperature   = s.Temperature * 0.01f;
+    }
+}
+
+
+#ifdef OVR_OS_ANDROID
+
+void SensorDeviceImpl::replaceWithPhoneMag(Vector3f* val)
+{
+
+	// Native calibrated.
+	pPhoneSensors->SetMagSource(PhoneSensors::MagnetometerSource_Native);
+
+	Vector3f magPhone;
+	pPhoneSensors->GetLatestMagValue(&magPhone);
+
+	// Phone value is in micro-Tesla. Convert it to Gauss and flip axes.
+	magPhone *= 10000.0f/1000000.0f;
+
+	Vector3f res;
+	res.x = -magPhone.y;
+	res.y = magPhone.x;
+	res.z = magPhone.z;
+
+	*val = res;
+}
+#endif 
+
+const int MAX_DEVICE_PROFILE_MAJOR_VERSION = 1;
+
+// Writes the current calibration for a particular device to a device profile file
+bool SensorDeviceImpl::SetMagCalibrationReport(const MagCalibrationReport &data)
+{
+    // Get device info
+    SensorInfo sinfo;
+    GetDeviceInfo(&sinfo);
+    
+    // A named calibration may be specified for calibration in different
+    // environments, otherwise the default calibration is used
+    const char* calibrationName = "default";
+
+    // Generate a mag calibration event
+    JSON* calibration = JSON::CreateObject();
+    // (hardcoded for now) the measurement and representation method 
+    calibration->AddStringItem("Version", "2.0");   
+    calibration->AddStringItem("Name", "default");
+
+    // time stamp the calibration
+    char time_str[64];
+   
+#ifdef OVR_OS_WIN32
+    struct tm caltime;
+    time_t now = time(0);
+    localtime_s(&caltime, &now);
+    strftime(time_str, 64, "%Y-%m-%d %H:%M:%S", &caltime);
+#else
+    struct tm* caltime;
+    time_t now = time(0);
+    caltime = localtime(&now);
+    strftime(time_str, 64, "%Y-%m-%d %H:%M:%S", caltime);
+#endif
+   
+    calibration->AddStringItem("Time", time_str);
+
+    // write the full calibration matrix
+    char matrix[256];
+    data.Calibration.ToString(matrix, 256);
+    calibration->AddStringItem("CalibrationMatrix", matrix);
+    // save just the offset, for backwards compatibility
+    // this can be removed when we don't want to support 0.2.4 anymore
+    Vector3f center(data.Calibration.M[0][3], data.Calibration.M[1][3], data.Calibration.M[2][3]);
+    Matrix4f tmp = data.Calibration; tmp.M[0][3] = tmp.M[1][3] = tmp.M[2][3] = 0; tmp.M[3][3] = 1;
+    center = tmp.Inverted().Transform(center);
+    Matrix4f oldcalmat; oldcalmat.M[0][3] = center.x; oldcalmat.M[1][3] = center.y; oldcalmat.M[2][3] = center.z; 
+    oldcalmat.ToString(matrix, 256);
+    calibration->AddStringItem("Calibration", matrix);
+    
+    String path = GetBaseOVRPath(true);
+    path += "/Devices.json";
+
+    // Look for a preexisting 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")
+        {   
+            int major = atoi(version->Value.ToCStr());
+            if (major > MAX_DEVICE_PROFILE_MAJOR_VERSION)
+            {
+                // don't use the file on unsupported major version number
+                root->Release();
+                root = NULL;
+            }
+        }
+        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);
+                    }
+
+
+                    /*
+                    this is removed temporarily, since this is a sensor fusion setting, not sensor itself
+                    should be moved to the correct place when Brant has finished the user profile implementation
+                    // 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);
+        // removed temporarily, see above
+        //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
+bool SensorDeviceImpl::GetMagCalibrationReport(MagCalibrationReport* data)
+{
+    data->Version = 0;
+    data->Calibration.SetIdentity();
+
+    // Get device info
+    SensorInfo sinfo;
+    GetDeviceInfo(&sinfo);
+
+    // A named calibration may be specified for calibration in different
+    // environments, otherwise the default calibration is used
+    const char* calibrationName = "default";
+
+    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")
+    {   
+        int major = atoi(version->Value.ToCStr());
+        if (major > MAX_DEVICE_PROFILE_MAJOR_VERSION)
+            return false;   // don't parse the file on unsupported major version number
+    }
+    else
+    {
+        return 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");
+                // as a temporary HACK, return no calibration if EnableYawCorrection is off
+                // this will force disable yaw correction in SensorFusion
+                // proper solution would load the value in the Profile, which SensorFusion can access
+                if (autoyaw && autoyaw->dValue == 0)
+                    return true;
+
+                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
+                            
+                            int major = 0;
+                            JSON* version = calibration->GetItemByName("Version");
+                            if (version)
+                                major = atoi(version->Value.ToCStr());
+
+                            if (major > data->Version && major <= 2)
+                            {
+                                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("CalibrationMatrix");
+                                if (!cal)
+                                    cal = calibration->GetItemByName("Calibration");
+                                if (cal)
+                                {
+                                    data->Calibration = Matrix4f::FromString(cal->Value.ToCStr());
+                                    data->Version = (UByte)major;
+                                }
+                            }
+                        } 
+                    }
+                    item = device->GetNextItem(item);
+                }
+
+                return true;
+            }
+        }
+
+        device = root->GetNextItem(device);
+    }
+    
+    return true;
+}
+
+
+bool SensorDeviceImpl::SetSerialReport(const SerialReport& data)
+{ 
+	bool result;
+	if (!GetManagerImpl()->GetThreadQueue()->
+            PushCallAndWaitResult(this, &Sensor2DeviceImpl::setSerialReport, &result, data))
+	{
+		return false;
+	}
+
+	return result;
+}
+
+bool SensorDeviceImpl::setSerialReport(const SerialReport& data)
+{
+    SerialImpl di(data);
+    return GetInternalDevice()->SetFeatureReport(di.Buffer, SerialImpl::PacketSize);
+}
+
+bool SensorDeviceImpl::GetSerialReport(SerialReport* data)
+{
+	bool result;
+	if (!GetManagerImpl()->GetThreadQueue()->
+            PushCallAndWaitResult(this, &Sensor2DeviceImpl::getSerialReport, &result, data))
+	{
+		return false;
+	}
+
+	return result;
+}
+
+bool SensorDeviceImpl::getSerialReport(SerialReport* data)
+{
+    SerialImpl di;
+    if (GetInternalDevice()->GetFeatureReport(di.Buffer, SerialImpl::PacketSize))
+    {
+        di.Unpack();
+        *data = di.Settings;
+        return true;
+    }
+
+    return false;
+}
+
+} // namespace OVR