OVR_SensorImpl.cpp
Go to the documentation of this file.
00001 /************************************************************************************
00002 
00003 Filename    :   OVR_SensorImpl.cpp
00004 Content     :   Oculus Sensor device implementation.
00005 Created     :   March 7, 2013
00006 Authors     :   Lee Cooper
00007 
00008 Copyright   :   Copyright 2013 Oculus VR, Inc. All Rights reserved.
00009 
00010 Use of this software is subject to the terms of the Oculus license
00011 agreement provided at the time of installation or download, or which
00012 otherwise accompanies this software in either electronic or hard copy form.
00013 
00014 *************************************************************************************/
00015 
00016 #include "OVR_SensorImpl.h"
00017 
00018 // HMDDeviceDesc can be created/updated through Sensor carrying DisplayInfo.
00019 
00020 #include "Kernel/OVR_Timer.h"
00021 
00022 namespace OVR {
00023     
00024 //-------------------------------------------------------------------------------------
00025 // ***** Oculus Sensor-specific packet data structures
00026 
00027 enum {    
00028     Sensor_VendorId  = Oculus_VendorId,
00029     Sensor_ProductId = 0x0001,
00030 
00031     // ST's VID used originally; should be removed in the future
00032     Sensor_OldVendorId  = 0x0483,
00033     Sensor_OldProductId = 0x5750,
00034 
00035     Sensor_DefaultReportRate = 500, // Hz
00036     Sensor_MaxReportRate     = 1000 // Hz
00037 };
00038 
00039 // Reported data is little-endian now
00040 static UInt16 DecodeUInt16(const UByte* buffer)
00041 {
00042     return (UInt16(buffer[1]) << 8) | UInt16(buffer[0]);
00043 }
00044 
00045 static SInt16 DecodeSInt16(const UByte* buffer)
00046 {
00047     return (SInt16(buffer[1]) << 8) | SInt16(buffer[0]);
00048 }
00049 
00050 static UInt32 DecodeUInt32(const UByte* buffer)
00051 {    
00052     return (buffer[0]) | UInt32(buffer[1] << 8) | UInt32(buffer[2] << 16) | UInt32(buffer[3] << 24);    
00053 }
00054 
00055 static float DecodeFloat(const UByte* buffer)
00056 {
00057     union {
00058         UInt32 U;
00059         float  F;
00060     };
00061 
00062     U = DecodeUInt32(buffer);
00063     return F;
00064 }
00065 
00066 
00067 static void UnpackSensor(const UByte* buffer, SInt32* x, SInt32* y, SInt32* z)
00068 {
00069     // Sign extending trick
00070     // from http://graphics.stanford.edu/~seander/bithacks.html#FixedSignExtend
00071     struct {SInt32 x:21;} s;
00072 
00073     *x = s.x = (buffer[0] << 13) | (buffer[1] << 5) | ((buffer[2] & 0xF8) >> 3);
00074     *y = s.x = ((buffer[2] & 0x07) << 18) | (buffer[3] << 10) | (buffer[4] << 2) |
00075                ((buffer[5] & 0xC0) >> 6);
00076     *z = s.x = ((buffer[5] & 0x3F) << 15) | (buffer[6] << 7) | (buffer[7] >> 1);
00077 }
00078 
00079 // Messages we care for
00080 enum TrackerMessageType
00081 {
00082     TrackerMessage_None              = 0,
00083     TrackerMessage_Sensors           = 1,
00084     TrackerMessage_Unknown           = 0x100,
00085     TrackerMessage_SizeError         = 0x101,
00086 };
00087 
00088 struct TrackerSample
00089 {
00090     SInt32 AccelX, AccelY, AccelZ;
00091     SInt32 GyroX, GyroY, GyroZ;
00092 };
00093 
00094 
00095 struct TrackerSensors
00096 {
00097     UByte       SampleCount;
00098     UInt16      Timestamp;
00099     UInt16      LastCommandID;
00100     SInt16      Temperature;
00101 
00102     TrackerSample Samples[3];
00103 
00104     SInt16      MagX, MagY, MagZ;
00105 
00106     TrackerMessageType Decode(const UByte* buffer, int size)
00107     {
00108         if (size < 62)
00109             return TrackerMessage_SizeError;
00110 
00111         SampleCount             = buffer[1];
00112         Timestamp               = DecodeUInt16(buffer + 2);
00113         LastCommandID   = DecodeUInt16(buffer + 4);
00114         Temperature             = DecodeSInt16(buffer + 6);
00115         
00116         //if (SampleCount > 2)        
00117         //    OVR_DEBUG_LOG_TEXT(("TackerSensor::Decode SampleCount=%d\n", SampleCount));        
00118 
00119         // Only unpack as many samples as there actually are
00120         UByte iterationCount = (SampleCount > 2) ? 3 : SampleCount;
00121 
00122         for (UByte i = 0; i < iterationCount; i++)
00123         {
00124             UnpackSensor(buffer + 8 + 16 * i,  &Samples[i].AccelX, &Samples[i].AccelY, &Samples[i].AccelZ);
00125             UnpackSensor(buffer + 16 + 16 * i, &Samples[i].GyroX,  &Samples[i].GyroY,  &Samples[i].GyroZ);
00126         }
00127 
00128         MagX = DecodeSInt16(buffer + 56);
00129         MagY = DecodeSInt16(buffer + 58);
00130         MagZ = DecodeSInt16(buffer + 60);
00131 
00132         return TrackerMessage_Sensors;
00133     }
00134 };
00135 
00136 struct TrackerMessage
00137 {
00138     TrackerMessageType Type;
00139     TrackerSensors     Sensors;
00140 };
00141 
00142 bool DecodeTrackerMessage(TrackerMessage* message, UByte* buffer, int size)
00143 {
00144     memset(message, 0, sizeof(TrackerMessage));
00145 
00146     if (size < 4)
00147     {
00148         message->Type = TrackerMessage_SizeError;
00149         return false;
00150     }
00151 
00152     switch (buffer[0])
00153     {
00154     case TrackerMessage_Sensors:
00155         message->Type = message->Sensors.Decode(buffer, size);
00156         break;
00157 
00158     default:
00159         message->Type = TrackerMessage_Unknown;
00160         break;
00161     }
00162 
00163     return (message->Type < TrackerMessage_Unknown) && (message->Type != TrackerMessage_None);
00164 }
00165 
00166 
00167 // ***** SensorRangeImpl Implementation
00168 
00169 // Sensor HW only accepts specific maximum range values, used to maximize
00170 // the 16-bit sensor outputs. Use these ramps to specify and report appropriate values.
00171 static const UInt16 AccelRangeRamp[] = { 2, 4, 8, 16 };
00172 static const UInt16 GyroRangeRamp[]  = { 250, 500, 1000, 2000 };
00173 static const UInt16 MagRangeRamp[]   = { 880, 1300, 1900, 2500 };
00174 
00175 static UInt16 SelectSensorRampValue(const UInt16* ramp, unsigned count,
00176                                     float val, float factor, const char* label)
00177 {    
00178     UInt16 threshold = (UInt16)(val * factor);
00179 
00180     for (unsigned i = 0; i<count; i++)
00181     {
00182         if (ramp[i] >= threshold)
00183             return ramp[i];
00184     }
00185     OVR_DEBUG_LOG(("SensorDevice::SetRange - %s clamped to %0.4f",
00186                    label, float(ramp[count-1]) / factor));
00187     OVR_UNUSED2(factor, label);
00188     return ramp[count-1];
00189 }
00190 
00191 // SensorScaleImpl provides buffer packing logic for the Sensor Range
00192 // record that can be applied to DK1 sensor through Get/SetFeature. We expose this
00193 // through SensorRange class, which has different units.
00194 struct SensorRangeImpl
00195 {
00196     enum  { PacketSize = 8 };
00197     UByte   Buffer[PacketSize];
00198     
00199     UInt16  CommandId;
00200     UInt16  AccelScale;
00201     UInt16  GyroScale;
00202     UInt16  MagScale;
00203 
00204     SensorRangeImpl(const SensorRange& r, UInt16 commandId = 0)
00205     {
00206         SetSensorRange(r, commandId);
00207     }
00208 
00209     void SetSensorRange(const SensorRange& r, UInt16 commandId = 0)
00210     {
00211         CommandId  = commandId;
00212         AccelScale = SelectSensorRampValue(AccelRangeRamp, sizeof(AccelRangeRamp)/sizeof(AccelRangeRamp[0]),
00213                                            r.MaxAcceleration, (1.0f / 9.81f), "MaxAcceleration");
00214         GyroScale  = SelectSensorRampValue(GyroRangeRamp, sizeof(GyroRangeRamp)/sizeof(GyroRangeRamp[0]),
00215                                            r.MaxRotationRate, Math<float>::RadToDegreeFactor, "MaxRotationRate");
00216         MagScale   = SelectSensorRampValue(MagRangeRamp, sizeof(MagRangeRamp)/sizeof(MagRangeRamp[0]),
00217                                            r.MaxMagneticField, 1000.0f, "MaxMagneticField");
00218         Pack();
00219     }
00220 
00221     void GetSensorRange(SensorRange* r)
00222     {
00223         r->MaxAcceleration = AccelScale * 9.81f;
00224         r->MaxRotationRate = DegreeToRad((float)GyroScale);
00225         r->MaxMagneticField= MagScale * 0.001f;
00226     }
00227 
00228     static SensorRange GetMaxSensorRange()
00229     {
00230         return SensorRange(AccelRangeRamp[sizeof(AccelRangeRamp)/sizeof(AccelRangeRamp[0]) - 1] * 9.81f,
00231                            GyroRangeRamp[sizeof(GyroRangeRamp)/sizeof(GyroRangeRamp[0]) - 1] *
00232                                 Math<float>::DegreeToRadFactor,
00233                            MagRangeRamp[sizeof(MagRangeRamp)/sizeof(MagRangeRamp[0]) - 1] * 0.001f);
00234     }
00235 
00236     void  Pack()
00237     {
00238         Buffer[0] = 4;
00239         Buffer[1] = UByte(CommandId & 0xFF);
00240         Buffer[2] = UByte(CommandId >> 8);
00241         Buffer[3] = UByte(AccelScale);
00242         Buffer[4] = UByte(GyroScale & 0xFF);
00243         Buffer[5] = UByte(GyroScale >> 8);
00244         Buffer[6] = UByte(MagScale & 0xFF);
00245         Buffer[7] = UByte(MagScale >> 8);
00246     }
00247 
00248     void Unpack()
00249     {
00250         CommandId = Buffer[1] | (UInt16(Buffer[2]) << 8);
00251         AccelScale= Buffer[3];
00252         GyroScale = Buffer[4] | (UInt16(Buffer[5]) << 8);
00253         MagScale  = Buffer[6] | (UInt16(Buffer[7]) << 8);
00254     }
00255 };
00256 
00257 
00258 // Sensor configuration command, ReportId == 2.
00259 
00260 struct SensorConfigImpl
00261 {
00262     enum  { PacketSize = 7 };
00263     UByte   Buffer[PacketSize];
00264 
00265     // Flag values for Flags.
00266     enum {
00267         Flag_RawMode            = 0x01,
00268         Flag_CallibrationTest   = 0x02, // Internal test mode
00269         Flag_UseCallibration    = 0x04,
00270         Flag_AutoCallibration   = 0x08,
00271         Flag_MotionKeepAlive    = 0x10,
00272         Flag_CommandKeepAlive   = 0x20,
00273         Flag_SensorCoordinates  = 0x40
00274     };
00275 
00276     UInt16  CommandId;
00277     UByte   Flags;
00278     UInt16  PacketInterval;
00279     UInt16  KeepAliveIntervalMs;
00280 
00281     SensorConfigImpl() : CommandId(0), Flags(0), PacketInterval(0), KeepAliveIntervalMs(0)
00282     {
00283         memset(Buffer, 0, PacketSize);
00284         Buffer[0] = 2;
00285     }
00286 
00287     void    SetSensorCoordinates(bool sensorCoordinates)
00288     { Flags = (Flags & ~Flag_SensorCoordinates) | (sensorCoordinates ? Flag_SensorCoordinates : 0); }
00289     bool    IsUsingSensorCoordinates() const
00290     { return (Flags & Flag_SensorCoordinates) != 0; }
00291 
00292     void Pack()
00293     {
00294         Buffer[0] = 2;
00295         Buffer[1] = UByte(CommandId & 0xFF);
00296         Buffer[2] = UByte(CommandId >> 8);
00297         Buffer[3] = Flags;
00298         Buffer[4] = UByte(PacketInterval);
00299         Buffer[5] = UByte(KeepAliveIntervalMs & 0xFF);
00300         Buffer[6] = UByte(KeepAliveIntervalMs >> 8);
00301     }
00302 
00303     void Unpack()
00304     {
00305         CommandId          = Buffer[1] | (UInt16(Buffer[2]) << 8);
00306         Flags              = Buffer[3];
00307         PacketInterval     = Buffer[4];
00308         KeepAliveIntervalMs= Buffer[5] | (UInt16(Buffer[6]) << 8);
00309     }
00310     
00311 };
00312 
00313 
00314 // SensorKeepAlive - feature report that needs to be sent at regular intervals for sensor
00315 // to receive commands.
00316 struct SensorKeepAliveImpl
00317 {
00318     enum  { PacketSize = 5 };
00319     UByte   Buffer[PacketSize];
00320 
00321     UInt16  CommandId;
00322     UInt16  KeepAliveIntervalMs;
00323 
00324     SensorKeepAliveImpl(UInt16 interval = 0, UInt16 commandId = 0)
00325         : CommandId(commandId), KeepAliveIntervalMs(interval)
00326     {
00327         Pack();
00328     }
00329 
00330     void  Pack()
00331     {
00332         Buffer[0] = 8;
00333         Buffer[1] = UByte(CommandId & 0xFF);
00334         Buffer[2] = UByte(CommandId >> 8);
00335         Buffer[3] = UByte(KeepAliveIntervalMs & 0xFF);
00336         Buffer[4] = UByte(KeepAliveIntervalMs >> 8);
00337     }
00338 
00339     void Unpack()
00340     {
00341         CommandId          = Buffer[1] | (UInt16(Buffer[2]) << 8);
00342         KeepAliveIntervalMs= Buffer[3] | (UInt16(Buffer[4]) << 8);
00343     }
00344 };
00345 
00346 
00347 //-------------------------------------------------------------------------------------
00348 // ***** SensorDisplayInfoImpl
00349 SensorDisplayInfoImpl::SensorDisplayInfoImpl()
00350  :  CommandId(0), DistortionType(Base_None)
00351 {
00352     memset(Buffer, 0, PacketSize);
00353     Buffer[0] = 9;
00354 }
00355 
00356 void SensorDisplayInfoImpl::Unpack()
00357 {
00358     CommandId               = Buffer[1] | (UInt16(Buffer[2]) << 8);
00359     DistortionType          = Buffer[3];
00360     HResolution             = DecodeUInt16(Buffer+4);
00361     VResolution             = DecodeUInt16(Buffer+6);
00362     HScreenSize             = DecodeUInt32(Buffer+8) *  (1/1000000.f);
00363     VScreenSize             = DecodeUInt32(Buffer+12) * (1/1000000.f);
00364     VCenter                 = DecodeUInt32(Buffer+16) * (1/1000000.f);
00365     LensSeparation          = DecodeUInt32(Buffer+20) * (1/1000000.f);
00366     EyeToScreenDistance[0]  = DecodeUInt32(Buffer+24) * (1/1000000.f);
00367     EyeToScreenDistance[1]  = DecodeUInt32(Buffer+28) * (1/1000000.f);
00368     DistortionK[0]          = DecodeFloat(Buffer+32);
00369     DistortionK[1]          = DecodeFloat(Buffer+36);
00370     DistortionK[2]          = DecodeFloat(Buffer+40);
00371     DistortionK[3]          = DecodeFloat(Buffer+44);
00372     DistortionK[4]          = DecodeFloat(Buffer+48);
00373     DistortionK[5]          = DecodeFloat(Buffer+52);
00374 }
00375 
00376 
00377 //-------------------------------------------------------------------------------------
00378 // ***** SensorDeviceFactory
00379 
00380 SensorDeviceFactory SensorDeviceFactory::Instance;
00381 
00382 void SensorDeviceFactory::EnumerateDevices(EnumerateVisitor& visitor)
00383 {
00384 
00385     class SensorEnumerator : public HIDEnumerateVisitor
00386     {
00387         // Assign not supported; suppress MSVC warning.
00388         void operator = (const SensorEnumerator&) { }
00389 
00390         DeviceFactory*     pFactory;
00391         EnumerateVisitor&  ExternalVisitor;   
00392     public:
00393         SensorEnumerator(DeviceFactory* factory, EnumerateVisitor& externalVisitor)
00394             : pFactory(factory), ExternalVisitor(externalVisitor) { }
00395 
00396         virtual bool MatchVendorProduct(UInt16 vendorId, UInt16 productId)
00397         {
00398             return pFactory->MatchVendorProduct(vendorId, productId);
00399         }
00400 
00401         virtual void Visit(HIDDevice& device, const HIDDeviceDesc& desc)
00402         {
00403             SensorDeviceCreateDesc createDesc(pFactory, desc);
00404             ExternalVisitor.Visit(createDesc);
00405 
00406             // Check if the sensor returns DisplayInfo. If so, try to use it to override potentially
00407             // mismatching monitor information (in case wrong EDID is reported by splitter),
00408             // or to create a new "virtualized" HMD Device.
00409             
00410             SensorDisplayInfoImpl displayInfo;
00411 
00412             if (device.GetFeatureReport(displayInfo.Buffer, SensorDisplayInfoImpl::PacketSize))
00413             {
00414                 displayInfo.Unpack();
00415 
00416                 // If we got display info, try to match / create HMDDevice as well
00417                 // so that sensor settings give preference.
00418                 if (displayInfo.DistortionType & SensorDisplayInfoImpl::Mask_BaseFmt)
00419                 {
00420                     SensorDeviceImpl::EnumerateHMDFromSensorDisplayInfo(displayInfo, ExternalVisitor);
00421                 }
00422             }
00423         }
00424     };
00425 
00426     //double start = Timer::GetProfileSeconds();
00427 
00428     SensorEnumerator sensorEnumerator(this, visitor);
00429     GetManagerImpl()->GetHIDDeviceManager()->Enumerate(&sensorEnumerator);
00430 
00431     //double totalSeconds = Timer::GetProfileSeconds() - start; 
00432 }
00433 
00434 bool SensorDeviceFactory::MatchVendorProduct(UInt16 vendorId, UInt16 productId) const
00435 {
00436     return ((vendorId == Sensor_VendorId) && (productId == Sensor_ProductId)) ||
00437         ((vendorId == Sensor_OldVendorId) && (productId == Sensor_OldProductId));
00438 }
00439 
00440 bool SensorDeviceFactory::DetectHIDDevice(DeviceManager* pdevMgr, const HIDDeviceDesc& desc)
00441 {
00442     if (MatchVendorProduct(desc.VendorId, desc.ProductId))
00443     {
00444         SensorDeviceCreateDesc createDesc(this, desc);
00445         return pdevMgr->AddDevice_NeedsLock(createDesc).GetPtr() != NULL;
00446     }
00447     return false;
00448 }
00449 
00450 //-------------------------------------------------------------------------------------
00451 // ***** SensorDeviceCreateDesc
00452 
00453 DeviceBase* SensorDeviceCreateDesc::NewDeviceInstance()
00454 {
00455     return new SensorDeviceImpl(this);
00456 }
00457 
00458 bool SensorDeviceCreateDesc::GetDeviceInfo(DeviceInfo* info) const
00459 {
00460     if ((info->InfoClassType != Device_Sensor) &&
00461         (info->InfoClassType != Device_None))
00462         return false;
00463 
00464     OVR_strcpy(info->ProductName,  DeviceInfo::MaxNameLength, HIDDesc.Product.ToCStr());
00465     OVR_strcpy(info->Manufacturer, DeviceInfo::MaxNameLength, HIDDesc.Manufacturer.ToCStr());
00466     info->Type    = Device_Sensor;
00467     info->Version = 0;
00468 
00469     if (info->InfoClassType == Device_Sensor)
00470     {
00471         SensorInfo* sinfo = (SensorInfo*)info;
00472         sinfo->VendorId  = HIDDesc.VendorId;
00473         sinfo->ProductId = HIDDesc.ProductId;
00474         sinfo->MaxRanges = SensorRangeImpl::GetMaxSensorRange();
00475         OVR_strcpy(sinfo->SerialNumber, sizeof(sinfo->SerialNumber),HIDDesc.SerialNumber.ToCStr());
00476     }
00477     return true;
00478 }
00479 
00480 
00481 //-------------------------------------------------------------------------------------
00482 // ***** SensorDevice
00483 
00484 SensorDeviceImpl::SensorDeviceImpl(SensorDeviceCreateDesc* createDesc)
00485     : OVR::HIDDeviceImpl<OVR::SensorDevice>(createDesc, 0),
00486       Coordinates(SensorDevice::Coord_Sensor),
00487       HWCoordinates(SensorDevice::Coord_HMD), // HW reports HMD coordinates by default.
00488       NextKeepAliveTicks(0),
00489       MaxValidRange(SensorRangeImpl::GetMaxSensorRange())
00490 {
00491     SequenceValid  = false;
00492     LastSampleCount= 0;
00493     LastTimestamp   = 0;
00494 
00495     OldCommandId = 0;
00496 }
00497 
00498 SensorDeviceImpl::~SensorDeviceImpl()
00499 {
00500     // Check that Shutdown() was called.
00501     OVR_ASSERT(!pCreateDesc->pDevice);    
00502 }
00503 
00504 // Internal creation APIs.
00505 bool SensorDeviceImpl::Initialize(DeviceBase* parent)
00506 {
00507     if (HIDDeviceImpl<OVR::SensorDevice>::Initialize(parent))
00508     {
00509         openDevice();
00510 
00511         LogText("OVR::SensorDevice initialized.\n");
00512 
00513         return true;
00514     }
00515 
00516     return false;
00517 }
00518 
00519 void SensorDeviceImpl::openDevice()
00520 {
00521 
00522     // Read the currently configured range from sensor.
00523     SensorRangeImpl sr(SensorRange(), 0);
00524 
00525     if (GetInternalDevice()->GetFeatureReport(sr.Buffer, SensorRangeImpl::PacketSize))
00526     {
00527         sr.Unpack();
00528         sr.GetSensorRange(&CurrentRange);
00529     }
00530 
00531 
00532     // If the sensor has "DisplayInfo" data, use HMD coordinate frame by default.
00533     SensorDisplayInfoImpl displayInfo;
00534     if (GetInternalDevice()->GetFeatureReport(displayInfo.Buffer, SensorDisplayInfoImpl::PacketSize))
00535     {
00536         displayInfo.Unpack();
00537         Coordinates = (displayInfo.DistortionType & SensorDisplayInfoImpl::Mask_BaseFmt) ?
00538                       Coord_HMD : Coord_Sensor;
00539     }
00540 
00541     // Read/Apply sensor config.
00542     setCoordinateFrame(Coordinates);
00543     setReportRate(Sensor_DefaultReportRate);
00544 
00545     // Set Keep-alive at 10 seconds.
00546     SensorKeepAliveImpl skeepAlive(10 * 1000);
00547     GetInternalDevice()->SetFeatureReport(skeepAlive.Buffer, SensorKeepAliveImpl::PacketSize);
00548 }
00549 
00550 void SensorDeviceImpl::closeDeviceOnError()
00551 {
00552     LogText("OVR::SensorDevice - Lost connection to '%s'\n", getHIDDesc()->Path.ToCStr());
00553     NextKeepAliveTicks = 0;
00554 }
00555 
00556 void SensorDeviceImpl::Shutdown()
00557 {   
00558     HIDDeviceImpl<OVR::SensorDevice>::Shutdown();
00559 
00560     LogText("OVR::SensorDevice - Closed '%s'\n", getHIDDesc()->Path.ToCStr());
00561 }
00562 
00563 
00564 void SensorDeviceImpl::OnInputReport(UByte* pData, UInt32 length)
00565 {
00566 
00567     bool processed = false;
00568     if (!processed)
00569     {
00570 
00571         TrackerMessage message;
00572         if (DecodeTrackerMessage(&message, pData, length))
00573         {
00574             processed = true;
00575             onTrackerMessage(&message);
00576         }
00577     }
00578 }
00579 
00580 UInt64 SensorDeviceImpl::OnTicks(UInt64 ticksMks)
00581 {
00582 
00583     if (ticksMks >= NextKeepAliveTicks)
00584     {
00585         // Use 3-seconds keep alive by default.
00586         UInt64 keepAliveDelta = Timer::MksPerSecond * 3;
00587 
00588         // Set Keep-alive at 10 seconds.
00589         SensorKeepAliveImpl skeepAlive(10 * 1000);
00590         // OnTicks is called from background thread so we don't need to add this to the command queue.
00591         GetInternalDevice()->SetFeatureReport(skeepAlive.Buffer, SensorKeepAliveImpl::PacketSize);
00592 
00593                 // Emit keep-alive every few seconds.
00594         NextKeepAliveTicks = ticksMks + keepAliveDelta;
00595     }
00596     return NextKeepAliveTicks - ticksMks;
00597 }
00598 
00599 bool SensorDeviceImpl::SetRange(const SensorRange& range, bool waitFlag)
00600 {
00601     bool                 result = 0;
00602     ThreadCommandQueue * threadQueue = GetManagerImpl()->GetThreadQueue();
00603 
00604     if (!waitFlag)
00605     {
00606         return threadQueue->PushCall(this, &SensorDeviceImpl::setRange, range);
00607     }
00608     
00609     if (!threadQueue->PushCallAndWaitResult(this, 
00610                                             &SensorDeviceImpl::setRange,
00611                                             &result, 
00612                                             range))
00613     {
00614         return false;
00615     }
00616 
00617     return result;
00618 }
00619 
00620 void SensorDeviceImpl::GetRange(SensorRange* range) const
00621 {
00622     Lock::Locker lockScope(GetLock());
00623     *range = CurrentRange;
00624 }
00625 
00626 bool SensorDeviceImpl::setRange(const SensorRange& range)
00627 {
00628     SensorRangeImpl sr(range);
00629     
00630     if (GetInternalDevice()->SetFeatureReport(sr.Buffer, SensorRangeImpl::PacketSize))
00631     {
00632         Lock::Locker lockScope(GetLock());
00633         sr.GetSensorRange(&CurrentRange);
00634         return true;
00635     }
00636     
00637     return false;
00638 }
00639 
00640 void SensorDeviceImpl::SetCoordinateFrame(CoordinateFrame coordframe)
00641 { 
00642     // Push call with wait.
00643     GetManagerImpl()->GetThreadQueue()->
00644         PushCall(this, &SensorDeviceImpl::setCoordinateFrame, coordframe, true);
00645 }
00646 
00647 SensorDevice::CoordinateFrame SensorDeviceImpl::GetCoordinateFrame() const
00648 {
00649     return Coordinates;
00650 }
00651 
00652 Void SensorDeviceImpl::setCoordinateFrame(CoordinateFrame coordframe)
00653 {
00654 
00655     Coordinates = coordframe;
00656 
00657     // Read the original coordinate frame, then try to change it.
00658     SensorConfigImpl scfg;
00659     if (GetInternalDevice()->GetFeatureReport(scfg.Buffer, SensorConfigImpl::PacketSize))
00660     {
00661         scfg.Unpack();
00662     }
00663 
00664     scfg.SetSensorCoordinates(coordframe == Coord_Sensor);
00665     scfg.Pack();
00666 
00667     GetInternalDevice()->SetFeatureReport(scfg.Buffer, SensorConfigImpl::PacketSize);
00668     
00669     // Re-read the state, in case of older firmware that doesn't support Sensor coordinates.
00670     if (GetInternalDevice()->GetFeatureReport(scfg.Buffer, SensorConfigImpl::PacketSize))
00671     {
00672         scfg.Unpack();
00673         HWCoordinates = scfg.IsUsingSensorCoordinates() ? Coord_Sensor : Coord_HMD;
00674     }
00675     else
00676     {
00677         HWCoordinates = Coord_HMD;
00678     }
00679     return 0;
00680 }
00681 
00682 void SensorDeviceImpl::SetReportRate(unsigned rateHz)
00683 { 
00684     // Push call with wait.
00685     GetManagerImpl()->GetThreadQueue()->
00686         PushCall(this, &SensorDeviceImpl::setReportRate, rateHz, true);
00687 }
00688 
00689 unsigned SensorDeviceImpl::GetReportRate() const
00690 {
00691     // Read the original configuration
00692     SensorConfigImpl scfg;
00693     if (GetInternalDevice()->GetFeatureReport(scfg.Buffer, SensorConfigImpl::PacketSize))
00694     {
00695         scfg.Unpack();
00696         return Sensor_MaxReportRate / (scfg.PacketInterval + 1);
00697     }
00698     return 0; // error
00699 }
00700 
00701 Void SensorDeviceImpl::setReportRate(unsigned rateHz)
00702 {
00703     // Read the original configuration
00704     SensorConfigImpl scfg;
00705     if (GetInternalDevice()->GetFeatureReport(scfg.Buffer, SensorConfigImpl::PacketSize))
00706     {
00707         scfg.Unpack();
00708     }
00709 
00710     if (rateHz > Sensor_MaxReportRate)
00711         rateHz = Sensor_MaxReportRate;
00712     else if (rateHz == 0)
00713         rateHz = Sensor_DefaultReportRate;
00714 
00715     scfg.PacketInterval = UInt16((Sensor_MaxReportRate / rateHz) - 1);
00716 
00717     scfg.Pack();
00718 
00719     GetInternalDevice()->SetFeatureReport(scfg.Buffer, SensorConfigImpl::PacketSize);
00720     return 0;
00721 }
00722 
00723 void SensorDeviceImpl::SetMessageHandler(MessageHandler* handler)
00724 {
00725     if (handler)
00726     {
00727         SequenceValid = false;
00728         DeviceBase::SetMessageHandler(handler);
00729     }
00730     else
00731     {       
00732         DeviceBase::SetMessageHandler(handler);
00733     }    
00734 }
00735 
00736 // Sensor reports data in the following coordinate system:
00737 // Accelerometer: 10^-4 m/s^2; X forward, Y right, Z Down.
00738 // Gyro:          10^-4 rad/s; X positive roll right, Y positive pitch up; Z positive yaw right.
00739 
00740 
00741 // We need to convert it to the following RHS coordinate system:
00742 // X right, Y Up, Z Back (out of screen)
00743 //
00744 Vector3f AccelFromBodyFrameUpdate(const TrackerSensors& update, UByte sampleNumber,
00745                                   bool convertHMDToSensor = false)
00746 {
00747     const TrackerSample& sample = update.Samples[sampleNumber];
00748     float                ax = (float)sample.AccelX;
00749     float                ay = (float)sample.AccelY;
00750     float                az = (float)sample.AccelZ;
00751 
00752     Vector3f val = convertHMDToSensor ? Vector3f(ax, az, -ay) :  Vector3f(ax, ay, az);
00753     return val * 0.0001f;
00754 }
00755 
00756 
00757 Vector3f MagFromBodyFrameUpdate(const TrackerSensors& update,
00758                                 bool convertHMDToSensor = false)
00759 {   
00760     // Note: Y and Z are swapped in comparison to the Accel.  
00761     // This accounts for DK1 sensor firmware axis swap, which should be undone in future releases.
00762     if (!convertHMDToSensor)
00763     {
00764         return Vector3f( (float)update.MagX,
00765                          (float)update.MagZ,
00766                          (float)update.MagY) * 0.0001f;
00767     }    
00768 
00769     return Vector3f( (float)update.MagX,
00770                      (float)update.MagY,
00771                     -(float)update.MagZ) * 0.0001f;
00772 }
00773 
00774 Vector3f EulerFromBodyFrameUpdate(const TrackerSensors& update, UByte sampleNumber,
00775                                   bool convertHMDToSensor = false)
00776 {
00777     const TrackerSample& sample = update.Samples[sampleNumber];
00778     float                gx = (float)sample.GyroX;
00779     float                gy = (float)sample.GyroY;
00780     float                gz = (float)sample.GyroZ;
00781 
00782     Vector3f val = convertHMDToSensor ? Vector3f(gx, gz, -gy) :  Vector3f(gx, gy, gz);
00783     return val * 0.0001f;
00784 }
00785 
00786 
00787 void SensorDeviceImpl::onTrackerMessage(TrackerMessage* message)
00788 {
00789     if (message->Type != TrackerMessage_Sensors)
00790         return;
00791     
00792     const float     timeUnit   = (1.0f / 1000.f);
00793     TrackerSensors& s = message->Sensors;
00794     
00795 
00796     // Call OnMessage() within a lock to avoid conflicts with handlers.
00797     Lock::Locker scopeLock(HandlerRef.GetLock());
00798 
00799 
00800     if (SequenceValid)
00801     {
00802         unsigned timestampDelta;
00803 
00804         if (s.Timestamp < LastTimestamp)
00805             timestampDelta = ((((int)s.Timestamp) + 0x10000) - (int)LastTimestamp);
00806         else
00807             timestampDelta = (s.Timestamp - LastTimestamp);
00808 
00809         // If we missed a small number of samples, replicate the last sample.
00810         if ((timestampDelta > LastSampleCount) && (timestampDelta <= 254))
00811         {
00812             if (HandlerRef.GetHandler())
00813             {
00814                 MessageBodyFrame sensors(this);
00815                 sensors.TimeDelta     = (timestampDelta - LastSampleCount) * timeUnit;
00816                 sensors.Acceleration  = LastAcceleration;
00817                 sensors.RotationRate  = LastRotationRate;
00818                 sensors.MagneticField = LastMagneticField;
00819                 sensors.Temperature   = LastTemperature;
00820 
00821                 HandlerRef.GetHandler()->OnMessage(sensors);
00822             }
00823         }
00824     }
00825     else
00826     {
00827         LastAcceleration = Vector3f(0);
00828         LastRotationRate = Vector3f(0);
00829         LastMagneticField= Vector3f(0);
00830         LastTemperature  = 0;
00831         SequenceValid    = true;
00832     }
00833 
00834     LastSampleCount = s.SampleCount;
00835     LastTimestamp   = s.Timestamp;
00836 
00837     bool convertHMDToSensor = (Coordinates == Coord_Sensor) && (HWCoordinates == Coord_HMD);
00838 
00839     if (HandlerRef.GetHandler())
00840     {
00841         MessageBodyFrame sensors(this);                
00842         UByte            iterations = s.SampleCount;
00843 
00844         if (s.SampleCount > 3)
00845         {
00846             iterations        = 3;
00847             sensors.TimeDelta = (s.SampleCount - 2) * timeUnit;
00848         }
00849         else
00850         {
00851             sensors.TimeDelta = timeUnit;
00852         }
00853 
00854         for (UByte i = 0; i < iterations; i++)
00855         {            
00856             sensors.Acceleration = AccelFromBodyFrameUpdate(s, i, convertHMDToSensor);
00857             sensors.RotationRate = EulerFromBodyFrameUpdate(s, i, convertHMDToSensor);
00858             sensors.MagneticField= MagFromBodyFrameUpdate(s, convertHMDToSensor);
00859             sensors.Temperature  = s.Temperature * 0.01f;
00860             HandlerRef.GetHandler()->OnMessage(sensors);
00861             // TimeDelta for the last two sample is always fixed.
00862             sensors.TimeDelta = timeUnit;
00863         }
00864 
00865         LastAcceleration = sensors.Acceleration;
00866         LastRotationRate = sensors.RotationRate;
00867         LastMagneticField= sensors.MagneticField;
00868         LastTemperature  = sensors.Temperature;
00869     }
00870     else
00871     {
00872         UByte i = (s.SampleCount > 3) ? 2 : (s.SampleCount - 1);
00873         LastAcceleration  = AccelFromBodyFrameUpdate(s, i, convertHMDToSensor);
00874         LastRotationRate  = EulerFromBodyFrameUpdate(s, i, convertHMDToSensor);
00875         LastMagneticField = MagFromBodyFrameUpdate(s, convertHMDToSensor);
00876         LastTemperature   = s.Temperature * 0.01f;
00877     }
00878 }
00879 
00880 } // namespace OVR
00881 
00882 


oculus_sdk
Author(s):
autogenerated on Mon Oct 6 2014 03:01:19