00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016 #include "OVR_SensorImpl.h"
00017
00018
00019
00020 #include "Kernel/OVR_Timer.h"
00021
00022 namespace OVR {
00023
00024
00025
00026
00027 enum {
00028 Sensor_VendorId = Oculus_VendorId,
00029 Sensor_ProductId = 0x0001,
00030
00031
00032 Sensor_OldVendorId = 0x0483,
00033 Sensor_OldProductId = 0x5750,
00034
00035 Sensor_DefaultReportRate = 500,
00036 Sensor_MaxReportRate = 1000
00037 };
00038
00039
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
00070
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
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
00117
00118
00119
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
00168
00169
00170
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
00192
00193
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
00259
00260 struct SensorConfigImpl
00261 {
00262 enum { PacketSize = 7 };
00263 UByte Buffer[PacketSize];
00264
00265
00266 enum {
00267 Flag_RawMode = 0x01,
00268 Flag_CallibrationTest = 0x02,
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
00315
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
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
00379
00380 SensorDeviceFactory SensorDeviceFactory::Instance;
00381
00382 void SensorDeviceFactory::EnumerateDevices(EnumerateVisitor& visitor)
00383 {
00384
00385 class SensorEnumerator : public HIDEnumerateVisitor
00386 {
00387
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
00407
00408
00409
00410 SensorDisplayInfoImpl displayInfo;
00411
00412 if (device.GetFeatureReport(displayInfo.Buffer, SensorDisplayInfoImpl::PacketSize))
00413 {
00414 displayInfo.Unpack();
00415
00416
00417
00418 if (displayInfo.DistortionType & SensorDisplayInfoImpl::Mask_BaseFmt)
00419 {
00420 SensorDeviceImpl::EnumerateHMDFromSensorDisplayInfo(displayInfo, ExternalVisitor);
00421 }
00422 }
00423 }
00424 };
00425
00426
00427
00428 SensorEnumerator sensorEnumerator(this, visitor);
00429 GetManagerImpl()->GetHIDDeviceManager()->Enumerate(&sensorEnumerator);
00430
00431
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
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
00483
00484 SensorDeviceImpl::SensorDeviceImpl(SensorDeviceCreateDesc* createDesc)
00485 : OVR::HIDDeviceImpl<OVR::SensorDevice>(createDesc, 0),
00486 Coordinates(SensorDevice::Coord_Sensor),
00487 HWCoordinates(SensorDevice::Coord_HMD),
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
00501 OVR_ASSERT(!pCreateDesc->pDevice);
00502 }
00503
00504
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
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
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
00542 setCoordinateFrame(Coordinates);
00543 setReportRate(Sensor_DefaultReportRate);
00544
00545
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
00586 UInt64 keepAliveDelta = Timer::MksPerSecond * 3;
00587
00588
00589 SensorKeepAliveImpl skeepAlive(10 * 1000);
00590
00591 GetInternalDevice()->SetFeatureReport(skeepAlive.Buffer, SensorKeepAliveImpl::PacketSize);
00592
00593
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
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
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
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
00685 GetManagerImpl()->GetThreadQueue()->
00686 PushCall(this, &SensorDeviceImpl::setReportRate, rateHz, true);
00687 }
00688
00689 unsigned SensorDeviceImpl::GetReportRate() const
00690 {
00691
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;
00699 }
00700
00701 Void SensorDeviceImpl::setReportRate(unsigned rateHz)
00702 {
00703
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
00737
00738
00739
00740
00741
00742
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
00761
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
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
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
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 }
00881
00882