1.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
     1.2 +++ b/libovr/Src/OVR_SensorImpl.cpp	Sat Sep 14 16:14:59 2013 +0300
     1.3 @@ -0,0 +1,882 @@
     1.4 +/************************************************************************************
     1.5 +
     1.6 +Filename    :   OVR_SensorImpl.cpp
     1.7 +Content     :   Oculus Sensor device implementation.
     1.8 +Created     :   March 7, 2013
     1.9 +Authors     :   Lee Cooper
    1.10 +
    1.11 +Copyright   :   Copyright 2013 Oculus VR, Inc. All Rights reserved.
    1.12 +
    1.13 +Use of this software is subject to the terms of the Oculus license
    1.14 +agreement provided at the time of installation or download, or which
    1.15 +otherwise accompanies this software in either electronic or hard copy form.
    1.16 +
    1.17 +*************************************************************************************/
    1.18 +
    1.19 +#include "OVR_SensorImpl.h"
    1.20 +
    1.21 +// HMDDeviceDesc can be created/updated through Sensor carrying DisplayInfo.
    1.22 +
    1.23 +#include "Kernel/OVR_Timer.h"
    1.24 +
    1.25 +namespace OVR {
    1.26 +    
    1.27 +//-------------------------------------------------------------------------------------
    1.28 +// ***** Oculus Sensor-specific packet data structures
    1.29 +
    1.30 +enum {    
    1.31 +    Sensor_VendorId  = Oculus_VendorId,
    1.32 +    Sensor_ProductId = 0x0001,
    1.33 +
    1.34 +    // ST's VID used originally; should be removed in the future
    1.35 +    Sensor_OldVendorId  = 0x0483,
    1.36 +    Sensor_OldProductId = 0x5750,
    1.37 +
    1.38 +    Sensor_DefaultReportRate = 500, // Hz
    1.39 +    Sensor_MaxReportRate     = 1000 // Hz
    1.40 +};
    1.41 +
    1.42 +// Reported data is little-endian now
    1.43 +static UInt16 DecodeUInt16(const UByte* buffer)
    1.44 +{
    1.45 +    return (UInt16(buffer[1]) << 8) | UInt16(buffer[0]);
    1.46 +}
    1.47 +
    1.48 +static SInt16 DecodeSInt16(const UByte* buffer)
    1.49 +{
    1.50 +    return (SInt16(buffer[1]) << 8) | SInt16(buffer[0]);
    1.51 +}
    1.52 +
    1.53 +static UInt32 DecodeUInt32(const UByte* buffer)
    1.54 +{    
    1.55 +    return (buffer[0]) | UInt32(buffer[1] << 8) | UInt32(buffer[2] << 16) | UInt32(buffer[3] << 24);    
    1.56 +}
    1.57 +
    1.58 +static float DecodeFloat(const UByte* buffer)
    1.59 +{
    1.60 +    union {
    1.61 +        UInt32 U;
    1.62 +        float  F;
    1.63 +    };
    1.64 +
    1.65 +    U = DecodeUInt32(buffer);
    1.66 +    return F;
    1.67 +}
    1.68 +
    1.69 +
    1.70 +static void UnpackSensor(const UByte* buffer, SInt32* x, SInt32* y, SInt32* z)
    1.71 +{
    1.72 +    // Sign extending trick
    1.73 +    // from http://graphics.stanford.edu/~seander/bithacks.html#FixedSignExtend
    1.74 +    struct {SInt32 x:21;} s;
    1.75 +
    1.76 +    *x = s.x = (buffer[0] << 13) | (buffer[1] << 5) | ((buffer[2] & 0xF8) >> 3);
    1.77 +    *y = s.x = ((buffer[2] & 0x07) << 18) | (buffer[3] << 10) | (buffer[4] << 2) |
    1.78 +               ((buffer[5] & 0xC0) >> 6);
    1.79 +    *z = s.x = ((buffer[5] & 0x3F) << 15) | (buffer[6] << 7) | (buffer[7] >> 1);
    1.80 +}
    1.81 +
    1.82 +// Messages we care for
    1.83 +enum TrackerMessageType
    1.84 +{
    1.85 +    TrackerMessage_None              = 0,
    1.86 +    TrackerMessage_Sensors           = 1,
    1.87 +    TrackerMessage_Unknown           = 0x100,
    1.88 +    TrackerMessage_SizeError         = 0x101,
    1.89 +};
    1.90 +
    1.91 +struct TrackerSample
    1.92 +{
    1.93 +    SInt32 AccelX, AccelY, AccelZ;
    1.94 +    SInt32 GyroX, GyroY, GyroZ;
    1.95 +};
    1.96 +
    1.97 +
    1.98 +struct TrackerSensors
    1.99 +{
   1.100 +    UByte	SampleCount;
   1.101 +    UInt16	Timestamp;
   1.102 +    UInt16	LastCommandID;
   1.103 +    SInt16	Temperature;
   1.104 +
   1.105 +    TrackerSample Samples[3];
   1.106 +
   1.107 +    SInt16	MagX, MagY, MagZ;
   1.108 +
   1.109 +    TrackerMessageType Decode(const UByte* buffer, int size)
   1.110 +    {
   1.111 +        if (size < 62)
   1.112 +            return TrackerMessage_SizeError;
   1.113 +
   1.114 +        SampleCount		= buffer[1];
   1.115 +        Timestamp		= DecodeUInt16(buffer + 2);
   1.116 +        LastCommandID	= DecodeUInt16(buffer + 4);
   1.117 +        Temperature		= DecodeSInt16(buffer + 6);
   1.118 +        
   1.119 +        //if (SampleCount > 2)        
   1.120 +        //    OVR_DEBUG_LOG_TEXT(("TackerSensor::Decode SampleCount=%d\n", SampleCount));        
   1.121 +
   1.122 +        // Only unpack as many samples as there actually are
   1.123 +        UByte iterationCount = (SampleCount > 2) ? 3 : SampleCount;
   1.124 +
   1.125 +        for (UByte i = 0; i < iterationCount; i++)
   1.126 +        {
   1.127 +            UnpackSensor(buffer + 8 + 16 * i,  &Samples[i].AccelX, &Samples[i].AccelY, &Samples[i].AccelZ);
   1.128 +            UnpackSensor(buffer + 16 + 16 * i, &Samples[i].GyroX,  &Samples[i].GyroY,  &Samples[i].GyroZ);
   1.129 +        }
   1.130 +
   1.131 +        MagX = DecodeSInt16(buffer + 56);
   1.132 +        MagY = DecodeSInt16(buffer + 58);
   1.133 +        MagZ = DecodeSInt16(buffer + 60);
   1.134 +
   1.135 +        return TrackerMessage_Sensors;
   1.136 +    }
   1.137 +};
   1.138 +
   1.139 +struct TrackerMessage
   1.140 +{
   1.141 +    TrackerMessageType Type;
   1.142 +    TrackerSensors     Sensors;
   1.143 +};
   1.144 +
   1.145 +bool DecodeTrackerMessage(TrackerMessage* message, UByte* buffer, int size)
   1.146 +{
   1.147 +    memset(message, 0, sizeof(TrackerMessage));
   1.148 +
   1.149 +    if (size < 4)
   1.150 +    {
   1.151 +        message->Type = TrackerMessage_SizeError;
   1.152 +        return false;
   1.153 +    }
   1.154 +
   1.155 +    switch (buffer[0])
   1.156 +    {
   1.157 +    case TrackerMessage_Sensors:
   1.158 +        message->Type = message->Sensors.Decode(buffer, size);
   1.159 +        break;
   1.160 +
   1.161 +    default:
   1.162 +        message->Type = TrackerMessage_Unknown;
   1.163 +        break;
   1.164 +    }
   1.165 +
   1.166 +    return (message->Type < TrackerMessage_Unknown) && (message->Type != TrackerMessage_None);
   1.167 +}
   1.168 +
   1.169 +
   1.170 +// ***** SensorRangeImpl Implementation
   1.171 +
   1.172 +// Sensor HW only accepts specific maximum range values, used to maximize
   1.173 +// the 16-bit sensor outputs. Use these ramps to specify and report appropriate values.
   1.174 +static const UInt16 AccelRangeRamp[] = { 2, 4, 8, 16 };
   1.175 +static const UInt16 GyroRangeRamp[]  = { 250, 500, 1000, 2000 };
   1.176 +static const UInt16 MagRangeRamp[]   = { 880, 1300, 1900, 2500 };
   1.177 +
   1.178 +static UInt16 SelectSensorRampValue(const UInt16* ramp, unsigned count,
   1.179 +                                    float val, float factor, const char* label)
   1.180 +{    
   1.181 +    UInt16 threshold = (UInt16)(val * factor);
   1.182 +
   1.183 +    for (unsigned i = 0; i<count; i++)
   1.184 +    {
   1.185 +        if (ramp[i] >= threshold)
   1.186 +            return ramp[i];
   1.187 +    }
   1.188 +    OVR_DEBUG_LOG(("SensorDevice::SetRange - %s clamped to %0.4f",
   1.189 +                   label, float(ramp[count-1]) / factor));
   1.190 +    OVR_UNUSED2(factor, label);
   1.191 +    return ramp[count-1];
   1.192 +}
   1.193 +
   1.194 +// SensorScaleImpl provides buffer packing logic for the Sensor Range
   1.195 +// record that can be applied to DK1 sensor through Get/SetFeature. We expose this
   1.196 +// through SensorRange class, which has different units.
   1.197 +struct SensorRangeImpl
   1.198 +{
   1.199 +    enum  { PacketSize = 8 };
   1.200 +    UByte   Buffer[PacketSize];
   1.201 +    
   1.202 +    UInt16  CommandId;
   1.203 +    UInt16  AccelScale;
   1.204 +    UInt16  GyroScale;
   1.205 +    UInt16  MagScale;
   1.206 +
   1.207 +    SensorRangeImpl(const SensorRange& r, UInt16 commandId = 0)
   1.208 +    {
   1.209 +        SetSensorRange(r, commandId);
   1.210 +    }
   1.211 +
   1.212 +    void SetSensorRange(const SensorRange& r, UInt16 commandId = 0)
   1.213 +    {
   1.214 +        CommandId  = commandId;
   1.215 +        AccelScale = SelectSensorRampValue(AccelRangeRamp, sizeof(AccelRangeRamp)/sizeof(AccelRangeRamp[0]),
   1.216 +                                           r.MaxAcceleration, (1.0f / 9.81f), "MaxAcceleration");
   1.217 +        GyroScale  = SelectSensorRampValue(GyroRangeRamp, sizeof(GyroRangeRamp)/sizeof(GyroRangeRamp[0]),
   1.218 +                                           r.MaxRotationRate, Math<float>::RadToDegreeFactor, "MaxRotationRate");
   1.219 +        MagScale   = SelectSensorRampValue(MagRangeRamp, sizeof(MagRangeRamp)/sizeof(MagRangeRamp[0]),
   1.220 +                                           r.MaxMagneticField, 1000.0f, "MaxMagneticField");
   1.221 +        Pack();
   1.222 +    }
   1.223 +
   1.224 +    void GetSensorRange(SensorRange* r)
   1.225 +    {
   1.226 +        r->MaxAcceleration = AccelScale * 9.81f;
   1.227 +        r->MaxRotationRate = DegreeToRad((float)GyroScale);
   1.228 +        r->MaxMagneticField= MagScale * 0.001f;
   1.229 +    }
   1.230 +
   1.231 +    static SensorRange GetMaxSensorRange()
   1.232 +    {
   1.233 +        return SensorRange(AccelRangeRamp[sizeof(AccelRangeRamp)/sizeof(AccelRangeRamp[0]) - 1] * 9.81f,
   1.234 +                           GyroRangeRamp[sizeof(GyroRangeRamp)/sizeof(GyroRangeRamp[0]) - 1] *
   1.235 +                                Math<float>::DegreeToRadFactor,
   1.236 +                           MagRangeRamp[sizeof(MagRangeRamp)/sizeof(MagRangeRamp[0]) - 1] * 0.001f);
   1.237 +    }
   1.238 +
   1.239 +    void  Pack()
   1.240 +    {
   1.241 +        Buffer[0] = 4;
   1.242 +        Buffer[1] = UByte(CommandId & 0xFF);
   1.243 +        Buffer[2] = UByte(CommandId >> 8);
   1.244 +        Buffer[3] = UByte(AccelScale);
   1.245 +        Buffer[4] = UByte(GyroScale & 0xFF);
   1.246 +        Buffer[5] = UByte(GyroScale >> 8);
   1.247 +        Buffer[6] = UByte(MagScale & 0xFF);
   1.248 +        Buffer[7] = UByte(MagScale >> 8);
   1.249 +    }
   1.250 +
   1.251 +    void Unpack()
   1.252 +    {
   1.253 +        CommandId = Buffer[1] | (UInt16(Buffer[2]) << 8);
   1.254 +        AccelScale= Buffer[3];
   1.255 +        GyroScale = Buffer[4] | (UInt16(Buffer[5]) << 8);
   1.256 +        MagScale  = Buffer[6] | (UInt16(Buffer[7]) << 8);
   1.257 +    }
   1.258 +};
   1.259 +
   1.260 +
   1.261 +// Sensor configuration command, ReportId == 2.
   1.262 +
   1.263 +struct SensorConfigImpl
   1.264 +{
   1.265 +    enum  { PacketSize = 7 };
   1.266 +    UByte   Buffer[PacketSize];
   1.267 +
   1.268 +    // Flag values for Flags.
   1.269 +    enum {
   1.270 +        Flag_RawMode            = 0x01,
   1.271 +        Flag_CallibrationTest   = 0x02, // Internal test mode
   1.272 +        Flag_UseCallibration    = 0x04,
   1.273 +        Flag_AutoCallibration   = 0x08,
   1.274 +        Flag_MotionKeepAlive    = 0x10,
   1.275 +        Flag_CommandKeepAlive   = 0x20,
   1.276 +        Flag_SensorCoordinates  = 0x40
   1.277 +    };
   1.278 +
   1.279 +    UInt16  CommandId;
   1.280 +    UByte   Flags;
   1.281 +    UInt16  PacketInterval;
   1.282 +    UInt16  KeepAliveIntervalMs;
   1.283 +
   1.284 +    SensorConfigImpl() : CommandId(0), Flags(0), PacketInterval(0), KeepAliveIntervalMs(0)
   1.285 +    {
   1.286 +        memset(Buffer, 0, PacketSize);
   1.287 +        Buffer[0] = 2;
   1.288 +    }
   1.289 +
   1.290 +    void    SetSensorCoordinates(bool sensorCoordinates)
   1.291 +    { Flags = (Flags & ~Flag_SensorCoordinates) | (sensorCoordinates ? Flag_SensorCoordinates : 0); }
   1.292 +    bool    IsUsingSensorCoordinates() const
   1.293 +    { return (Flags & Flag_SensorCoordinates) != 0; }
   1.294 +
   1.295 +    void Pack()
   1.296 +    {
   1.297 +        Buffer[0] = 2;
   1.298 +        Buffer[1] = UByte(CommandId & 0xFF);
   1.299 +        Buffer[2] = UByte(CommandId >> 8);
   1.300 +        Buffer[3] = Flags;
   1.301 +        Buffer[4] = UByte(PacketInterval);
   1.302 +        Buffer[5] = UByte(KeepAliveIntervalMs & 0xFF);
   1.303 +        Buffer[6] = UByte(KeepAliveIntervalMs >> 8);
   1.304 +    }
   1.305 +
   1.306 +    void Unpack()
   1.307 +    {
   1.308 +        CommandId          = Buffer[1] | (UInt16(Buffer[2]) << 8);
   1.309 +        Flags              = Buffer[3];
   1.310 +        PacketInterval     = Buffer[4];
   1.311 +        KeepAliveIntervalMs= Buffer[5] | (UInt16(Buffer[6]) << 8);
   1.312 +    }
   1.313 +    
   1.314 +};
   1.315 +
   1.316 +
   1.317 +// SensorKeepAlive - feature report that needs to be sent at regular intervals for sensor
   1.318 +// to receive commands.
   1.319 +struct SensorKeepAliveImpl
   1.320 +{
   1.321 +    enum  { PacketSize = 5 };
   1.322 +    UByte   Buffer[PacketSize];
   1.323 +
   1.324 +    UInt16  CommandId;
   1.325 +    UInt16  KeepAliveIntervalMs;
   1.326 +
   1.327 +    SensorKeepAliveImpl(UInt16 interval = 0, UInt16 commandId = 0)
   1.328 +        : CommandId(commandId), KeepAliveIntervalMs(interval)
   1.329 +    {
   1.330 +        Pack();
   1.331 +    }
   1.332 +
   1.333 +    void  Pack()
   1.334 +    {
   1.335 +        Buffer[0] = 8;
   1.336 +        Buffer[1] = UByte(CommandId & 0xFF);
   1.337 +        Buffer[2] = UByte(CommandId >> 8);
   1.338 +        Buffer[3] = UByte(KeepAliveIntervalMs & 0xFF);
   1.339 +        Buffer[4] = UByte(KeepAliveIntervalMs >> 8);
   1.340 +    }
   1.341 +
   1.342 +    void Unpack()
   1.343 +    {
   1.344 +        CommandId          = Buffer[1] | (UInt16(Buffer[2]) << 8);
   1.345 +        KeepAliveIntervalMs= Buffer[3] | (UInt16(Buffer[4]) << 8);
   1.346 +    }
   1.347 +};
   1.348 +
   1.349 +
   1.350 +//-------------------------------------------------------------------------------------
   1.351 +// ***** SensorDisplayInfoImpl
   1.352 +SensorDisplayInfoImpl::SensorDisplayInfoImpl()
   1.353 + :  CommandId(0), DistortionType(Base_None)
   1.354 +{
   1.355 +    memset(Buffer, 0, PacketSize);
   1.356 +    Buffer[0] = 9;
   1.357 +}
   1.358 +
   1.359 +void SensorDisplayInfoImpl::Unpack()
   1.360 +{
   1.361 +    CommandId               = Buffer[1] | (UInt16(Buffer[2]) << 8);
   1.362 +    DistortionType          = Buffer[3];
   1.363 +    HResolution             = DecodeUInt16(Buffer+4);
   1.364 +    VResolution             = DecodeUInt16(Buffer+6);
   1.365 +    HScreenSize             = DecodeUInt32(Buffer+8) *  (1/1000000.f);
   1.366 +    VScreenSize             = DecodeUInt32(Buffer+12) * (1/1000000.f);
   1.367 +    VCenter                 = DecodeUInt32(Buffer+16) * (1/1000000.f);
   1.368 +    LensSeparation          = DecodeUInt32(Buffer+20) * (1/1000000.f);
   1.369 +    EyeToScreenDistance[0]  = DecodeUInt32(Buffer+24) * (1/1000000.f);
   1.370 +    EyeToScreenDistance[1]  = DecodeUInt32(Buffer+28) * (1/1000000.f);
   1.371 +    DistortionK[0]          = DecodeFloat(Buffer+32);
   1.372 +    DistortionK[1]          = DecodeFloat(Buffer+36);
   1.373 +    DistortionK[2]          = DecodeFloat(Buffer+40);
   1.374 +    DistortionK[3]          = DecodeFloat(Buffer+44);
   1.375 +    DistortionK[4]          = DecodeFloat(Buffer+48);
   1.376 +    DistortionK[5]          = DecodeFloat(Buffer+52);
   1.377 +}
   1.378 +
   1.379 +
   1.380 +//-------------------------------------------------------------------------------------
   1.381 +// ***** SensorDeviceFactory
   1.382 +
   1.383 +SensorDeviceFactory SensorDeviceFactory::Instance;
   1.384 +
   1.385 +void SensorDeviceFactory::EnumerateDevices(EnumerateVisitor& visitor)
   1.386 +{
   1.387 +
   1.388 +    class SensorEnumerator : public HIDEnumerateVisitor
   1.389 +    {
   1.390 +        // Assign not supported; suppress MSVC warning.
   1.391 +        void operator = (const SensorEnumerator&) { }
   1.392 +
   1.393 +        DeviceFactory*     pFactory;
   1.394 +        EnumerateVisitor&  ExternalVisitor;   
   1.395 +    public:
   1.396 +        SensorEnumerator(DeviceFactory* factory, EnumerateVisitor& externalVisitor)
   1.397 +            : pFactory(factory), ExternalVisitor(externalVisitor) { }
   1.398 +
   1.399 +        virtual bool MatchVendorProduct(UInt16 vendorId, UInt16 productId)
   1.400 +        {
   1.401 +            return pFactory->MatchVendorProduct(vendorId, productId);
   1.402 +        }
   1.403 +
   1.404 +        virtual void Visit(HIDDevice& device, const HIDDeviceDesc& desc)
   1.405 +        {
   1.406 +            SensorDeviceCreateDesc createDesc(pFactory, desc);
   1.407 +            ExternalVisitor.Visit(createDesc);
   1.408 +
   1.409 +            // Check if the sensor returns DisplayInfo. If so, try to use it to override potentially
   1.410 +            // mismatching monitor information (in case wrong EDID is reported by splitter),
   1.411 +            // or to create a new "virtualized" HMD Device.
   1.412 +            
   1.413 +            SensorDisplayInfoImpl displayInfo;
   1.414 +
   1.415 +            if (device.GetFeatureReport(displayInfo.Buffer, SensorDisplayInfoImpl::PacketSize))
   1.416 +            {
   1.417 +                displayInfo.Unpack();
   1.418 +
   1.419 +                // If we got display info, try to match / create HMDDevice as well
   1.420 +                // so that sensor settings give preference.
   1.421 +                if (displayInfo.DistortionType & SensorDisplayInfoImpl::Mask_BaseFmt)
   1.422 +                {
   1.423 +                    SensorDeviceImpl::EnumerateHMDFromSensorDisplayInfo(displayInfo, ExternalVisitor);
   1.424 +                }
   1.425 +            }
   1.426 +        }
   1.427 +    };
   1.428 +
   1.429 +    //double start = Timer::GetProfileSeconds();
   1.430 +
   1.431 +    SensorEnumerator sensorEnumerator(this, visitor);
   1.432 +    GetManagerImpl()->GetHIDDeviceManager()->Enumerate(&sensorEnumerator);
   1.433 +
   1.434 +    //double totalSeconds = Timer::GetProfileSeconds() - start; 
   1.435 +}
   1.436 +
   1.437 +bool SensorDeviceFactory::MatchVendorProduct(UInt16 vendorId, UInt16 productId) const
   1.438 +{
   1.439 +    return ((vendorId == Sensor_VendorId) && (productId == Sensor_ProductId)) ||
   1.440 +        ((vendorId == Sensor_OldVendorId) && (productId == Sensor_OldProductId));
   1.441 +}
   1.442 +
   1.443 +bool SensorDeviceFactory::DetectHIDDevice(DeviceManager* pdevMgr, const HIDDeviceDesc& desc)
   1.444 +{
   1.445 +    if (MatchVendorProduct(desc.VendorId, desc.ProductId))
   1.446 +    {
   1.447 +        SensorDeviceCreateDesc createDesc(this, desc);
   1.448 +        return pdevMgr->AddDevice_NeedsLock(createDesc).GetPtr() != NULL;
   1.449 +    }
   1.450 +    return false;
   1.451 +}
   1.452 +
   1.453 +//-------------------------------------------------------------------------------------
   1.454 +// ***** SensorDeviceCreateDesc
   1.455 +
   1.456 +DeviceBase* SensorDeviceCreateDesc::NewDeviceInstance()
   1.457 +{
   1.458 +    return new SensorDeviceImpl(this);
   1.459 +}
   1.460 +
   1.461 +bool SensorDeviceCreateDesc::GetDeviceInfo(DeviceInfo* info) const
   1.462 +{
   1.463 +    if ((info->InfoClassType != Device_Sensor) &&
   1.464 +        (info->InfoClassType != Device_None))
   1.465 +        return false;
   1.466 +
   1.467 +    OVR_strcpy(info->ProductName,  DeviceInfo::MaxNameLength, HIDDesc.Product.ToCStr());
   1.468 +    OVR_strcpy(info->Manufacturer, DeviceInfo::MaxNameLength, HIDDesc.Manufacturer.ToCStr());
   1.469 +    info->Type    = Device_Sensor;
   1.470 +    info->Version = 0;
   1.471 +
   1.472 +    if (info->InfoClassType == Device_Sensor)
   1.473 +    {
   1.474 +        SensorInfo* sinfo = (SensorInfo*)info;
   1.475 +        sinfo->VendorId  = HIDDesc.VendorId;
   1.476 +        sinfo->ProductId = HIDDesc.ProductId;
   1.477 +        sinfo->MaxRanges = SensorRangeImpl::GetMaxSensorRange();
   1.478 +        OVR_strcpy(sinfo->SerialNumber, sizeof(sinfo->SerialNumber),HIDDesc.SerialNumber.ToCStr());
   1.479 +    }
   1.480 +    return true;
   1.481 +}
   1.482 +
   1.483 +
   1.484 +//-------------------------------------------------------------------------------------
   1.485 +// ***** SensorDevice
   1.486 +
   1.487 +SensorDeviceImpl::SensorDeviceImpl(SensorDeviceCreateDesc* createDesc)
   1.488 +    : OVR::HIDDeviceImpl<OVR::SensorDevice>(createDesc, 0),
   1.489 +      Coordinates(SensorDevice::Coord_Sensor),
   1.490 +      HWCoordinates(SensorDevice::Coord_HMD), // HW reports HMD coordinates by default.
   1.491 +      NextKeepAliveTicks(0),
   1.492 +      MaxValidRange(SensorRangeImpl::GetMaxSensorRange())
   1.493 +{
   1.494 +    SequenceValid  = false;
   1.495 +    LastSampleCount= 0;
   1.496 +    LastTimestamp   = 0;
   1.497 +
   1.498 +    OldCommandId = 0;
   1.499 +}
   1.500 +
   1.501 +SensorDeviceImpl::~SensorDeviceImpl()
   1.502 +{
   1.503 +    // Check that Shutdown() was called.
   1.504 +    OVR_ASSERT(!pCreateDesc->pDevice);    
   1.505 +}
   1.506 +
   1.507 +// Internal creation APIs.
   1.508 +bool SensorDeviceImpl::Initialize(DeviceBase* parent)
   1.509 +{
   1.510 +    if (HIDDeviceImpl<OVR::SensorDevice>::Initialize(parent))
   1.511 +    {
   1.512 +        openDevice();
   1.513 +
   1.514 +        LogText("OVR::SensorDevice initialized.\n");
   1.515 +
   1.516 +        return true;
   1.517 +    }
   1.518 +
   1.519 +    return false;
   1.520 +}
   1.521 +
   1.522 +void SensorDeviceImpl::openDevice()
   1.523 +{
   1.524 +
   1.525 +    // Read the currently configured range from sensor.
   1.526 +    SensorRangeImpl sr(SensorRange(), 0);
   1.527 +
   1.528 +    if (GetInternalDevice()->GetFeatureReport(sr.Buffer, SensorRangeImpl::PacketSize))
   1.529 +    {
   1.530 +        sr.Unpack();
   1.531 +        sr.GetSensorRange(&CurrentRange);
   1.532 +    }
   1.533 +
   1.534 +
   1.535 +    // If the sensor has "DisplayInfo" data, use HMD coordinate frame by default.
   1.536 +    SensorDisplayInfoImpl displayInfo;
   1.537 +    if (GetInternalDevice()->GetFeatureReport(displayInfo.Buffer, SensorDisplayInfoImpl::PacketSize))
   1.538 +    {
   1.539 +        displayInfo.Unpack();
   1.540 +        Coordinates = (displayInfo.DistortionType & SensorDisplayInfoImpl::Mask_BaseFmt) ?
   1.541 +                      Coord_HMD : Coord_Sensor;
   1.542 +    }
   1.543 +
   1.544 +    // Read/Apply sensor config.
   1.545 +    setCoordinateFrame(Coordinates);
   1.546 +    setReportRate(Sensor_DefaultReportRate);
   1.547 +
   1.548 +    // Set Keep-alive at 10 seconds.
   1.549 +    SensorKeepAliveImpl skeepAlive(10 * 1000);
   1.550 +    GetInternalDevice()->SetFeatureReport(skeepAlive.Buffer, SensorKeepAliveImpl::PacketSize);
   1.551 +}
   1.552 +
   1.553 +void SensorDeviceImpl::closeDeviceOnError()
   1.554 +{
   1.555 +    LogText("OVR::SensorDevice - Lost connection to '%s'\n", getHIDDesc()->Path.ToCStr());
   1.556 +    NextKeepAliveTicks = 0;
   1.557 +}
   1.558 +
   1.559 +void SensorDeviceImpl::Shutdown()
   1.560 +{   
   1.561 +    HIDDeviceImpl<OVR::SensorDevice>::Shutdown();
   1.562 +
   1.563 +    LogText("OVR::SensorDevice - Closed '%s'\n", getHIDDesc()->Path.ToCStr());
   1.564 +}
   1.565 +
   1.566 +
   1.567 +void SensorDeviceImpl::OnInputReport(UByte* pData, UInt32 length)
   1.568 +{
   1.569 +
   1.570 +    bool processed = false;
   1.571 +    if (!processed)
   1.572 +    {
   1.573 +
   1.574 +        TrackerMessage message;
   1.575 +        if (DecodeTrackerMessage(&message, pData, length))
   1.576 +        {
   1.577 +            processed = true;
   1.578 +            onTrackerMessage(&message);
   1.579 +        }
   1.580 +    }
   1.581 +}
   1.582 +
   1.583 +UInt64 SensorDeviceImpl::OnTicks(UInt64 ticksMks)
   1.584 +{
   1.585 +
   1.586 +    if (ticksMks >= NextKeepAliveTicks)
   1.587 +    {
   1.588 +        // Use 3-seconds keep alive by default.
   1.589 +        UInt64 keepAliveDelta = Timer::MksPerSecond * 3;
   1.590 +
   1.591 +        // Set Keep-alive at 10 seconds.
   1.592 +        SensorKeepAliveImpl skeepAlive(10 * 1000);
   1.593 +        // OnTicks is called from background thread so we don't need to add this to the command queue.
   1.594 +        GetInternalDevice()->SetFeatureReport(skeepAlive.Buffer, SensorKeepAliveImpl::PacketSize);
   1.595 +
   1.596 +		// Emit keep-alive every few seconds.
   1.597 +        NextKeepAliveTicks = ticksMks + keepAliveDelta;
   1.598 +    }
   1.599 +    return NextKeepAliveTicks - ticksMks;
   1.600 +}
   1.601 +
   1.602 +bool SensorDeviceImpl::SetRange(const SensorRange& range, bool waitFlag)
   1.603 +{
   1.604 +    bool                 result = 0;
   1.605 +    ThreadCommandQueue * threadQueue = GetManagerImpl()->GetThreadQueue();
   1.606 +
   1.607 +    if (!waitFlag)
   1.608 +    {
   1.609 +        return threadQueue->PushCall(this, &SensorDeviceImpl::setRange, range);
   1.610 +    }
   1.611 +    
   1.612 +    if (!threadQueue->PushCallAndWaitResult(this, 
   1.613 +                                            &SensorDeviceImpl::setRange,
   1.614 +                                            &result, 
   1.615 +                                            range))
   1.616 +    {
   1.617 +        return false;
   1.618 +    }
   1.619 +
   1.620 +    return result;
   1.621 +}
   1.622 +
   1.623 +void SensorDeviceImpl::GetRange(SensorRange* range) const
   1.624 +{
   1.625 +    Lock::Locker lockScope(GetLock());
   1.626 +    *range = CurrentRange;
   1.627 +}
   1.628 +
   1.629 +bool SensorDeviceImpl::setRange(const SensorRange& range)
   1.630 +{
   1.631 +    SensorRangeImpl sr(range);
   1.632 +    
   1.633 +    if (GetInternalDevice()->SetFeatureReport(sr.Buffer, SensorRangeImpl::PacketSize))
   1.634 +    {
   1.635 +        Lock::Locker lockScope(GetLock());
   1.636 +        sr.GetSensorRange(&CurrentRange);
   1.637 +        return true;
   1.638 +    }
   1.639 +    
   1.640 +    return false;
   1.641 +}
   1.642 +
   1.643 +void SensorDeviceImpl::SetCoordinateFrame(CoordinateFrame coordframe)
   1.644 +{ 
   1.645 +    // Push call with wait.
   1.646 +    GetManagerImpl()->GetThreadQueue()->
   1.647 +        PushCall(this, &SensorDeviceImpl::setCoordinateFrame, coordframe, true);
   1.648 +}
   1.649 +
   1.650 +SensorDevice::CoordinateFrame SensorDeviceImpl::GetCoordinateFrame() const
   1.651 +{
   1.652 +    return Coordinates;
   1.653 +}
   1.654 +
   1.655 +Void SensorDeviceImpl::setCoordinateFrame(CoordinateFrame coordframe)
   1.656 +{
   1.657 +
   1.658 +    Coordinates = coordframe;
   1.659 +
   1.660 +    // Read the original coordinate frame, then try to change it.
   1.661 +    SensorConfigImpl scfg;
   1.662 +    if (GetInternalDevice()->GetFeatureReport(scfg.Buffer, SensorConfigImpl::PacketSize))
   1.663 +    {
   1.664 +        scfg.Unpack();
   1.665 +    }
   1.666 +
   1.667 +    scfg.SetSensorCoordinates(coordframe == Coord_Sensor);
   1.668 +    scfg.Pack();
   1.669 +
   1.670 +    GetInternalDevice()->SetFeatureReport(scfg.Buffer, SensorConfigImpl::PacketSize);
   1.671 +    
   1.672 +    // Re-read the state, in case of older firmware that doesn't support Sensor coordinates.
   1.673 +    if (GetInternalDevice()->GetFeatureReport(scfg.Buffer, SensorConfigImpl::PacketSize))
   1.674 +    {
   1.675 +        scfg.Unpack();
   1.676 +        HWCoordinates = scfg.IsUsingSensorCoordinates() ? Coord_Sensor : Coord_HMD;
   1.677 +    }
   1.678 +    else
   1.679 +    {
   1.680 +        HWCoordinates = Coord_HMD;
   1.681 +    }
   1.682 +    return 0;
   1.683 +}
   1.684 +
   1.685 +void SensorDeviceImpl::SetReportRate(unsigned rateHz)
   1.686 +{ 
   1.687 +    // Push call with wait.
   1.688 +    GetManagerImpl()->GetThreadQueue()->
   1.689 +        PushCall(this, &SensorDeviceImpl::setReportRate, rateHz, true);
   1.690 +}
   1.691 +
   1.692 +unsigned SensorDeviceImpl::GetReportRate() const
   1.693 +{
   1.694 +    // Read the original configuration
   1.695 +    SensorConfigImpl scfg;
   1.696 +    if (GetInternalDevice()->GetFeatureReport(scfg.Buffer, SensorConfigImpl::PacketSize))
   1.697 +    {
   1.698 +        scfg.Unpack();
   1.699 +        return Sensor_MaxReportRate / (scfg.PacketInterval + 1);
   1.700 +    }
   1.701 +    return 0; // error
   1.702 +}
   1.703 +
   1.704 +Void SensorDeviceImpl::setReportRate(unsigned rateHz)
   1.705 +{
   1.706 +    // Read the original configuration
   1.707 +    SensorConfigImpl scfg;
   1.708 +    if (GetInternalDevice()->GetFeatureReport(scfg.Buffer, SensorConfigImpl::PacketSize))
   1.709 +    {
   1.710 +        scfg.Unpack();
   1.711 +    }
   1.712 +
   1.713 +    if (rateHz > Sensor_MaxReportRate)
   1.714 +        rateHz = Sensor_MaxReportRate;
   1.715 +    else if (rateHz == 0)
   1.716 +        rateHz = Sensor_DefaultReportRate;
   1.717 +
   1.718 +    scfg.PacketInterval = UInt16((Sensor_MaxReportRate / rateHz) - 1);
   1.719 +
   1.720 +    scfg.Pack();
   1.721 +
   1.722 +    GetInternalDevice()->SetFeatureReport(scfg.Buffer, SensorConfigImpl::PacketSize);
   1.723 +    return 0;
   1.724 +}
   1.725 +
   1.726 +void SensorDeviceImpl::SetMessageHandler(MessageHandler* handler)
   1.727 +{
   1.728 +    if (handler)
   1.729 +    {
   1.730 +        SequenceValid = false;
   1.731 +        DeviceBase::SetMessageHandler(handler);
   1.732 +    }
   1.733 +    else
   1.734 +    {       
   1.735 +        DeviceBase::SetMessageHandler(handler);
   1.736 +    }    
   1.737 +}
   1.738 +
   1.739 +// Sensor reports data in the following coordinate system:
   1.740 +// Accelerometer: 10^-4 m/s^2; X forward, Y right, Z Down.
   1.741 +// Gyro:          10^-4 rad/s; X positive roll right, Y positive pitch up; Z positive yaw right.
   1.742 +
   1.743 +
   1.744 +// We need to convert it to the following RHS coordinate system:
   1.745 +// X right, Y Up, Z Back (out of screen)
   1.746 +//
   1.747 +Vector3f AccelFromBodyFrameUpdate(const TrackerSensors& update, UByte sampleNumber,
   1.748 +                                  bool convertHMDToSensor = false)
   1.749 +{
   1.750 +    const TrackerSample& sample = update.Samples[sampleNumber];
   1.751 +    float                ax = (float)sample.AccelX;
   1.752 +    float                ay = (float)sample.AccelY;
   1.753 +    float                az = (float)sample.AccelZ;
   1.754 +
   1.755 +    Vector3f val = convertHMDToSensor ? Vector3f(ax, az, -ay) :  Vector3f(ax, ay, az);
   1.756 +    return val * 0.0001f;
   1.757 +}
   1.758 +
   1.759 +
   1.760 +Vector3f MagFromBodyFrameUpdate(const TrackerSensors& update,
   1.761 +                                bool convertHMDToSensor = false)
   1.762 +{   
   1.763 +    // Note: Y and Z are swapped in comparison to the Accel.  
   1.764 +    // This accounts for DK1 sensor firmware axis swap, which should be undone in future releases.
   1.765 +    if (!convertHMDToSensor)
   1.766 +    {
   1.767 +        return Vector3f( (float)update.MagX,
   1.768 +                         (float)update.MagZ,
   1.769 +                         (float)update.MagY) * 0.0001f;
   1.770 +    }    
   1.771 +
   1.772 +    return Vector3f( (float)update.MagX,
   1.773 +                     (float)update.MagY,
   1.774 +                    -(float)update.MagZ) * 0.0001f;
   1.775 +}
   1.776 +
   1.777 +Vector3f EulerFromBodyFrameUpdate(const TrackerSensors& update, UByte sampleNumber,
   1.778 +                                  bool convertHMDToSensor = false)
   1.779 +{
   1.780 +    const TrackerSample& sample = update.Samples[sampleNumber];
   1.781 +    float                gx = (float)sample.GyroX;
   1.782 +    float                gy = (float)sample.GyroY;
   1.783 +    float                gz = (float)sample.GyroZ;
   1.784 +
   1.785 +    Vector3f val = convertHMDToSensor ? Vector3f(gx, gz, -gy) :  Vector3f(gx, gy, gz);
   1.786 +    return val * 0.0001f;
   1.787 +}
   1.788 +
   1.789 +
   1.790 +void SensorDeviceImpl::onTrackerMessage(TrackerMessage* message)
   1.791 +{
   1.792 +    if (message->Type != TrackerMessage_Sensors)
   1.793 +        return;
   1.794 +    
   1.795 +    const float     timeUnit   = (1.0f / 1000.f);
   1.796 +    TrackerSensors& s = message->Sensors;
   1.797 +    
   1.798 +
   1.799 +    // Call OnMessage() within a lock to avoid conflicts with handlers.
   1.800 +    Lock::Locker scopeLock(HandlerRef.GetLock());
   1.801 +
   1.802 +
   1.803 +    if (SequenceValid)
   1.804 +    {
   1.805 +        unsigned timestampDelta;
   1.806 +
   1.807 +        if (s.Timestamp < LastTimestamp)
   1.808 +            timestampDelta = ((((int)s.Timestamp) + 0x10000) - (int)LastTimestamp);
   1.809 +        else
   1.810 +            timestampDelta = (s.Timestamp - LastTimestamp);
   1.811 +
   1.812 +        // If we missed a small number of samples, replicate the last sample.
   1.813 +        if ((timestampDelta > LastSampleCount) && (timestampDelta <= 254))
   1.814 +        {
   1.815 +            if (HandlerRef.GetHandler())
   1.816 +            {
   1.817 +                MessageBodyFrame sensors(this);
   1.818 +                sensors.TimeDelta     = (timestampDelta - LastSampleCount) * timeUnit;
   1.819 +                sensors.Acceleration  = LastAcceleration;
   1.820 +                sensors.RotationRate  = LastRotationRate;
   1.821 +                sensors.MagneticField = LastMagneticField;
   1.822 +                sensors.Temperature   = LastTemperature;
   1.823 +
   1.824 +                HandlerRef.GetHandler()->OnMessage(sensors);
   1.825 +            }
   1.826 +        }
   1.827 +    }
   1.828 +    else
   1.829 +    {
   1.830 +        LastAcceleration = Vector3f(0);
   1.831 +        LastRotationRate = Vector3f(0);
   1.832 +        LastMagneticField= Vector3f(0);
   1.833 +        LastTemperature  = 0;
   1.834 +        SequenceValid    = true;
   1.835 +    }
   1.836 +
   1.837 +    LastSampleCount = s.SampleCount;
   1.838 +    LastTimestamp   = s.Timestamp;
   1.839 +
   1.840 +    bool convertHMDToSensor = (Coordinates == Coord_Sensor) && (HWCoordinates == Coord_HMD);
   1.841 +
   1.842 +    if (HandlerRef.GetHandler())
   1.843 +    {
   1.844 +        MessageBodyFrame sensors(this);                
   1.845 +        UByte            iterations = s.SampleCount;
   1.846 +
   1.847 +        if (s.SampleCount > 3)
   1.848 +        {
   1.849 +            iterations        = 3;
   1.850 +            sensors.TimeDelta = (s.SampleCount - 2) * timeUnit;
   1.851 +        }
   1.852 +        else
   1.853 +        {
   1.854 +            sensors.TimeDelta = timeUnit;
   1.855 +        }
   1.856 +
   1.857 +        for (UByte i = 0; i < iterations; i++)
   1.858 +        {            
   1.859 +            sensors.Acceleration = AccelFromBodyFrameUpdate(s, i, convertHMDToSensor);
   1.860 +            sensors.RotationRate = EulerFromBodyFrameUpdate(s, i, convertHMDToSensor);
   1.861 +            sensors.MagneticField= MagFromBodyFrameUpdate(s, convertHMDToSensor);
   1.862 +            sensors.Temperature  = s.Temperature * 0.01f;
   1.863 +            HandlerRef.GetHandler()->OnMessage(sensors);
   1.864 +            // TimeDelta for the last two sample is always fixed.
   1.865 +            sensors.TimeDelta = timeUnit;
   1.866 +        }
   1.867 +
   1.868 +        LastAcceleration = sensors.Acceleration;
   1.869 +        LastRotationRate = sensors.RotationRate;
   1.870 +        LastMagneticField= sensors.MagneticField;
   1.871 +        LastTemperature  = sensors.Temperature;
   1.872 +    }
   1.873 +    else
   1.874 +    {
   1.875 +        UByte i = (s.SampleCount > 3) ? 2 : (s.SampleCount - 1);
   1.876 +        LastAcceleration  = AccelFromBodyFrameUpdate(s, i, convertHMDToSensor);
   1.877 +        LastRotationRate  = EulerFromBodyFrameUpdate(s, i, convertHMDToSensor);
   1.878 +        LastMagneticField = MagFromBodyFrameUpdate(s, convertHMDToSensor);
   1.879 +        LastTemperature   = s.Temperature * 0.01f;
   1.880 +    }
   1.881 +}
   1.882 +
   1.883 +} // namespace OVR
   1.884 +
   1.885 +