nuclear@1: /************************************************************************************
nuclear@1: 
nuclear@1: Filename    :   OVR_SensorImpl.cpp
nuclear@1: Content     :   Oculus Sensor device implementation.
nuclear@1: Created     :   March 7, 2013
nuclear@1: Authors     :   Lee Cooper
nuclear@1: 
nuclear@1: Copyright   :   Copyright 2013 Oculus VR, Inc. All Rights reserved.
nuclear@1: 
nuclear@1: Use of this software is subject to the terms of the Oculus license
nuclear@1: agreement provided at the time of installation or download, or which
nuclear@1: otherwise accompanies this software in either electronic or hard copy form.
nuclear@1: 
nuclear@1: *************************************************************************************/
nuclear@1: 
nuclear@1: #include "OVR_SensorImpl.h"
nuclear@1: 
nuclear@1: // HMDDeviceDesc can be created/updated through Sensor carrying DisplayInfo.
nuclear@1: 
nuclear@1: #include "Kernel/OVR_Timer.h"
nuclear@1: 
nuclear@1: namespace OVR {
nuclear@1:     
nuclear@1: //-------------------------------------------------------------------------------------
nuclear@1: // ***** Oculus Sensor-specific packet data structures
nuclear@1: 
nuclear@1: enum {    
nuclear@1:     Sensor_VendorId  = Oculus_VendorId,
nuclear@1:     Sensor_ProductId = 0x0001,
nuclear@1: 
nuclear@1:     // ST's VID used originally; should be removed in the future
nuclear@1:     Sensor_OldVendorId  = 0x0483,
nuclear@1:     Sensor_OldProductId = 0x5750,
nuclear@1: 
nuclear@1:     Sensor_DefaultReportRate = 500, // Hz
nuclear@1:     Sensor_MaxReportRate     = 1000 // Hz
nuclear@1: };
nuclear@1: 
nuclear@1: // Reported data is little-endian now
nuclear@1: static UInt16 DecodeUInt16(const UByte* buffer)
nuclear@1: {
nuclear@1:     return (UInt16(buffer[1]) << 8) | UInt16(buffer[0]);
nuclear@1: }
nuclear@1: 
nuclear@1: static SInt16 DecodeSInt16(const UByte* buffer)
nuclear@1: {
nuclear@1:     return (SInt16(buffer[1]) << 8) | SInt16(buffer[0]);
nuclear@1: }
nuclear@1: 
nuclear@1: static UInt32 DecodeUInt32(const UByte* buffer)
nuclear@1: {    
nuclear@1:     return (buffer[0]) | UInt32(buffer[1] << 8) | UInt32(buffer[2] << 16) | UInt32(buffer[3] << 24);    
nuclear@1: }
nuclear@1: 
nuclear@1: static float DecodeFloat(const UByte* buffer)
nuclear@1: {
nuclear@1:     union {
nuclear@1:         UInt32 U;
nuclear@1:         float  F;
nuclear@1:     };
nuclear@1: 
nuclear@1:     U = DecodeUInt32(buffer);
nuclear@1:     return F;
nuclear@1: }
nuclear@1: 
nuclear@1: 
nuclear@1: static void UnpackSensor(const UByte* buffer, SInt32* x, SInt32* y, SInt32* z)
nuclear@1: {
nuclear@1:     // Sign extending trick
nuclear@1:     // from http://graphics.stanford.edu/~seander/bithacks.html#FixedSignExtend
nuclear@1:     struct {SInt32 x:21;} s;
nuclear@1: 
nuclear@1:     *x = s.x = (buffer[0] << 13) | (buffer[1] << 5) | ((buffer[2] & 0xF8) >> 3);
nuclear@1:     *y = s.x = ((buffer[2] & 0x07) << 18) | (buffer[3] << 10) | (buffer[4] << 2) |
nuclear@1:                ((buffer[5] & 0xC0) >> 6);
nuclear@1:     *z = s.x = ((buffer[5] & 0x3F) << 15) | (buffer[6] << 7) | (buffer[7] >> 1);
nuclear@1: }
nuclear@1: 
nuclear@1: // Messages we care for
nuclear@1: enum TrackerMessageType
nuclear@1: {
nuclear@1:     TrackerMessage_None              = 0,
nuclear@1:     TrackerMessage_Sensors           = 1,
nuclear@1:     TrackerMessage_Unknown           = 0x100,
nuclear@1:     TrackerMessage_SizeError         = 0x101,
nuclear@1: };
nuclear@1: 
nuclear@1: struct TrackerSample
nuclear@1: {
nuclear@1:     SInt32 AccelX, AccelY, AccelZ;
nuclear@1:     SInt32 GyroX, GyroY, GyroZ;
nuclear@1: };
nuclear@1: 
nuclear@1: 
nuclear@1: struct TrackerSensors
nuclear@1: {
nuclear@1:     UByte	SampleCount;
nuclear@1:     UInt16	Timestamp;
nuclear@1:     UInt16	LastCommandID;
nuclear@1:     SInt16	Temperature;
nuclear@1: 
nuclear@1:     TrackerSample Samples[3];
nuclear@1: 
nuclear@1:     SInt16	MagX, MagY, MagZ;
nuclear@1: 
nuclear@1:     TrackerMessageType Decode(const UByte* buffer, int size)
nuclear@1:     {
nuclear@1:         if (size < 62)
nuclear@1:             return TrackerMessage_SizeError;
nuclear@1: 
nuclear@1:         SampleCount		= buffer[1];
nuclear@1:         Timestamp		= DecodeUInt16(buffer + 2);
nuclear@1:         LastCommandID	= DecodeUInt16(buffer + 4);
nuclear@1:         Temperature		= DecodeSInt16(buffer + 6);
nuclear@1:         
nuclear@1:         //if (SampleCount > 2)        
nuclear@1:         //    OVR_DEBUG_LOG_TEXT(("TackerSensor::Decode SampleCount=%d\n", SampleCount));        
nuclear@1: 
nuclear@1:         // Only unpack as many samples as there actually are
nuclear@1:         UByte iterationCount = (SampleCount > 2) ? 3 : SampleCount;
nuclear@1: 
nuclear@1:         for (UByte i = 0; i < iterationCount; i++)
nuclear@1:         {
nuclear@1:             UnpackSensor(buffer + 8 + 16 * i,  &Samples[i].AccelX, &Samples[i].AccelY, &Samples[i].AccelZ);
nuclear@1:             UnpackSensor(buffer + 16 + 16 * i, &Samples[i].GyroX,  &Samples[i].GyroY,  &Samples[i].GyroZ);
nuclear@1:         }
nuclear@1: 
nuclear@1:         MagX = DecodeSInt16(buffer + 56);
nuclear@1:         MagY = DecodeSInt16(buffer + 58);
nuclear@1:         MagZ = DecodeSInt16(buffer + 60);
nuclear@1: 
nuclear@1:         return TrackerMessage_Sensors;
nuclear@1:     }
nuclear@1: };
nuclear@1: 
nuclear@1: struct TrackerMessage
nuclear@1: {
nuclear@1:     TrackerMessageType Type;
nuclear@1:     TrackerSensors     Sensors;
nuclear@1: };
nuclear@1: 
nuclear@1: bool DecodeTrackerMessage(TrackerMessage* message, UByte* buffer, int size)
nuclear@1: {
nuclear@1:     memset(message, 0, sizeof(TrackerMessage));
nuclear@1: 
nuclear@1:     if (size < 4)
nuclear@1:     {
nuclear@1:         message->Type = TrackerMessage_SizeError;
nuclear@1:         return false;
nuclear@1:     }
nuclear@1: 
nuclear@1:     switch (buffer[0])
nuclear@1:     {
nuclear@1:     case TrackerMessage_Sensors:
nuclear@1:         message->Type = message->Sensors.Decode(buffer, size);
nuclear@1:         break;
nuclear@1: 
nuclear@1:     default:
nuclear@1:         message->Type = TrackerMessage_Unknown;
nuclear@1:         break;
nuclear@1:     }
nuclear@1: 
nuclear@1:     return (message->Type < TrackerMessage_Unknown) && (message->Type != TrackerMessage_None);
nuclear@1: }
nuclear@1: 
nuclear@1: 
nuclear@1: // ***** SensorRangeImpl Implementation
nuclear@1: 
nuclear@1: // Sensor HW only accepts specific maximum range values, used to maximize
nuclear@1: // the 16-bit sensor outputs. Use these ramps to specify and report appropriate values.
nuclear@1: static const UInt16 AccelRangeRamp[] = { 2, 4, 8, 16 };
nuclear@1: static const UInt16 GyroRangeRamp[]  = { 250, 500, 1000, 2000 };
nuclear@1: static const UInt16 MagRangeRamp[]   = { 880, 1300, 1900, 2500 };
nuclear@1: 
nuclear@1: static UInt16 SelectSensorRampValue(const UInt16* ramp, unsigned count,
nuclear@1:                                     float val, float factor, const char* label)
nuclear@1: {    
nuclear@1:     UInt16 threshold = (UInt16)(val * factor);
nuclear@1: 
nuclear@1:     for (unsigned i = 0; i<count; i++)
nuclear@1:     {
nuclear@1:         if (ramp[i] >= threshold)
nuclear@1:             return ramp[i];
nuclear@1:     }
nuclear@1:     OVR_DEBUG_LOG(("SensorDevice::SetRange - %s clamped to %0.4f",
nuclear@1:                    label, float(ramp[count-1]) / factor));
nuclear@1:     OVR_UNUSED2(factor, label);
nuclear@1:     return ramp[count-1];
nuclear@1: }
nuclear@1: 
nuclear@1: // SensorScaleImpl provides buffer packing logic for the Sensor Range
nuclear@1: // record that can be applied to DK1 sensor through Get/SetFeature. We expose this
nuclear@1: // through SensorRange class, which has different units.
nuclear@1: struct SensorRangeImpl
nuclear@1: {
nuclear@1:     enum  { PacketSize = 8 };
nuclear@1:     UByte   Buffer[PacketSize];
nuclear@1:     
nuclear@1:     UInt16  CommandId;
nuclear@1:     UInt16  AccelScale;
nuclear@1:     UInt16  GyroScale;
nuclear@1:     UInt16  MagScale;
nuclear@1: 
nuclear@1:     SensorRangeImpl(const SensorRange& r, UInt16 commandId = 0)
nuclear@1:     {
nuclear@1:         SetSensorRange(r, commandId);
nuclear@1:     }
nuclear@1: 
nuclear@1:     void SetSensorRange(const SensorRange& r, UInt16 commandId = 0)
nuclear@1:     {
nuclear@1:         CommandId  = commandId;
nuclear@1:         AccelScale = SelectSensorRampValue(AccelRangeRamp, sizeof(AccelRangeRamp)/sizeof(AccelRangeRamp[0]),
nuclear@1:                                            r.MaxAcceleration, (1.0f / 9.81f), "MaxAcceleration");
nuclear@1:         GyroScale  = SelectSensorRampValue(GyroRangeRamp, sizeof(GyroRangeRamp)/sizeof(GyroRangeRamp[0]),
nuclear@1:                                            r.MaxRotationRate, Math<float>::RadToDegreeFactor, "MaxRotationRate");
nuclear@1:         MagScale   = SelectSensorRampValue(MagRangeRamp, sizeof(MagRangeRamp)/sizeof(MagRangeRamp[0]),
nuclear@1:                                            r.MaxMagneticField, 1000.0f, "MaxMagneticField");
nuclear@1:         Pack();
nuclear@1:     }
nuclear@1: 
nuclear@1:     void GetSensorRange(SensorRange* r)
nuclear@1:     {
nuclear@1:         r->MaxAcceleration = AccelScale * 9.81f;
nuclear@1:         r->MaxRotationRate = DegreeToRad((float)GyroScale);
nuclear@1:         r->MaxMagneticField= MagScale * 0.001f;
nuclear@1:     }
nuclear@1: 
nuclear@1:     static SensorRange GetMaxSensorRange()
nuclear@1:     {
nuclear@1:         return SensorRange(AccelRangeRamp[sizeof(AccelRangeRamp)/sizeof(AccelRangeRamp[0]) - 1] * 9.81f,
nuclear@1:                            GyroRangeRamp[sizeof(GyroRangeRamp)/sizeof(GyroRangeRamp[0]) - 1] *
nuclear@1:                                 Math<float>::DegreeToRadFactor,
nuclear@1:                            MagRangeRamp[sizeof(MagRangeRamp)/sizeof(MagRangeRamp[0]) - 1] * 0.001f);
nuclear@1:     }
nuclear@1: 
nuclear@1:     void  Pack()
nuclear@1:     {
nuclear@1:         Buffer[0] = 4;
nuclear@1:         Buffer[1] = UByte(CommandId & 0xFF);
nuclear@1:         Buffer[2] = UByte(CommandId >> 8);
nuclear@1:         Buffer[3] = UByte(AccelScale);
nuclear@1:         Buffer[4] = UByte(GyroScale & 0xFF);
nuclear@1:         Buffer[5] = UByte(GyroScale >> 8);
nuclear@1:         Buffer[6] = UByte(MagScale & 0xFF);
nuclear@1:         Buffer[7] = UByte(MagScale >> 8);
nuclear@1:     }
nuclear@1: 
nuclear@1:     void Unpack()
nuclear@1:     {
nuclear@1:         CommandId = Buffer[1] | (UInt16(Buffer[2]) << 8);
nuclear@1:         AccelScale= Buffer[3];
nuclear@1:         GyroScale = Buffer[4] | (UInt16(Buffer[5]) << 8);
nuclear@1:         MagScale  = Buffer[6] | (UInt16(Buffer[7]) << 8);
nuclear@1:     }
nuclear@1: };
nuclear@1: 
nuclear@1: 
nuclear@1: // Sensor configuration command, ReportId == 2.
nuclear@1: 
nuclear@1: struct SensorConfigImpl
nuclear@1: {
nuclear@1:     enum  { PacketSize = 7 };
nuclear@1:     UByte   Buffer[PacketSize];
nuclear@1: 
nuclear@1:     // Flag values for Flags.
nuclear@1:     enum {
nuclear@1:         Flag_RawMode            = 0x01,
nuclear@1:         Flag_CallibrationTest   = 0x02, // Internal test mode
nuclear@1:         Flag_UseCallibration    = 0x04,
nuclear@1:         Flag_AutoCallibration   = 0x08,
nuclear@1:         Flag_MotionKeepAlive    = 0x10,
nuclear@1:         Flag_CommandKeepAlive   = 0x20,
nuclear@1:         Flag_SensorCoordinates  = 0x40
nuclear@1:     };
nuclear@1: 
nuclear@1:     UInt16  CommandId;
nuclear@1:     UByte   Flags;
nuclear@1:     UInt16  PacketInterval;
nuclear@1:     UInt16  KeepAliveIntervalMs;
nuclear@1: 
nuclear@1:     SensorConfigImpl() : CommandId(0), Flags(0), PacketInterval(0), KeepAliveIntervalMs(0)
nuclear@1:     {
nuclear@1:         memset(Buffer, 0, PacketSize);
nuclear@1:         Buffer[0] = 2;
nuclear@1:     }
nuclear@1: 
nuclear@1:     void    SetSensorCoordinates(bool sensorCoordinates)
nuclear@1:     { Flags = (Flags & ~Flag_SensorCoordinates) | (sensorCoordinates ? Flag_SensorCoordinates : 0); }
nuclear@1:     bool    IsUsingSensorCoordinates() const
nuclear@1:     { return (Flags & Flag_SensorCoordinates) != 0; }
nuclear@1: 
nuclear@1:     void Pack()
nuclear@1:     {
nuclear@1:         Buffer[0] = 2;
nuclear@1:         Buffer[1] = UByte(CommandId & 0xFF);
nuclear@1:         Buffer[2] = UByte(CommandId >> 8);
nuclear@1:         Buffer[3] = Flags;
nuclear@1:         Buffer[4] = UByte(PacketInterval);
nuclear@1:         Buffer[5] = UByte(KeepAliveIntervalMs & 0xFF);
nuclear@1:         Buffer[6] = UByte(KeepAliveIntervalMs >> 8);
nuclear@1:     }
nuclear@1: 
nuclear@1:     void Unpack()
nuclear@1:     {
nuclear@1:         CommandId          = Buffer[1] | (UInt16(Buffer[2]) << 8);
nuclear@1:         Flags              = Buffer[3];
nuclear@1:         PacketInterval     = Buffer[4];
nuclear@1:         KeepAliveIntervalMs= Buffer[5] | (UInt16(Buffer[6]) << 8);
nuclear@1:     }
nuclear@1:     
nuclear@1: };
nuclear@1: 
nuclear@1: 
nuclear@1: // SensorKeepAlive - feature report that needs to be sent at regular intervals for sensor
nuclear@1: // to receive commands.
nuclear@1: struct SensorKeepAliveImpl
nuclear@1: {
nuclear@1:     enum  { PacketSize = 5 };
nuclear@1:     UByte   Buffer[PacketSize];
nuclear@1: 
nuclear@1:     UInt16  CommandId;
nuclear@1:     UInt16  KeepAliveIntervalMs;
nuclear@1: 
nuclear@1:     SensorKeepAliveImpl(UInt16 interval = 0, UInt16 commandId = 0)
nuclear@1:         : CommandId(commandId), KeepAliveIntervalMs(interval)
nuclear@1:     {
nuclear@1:         Pack();
nuclear@1:     }
nuclear@1: 
nuclear@1:     void  Pack()
nuclear@1:     {
nuclear@1:         Buffer[0] = 8;
nuclear@1:         Buffer[1] = UByte(CommandId & 0xFF);
nuclear@1:         Buffer[2] = UByte(CommandId >> 8);
nuclear@1:         Buffer[3] = UByte(KeepAliveIntervalMs & 0xFF);
nuclear@1:         Buffer[4] = UByte(KeepAliveIntervalMs >> 8);
nuclear@1:     }
nuclear@1: 
nuclear@1:     void Unpack()
nuclear@1:     {
nuclear@1:         CommandId          = Buffer[1] | (UInt16(Buffer[2]) << 8);
nuclear@1:         KeepAliveIntervalMs= Buffer[3] | (UInt16(Buffer[4]) << 8);
nuclear@1:     }
nuclear@1: };
nuclear@1: 
nuclear@1: 
nuclear@1: //-------------------------------------------------------------------------------------
nuclear@1: // ***** SensorDisplayInfoImpl
nuclear@1: SensorDisplayInfoImpl::SensorDisplayInfoImpl()
nuclear@1:  :  CommandId(0), DistortionType(Base_None)
nuclear@1: {
nuclear@1:     memset(Buffer, 0, PacketSize);
nuclear@1:     Buffer[0] = 9;
nuclear@1: }
nuclear@1: 
nuclear@1: void SensorDisplayInfoImpl::Unpack()
nuclear@1: {
nuclear@1:     CommandId               = Buffer[1] | (UInt16(Buffer[2]) << 8);
nuclear@1:     DistortionType          = Buffer[3];
nuclear@1:     HResolution             = DecodeUInt16(Buffer+4);
nuclear@1:     VResolution             = DecodeUInt16(Buffer+6);
nuclear@1:     HScreenSize             = DecodeUInt32(Buffer+8) *  (1/1000000.f);
nuclear@1:     VScreenSize             = DecodeUInt32(Buffer+12) * (1/1000000.f);
nuclear@1:     VCenter                 = DecodeUInt32(Buffer+16) * (1/1000000.f);
nuclear@1:     LensSeparation          = DecodeUInt32(Buffer+20) * (1/1000000.f);
nuclear@1:     EyeToScreenDistance[0]  = DecodeUInt32(Buffer+24) * (1/1000000.f);
nuclear@1:     EyeToScreenDistance[1]  = DecodeUInt32(Buffer+28) * (1/1000000.f);
nuclear@1:     DistortionK[0]          = DecodeFloat(Buffer+32);
nuclear@1:     DistortionK[1]          = DecodeFloat(Buffer+36);
nuclear@1:     DistortionK[2]          = DecodeFloat(Buffer+40);
nuclear@1:     DistortionK[3]          = DecodeFloat(Buffer+44);
nuclear@1:     DistortionK[4]          = DecodeFloat(Buffer+48);
nuclear@1:     DistortionK[5]          = DecodeFloat(Buffer+52);
nuclear@1: }
nuclear@1: 
nuclear@1: 
nuclear@1: //-------------------------------------------------------------------------------------
nuclear@1: // ***** SensorDeviceFactory
nuclear@1: 
nuclear@1: SensorDeviceFactory SensorDeviceFactory::Instance;
nuclear@1: 
nuclear@1: void SensorDeviceFactory::EnumerateDevices(EnumerateVisitor& visitor)
nuclear@1: {
nuclear@1: 
nuclear@1:     class SensorEnumerator : public HIDEnumerateVisitor
nuclear@1:     {
nuclear@1:         // Assign not supported; suppress MSVC warning.
nuclear@1:         void operator = (const SensorEnumerator&) { }
nuclear@1: 
nuclear@1:         DeviceFactory*     pFactory;
nuclear@1:         EnumerateVisitor&  ExternalVisitor;   
nuclear@1:     public:
nuclear@1:         SensorEnumerator(DeviceFactory* factory, EnumerateVisitor& externalVisitor)
nuclear@1:             : pFactory(factory), ExternalVisitor(externalVisitor) { }
nuclear@1: 
nuclear@1:         virtual bool MatchVendorProduct(UInt16 vendorId, UInt16 productId)
nuclear@1:         {
nuclear@1:             return pFactory->MatchVendorProduct(vendorId, productId);
nuclear@1:         }
nuclear@1: 
nuclear@1:         virtual void Visit(HIDDevice& device, const HIDDeviceDesc& desc)
nuclear@1:         {
nuclear@1:             SensorDeviceCreateDesc createDesc(pFactory, desc);
nuclear@1:             ExternalVisitor.Visit(createDesc);
nuclear@1: 
nuclear@1:             // Check if the sensor returns DisplayInfo. If so, try to use it to override potentially
nuclear@1:             // mismatching monitor information (in case wrong EDID is reported by splitter),
nuclear@1:             // or to create a new "virtualized" HMD Device.
nuclear@1:             
nuclear@1:             SensorDisplayInfoImpl displayInfo;
nuclear@1: 
nuclear@1:             if (device.GetFeatureReport(displayInfo.Buffer, SensorDisplayInfoImpl::PacketSize))
nuclear@1:             {
nuclear@1:                 displayInfo.Unpack();
nuclear@1: 
nuclear@1:                 // If we got display info, try to match / create HMDDevice as well
nuclear@1:                 // so that sensor settings give preference.
nuclear@1:                 if (displayInfo.DistortionType & SensorDisplayInfoImpl::Mask_BaseFmt)
nuclear@1:                 {
nuclear@1:                     SensorDeviceImpl::EnumerateHMDFromSensorDisplayInfo(displayInfo, ExternalVisitor);
nuclear@1:                 }
nuclear@1:             }
nuclear@1:         }
nuclear@1:     };
nuclear@1: 
nuclear@1:     //double start = Timer::GetProfileSeconds();
nuclear@1: 
nuclear@1:     SensorEnumerator sensorEnumerator(this, visitor);
nuclear@1:     GetManagerImpl()->GetHIDDeviceManager()->Enumerate(&sensorEnumerator);
nuclear@1: 
nuclear@1:     //double totalSeconds = Timer::GetProfileSeconds() - start; 
nuclear@1: }
nuclear@1: 
nuclear@1: bool SensorDeviceFactory::MatchVendorProduct(UInt16 vendorId, UInt16 productId) const
nuclear@1: {
nuclear@1:     return ((vendorId == Sensor_VendorId) && (productId == Sensor_ProductId)) ||
nuclear@1:         ((vendorId == Sensor_OldVendorId) && (productId == Sensor_OldProductId));
nuclear@1: }
nuclear@1: 
nuclear@1: bool SensorDeviceFactory::DetectHIDDevice(DeviceManager* pdevMgr, const HIDDeviceDesc& desc)
nuclear@1: {
nuclear@1:     if (MatchVendorProduct(desc.VendorId, desc.ProductId))
nuclear@1:     {
nuclear@1:         SensorDeviceCreateDesc createDesc(this, desc);
nuclear@1:         return pdevMgr->AddDevice_NeedsLock(createDesc).GetPtr() != NULL;
nuclear@1:     }
nuclear@1:     return false;
nuclear@1: }
nuclear@1: 
nuclear@1: //-------------------------------------------------------------------------------------
nuclear@1: // ***** SensorDeviceCreateDesc
nuclear@1: 
nuclear@1: DeviceBase* SensorDeviceCreateDesc::NewDeviceInstance()
nuclear@1: {
nuclear@1:     return new SensorDeviceImpl(this);
nuclear@1: }
nuclear@1: 
nuclear@1: bool SensorDeviceCreateDesc::GetDeviceInfo(DeviceInfo* info) const
nuclear@1: {
nuclear@1:     if ((info->InfoClassType != Device_Sensor) &&
nuclear@1:         (info->InfoClassType != Device_None))
nuclear@1:         return false;
nuclear@1: 
nuclear@1:     OVR_strcpy(info->ProductName,  DeviceInfo::MaxNameLength, HIDDesc.Product.ToCStr());
nuclear@1:     OVR_strcpy(info->Manufacturer, DeviceInfo::MaxNameLength, HIDDesc.Manufacturer.ToCStr());
nuclear@1:     info->Type    = Device_Sensor;
nuclear@1:     info->Version = 0;
nuclear@1: 
nuclear@1:     if (info->InfoClassType == Device_Sensor)
nuclear@1:     {
nuclear@1:         SensorInfo* sinfo = (SensorInfo*)info;
nuclear@1:         sinfo->VendorId  = HIDDesc.VendorId;
nuclear@1:         sinfo->ProductId = HIDDesc.ProductId;
nuclear@1:         sinfo->MaxRanges = SensorRangeImpl::GetMaxSensorRange();
nuclear@1:         OVR_strcpy(sinfo->SerialNumber, sizeof(sinfo->SerialNumber),HIDDesc.SerialNumber.ToCStr());
nuclear@1:     }
nuclear@1:     return true;
nuclear@1: }
nuclear@1: 
nuclear@1: 
nuclear@1: //-------------------------------------------------------------------------------------
nuclear@1: // ***** SensorDevice
nuclear@1: 
nuclear@1: SensorDeviceImpl::SensorDeviceImpl(SensorDeviceCreateDesc* createDesc)
nuclear@1:     : OVR::HIDDeviceImpl<OVR::SensorDevice>(createDesc, 0),
nuclear@1:       Coordinates(SensorDevice::Coord_Sensor),
nuclear@1:       HWCoordinates(SensorDevice::Coord_HMD), // HW reports HMD coordinates by default.
nuclear@1:       NextKeepAliveTicks(0),
nuclear@1:       MaxValidRange(SensorRangeImpl::GetMaxSensorRange())
nuclear@1: {
nuclear@1:     SequenceValid  = false;
nuclear@1:     LastSampleCount= 0;
nuclear@1:     LastTimestamp   = 0;
nuclear@1: 
nuclear@1:     OldCommandId = 0;
nuclear@1: }
nuclear@1: 
nuclear@1: SensorDeviceImpl::~SensorDeviceImpl()
nuclear@1: {
nuclear@1:     // Check that Shutdown() was called.
nuclear@1:     OVR_ASSERT(!pCreateDesc->pDevice);    
nuclear@1: }
nuclear@1: 
nuclear@1: // Internal creation APIs.
nuclear@1: bool SensorDeviceImpl::Initialize(DeviceBase* parent)
nuclear@1: {
nuclear@1:     if (HIDDeviceImpl<OVR::SensorDevice>::Initialize(parent))
nuclear@1:     {
nuclear@1:         openDevice();
nuclear@1: 
nuclear@1:         LogText("OVR::SensorDevice initialized.\n");
nuclear@1: 
nuclear@1:         return true;
nuclear@1:     }
nuclear@1: 
nuclear@1:     return false;
nuclear@1: }
nuclear@1: 
nuclear@1: void SensorDeviceImpl::openDevice()
nuclear@1: {
nuclear@1: 
nuclear@1:     // Read the currently configured range from sensor.
nuclear@1:     SensorRangeImpl sr(SensorRange(), 0);
nuclear@1: 
nuclear@1:     if (GetInternalDevice()->GetFeatureReport(sr.Buffer, SensorRangeImpl::PacketSize))
nuclear@1:     {
nuclear@1:         sr.Unpack();
nuclear@1:         sr.GetSensorRange(&CurrentRange);
nuclear@1:     }
nuclear@1: 
nuclear@1: 
nuclear@1:     // If the sensor has "DisplayInfo" data, use HMD coordinate frame by default.
nuclear@1:     SensorDisplayInfoImpl displayInfo;
nuclear@1:     if (GetInternalDevice()->GetFeatureReport(displayInfo.Buffer, SensorDisplayInfoImpl::PacketSize))
nuclear@1:     {
nuclear@1:         displayInfo.Unpack();
nuclear@1:         Coordinates = (displayInfo.DistortionType & SensorDisplayInfoImpl::Mask_BaseFmt) ?
nuclear@1:                       Coord_HMD : Coord_Sensor;
nuclear@1:     }
nuclear@1: 
nuclear@1:     // Read/Apply sensor config.
nuclear@1:     setCoordinateFrame(Coordinates);
nuclear@1:     setReportRate(Sensor_DefaultReportRate);
nuclear@1: 
nuclear@1:     // Set Keep-alive at 10 seconds.
nuclear@1:     SensorKeepAliveImpl skeepAlive(10 * 1000);
nuclear@1:     GetInternalDevice()->SetFeatureReport(skeepAlive.Buffer, SensorKeepAliveImpl::PacketSize);
nuclear@1: }
nuclear@1: 
nuclear@1: void SensorDeviceImpl::closeDeviceOnError()
nuclear@1: {
nuclear@1:     LogText("OVR::SensorDevice - Lost connection to '%s'\n", getHIDDesc()->Path.ToCStr());
nuclear@1:     NextKeepAliveTicks = 0;
nuclear@1: }
nuclear@1: 
nuclear@1: void SensorDeviceImpl::Shutdown()
nuclear@1: {   
nuclear@1:     HIDDeviceImpl<OVR::SensorDevice>::Shutdown();
nuclear@1: 
nuclear@1:     LogText("OVR::SensorDevice - Closed '%s'\n", getHIDDesc()->Path.ToCStr());
nuclear@1: }
nuclear@1: 
nuclear@1: 
nuclear@1: void SensorDeviceImpl::OnInputReport(UByte* pData, UInt32 length)
nuclear@1: {
nuclear@1: 
nuclear@1:     bool processed = false;
nuclear@1:     if (!processed)
nuclear@1:     {
nuclear@1: 
nuclear@1:         TrackerMessage message;
nuclear@1:         if (DecodeTrackerMessage(&message, pData, length))
nuclear@1:         {
nuclear@1:             processed = true;
nuclear@1:             onTrackerMessage(&message);
nuclear@1:         }
nuclear@1:     }
nuclear@1: }
nuclear@1: 
nuclear@1: UInt64 SensorDeviceImpl::OnTicks(UInt64 ticksMks)
nuclear@1: {
nuclear@1: 
nuclear@1:     if (ticksMks >= NextKeepAliveTicks)
nuclear@1:     {
nuclear@1:         // Use 3-seconds keep alive by default.
nuclear@1:         UInt64 keepAliveDelta = Timer::MksPerSecond * 3;
nuclear@1: 
nuclear@1:         // Set Keep-alive at 10 seconds.
nuclear@1:         SensorKeepAliveImpl skeepAlive(10 * 1000);
nuclear@1:         // OnTicks is called from background thread so we don't need to add this to the command queue.
nuclear@1:         GetInternalDevice()->SetFeatureReport(skeepAlive.Buffer, SensorKeepAliveImpl::PacketSize);
nuclear@1: 
nuclear@1: 		// Emit keep-alive every few seconds.
nuclear@1:         NextKeepAliveTicks = ticksMks + keepAliveDelta;
nuclear@1:     }
nuclear@1:     return NextKeepAliveTicks - ticksMks;
nuclear@1: }
nuclear@1: 
nuclear@1: bool SensorDeviceImpl::SetRange(const SensorRange& range, bool waitFlag)
nuclear@1: {
nuclear@1:     bool                 result = 0;
nuclear@1:     ThreadCommandQueue * threadQueue = GetManagerImpl()->GetThreadQueue();
nuclear@1: 
nuclear@1:     if (!waitFlag)
nuclear@1:     {
nuclear@1:         return threadQueue->PushCall(this, &SensorDeviceImpl::setRange, range);
nuclear@1:     }
nuclear@1:     
nuclear@1:     if (!threadQueue->PushCallAndWaitResult(this, 
nuclear@1:                                             &SensorDeviceImpl::setRange,
nuclear@1:                                             &result, 
nuclear@1:                                             range))
nuclear@1:     {
nuclear@1:         return false;
nuclear@1:     }
nuclear@1: 
nuclear@1:     return result;
nuclear@1: }
nuclear@1: 
nuclear@1: void SensorDeviceImpl::GetRange(SensorRange* range) const
nuclear@1: {
nuclear@1:     Lock::Locker lockScope(GetLock());
nuclear@1:     *range = CurrentRange;
nuclear@1: }
nuclear@1: 
nuclear@1: bool SensorDeviceImpl::setRange(const SensorRange& range)
nuclear@1: {
nuclear@1:     SensorRangeImpl sr(range);
nuclear@1:     
nuclear@1:     if (GetInternalDevice()->SetFeatureReport(sr.Buffer, SensorRangeImpl::PacketSize))
nuclear@1:     {
nuclear@1:         Lock::Locker lockScope(GetLock());
nuclear@1:         sr.GetSensorRange(&CurrentRange);
nuclear@1:         return true;
nuclear@1:     }
nuclear@1:     
nuclear@1:     return false;
nuclear@1: }
nuclear@1: 
nuclear@1: void SensorDeviceImpl::SetCoordinateFrame(CoordinateFrame coordframe)
nuclear@1: { 
nuclear@1:     // Push call with wait.
nuclear@1:     GetManagerImpl()->GetThreadQueue()->
nuclear@1:         PushCall(this, &SensorDeviceImpl::setCoordinateFrame, coordframe, true);
nuclear@1: }
nuclear@1: 
nuclear@1: SensorDevice::CoordinateFrame SensorDeviceImpl::GetCoordinateFrame() const
nuclear@1: {
nuclear@1:     return Coordinates;
nuclear@1: }
nuclear@1: 
nuclear@1: Void SensorDeviceImpl::setCoordinateFrame(CoordinateFrame coordframe)
nuclear@1: {
nuclear@1: 
nuclear@1:     Coordinates = coordframe;
nuclear@1: 
nuclear@1:     // Read the original coordinate frame, then try to change it.
nuclear@1:     SensorConfigImpl scfg;
nuclear@1:     if (GetInternalDevice()->GetFeatureReport(scfg.Buffer, SensorConfigImpl::PacketSize))
nuclear@1:     {
nuclear@1:         scfg.Unpack();
nuclear@1:     }
nuclear@1: 
nuclear@1:     scfg.SetSensorCoordinates(coordframe == Coord_Sensor);
nuclear@1:     scfg.Pack();
nuclear@1: 
nuclear@1:     GetInternalDevice()->SetFeatureReport(scfg.Buffer, SensorConfigImpl::PacketSize);
nuclear@1:     
nuclear@1:     // Re-read the state, in case of older firmware that doesn't support Sensor coordinates.
nuclear@1:     if (GetInternalDevice()->GetFeatureReport(scfg.Buffer, SensorConfigImpl::PacketSize))
nuclear@1:     {
nuclear@1:         scfg.Unpack();
nuclear@1:         HWCoordinates = scfg.IsUsingSensorCoordinates() ? Coord_Sensor : Coord_HMD;
nuclear@1:     }
nuclear@1:     else
nuclear@1:     {
nuclear@1:         HWCoordinates = Coord_HMD;
nuclear@1:     }
nuclear@1:     return 0;
nuclear@1: }
nuclear@1: 
nuclear@1: void SensorDeviceImpl::SetReportRate(unsigned rateHz)
nuclear@1: { 
nuclear@1:     // Push call with wait.
nuclear@1:     GetManagerImpl()->GetThreadQueue()->
nuclear@1:         PushCall(this, &SensorDeviceImpl::setReportRate, rateHz, true);
nuclear@1: }
nuclear@1: 
nuclear@1: unsigned SensorDeviceImpl::GetReportRate() const
nuclear@1: {
nuclear@1:     // Read the original configuration
nuclear@1:     SensorConfigImpl scfg;
nuclear@1:     if (GetInternalDevice()->GetFeatureReport(scfg.Buffer, SensorConfigImpl::PacketSize))
nuclear@1:     {
nuclear@1:         scfg.Unpack();
nuclear@1:         return Sensor_MaxReportRate / (scfg.PacketInterval + 1);
nuclear@1:     }
nuclear@1:     return 0; // error
nuclear@1: }
nuclear@1: 
nuclear@1: Void SensorDeviceImpl::setReportRate(unsigned rateHz)
nuclear@1: {
nuclear@1:     // Read the original configuration
nuclear@1:     SensorConfigImpl scfg;
nuclear@1:     if (GetInternalDevice()->GetFeatureReport(scfg.Buffer, SensorConfigImpl::PacketSize))
nuclear@1:     {
nuclear@1:         scfg.Unpack();
nuclear@1:     }
nuclear@1: 
nuclear@1:     if (rateHz > Sensor_MaxReportRate)
nuclear@1:         rateHz = Sensor_MaxReportRate;
nuclear@1:     else if (rateHz == 0)
nuclear@1:         rateHz = Sensor_DefaultReportRate;
nuclear@1: 
nuclear@1:     scfg.PacketInterval = UInt16((Sensor_MaxReportRate / rateHz) - 1);
nuclear@1: 
nuclear@1:     scfg.Pack();
nuclear@1: 
nuclear@1:     GetInternalDevice()->SetFeatureReport(scfg.Buffer, SensorConfigImpl::PacketSize);
nuclear@1:     return 0;
nuclear@1: }
nuclear@1: 
nuclear@1: void SensorDeviceImpl::SetMessageHandler(MessageHandler* handler)
nuclear@1: {
nuclear@1:     if (handler)
nuclear@1:     {
nuclear@1:         SequenceValid = false;
nuclear@1:         DeviceBase::SetMessageHandler(handler);
nuclear@1:     }
nuclear@1:     else
nuclear@1:     {       
nuclear@1:         DeviceBase::SetMessageHandler(handler);
nuclear@1:     }    
nuclear@1: }
nuclear@1: 
nuclear@1: // Sensor reports data in the following coordinate system:
nuclear@1: // Accelerometer: 10^-4 m/s^2; X forward, Y right, Z Down.
nuclear@1: // Gyro:          10^-4 rad/s; X positive roll right, Y positive pitch up; Z positive yaw right.
nuclear@1: 
nuclear@1: 
nuclear@1: // We need to convert it to the following RHS coordinate system:
nuclear@1: // X right, Y Up, Z Back (out of screen)
nuclear@1: //
nuclear@1: Vector3f AccelFromBodyFrameUpdate(const TrackerSensors& update, UByte sampleNumber,
nuclear@1:                                   bool convertHMDToSensor = false)
nuclear@1: {
nuclear@1:     const TrackerSample& sample = update.Samples[sampleNumber];
nuclear@1:     float                ax = (float)sample.AccelX;
nuclear@1:     float                ay = (float)sample.AccelY;
nuclear@1:     float                az = (float)sample.AccelZ;
nuclear@1: 
nuclear@1:     Vector3f val = convertHMDToSensor ? Vector3f(ax, az, -ay) :  Vector3f(ax, ay, az);
nuclear@1:     return val * 0.0001f;
nuclear@1: }
nuclear@1: 
nuclear@1: 
nuclear@1: Vector3f MagFromBodyFrameUpdate(const TrackerSensors& update,
nuclear@1:                                 bool convertHMDToSensor = false)
nuclear@1: {   
nuclear@1:     // Note: Y and Z are swapped in comparison to the Accel.  
nuclear@1:     // This accounts for DK1 sensor firmware axis swap, which should be undone in future releases.
nuclear@1:     if (!convertHMDToSensor)
nuclear@1:     {
nuclear@1:         return Vector3f( (float)update.MagX,
nuclear@1:                          (float)update.MagZ,
nuclear@1:                          (float)update.MagY) * 0.0001f;
nuclear@1:     }    
nuclear@1: 
nuclear@1:     return Vector3f( (float)update.MagX,
nuclear@1:                      (float)update.MagY,
nuclear@1:                     -(float)update.MagZ) * 0.0001f;
nuclear@1: }
nuclear@1: 
nuclear@1: Vector3f EulerFromBodyFrameUpdate(const TrackerSensors& update, UByte sampleNumber,
nuclear@1:                                   bool convertHMDToSensor = false)
nuclear@1: {
nuclear@1:     const TrackerSample& sample = update.Samples[sampleNumber];
nuclear@1:     float                gx = (float)sample.GyroX;
nuclear@1:     float                gy = (float)sample.GyroY;
nuclear@1:     float                gz = (float)sample.GyroZ;
nuclear@1: 
nuclear@1:     Vector3f val = convertHMDToSensor ? Vector3f(gx, gz, -gy) :  Vector3f(gx, gy, gz);
nuclear@1:     return val * 0.0001f;
nuclear@1: }
nuclear@1: 
nuclear@1: 
nuclear@1: void SensorDeviceImpl::onTrackerMessage(TrackerMessage* message)
nuclear@1: {
nuclear@1:     if (message->Type != TrackerMessage_Sensors)
nuclear@1:         return;
nuclear@1:     
nuclear@1:     const float     timeUnit   = (1.0f / 1000.f);
nuclear@1:     TrackerSensors& s = message->Sensors;
nuclear@1:     
nuclear@1: 
nuclear@1:     // Call OnMessage() within a lock to avoid conflicts with handlers.
nuclear@1:     Lock::Locker scopeLock(HandlerRef.GetLock());
nuclear@1: 
nuclear@1: 
nuclear@1:     if (SequenceValid)
nuclear@1:     {
nuclear@1:         unsigned timestampDelta;
nuclear@1: 
nuclear@1:         if (s.Timestamp < LastTimestamp)
nuclear@1:             timestampDelta = ((((int)s.Timestamp) + 0x10000) - (int)LastTimestamp);
nuclear@1:         else
nuclear@1:             timestampDelta = (s.Timestamp - LastTimestamp);
nuclear@1: 
nuclear@1:         // If we missed a small number of samples, replicate the last sample.
nuclear@1:         if ((timestampDelta > LastSampleCount) && (timestampDelta <= 254))
nuclear@1:         {
nuclear@1:             if (HandlerRef.GetHandler())
nuclear@1:             {
nuclear@1:                 MessageBodyFrame sensors(this);
nuclear@1:                 sensors.TimeDelta     = (timestampDelta - LastSampleCount) * timeUnit;
nuclear@1:                 sensors.Acceleration  = LastAcceleration;
nuclear@1:                 sensors.RotationRate  = LastRotationRate;
nuclear@1:                 sensors.MagneticField = LastMagneticField;
nuclear@1:                 sensors.Temperature   = LastTemperature;
nuclear@1: 
nuclear@1:                 HandlerRef.GetHandler()->OnMessage(sensors);
nuclear@1:             }
nuclear@1:         }
nuclear@1:     }
nuclear@1:     else
nuclear@1:     {
nuclear@1:         LastAcceleration = Vector3f(0);
nuclear@1:         LastRotationRate = Vector3f(0);
nuclear@1:         LastMagneticField= Vector3f(0);
nuclear@1:         LastTemperature  = 0;
nuclear@1:         SequenceValid    = true;
nuclear@1:     }
nuclear@1: 
nuclear@1:     LastSampleCount = s.SampleCount;
nuclear@1:     LastTimestamp   = s.Timestamp;
nuclear@1: 
nuclear@1:     bool convertHMDToSensor = (Coordinates == Coord_Sensor) && (HWCoordinates == Coord_HMD);
nuclear@1: 
nuclear@1:     if (HandlerRef.GetHandler())
nuclear@1:     {
nuclear@1:         MessageBodyFrame sensors(this);                
nuclear@1:         UByte            iterations = s.SampleCount;
nuclear@1: 
nuclear@1:         if (s.SampleCount > 3)
nuclear@1:         {
nuclear@1:             iterations        = 3;
nuclear@1:             sensors.TimeDelta = (s.SampleCount - 2) * timeUnit;
nuclear@1:         }
nuclear@1:         else
nuclear@1:         {
nuclear@1:             sensors.TimeDelta = timeUnit;
nuclear@1:         }
nuclear@1: 
nuclear@1:         for (UByte i = 0; i < iterations; i++)
nuclear@1:         {            
nuclear@1:             sensors.Acceleration = AccelFromBodyFrameUpdate(s, i, convertHMDToSensor);
nuclear@1:             sensors.RotationRate = EulerFromBodyFrameUpdate(s, i, convertHMDToSensor);
nuclear@1:             sensors.MagneticField= MagFromBodyFrameUpdate(s, convertHMDToSensor);
nuclear@1:             sensors.Temperature  = s.Temperature * 0.01f;
nuclear@1:             HandlerRef.GetHandler()->OnMessage(sensors);
nuclear@1:             // TimeDelta for the last two sample is always fixed.
nuclear@1:             sensors.TimeDelta = timeUnit;
nuclear@1:         }
nuclear@1: 
nuclear@1:         LastAcceleration = sensors.Acceleration;
nuclear@1:         LastRotationRate = sensors.RotationRate;
nuclear@1:         LastMagneticField= sensors.MagneticField;
nuclear@1:         LastTemperature  = sensors.Temperature;
nuclear@1:     }
nuclear@1:     else
nuclear@1:     {
nuclear@1:         UByte i = (s.SampleCount > 3) ? 2 : (s.SampleCount - 1);
nuclear@1:         LastAcceleration  = AccelFromBodyFrameUpdate(s, i, convertHMDToSensor);
nuclear@1:         LastRotationRate  = EulerFromBodyFrameUpdate(s, i, convertHMDToSensor);
nuclear@1:         LastMagneticField = MagFromBodyFrameUpdate(s, convertHMDToSensor);
nuclear@1:         LastTemperature   = s.Temperature * 0.01f;
nuclear@1:     }
nuclear@1: }
nuclear@1: 
nuclear@1: } // namespace OVR
nuclear@1: 
nuclear@1: