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

1 files changed, 1124 insertions, 0 deletions

diff --git a/LibOVR/Src/OVR_Sensor2Impl.cpp b/LibOVR/Src/OVR_Sensor2Impl.cpp
new file mode 100644
index 0000000..95d486c
--- /dev/null
+++ b/LibOVR/Src/OVR_Sensor2Impl.cpp
@@ -0,0 +1,1124 @@
+/************************************************************************************
+
+Filename    :   OVR_Sensor2Impl.cpp
+Content     :   DK2 sensor device specific implementation.
+Created     :   January 21, 2013
+Authors     :   Lee Cooper
+
+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_Sensor2Impl.h"
+#include "OVR_SensorImpl_Common.h"
+#include "OVR_Sensor2ImplUtil.h"
+#include "Kernel/OVR_Alg.h"
+
+//extern FILE *SF_LOG_fp;
+
+namespace OVR {
+
+//-------------------------------------------------------------------------------------
+// ***** Oculus Sensor2-specific packet data structures
+
+enum {    
+    Sensor2_VendorId            = Oculus_VendorId,
+    Sensor2_ProductId           = 0x0021,
+
+    Sensor2_BootLoader          = 0x1001,
+
+    Sensor2_DefaultReportRate   = 1000, // Hz
+};
+
+
+// Messages we care for
+enum Tracker2MessageType
+{
+    Tracker2Message_None              = 0,
+    Tracker2Message_Sensors           = 11,
+    Tracker2Message_Unknown           = 0x100,
+    Tracker2Message_SizeError         = 0x101,
+};
+
+
+struct Tracker2Sensors
+{
+    UInt16	LastCommandID;
+    UByte	NumSamples;
+    UInt16	RunningSampleCount;				// Named 'SampleCount' in the firmware docs.
+    SInt16	Temperature;
+	UInt32	SampleTimestamp;
+    TrackerSample Samples[2];
+    SInt16	MagX, MagY, MagZ;
+    UInt16	FrameCount;
+	UInt32	FrameTimestamp;
+    UByte	FrameID;
+    UByte	CameraPattern;
+    UInt16	CameraFrameCount;				// Named 'CameraCount' in the firmware docs.
+	UInt32	CameraTimestamp;
+
+    Tracker2MessageType Decode(const UByte* buffer, int size)
+    {
+        if (size < 64)
+            return Tracker2Message_SizeError;
+
+		LastCommandID		= DecodeUInt16(buffer + 1);
+        NumSamples			= buffer[3];
+        RunningSampleCount	= DecodeUInt16(buffer + 4);
+        Temperature			= DecodeSInt16(buffer + 6);
+        SampleTimestamp		= DecodeUInt32(buffer + 8);
+        
+		// Only unpack as many samples as there actually are.
+        UByte iterationCount = (NumSamples > 1) ? 2 : NumSamples;
+
+        for (UByte i = 0; i < iterationCount; i++)
+        {
+			UnpackSensor(buffer + 12 + 16 * i, &Samples[i].AccelX, &Samples[i].AccelY, &Samples[i].AccelZ);
+            UnpackSensor(buffer + 20 + 16 * i, &Samples[i].GyroX,  &Samples[i].GyroY,  &Samples[i].GyroZ);
+		}
+
+        MagX = DecodeSInt16(buffer + 44);
+        MagY = DecodeSInt16(buffer + 46);
+        MagZ = DecodeSInt16(buffer + 48);
+
+		FrameCount = DecodeUInt16(buffer + 50);
+
+		FrameTimestamp		= DecodeUInt32(buffer + 52);
+		FrameID				= buffer[56];
+		CameraPattern		= buffer[57];
+		CameraFrameCount	= DecodeUInt16(buffer + 58);
+		CameraTimestamp		= DecodeUInt32(buffer + 60);
+        
+        return Tracker2Message_Sensors;
+    }
+};
+
+struct Tracker2Message
+{
+    Tracker2MessageType Type;
+    Tracker2Sensors     Sensors;
+};
+
+// 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 Tracker2Sensors& update, UByte sampleNumber)
+{
+    const TrackerSample& sample = update.Samples[sampleNumber];
+    float                ax = (float)sample.AccelX;
+    float                ay = (float)sample.AccelY;
+    float                az = (float)sample.AccelZ;
+
+    return Vector3f(ax, ay, az) * 0.0001f;
+}
+
+
+Vector3f MagFromBodyFrameUpdate(const Tracker2Sensors& update)
+{   
+    return Vector3f( (float)update.MagX, (float)update.MagY, (float)update.MagZ) * 0.0001f;
+}
+
+Vector3f EulerFromBodyFrameUpdate(const Tracker2Sensors& update, UByte sampleNumber)
+{
+    const TrackerSample& sample = update.Samples[sampleNumber];
+    float                gx = (float)sample.GyroX;
+    float                gy = (float)sample.GyroY;
+    float                gz = (float)sample.GyroZ;
+
+    return Vector3f(gx, gy, gz) * 0.0001f;
+}
+
+bool  Sensor2DeviceImpl::decodeTracker2Message(Tracker2Message* message, UByte* buffer, int size)
+{
+    memset(message, 0, sizeof(Tracker2Message));
+
+    if (size < 4)
+    {
+        message->Type = Tracker2Message_SizeError;
+        return false;
+    }
+
+    switch (buffer[0])
+    {
+    case Tracker2Message_Sensors:
+        message->Type = message->Sensors.Decode(buffer, size);
+        break;
+
+    default:
+        message->Type = Tracker2Message_Unknown;
+        break;
+    }
+
+    return (message->Type < Tracker2Message_Unknown) && (message->Type != Tracker2Message_None);
+}
+
+//-------------------------------------------------------------------------------------
+// ***** Sensor2Device
+
+Sensor2DeviceImpl::Sensor2DeviceImpl(SensorDeviceCreateDesc* createDesc)
+    :   SensorDeviceImpl(createDesc),
+        LastNumSamples(0),
+        LastRunningSampleCount(0),
+        FullCameraFrameCount(0),
+        LastCameraTime("C"),
+        LastFrameTime("F"),
+        LastSensorTime("S"),
+        LastFrameTimestamp(0)
+{
+    // 15 samples ok in min-window for DK2 since it uses microsecond clock.
+    TimeFilter = SensorTimeFilter(SensorTimeFilter::Settings(15));
+
+    pCalibration = new SensorCalibration(this);
+}
+
+Sensor2DeviceImpl::~Sensor2DeviceImpl()
+{
+    delete pCalibration;
+}
+
+void Sensor2DeviceImpl::openDevice()
+{
+
+    // Read the currently configured range from sensor.
+    SensorRangeImpl sr(SensorRange(), 0);
+
+    if (GetInternalDevice()->GetFeatureReport(sr.Buffer, SensorRangeImpl::PacketSize))
+    {
+        sr.Unpack();
+        sr.GetSensorRange(&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;
+    }
+	Coordinates = Coord_HMD; // TODO temporary to force it behave
+
+    // Read/Apply sensor config.
+    setCoordinateFrame(Coordinates);
+    setReportRate(Sensor2_DefaultReportRate);
+    setOnboardCalibrationEnabled(false);
+
+    // Must send DK2 keep-alive. Set Keep-alive at 10 seconds.
+    KeepAliveMuxReport keepAlive;
+    keepAlive.CommandId = 0;
+    keepAlive.INReport = 11;
+    keepAlive.Interval = 10 * 1000;
+
+    // Device creation is done from background thread so we don't need to add this to the command queue.
+    KeepAliveMuxImpl keepAliveImpl(keepAlive);
+    GetInternalDevice()->SetFeatureReport(keepAliveImpl.Buffer, KeepAliveMuxImpl::PacketSize);
+
+    // Read the temperature  data from the device
+    pCalibration->Initialize();
+}
+
+bool Sensor2DeviceImpl::SetTrackingReport(const TrackingReport& data)
+{ 
+	bool result;
+	if (!GetManagerImpl()->GetThreadQueue()->
+            PushCallAndWaitResult(this, &Sensor2DeviceImpl::setTrackingReport, &result, data))
+	{
+		return false;
+	}
+
+	return result;
+}
+
+bool Sensor2DeviceImpl::setTrackingReport(const TrackingReport& data)
+{
+    TrackingImpl ci(data);
+    return GetInternalDevice()->SetFeatureReport(ci.Buffer, TrackingImpl::PacketSize);
+}
+
+bool Sensor2DeviceImpl::GetTrackingReport(TrackingReport* data)
+{
+	bool result;
+	if (!GetManagerImpl()->GetThreadQueue()->
+            PushCallAndWaitResult(this, &Sensor2DeviceImpl::getTrackingReport, &result, data))
+	{
+		return false;
+	}
+
+	return result;
+}
+
+bool Sensor2DeviceImpl::getTrackingReport(TrackingReport* data)
+{
+    TrackingImpl ci;
+    if (GetInternalDevice()->GetFeatureReport(ci.Buffer, TrackingImpl::PacketSize))
+    {
+        ci.Unpack();
+        *data = ci.Settings;
+        return true;
+    }
+
+    return false;
+}
+
+bool Sensor2DeviceImpl::SetDisplayReport(const DisplayReport& data)
+{ 
+	bool result;
+	if (!GetManagerImpl()->GetThreadQueue()->
+            PushCallAndWaitResult(this, &Sensor2DeviceImpl::setDisplayReport, &result, data))
+	{
+		return false;
+	}
+
+	return result;
+}
+
+bool Sensor2DeviceImpl::setDisplayReport(const DisplayReport& data)
+{
+    DisplayImpl di(data);
+    return GetInternalDevice()->SetFeatureReport(di.Buffer, DisplayImpl::PacketSize);
+}
+
+bool Sensor2DeviceImpl::GetDisplayReport(DisplayReport* data)
+{
+	bool result;
+	if (!GetManagerImpl()->GetThreadQueue()->
+            PushCallAndWaitResult(this, &Sensor2DeviceImpl::getDisplayReport, &result, data))
+	{
+		return false;
+	}
+
+	return result;
+}
+
+bool Sensor2DeviceImpl::getDisplayReport(DisplayReport* data)
+{
+    DisplayImpl di;
+    if (GetInternalDevice()->GetFeatureReport(di.Buffer, DisplayImpl::PacketSize))
+    {
+        di.Unpack();
+        *data = di.Settings;
+        return true;
+    }
+
+    return false;
+}
+
+bool Sensor2DeviceImpl::SetMagCalibrationReport(const MagCalibrationReport& data)
+{ 
+	bool result;
+	if (!GetManagerImpl()->GetThreadQueue()->
+            PushCallAndWaitResult(this, &Sensor2DeviceImpl::setMagCalibrationReport, &result, data))
+	{
+		return false;
+	}
+
+	return result;
+}
+
+bool Sensor2DeviceImpl::setMagCalibrationReport(const MagCalibrationReport& data)
+{
+    MagCalibrationImpl mci(data);
+    return GetInternalDevice()->SetFeatureReport(mci.Buffer, MagCalibrationImpl::PacketSize);
+}
+
+bool Sensor2DeviceImpl::GetMagCalibrationReport(MagCalibrationReport* data)
+{
+    // direct call if we are already on the device manager thread
+    if (GetCurrentThreadId() == GetManagerImpl()->GetThreadId())
+    {
+        return getMagCalibrationReport(data);
+    }
+
+	bool result;
+	if (!GetManagerImpl()->GetThreadQueue()->
+            PushCallAndWaitResult(this, &Sensor2DeviceImpl::getMagCalibrationReport, &result, data))
+	{
+		return false;
+	}
+
+	return result;
+}
+
+bool Sensor2DeviceImpl::getMagCalibrationReport(MagCalibrationReport* data)
+{
+    MagCalibrationImpl mci;
+    if (GetInternalDevice()->GetFeatureReport(mci.Buffer, MagCalibrationImpl::PacketSize))
+    {
+        mci.Unpack();
+        *data = mci.Settings;
+        return true;
+    }
+
+    return false;
+}
+
+bool Sensor2DeviceImpl::SetPositionCalibrationReport(const PositionCalibrationReport& data)
+{ 
+    bool result;
+	if (!GetManagerImpl()->GetThreadQueue()->
+            PushCallAndWaitResult(this, &Sensor2DeviceImpl::setPositionCalibrationReport, &result, data))
+	{
+		return false;
+	}
+
+	return result;
+}
+
+bool Sensor2DeviceImpl::setPositionCalibrationReport(const PositionCalibrationReport& data)
+{
+	UByte version = GetDeviceInterfaceVersion();
+	if (version < 5)
+	{
+		PositionCalibrationImpl_Pre5 pci(data);
+		return GetInternalDevice()->SetFeatureReport(pci.Buffer, PositionCalibrationImpl_Pre5::PacketSize);
+	}
+	
+	PositionCalibrationImpl pci(data);
+    return GetInternalDevice()->SetFeatureReport(pci.Buffer, PositionCalibrationImpl::PacketSize);
+}
+
+bool Sensor2DeviceImpl::getPositionCalibrationReport(PositionCalibrationReport* data)
+{
+	UByte version = GetDeviceInterfaceVersion();
+	if (version < 5)
+	{
+		PositionCalibrationImpl_Pre5 pci;
+		if (GetInternalDevice()->GetFeatureReport(pci.Buffer, PositionCalibrationImpl_Pre5::PacketSize))
+		{
+			pci.Unpack();
+			*data = pci.Settings;
+			return true;
+		}
+
+		return false;
+	}
+
+    PositionCalibrationImpl pci;
+    if (GetInternalDevice()->GetFeatureReport(pci.Buffer, PositionCalibrationImpl::PacketSize))
+    {
+        pci.Unpack();
+        *data = pci.Settings;
+        return true;
+    }
+
+    return false;
+}
+
+bool Sensor2DeviceImpl::GetAllPositionCalibrationReports(Array<PositionCalibrationReport>* data)
+{
+    bool result;
+    if (!GetManagerImpl()->GetThreadQueue()->
+        PushCallAndWaitResult(this, &Sensor2DeviceImpl::getAllPositionCalibrationReports, &result, data))
+    {
+        return false;
+    }
+
+    return result;
+}
+
+bool Sensor2DeviceImpl::getAllPositionCalibrationReports(Array<PositionCalibrationReport>* data)
+{
+    PositionCalibrationReport pc;
+    bool result = getPositionCalibrationReport(&pc);
+    if (!result)
+        return false;
+
+    int positions = pc.NumPositions;
+    data->Clear();
+    data->Resize(positions);
+
+    for (int i = 0; i < positions; i++)
+    {
+        result = getPositionCalibrationReport(&pc);
+        if (!result)
+            return false;
+        OVR_ASSERT(pc.NumPositions == positions);
+
+        (*data)[pc.PositionIndex] = pc;
+        // IMU should be the last one
+        OVR_ASSERT(pc.PositionType == (pc.PositionIndex == positions - 1) ? 
+            PositionCalibrationReport::PositionType_IMU : PositionCalibrationReport::PositionType_LED);
+    }
+    return true;
+}
+
+bool Sensor2DeviceImpl::SetCustomPatternReport(const CustomPatternReport& data)
+{ 
+	bool result;
+	if (!GetManagerImpl()->GetThreadQueue()->
+            PushCallAndWaitResult(this, &Sensor2DeviceImpl::setCustomPatternReport, &result, data))
+	{
+		return false;
+	}
+
+	return result;
+}
+
+bool Sensor2DeviceImpl::setCustomPatternReport(const CustomPatternReport& data)
+{
+    CustomPatternImpl cpi(data);
+    return GetInternalDevice()->SetFeatureReport(cpi.Buffer, CustomPatternImpl::PacketSize);
+}
+
+bool Sensor2DeviceImpl::GetCustomPatternReport(CustomPatternReport* data)
+{
+	bool result;
+	if (!GetManagerImpl()->GetThreadQueue()->
+            PushCallAndWaitResult(this, &Sensor2DeviceImpl::getCustomPatternReport, &result, data))
+	{
+		return false;
+	}
+
+	return result;
+}
+
+bool Sensor2DeviceImpl::getCustomPatternReport(CustomPatternReport* data)
+{
+    CustomPatternImpl cpi;
+    if (GetInternalDevice()->GetFeatureReport(cpi.Buffer, CustomPatternImpl::PacketSize))
+    {
+        cpi.Unpack();
+        *data = cpi.Settings;
+        return true;
+    }
+
+    return false;
+}
+
+bool Sensor2DeviceImpl::SetManufacturingReport(const ManufacturingReport& data)
+{ 
+	bool result;
+	if (!GetManagerImpl()->GetThreadQueue()->
+            PushCallAndWaitResult(this, &Sensor2DeviceImpl::setManufacturingReport, &result, data))
+	{
+		return false;
+	}
+
+	return result;
+}
+
+bool Sensor2DeviceImpl::setManufacturingReport(const ManufacturingReport& data)
+{
+    ManufacturingImpl mi(data);
+    return GetInternalDevice()->SetFeatureReport(mi.Buffer, ManufacturingImpl::PacketSize);
+}
+
+bool Sensor2DeviceImpl::GetManufacturingReport(ManufacturingReport* data)
+{
+	bool result;
+	if (!GetManagerImpl()->GetThreadQueue()->
+            PushCallAndWaitResult(this, &Sensor2DeviceImpl::getManufacturingReport, &result, data))
+	{
+		return false;
+	}
+
+	return result;
+}
+
+bool Sensor2DeviceImpl::getManufacturingReport(ManufacturingReport* data)
+{
+    ManufacturingImpl mi;
+    if (GetInternalDevice()->GetFeatureReport(mi.Buffer, ManufacturingImpl::PacketSize))
+    {
+        mi.Unpack();
+        *data = mi.Settings;
+        return true;
+    }
+
+    return false;
+}
+
+bool Sensor2DeviceImpl::SetLensDistortionReport(const LensDistortionReport& data)
+{ 
+	bool result;
+	if (!GetManagerImpl()->GetThreadQueue()->
+            PushCallAndWaitResult(this, &Sensor2DeviceImpl::setLensDistortionReport, &result, data))
+	{
+		return false;
+	}
+
+	return result;
+}
+
+bool Sensor2DeviceImpl::setLensDistortionReport(const LensDistortionReport& data)
+{
+    LensDistortionImpl ui(data);
+    return GetInternalDevice()->SetFeatureReport(ui.Buffer, LensDistortionImpl::PacketSize);
+}
+
+bool Sensor2DeviceImpl::GetLensDistortionReport(LensDistortionReport* data)
+{
+	bool result;
+	if (!GetManagerImpl()->GetThreadQueue()->
+            PushCallAndWaitResult(this, &Sensor2DeviceImpl::getLensDistortionReport, &result, data))
+	{
+		return false;
+	}
+
+	return result;
+}
+
+bool Sensor2DeviceImpl::getLensDistortionReport(LensDistortionReport* data)
+{
+    LensDistortionImpl ui;
+    if (GetInternalDevice()->GetFeatureReport(ui.Buffer, LensDistortionImpl::PacketSize))
+    {
+        ui.Unpack();
+        *data = ui.Settings;
+        return true;
+    }
+
+    return false;
+}
+
+bool Sensor2DeviceImpl::SetUUIDReport(const UUIDReport& data)
+{ 
+	bool result;
+	if (!GetManagerImpl()->GetThreadQueue()->
+            PushCallAndWaitResult(this, &Sensor2DeviceImpl::setUUIDReport, &result, data))
+	{
+		return false;
+	}
+
+	return result;
+}
+
+bool Sensor2DeviceImpl::setUUIDReport(const UUIDReport& data)
+{
+    UUIDImpl ui(data);
+    return GetInternalDevice()->SetFeatureReport(ui.Buffer, UUIDImpl::PacketSize);
+}
+
+bool Sensor2DeviceImpl::GetUUIDReport(UUIDReport* data)
+{
+	bool result;
+	if (!GetManagerImpl()->GetThreadQueue()->
+            PushCallAndWaitResult(this, &Sensor2DeviceImpl::getUUIDReport, &result, data))
+	{
+		return false;
+	}
+
+	return result;
+}
+
+bool Sensor2DeviceImpl::getUUIDReport(UUIDReport* data)
+{
+    UUIDImpl ui;
+    if (GetInternalDevice()->GetFeatureReport(ui.Buffer, UUIDImpl::PacketSize))
+    {
+        ui.Unpack();
+        *data = ui.Settings;
+        return true;
+    }
+
+    return false;
+}
+
+bool Sensor2DeviceImpl::SetKeepAliveMuxReport(const KeepAliveMuxReport& data)
+{ 
+	bool result;
+	if (!GetManagerImpl()->GetThreadQueue()->
+            PushCallAndWaitResult(this, &Sensor2DeviceImpl::setKeepAliveMuxReport, &result, data))
+	{
+		return false;
+	}
+
+	return result;
+}
+
+bool Sensor2DeviceImpl::setKeepAliveMuxReport(const KeepAliveMuxReport& data)
+{
+    KeepAliveMuxImpl kami(data);
+    return GetInternalDevice()->SetFeatureReport(kami.Buffer, KeepAliveMuxImpl::PacketSize);
+}
+
+bool Sensor2DeviceImpl::GetKeepAliveMuxReport(KeepAliveMuxReport* data)
+{
+	bool result;
+	if (!GetManagerImpl()->GetThreadQueue()->
+            PushCallAndWaitResult(this, &Sensor2DeviceImpl::getKeepAliveMuxReport, &result, data))
+	{
+		return false;
+	}
+
+	return result;
+}
+
+bool Sensor2DeviceImpl::getKeepAliveMuxReport(KeepAliveMuxReport* data)
+{
+    KeepAliveMuxImpl kami;
+    if (GetInternalDevice()->GetFeatureReport(kami.Buffer, KeepAliveMuxImpl::PacketSize))
+    {
+        kami.Unpack();
+        *data = kami.Settings;
+        return true;
+    }
+
+    return false;
+}
+
+bool Sensor2DeviceImpl::SetTemperatureReport(const TemperatureReport& data)
+{
+    // direct call if we are already on the device manager thread
+    if (GetCurrentThreadId() == GetManagerImpl()->GetThreadId())
+    {
+        return setTemperatureReport(data);
+    }
+
+    bool result;
+	if (!GetManagerImpl()->GetThreadQueue()->
+            PushCallAndWaitResult(this, &Sensor2DeviceImpl::setTemperatureReport, &result, data))
+	{
+		return false;
+	}
+
+	return result;
+}
+
+bool Sensor2DeviceImpl::setTemperatureReport(const TemperatureReport& data)
+{
+    TemperatureImpl ti(data);
+    return GetInternalDevice()->SetFeatureReport(ti.Buffer, TemperatureImpl::PacketSize);
+}
+
+bool Sensor2DeviceImpl::getTemperatureReport(TemperatureReport* data)
+{
+    TemperatureImpl ti;
+    if (GetInternalDevice()->GetFeatureReport(ti.Buffer, TemperatureImpl::PacketSize))
+    {
+        ti.Unpack();
+        *data = ti.Settings;
+        return true;
+    }
+
+    return false;
+}
+
+bool Sensor2DeviceImpl::GetAllTemperatureReports(Array<Array<TemperatureReport> >* data)
+{
+    // direct call if we are already on the device manager thread
+    if (GetCurrentThreadId() == GetManagerImpl()->GetThreadId())
+    {
+        return getAllTemperatureReports(data);
+    }
+
+    bool result;
+    if (!GetManagerImpl()->GetThreadQueue()->
+        PushCallAndWaitResult(this, &Sensor2DeviceImpl::getAllTemperatureReports, &result, data))
+    {
+        return false;
+    }
+
+    return result;
+}
+
+bool Sensor2DeviceImpl::getAllTemperatureReports(Array<Array<TemperatureReport> >* data)
+{
+    TemperatureReport t;
+    bool result = getTemperatureReport(&t);
+    if (!result)
+        return false;
+
+    int bins = t.NumBins, samples = t.NumSamples;
+    data->Clear();
+    data->Resize(bins);
+    for (int i = 0; i < bins; i++)
+        (*data)[i].Resize(samples);
+
+    for (int i = 0; i < bins; i++)
+        for (int j = 0; j < samples; j++)
+        {
+            result = getTemperatureReport(&t);
+            if (!result)
+                return false;
+            OVR_ASSERT(t.NumBins == bins && t.NumSamples == samples);
+
+            (*data)[t.Bin][t.Sample] = t;
+        }
+    return true;
+}
+
+bool Sensor2DeviceImpl::GetGyroOffsetReport(GyroOffsetReport* data)
+{
+    // direct call if we are already on the device manager thread
+    if (GetCurrentThreadId() == GetManagerImpl()->GetThreadId())
+    {
+        return getGyroOffsetReport(data);
+    }
+
+    bool result;
+	if (!GetManagerImpl()->GetThreadQueue()->
+            PushCallAndWaitResult(this, &Sensor2DeviceImpl::getGyroOffsetReport, &result, data))
+	{
+		return false;
+	}
+
+	return result;
+}
+
+bool Sensor2DeviceImpl::getGyroOffsetReport(GyroOffsetReport* data)
+{
+    GyroOffsetImpl goi;
+    if (GetInternalDevice()->GetFeatureReport(goi.Buffer, GyroOffsetImpl::PacketSize))
+    {
+        goi.Unpack();
+        *data = goi.Settings;
+        return true;
+    }
+
+    return false;
+}
+
+void Sensor2DeviceImpl::onTrackerMessage(Tracker2Message* message)
+{
+    if (message->Type != Tracker2Message_Sensors)
+        return;
+    
+    const float     sampleIntervalTimeUnit   = (1.0f / 1000.f);
+    double          scaledSampleIntervalTimeUnit  = sampleIntervalTimeUnit;
+    Tracker2Sensors& s = message->Sensors;
+    
+    double       absoluteTimeSeconds = 0.0;
+
+    if (SequenceValid)
+    {
+        UInt32 runningSampleCountDelta;
+
+        if (s.RunningSampleCount < LastRunningSampleCount)
+        {
+            // The running sample count on the device rolled around the 16 bit counter
+            // (expect to happen about once per minute), so RunningSampleCount 
+            // needs a high word increment.
+            runningSampleCountDelta = ((((int)s.RunningSampleCount) + 0x10000) - (int)LastRunningSampleCount);
+        }
+        else
+        {
+            runningSampleCountDelta = (s.RunningSampleCount - LastRunningSampleCount);
+        }
+
+        absoluteTimeSeconds = LastSensorTime.TimeSeconds;
+        scaledSampleIntervalTimeUnit = TimeFilter.ScaleTimeUnit(sampleIntervalTimeUnit);
+ 
+        // If we missed a small number of samples, replicate the last sample.
+        if ((runningSampleCountDelta > LastNumSamples) && (runningSampleCountDelta <= 254))
+        {
+            if (HandlerRef.HasHandlers())
+            {
+                MessageBodyFrame sensors(this);
+
+                sensors.AbsoluteTimeSeconds = absoluteTimeSeconds - s.NumSamples * scaledSampleIntervalTimeUnit;
+                sensors.TimeDelta     = (float) ((runningSampleCountDelta - LastNumSamples) * scaledSampleIntervalTimeUnit);
+                sensors.Acceleration  = LastAcceleration;
+                sensors.RotationRate  = LastRotationRate;
+                sensors.MagneticField = LastMagneticField;
+                sensors.Temperature   = LastTemperature;
+
+                pCalibration->Apply(sensors);
+                HandlerRef.Call(sensors);
+            }
+        }
+    }
+    else
+    {
+        LastAcceleration = Vector3f(0);
+        LastRotationRate = Vector3f(0);
+        LastMagneticField= Vector3f(0);
+        LastTemperature  = 0;
+        SequenceValid    = true;
+    }
+
+    LastNumSamples = s.NumSamples;
+    LastRunningSampleCount = s.RunningSampleCount;
+
+    if (HandlerRef.HasHandlers())
+    {
+        MessageBodyFrame sensors(this);        
+        UByte            iterations = s.NumSamples;
+
+        if (s.NumSamples > 2)
+        {
+            iterations        = 2;
+            sensors.TimeDelta = (float) ((s.NumSamples - 1) * scaledSampleIntervalTimeUnit);
+        }
+        else
+        {
+            sensors.TimeDelta = (float) scaledSampleIntervalTimeUnit;
+        }
+
+        for (UByte i = 0; i < iterations; i++)
+        {            
+            sensors.AbsoluteTimeSeconds = absoluteTimeSeconds - ( iterations - 1 - i ) * scaledSampleIntervalTimeUnit;
+            sensors.Acceleration = AccelFromBodyFrameUpdate(s, i);
+            sensors.RotationRate = EulerFromBodyFrameUpdate(s, i);
+            sensors.MagneticField= MagFromBodyFrameUpdate(s);
+            sensors.Temperature  = s.Temperature * 0.01f;
+
+            pCalibration->Apply(sensors);
+            HandlerRef.Call(sensors);
+
+            // TimeDelta for the last two sample is always fixed.
+            sensors.TimeDelta = (float) scaledSampleIntervalTimeUnit;
+        }
+
+        // Send pixel read only when frame timestamp changes.
+        if (LastFrameTimestamp != s.FrameTimestamp)
+        {
+            MessagePixelRead pixelRead(this);
+            // Prepare message for pixel read
+            pixelRead.PixelReadValue    = s.FrameID;
+            pixelRead.RawFrameTime      = s.FrameTimestamp;
+            pixelRead.RawSensorTime     = s.SampleTimestamp;
+            pixelRead.SensorTimeSeconds = LastSensorTime.TimeSeconds;
+            pixelRead.FrameTimeSeconds  = LastFrameTime.TimeSeconds;
+
+            HandlerRef.Call(pixelRead);
+            LastFrameTimestamp = s.FrameTimestamp;
+        }
+
+        UInt16 lowFrameCount = (UInt16) FullCameraFrameCount;
+        // Send message only when frame counter changes
+        if (lowFrameCount != s.CameraFrameCount)
+        {
+            // check for the rollover in the counter
+            if (s.CameraFrameCount < lowFrameCount)
+                FullCameraFrameCount += 0x10000;
+            // update the low bits
+            FullCameraFrameCount = (FullCameraFrameCount & ~0xFFFF) | s.CameraFrameCount;
+
+            MessageExposureFrame vision(this);
+            vision.CameraPattern = s.CameraPattern;
+            vision.CameraFrameCount = FullCameraFrameCount;
+            vision.CameraTimeSeconds = LastCameraTime.TimeSeconds;
+
+            HandlerRef.Call(vision);
+        }
+
+        LastAcceleration = sensors.Acceleration;
+        LastRotationRate = sensors.RotationRate;
+        LastMagneticField= sensors.MagneticField;
+        LastTemperature  = sensors.Temperature;
+
+        //LastPixelRead = pixelRead.PixelReadValue;
+        //LastPixelReadTimeStamp = LastFrameTime;
+    }
+    else
+    {
+        if (s.NumSamples != 0)
+		{
+			UByte i = (s.NumSamples > 1) ? 1 : 0;
+			LastAcceleration  = AccelFromBodyFrameUpdate(s, i);
+			LastRotationRate  = EulerFromBodyFrameUpdate(s, i);
+			LastMagneticField = MagFromBodyFrameUpdate(s);
+			LastTemperature   = s.Temperature * 0.01f;
+		}
+    }
+}
+
+// Helper function to handle wrap-around of timestamps from Tracker2Message and convert them
+// to system time.
+//   - Any timestamps that didn't increment keep their old system time.
+//   - This is a bit tricky since we don't know which one of timestamps has most recent time.
+//   - The first timestamp must be the IMU one; we assume that others can't be too much ahead of it
+
+void UpdateDK2Timestamps(SensorTimeFilter& tf,
+                         SensorTimestampMapping** timestamps, UInt32 *rawValues, int count)
+{
+    int     updateIndices[4];
+    int     updateCount = 0;
+    int     i;
+    double  now = Timer::GetSeconds();
+
+    OVR_ASSERT(count <= sizeof(updateIndices)/sizeof(int));
+
+    // Update timestamp wrapping for any values that changed.
+    for (i = 0; i < count; i++)
+    {        
+        UInt32 lowMks = (UInt32)timestamps[i]->TimestampMks;  // Low 32-bits are raw old timestamp.
+
+        if (rawValues[i] != lowMks)
+        {
+            if (i == 0)
+            {
+                // Only check for rollover in the IMU timestamp
+                if (rawValues[i] < lowMks)
+                {
+                    LogText("Timestamp %d rollover, was: %u, now: %u\n", i, lowMks, rawValues[i]);
+                    timestamps[i]->TimestampMks += 0x100000000;
+                }
+                // Update the low bits
+                timestamps[i]->TimestampMks = (timestamps[i]->TimestampMks & 0xFFFFFFFF00000000) | rawValues[i];
+            }
+            else
+            {
+                // Take the high bits from the main timestamp first (not a typo in the first argument!)
+                timestamps[i]->TimestampMks = 
+                    (timestamps[0]->TimestampMks & 0xFFFFFFFF00000000) | rawValues[i];
+                // Now force it into the reasonable range around the expanded main timestamp
+                if (timestamps[i]->TimestampMks > timestamps[0]->TimestampMks + 0x1000000)
+                    timestamps[i]->TimestampMks -= 0x100000000;
+                else if (timestamps[i]->TimestampMks + 0x100000000 < timestamps[0]->TimestampMks + 0x1000000)
+                    timestamps[i]->TimestampMks += 0x100000000;
+            }
+
+            updateIndices[updateCount] = i;
+            updateCount++;
+        }
+    }
+
+
+    // TBD: Simplify. Update indices should no longer be needed with new TimeFilter accepting
+    //      previous values.
+    // We might want to have multi-element checking time roll-over.
+
+    static const double mksToSec = 1.0 / 1000000.0;
+
+    for (int i = 0; i < updateCount; i++)
+    {
+        SensorTimestampMapping& ts  = *timestamps[updateIndices[i]];
+
+        ts.TimeSeconds = tf.SampleToSystemTime(((double)ts.TimestampMks) * mksToSec,
+                                               now, ts.TimeSeconds, ts.DebugTag);
+    }
+}
+
+
+void Sensor2DeviceImpl::OnInputReport(UByte* pData, UInt32 length)
+{
+	bool processed = false;
+    if (!processed)
+    {
+		Tracker2Message message;
+        if (decodeTracker2Message(&message, pData, length))
+        {
+            processed = true;
+
+            // Process microsecond timestamps from DK2 tracker.
+            // Mapped and raw values must correspond to one another in each array.
+            // IMU timestamp must be the first one!
+            SensorTimestampMapping* tsMaps[3] =
+            {                
+                &LastSensorTime,
+                &LastCameraTime,
+                &LastFrameTime
+            };
+            UInt32 tsRawMks[3] =
+            {                
+                message.Sensors.SampleTimestamp,
+                message.Sensors.CameraTimestamp,
+                message.Sensors.FrameTimestamp
+            };
+            // Handle wrap-around and convert samples to system time for any samples that changed.
+            UpdateDK2Timestamps(TimeFilter, tsMaps, tsRawMks, sizeof(tsRawMks)/sizeof(tsRawMks[0]));            
+
+            onTrackerMessage(&message);
+
+            /*
+            if (SF_LOG_fp)
+            {
+                static UInt32 lastFrameTs  = 0;
+                static UInt32 lastCameraTs = 0;
+                
+                if ((lastFrameTs != message.Sensors.FrameTimestamp) ||
+                    (lastCameraTs = message.Sensors.CameraTimestamp))
+                    fprintf(SF_LOG_fp, "msg cameraTs: 0x%X frameTs: 0x%X sensorTs: 0x%X\n",
+                            message.Sensors.CameraTimestamp, message.Sensors.FrameTimestamp,
+                            message.Sensors.SampleTimestamp);
+
+                lastFrameTs  = message.Sensors.FrameTimestamp;
+                lastCameraTs = message.Sensors.CameraTimestamp;
+            }
+            */            
+
+#if 0
+            // Checks for DK2 firmware bug.
+            static unsigned SLastSampleTime = 0;
+            if ((SLastSampleTime >  message.Sensors.SampleTimestamp) && message.Sensors.SampleTimestamp > 1000000 )
+            {
+                fprintf(SF_LOG_fp, "*** Sample Timestamp Wrap! ***\n");
+                OVR_ASSERT (SLastSampleTime <= message.Sensors.SampleTimestamp);
+            }            
+            SLastSampleTime = message.Sensors.SampleTimestamp;
+
+            static unsigned SLastCameraTime = 0;
+            if ((SLastCameraTime > message.Sensors.CameraTimestamp) && message.Sensors.CameraTimestamp > 1000000 )
+            {
+                fprintf(SF_LOG_fp, "*** Camera Timestamp Wrap! ***\n");
+                OVR_ASSERT (SLastCameraTime <= message.Sensors.CameraTimestamp);
+            }            
+            SLastCameraTime = message.Sensors.CameraTimestamp;
+
+            static unsigned SLastFrameTime = 0;
+            if ((SLastFrameTime > message.Sensors.FrameTimestamp) && message.Sensors.FrameTimestamp > 1000000 )
+            {
+                fprintf(SF_LOG_fp, "*** Frame Timestamp Wrap! ***\n");
+                OVR_ASSERT (SLastFrameTime <= message.Sensors.FrameTimestamp);
+            }                        
+            SLastFrameTime = message.Sensors.FrameTimestamp;
+#endif            
+        }       
+    }
+}
+
+double Sensor2DeviceImpl::OnTicks(double tickSeconds)
+{
+
+    if (tickSeconds >= NextKeepAliveTickSeconds)
+    {
+        // Must send DK2 keep-alive. Set Keep-alive at 10 seconds.
+        KeepAliveMuxReport keepAlive;
+        keepAlive.CommandId = 0;
+        keepAlive.INReport = 11;
+        keepAlive.Interval = 10 * 1000;
+
+        // Device creation is done from background thread so we don't need to add this to the command queue.
+        KeepAliveMuxImpl keepAliveImpl(keepAlive);
+        GetInternalDevice()->SetFeatureReport(keepAliveImpl.Buffer, KeepAliveMuxImpl::PacketSize);
+
+		// Emit keep-alive every few seconds.
+        double keepAliveDelta = 3.0;        // Use 3-second interval.
+        NextKeepAliveTickSeconds = tickSeconds + keepAliveDelta;
+    }
+    return NextKeepAliveTickSeconds - tickSeconds;
+}
+
+/*
+// TBD: don't report calibration for now, until we figure out the logic between camera and mag yaw correction
+bool Sensor2DeviceImpl::IsMagCalibrated()
+{
+    return pCalibration->IsMagCalibrated();
+}
+*/
+
+} // namespace OVR