oculus1
view libovr/Src/OVR_SensorImpl.cpp @ 1:e2f9e4603129
added LibOVR and started a simple vr wrapper.
| author | John Tsiombikas <nuclear@member.fsf.org> |
|---|---|
| date | Sat, 14 Sep 2013 16:14:59 +0300 |
| parents | |
| children |
line source
1 /************************************************************************************
3 Filename : OVR_SensorImpl.cpp
4 Content : Oculus Sensor device implementation.
5 Created : March 7, 2013
6 Authors : Lee Cooper
8 Copyright : Copyright 2013 Oculus VR, Inc. All Rights reserved.
10 Use of this software is subject to the terms of the Oculus license
11 agreement provided at the time of installation or download, or which
12 otherwise accompanies this software in either electronic or hard copy form.
14 *************************************************************************************/
16 #include "OVR_SensorImpl.h"
18 // HMDDeviceDesc can be created/updated through Sensor carrying DisplayInfo.
20 #include "Kernel/OVR_Timer.h"
22 namespace OVR {
24 //-------------------------------------------------------------------------------------
25 // ***** Oculus Sensor-specific packet data structures
27 enum {
28 Sensor_VendorId = Oculus_VendorId,
29 Sensor_ProductId = 0x0001,
31 // ST's VID used originally; should be removed in the future
32 Sensor_OldVendorId = 0x0483,
33 Sensor_OldProductId = 0x5750,
35 Sensor_DefaultReportRate = 500, // Hz
36 Sensor_MaxReportRate = 1000 // Hz
37 };
39 // Reported data is little-endian now
40 static UInt16 DecodeUInt16(const UByte* buffer)
41 {
42 return (UInt16(buffer[1]) << 8) | UInt16(buffer[0]);
43 }
45 static SInt16 DecodeSInt16(const UByte* buffer)
46 {
47 return (SInt16(buffer[1]) << 8) | SInt16(buffer[0]);
48 }
50 static UInt32 DecodeUInt32(const UByte* buffer)
51 {
52 return (buffer[0]) | UInt32(buffer[1] << 8) | UInt32(buffer[2] << 16) | UInt32(buffer[3] << 24);
53 }
55 static float DecodeFloat(const UByte* buffer)
56 {
57 union {
58 UInt32 U;
59 float F;
60 };
62 U = DecodeUInt32(buffer);
63 return F;
64 }
67 static void UnpackSensor(const UByte* buffer, SInt32* x, SInt32* y, SInt32* z)
68 {
69 // Sign extending trick
70 // from http://graphics.stanford.edu/~seander/bithacks.html#FixedSignExtend
71 struct {SInt32 x:21;} s;
73 *x = s.x = (buffer[0] << 13) | (buffer[1] << 5) | ((buffer[2] & 0xF8) >> 3);
74 *y = s.x = ((buffer[2] & 0x07) << 18) | (buffer[3] << 10) | (buffer[4] << 2) |
75 ((buffer[5] & 0xC0) >> 6);
76 *z = s.x = ((buffer[5] & 0x3F) << 15) | (buffer[6] << 7) | (buffer[7] >> 1);
77 }
79 // Messages we care for
80 enum TrackerMessageType
81 {
82 TrackerMessage_None = 0,
83 TrackerMessage_Sensors = 1,
84 TrackerMessage_Unknown = 0x100,
85 TrackerMessage_SizeError = 0x101,
86 };
88 struct TrackerSample
89 {
90 SInt32 AccelX, AccelY, AccelZ;
91 SInt32 GyroX, GyroY, GyroZ;
92 };
95 struct TrackerSensors
96 {
97 UByte SampleCount;
98 UInt16 Timestamp;
99 UInt16 LastCommandID;
100 SInt16 Temperature;
102 TrackerSample Samples[3];
104 SInt16 MagX, MagY, MagZ;
106 TrackerMessageType Decode(const UByte* buffer, int size)
107 {
108 if (size < 62)
109 return TrackerMessage_SizeError;
111 SampleCount = buffer[1];
112 Timestamp = DecodeUInt16(buffer + 2);
113 LastCommandID = DecodeUInt16(buffer + 4);
114 Temperature = DecodeSInt16(buffer + 6);
116 //if (SampleCount > 2)
117 // OVR_DEBUG_LOG_TEXT(("TackerSensor::Decode SampleCount=%d\n", SampleCount));
119 // Only unpack as many samples as there actually are
120 UByte iterationCount = (SampleCount > 2) ? 3 : SampleCount;
122 for (UByte i = 0; i < iterationCount; i++)
123 {
124 UnpackSensor(buffer + 8 + 16 * i, &Samples[i].AccelX, &Samples[i].AccelY, &Samples[i].AccelZ);
125 UnpackSensor(buffer + 16 + 16 * i, &Samples[i].GyroX, &Samples[i].GyroY, &Samples[i].GyroZ);
126 }
128 MagX = DecodeSInt16(buffer + 56);
129 MagY = DecodeSInt16(buffer + 58);
130 MagZ = DecodeSInt16(buffer + 60);
132 return TrackerMessage_Sensors;
133 }
134 };
136 struct TrackerMessage
137 {
138 TrackerMessageType Type;
139 TrackerSensors Sensors;
140 };
142 bool DecodeTrackerMessage(TrackerMessage* message, UByte* buffer, int size)
143 {
144 memset(message, 0, sizeof(TrackerMessage));
146 if (size < 4)
147 {
148 message->Type = TrackerMessage_SizeError;
149 return false;
150 }
152 switch (buffer[0])
153 {
154 case TrackerMessage_Sensors:
155 message->Type = message->Sensors.Decode(buffer, size);
156 break;
158 default:
159 message->Type = TrackerMessage_Unknown;
160 break;
161 }
163 return (message->Type < TrackerMessage_Unknown) && (message->Type != TrackerMessage_None);
164 }
167 // ***** SensorRangeImpl Implementation
169 // Sensor HW only accepts specific maximum range values, used to maximize
170 // the 16-bit sensor outputs. Use these ramps to specify and report appropriate values.
171 static const UInt16 AccelRangeRamp[] = { 2, 4, 8, 16 };
172 static const UInt16 GyroRangeRamp[] = { 250, 500, 1000, 2000 };
173 static const UInt16 MagRangeRamp[] = { 880, 1300, 1900, 2500 };
175 static UInt16 SelectSensorRampValue(const UInt16* ramp, unsigned count,
176 float val, float factor, const char* label)
177 {
178 UInt16 threshold = (UInt16)(val * factor);
180 for (unsigned i = 0; i<count; i++)
181 {
182 if (ramp[i] >= threshold)
183 return ramp[i];
184 }
185 OVR_DEBUG_LOG(("SensorDevice::SetRange - %s clamped to %0.4f",
186 label, float(ramp[count-1]) / factor));
187 OVR_UNUSED2(factor, label);
188 return ramp[count-1];
189 }
191 // SensorScaleImpl provides buffer packing logic for the Sensor Range
192 // record that can be applied to DK1 sensor through Get/SetFeature. We expose this
193 // through SensorRange class, which has different units.
194 struct SensorRangeImpl
195 {
196 enum { PacketSize = 8 };
197 UByte Buffer[PacketSize];
199 UInt16 CommandId;
200 UInt16 AccelScale;
201 UInt16 GyroScale;
202 UInt16 MagScale;
204 SensorRangeImpl(const SensorRange& r, UInt16 commandId = 0)
205 {
206 SetSensorRange(r, commandId);
207 }
209 void SetSensorRange(const SensorRange& r, UInt16 commandId = 0)
210 {
211 CommandId = commandId;
212 AccelScale = SelectSensorRampValue(AccelRangeRamp, sizeof(AccelRangeRamp)/sizeof(AccelRangeRamp[0]),
213 r.MaxAcceleration, (1.0f / 9.81f), "MaxAcceleration");
214 GyroScale = SelectSensorRampValue(GyroRangeRamp, sizeof(GyroRangeRamp)/sizeof(GyroRangeRamp[0]),
215 r.MaxRotationRate, Math<float>::RadToDegreeFactor, "MaxRotationRate");
216 MagScale = SelectSensorRampValue(MagRangeRamp, sizeof(MagRangeRamp)/sizeof(MagRangeRamp[0]),
217 r.MaxMagneticField, 1000.0f, "MaxMagneticField");
218 Pack();
219 }
221 void GetSensorRange(SensorRange* r)
222 {
223 r->MaxAcceleration = AccelScale * 9.81f;
224 r->MaxRotationRate = DegreeToRad((float)GyroScale);
225 r->MaxMagneticField= MagScale * 0.001f;
226 }
228 static SensorRange GetMaxSensorRange()
229 {
230 return SensorRange(AccelRangeRamp[sizeof(AccelRangeRamp)/sizeof(AccelRangeRamp[0]) - 1] * 9.81f,
231 GyroRangeRamp[sizeof(GyroRangeRamp)/sizeof(GyroRangeRamp[0]) - 1] *
232 Math<float>::DegreeToRadFactor,
233 MagRangeRamp[sizeof(MagRangeRamp)/sizeof(MagRangeRamp[0]) - 1] * 0.001f);
234 }
236 void Pack()
237 {
238 Buffer[0] = 4;
239 Buffer[1] = UByte(CommandId & 0xFF);
240 Buffer[2] = UByte(CommandId >> 8);
241 Buffer[3] = UByte(AccelScale);
242 Buffer[4] = UByte(GyroScale & 0xFF);
243 Buffer[5] = UByte(GyroScale >> 8);
244 Buffer[6] = UByte(MagScale & 0xFF);
245 Buffer[7] = UByte(MagScale >> 8);
246 }
248 void Unpack()
249 {
250 CommandId = Buffer[1] | (UInt16(Buffer[2]) << 8);
251 AccelScale= Buffer[3];
252 GyroScale = Buffer[4] | (UInt16(Buffer[5]) << 8);
253 MagScale = Buffer[6] | (UInt16(Buffer[7]) << 8);
254 }
255 };
258 // Sensor configuration command, ReportId == 2.
260 struct SensorConfigImpl
261 {
262 enum { PacketSize = 7 };
263 UByte Buffer[PacketSize];
265 // Flag values for Flags.
266 enum {
267 Flag_RawMode = 0x01,
268 Flag_CallibrationTest = 0x02, // Internal test mode
269 Flag_UseCallibration = 0x04,
270 Flag_AutoCallibration = 0x08,
271 Flag_MotionKeepAlive = 0x10,
272 Flag_CommandKeepAlive = 0x20,
273 Flag_SensorCoordinates = 0x40
274 };
276 UInt16 CommandId;
277 UByte Flags;
278 UInt16 PacketInterval;
279 UInt16 KeepAliveIntervalMs;
281 SensorConfigImpl() : CommandId(0), Flags(0), PacketInterval(0), KeepAliveIntervalMs(0)
282 {
283 memset(Buffer, 0, PacketSize);
284 Buffer[0] = 2;
285 }
287 void SetSensorCoordinates(bool sensorCoordinates)
288 { Flags = (Flags & ~Flag_SensorCoordinates) | (sensorCoordinates ? Flag_SensorCoordinates : 0); }
289 bool IsUsingSensorCoordinates() const
290 { return (Flags & Flag_SensorCoordinates) != 0; }
292 void Pack()
293 {
294 Buffer[0] = 2;
295 Buffer[1] = UByte(CommandId & 0xFF);
296 Buffer[2] = UByte(CommandId >> 8);
297 Buffer[3] = Flags;
298 Buffer[4] = UByte(PacketInterval);
299 Buffer[5] = UByte(KeepAliveIntervalMs & 0xFF);
300 Buffer[6] = UByte(KeepAliveIntervalMs >> 8);
301 }
303 void Unpack()
304 {
305 CommandId = Buffer[1] | (UInt16(Buffer[2]) << 8);
306 Flags = Buffer[3];
307 PacketInterval = Buffer[4];
308 KeepAliveIntervalMs= Buffer[5] | (UInt16(Buffer[6]) << 8);
309 }
311 };
314 // SensorKeepAlive - feature report that needs to be sent at regular intervals for sensor
315 // to receive commands.
316 struct SensorKeepAliveImpl
317 {
318 enum { PacketSize = 5 };
319 UByte Buffer[PacketSize];
321 UInt16 CommandId;
322 UInt16 KeepAliveIntervalMs;
324 SensorKeepAliveImpl(UInt16 interval = 0, UInt16 commandId = 0)
325 : CommandId(commandId), KeepAliveIntervalMs(interval)
326 {
327 Pack();
328 }
330 void Pack()
331 {
332 Buffer[0] = 8;
333 Buffer[1] = UByte(CommandId & 0xFF);
334 Buffer[2] = UByte(CommandId >> 8);
335 Buffer[3] = UByte(KeepAliveIntervalMs & 0xFF);
336 Buffer[4] = UByte(KeepAliveIntervalMs >> 8);
337 }
339 void Unpack()
340 {
341 CommandId = Buffer[1] | (UInt16(Buffer[2]) << 8);
342 KeepAliveIntervalMs= Buffer[3] | (UInt16(Buffer[4]) << 8);
343 }
344 };
347 //-------------------------------------------------------------------------------------
348 // ***** SensorDisplayInfoImpl
349 SensorDisplayInfoImpl::SensorDisplayInfoImpl()
350 : CommandId(0), DistortionType(Base_None)
351 {
352 memset(Buffer, 0, PacketSize);
353 Buffer[0] = 9;
354 }
356 void SensorDisplayInfoImpl::Unpack()
357 {
358 CommandId = Buffer[1] | (UInt16(Buffer[2]) << 8);
359 DistortionType = Buffer[3];
360 HResolution = DecodeUInt16(Buffer+4);
361 VResolution = DecodeUInt16(Buffer+6);
362 HScreenSize = DecodeUInt32(Buffer+8) * (1/1000000.f);
363 VScreenSize = DecodeUInt32(Buffer+12) * (1/1000000.f);
364 VCenter = DecodeUInt32(Buffer+16) * (1/1000000.f);
365 LensSeparation = DecodeUInt32(Buffer+20) * (1/1000000.f);
366 EyeToScreenDistance[0] = DecodeUInt32(Buffer+24) * (1/1000000.f);
367 EyeToScreenDistance[1] = DecodeUInt32(Buffer+28) * (1/1000000.f);
368 DistortionK[0] = DecodeFloat(Buffer+32);
369 DistortionK[1] = DecodeFloat(Buffer+36);
370 DistortionK[2] = DecodeFloat(Buffer+40);
371 DistortionK[3] = DecodeFloat(Buffer+44);
372 DistortionK[4] = DecodeFloat(Buffer+48);
373 DistortionK[5] = DecodeFloat(Buffer+52);
374 }
377 //-------------------------------------------------------------------------------------
378 // ***** SensorDeviceFactory
380 SensorDeviceFactory SensorDeviceFactory::Instance;
382 void SensorDeviceFactory::EnumerateDevices(EnumerateVisitor& visitor)
383 {
385 class SensorEnumerator : public HIDEnumerateVisitor
386 {
387 // Assign not supported; suppress MSVC warning.
388 void operator = (const SensorEnumerator&) { }
390 DeviceFactory* pFactory;
391 EnumerateVisitor& ExternalVisitor;
392 public:
393 SensorEnumerator(DeviceFactory* factory, EnumerateVisitor& externalVisitor)
394 : pFactory(factory), ExternalVisitor(externalVisitor) { }
396 virtual bool MatchVendorProduct(UInt16 vendorId, UInt16 productId)
397 {
398 return pFactory->MatchVendorProduct(vendorId, productId);
399 }
401 virtual void Visit(HIDDevice& device, const HIDDeviceDesc& desc)
402 {
403 SensorDeviceCreateDesc createDesc(pFactory, desc);
404 ExternalVisitor.Visit(createDesc);
406 // Check if the sensor returns DisplayInfo. If so, try to use it to override potentially
407 // mismatching monitor information (in case wrong EDID is reported by splitter),
408 // or to create a new "virtualized" HMD Device.
410 SensorDisplayInfoImpl displayInfo;
412 if (device.GetFeatureReport(displayInfo.Buffer, SensorDisplayInfoImpl::PacketSize))
413 {
414 displayInfo.Unpack();
416 // If we got display info, try to match / create HMDDevice as well
417 // so that sensor settings give preference.
418 if (displayInfo.DistortionType & SensorDisplayInfoImpl::Mask_BaseFmt)
419 {
420 SensorDeviceImpl::EnumerateHMDFromSensorDisplayInfo(displayInfo, ExternalVisitor);
421 }
422 }
423 }
424 };
426 //double start = Timer::GetProfileSeconds();
428 SensorEnumerator sensorEnumerator(this, visitor);
429 GetManagerImpl()->GetHIDDeviceManager()->Enumerate(&sensorEnumerator);
431 //double totalSeconds = Timer::GetProfileSeconds() - start;
432 }
434 bool SensorDeviceFactory::MatchVendorProduct(UInt16 vendorId, UInt16 productId) const
435 {
436 return ((vendorId == Sensor_VendorId) && (productId == Sensor_ProductId)) ||
437 ((vendorId == Sensor_OldVendorId) && (productId == Sensor_OldProductId));
438 }
440 bool SensorDeviceFactory::DetectHIDDevice(DeviceManager* pdevMgr, const HIDDeviceDesc& desc)
441 {
442 if (MatchVendorProduct(desc.VendorId, desc.ProductId))
443 {
444 SensorDeviceCreateDesc createDesc(this, desc);
445 return pdevMgr->AddDevice_NeedsLock(createDesc).GetPtr() != NULL;
446 }
447 return false;
448 }
450 //-------------------------------------------------------------------------------------
451 // ***** SensorDeviceCreateDesc
453 DeviceBase* SensorDeviceCreateDesc::NewDeviceInstance()
454 {
455 return new SensorDeviceImpl(this);
456 }
458 bool SensorDeviceCreateDesc::GetDeviceInfo(DeviceInfo* info) const
459 {
460 if ((info->InfoClassType != Device_Sensor) &&
461 (info->InfoClassType != Device_None))
462 return false;
464 OVR_strcpy(info->ProductName, DeviceInfo::MaxNameLength, HIDDesc.Product.ToCStr());
465 OVR_strcpy(info->Manufacturer, DeviceInfo::MaxNameLength, HIDDesc.Manufacturer.ToCStr());
466 info->Type = Device_Sensor;
467 info->Version = 0;
469 if (info->InfoClassType == Device_Sensor)
470 {
471 SensorInfo* sinfo = (SensorInfo*)info;
472 sinfo->VendorId = HIDDesc.VendorId;
473 sinfo->ProductId = HIDDesc.ProductId;
474 sinfo->MaxRanges = SensorRangeImpl::GetMaxSensorRange();
475 OVR_strcpy(sinfo->SerialNumber, sizeof(sinfo->SerialNumber),HIDDesc.SerialNumber.ToCStr());
476 }
477 return true;
478 }
481 //-------------------------------------------------------------------------------------
482 // ***** SensorDevice
484 SensorDeviceImpl::SensorDeviceImpl(SensorDeviceCreateDesc* createDesc)
485 : OVR::HIDDeviceImpl<OVR::SensorDevice>(createDesc, 0),
486 Coordinates(SensorDevice::Coord_Sensor),
487 HWCoordinates(SensorDevice::Coord_HMD), // HW reports HMD coordinates by default.
488 NextKeepAliveTicks(0),
489 MaxValidRange(SensorRangeImpl::GetMaxSensorRange())
490 {
491 SequenceValid = false;
492 LastSampleCount= 0;
493 LastTimestamp = 0;
495 OldCommandId = 0;
496 }
498 SensorDeviceImpl::~SensorDeviceImpl()
499 {
500 // Check that Shutdown() was called.
501 OVR_ASSERT(!pCreateDesc->pDevice);
502 }
504 // Internal creation APIs.
505 bool SensorDeviceImpl::Initialize(DeviceBase* parent)
506 {
507 if (HIDDeviceImpl<OVR::SensorDevice>::Initialize(parent))
508 {
509 openDevice();
511 LogText("OVR::SensorDevice initialized.\n");
513 return true;
514 }
516 return false;
517 }
519 void SensorDeviceImpl::openDevice()
520 {
522 // Read the currently configured range from sensor.
523 SensorRangeImpl sr(SensorRange(), 0);
525 if (GetInternalDevice()->GetFeatureReport(sr.Buffer, SensorRangeImpl::PacketSize))
526 {
527 sr.Unpack();
528 sr.GetSensorRange(&CurrentRange);
529 }
532 // If the sensor has "DisplayInfo" data, use HMD coordinate frame by default.
533 SensorDisplayInfoImpl displayInfo;
534 if (GetInternalDevice()->GetFeatureReport(displayInfo.Buffer, SensorDisplayInfoImpl::PacketSize))
535 {
536 displayInfo.Unpack();
537 Coordinates = (displayInfo.DistortionType & SensorDisplayInfoImpl::Mask_BaseFmt) ?
538 Coord_HMD : Coord_Sensor;
539 }
541 // Read/Apply sensor config.
542 setCoordinateFrame(Coordinates);
543 setReportRate(Sensor_DefaultReportRate);
545 // Set Keep-alive at 10 seconds.
546 SensorKeepAliveImpl skeepAlive(10 * 1000);
547 GetInternalDevice()->SetFeatureReport(skeepAlive.Buffer, SensorKeepAliveImpl::PacketSize);
548 }
550 void SensorDeviceImpl::closeDeviceOnError()
551 {
552 LogText("OVR::SensorDevice - Lost connection to '%s'\n", getHIDDesc()->Path.ToCStr());
553 NextKeepAliveTicks = 0;
554 }
556 void SensorDeviceImpl::Shutdown()
557 {
558 HIDDeviceImpl<OVR::SensorDevice>::Shutdown();
560 LogText("OVR::SensorDevice - Closed '%s'\n", getHIDDesc()->Path.ToCStr());
561 }
564 void SensorDeviceImpl::OnInputReport(UByte* pData, UInt32 length)
565 {
567 bool processed = false;
568 if (!processed)
569 {
571 TrackerMessage message;
572 if (DecodeTrackerMessage(&message, pData, length))
573 {
574 processed = true;
575 onTrackerMessage(&message);
576 }
577 }
578 }
580 UInt64 SensorDeviceImpl::OnTicks(UInt64 ticksMks)
581 {
583 if (ticksMks >= NextKeepAliveTicks)
584 {
585 // Use 3-seconds keep alive by default.
586 UInt64 keepAliveDelta = Timer::MksPerSecond * 3;
588 // Set Keep-alive at 10 seconds.
589 SensorKeepAliveImpl skeepAlive(10 * 1000);
590 // OnTicks is called from background thread so we don't need to add this to the command queue.
591 GetInternalDevice()->SetFeatureReport(skeepAlive.Buffer, SensorKeepAliveImpl::PacketSize);
593 // Emit keep-alive every few seconds.
594 NextKeepAliveTicks = ticksMks + keepAliveDelta;
595 }
596 return NextKeepAliveTicks - ticksMks;
597 }
599 bool SensorDeviceImpl::SetRange(const SensorRange& range, bool waitFlag)
600 {
601 bool result = 0;
602 ThreadCommandQueue * threadQueue = GetManagerImpl()->GetThreadQueue();
604 if (!waitFlag)
605 {
606 return threadQueue->PushCall(this, &SensorDeviceImpl::setRange, range);
607 }
609 if (!threadQueue->PushCallAndWaitResult(this,
610 &SensorDeviceImpl::setRange,
611 &result,
612 range))
613 {
614 return false;
615 }
617 return result;
618 }
620 void SensorDeviceImpl::GetRange(SensorRange* range) const
621 {
622 Lock::Locker lockScope(GetLock());
623 *range = CurrentRange;
624 }
626 bool SensorDeviceImpl::setRange(const SensorRange& range)
627 {
628 SensorRangeImpl sr(range);
630 if (GetInternalDevice()->SetFeatureReport(sr.Buffer, SensorRangeImpl::PacketSize))
631 {
632 Lock::Locker lockScope(GetLock());
633 sr.GetSensorRange(&CurrentRange);
634 return true;
635 }
637 return false;
638 }
640 void SensorDeviceImpl::SetCoordinateFrame(CoordinateFrame coordframe)
641 {
642 // Push call with wait.
643 GetManagerImpl()->GetThreadQueue()->
644 PushCall(this, &SensorDeviceImpl::setCoordinateFrame, coordframe, true);
645 }
647 SensorDevice::CoordinateFrame SensorDeviceImpl::GetCoordinateFrame() const
648 {
649 return Coordinates;
650 }
652 Void SensorDeviceImpl::setCoordinateFrame(CoordinateFrame coordframe)
653 {
655 Coordinates = coordframe;
657 // Read the original coordinate frame, then try to change it.
658 SensorConfigImpl scfg;
659 if (GetInternalDevice()->GetFeatureReport(scfg.Buffer, SensorConfigImpl::PacketSize))
660 {
661 scfg.Unpack();
662 }
664 scfg.SetSensorCoordinates(coordframe == Coord_Sensor);
665 scfg.Pack();
667 GetInternalDevice()->SetFeatureReport(scfg.Buffer, SensorConfigImpl::PacketSize);
669 // Re-read the state, in case of older firmware that doesn't support Sensor coordinates.
670 if (GetInternalDevice()->GetFeatureReport(scfg.Buffer, SensorConfigImpl::PacketSize))
671 {
672 scfg.Unpack();
673 HWCoordinates = scfg.IsUsingSensorCoordinates() ? Coord_Sensor : Coord_HMD;
674 }
675 else
676 {
677 HWCoordinates = Coord_HMD;
678 }
679 return 0;
680 }
682 void SensorDeviceImpl::SetReportRate(unsigned rateHz)
683 {
684 // Push call with wait.
685 GetManagerImpl()->GetThreadQueue()->
686 PushCall(this, &SensorDeviceImpl::setReportRate, rateHz, true);
687 }
689 unsigned SensorDeviceImpl::GetReportRate() const
690 {
691 // Read the original configuration
692 SensorConfigImpl scfg;
693 if (GetInternalDevice()->GetFeatureReport(scfg.Buffer, SensorConfigImpl::PacketSize))
694 {
695 scfg.Unpack();
696 return Sensor_MaxReportRate / (scfg.PacketInterval + 1);
697 }
698 return 0; // error
699 }
701 Void SensorDeviceImpl::setReportRate(unsigned rateHz)
702 {
703 // Read the original configuration
704 SensorConfigImpl scfg;
705 if (GetInternalDevice()->GetFeatureReport(scfg.Buffer, SensorConfigImpl::PacketSize))
706 {
707 scfg.Unpack();
708 }
710 if (rateHz > Sensor_MaxReportRate)
711 rateHz = Sensor_MaxReportRate;
712 else if (rateHz == 0)
713 rateHz = Sensor_DefaultReportRate;
715 scfg.PacketInterval = UInt16((Sensor_MaxReportRate / rateHz) - 1);
717 scfg.Pack();
719 GetInternalDevice()->SetFeatureReport(scfg.Buffer, SensorConfigImpl::PacketSize);
720 return 0;
721 }
723 void SensorDeviceImpl::SetMessageHandler(MessageHandler* handler)
724 {
725 if (handler)
726 {
727 SequenceValid = false;
728 DeviceBase::SetMessageHandler(handler);
729 }
730 else
731 {
732 DeviceBase::SetMessageHandler(handler);
733 }
734 }
736 // Sensor reports data in the following coordinate system:
737 // Accelerometer: 10^-4 m/s^2; X forward, Y right, Z Down.
738 // Gyro: 10^-4 rad/s; X positive roll right, Y positive pitch up; Z positive yaw right.
741 // We need to convert it to the following RHS coordinate system:
742 // X right, Y Up, Z Back (out of screen)
743 //
744 Vector3f AccelFromBodyFrameUpdate(const TrackerSensors& update, UByte sampleNumber,
745 bool convertHMDToSensor = false)
746 {
747 const TrackerSample& sample = update.Samples[sampleNumber];
748 float ax = (float)sample.AccelX;
749 float ay = (float)sample.AccelY;
750 float az = (float)sample.AccelZ;
752 Vector3f val = convertHMDToSensor ? Vector3f(ax, az, -ay) : Vector3f(ax, ay, az);
753 return val * 0.0001f;
754 }
757 Vector3f MagFromBodyFrameUpdate(const TrackerSensors& update,
758 bool convertHMDToSensor = false)
759 {
760 // Note: Y and Z are swapped in comparison to the Accel.
761 // This accounts for DK1 sensor firmware axis swap, which should be undone in future releases.
762 if (!convertHMDToSensor)
763 {
764 return Vector3f( (float)update.MagX,
765 (float)update.MagZ,
766 (float)update.MagY) * 0.0001f;
767 }
769 return Vector3f( (float)update.MagX,
770 (float)update.MagY,
771 -(float)update.MagZ) * 0.0001f;
772 }
774 Vector3f EulerFromBodyFrameUpdate(const TrackerSensors& update, UByte sampleNumber,
775 bool convertHMDToSensor = false)
776 {
777 const TrackerSample& sample = update.Samples[sampleNumber];
778 float gx = (float)sample.GyroX;
779 float gy = (float)sample.GyroY;
780 float gz = (float)sample.GyroZ;
782 Vector3f val = convertHMDToSensor ? Vector3f(gx, gz, -gy) : Vector3f(gx, gy, gz);
783 return val * 0.0001f;
784 }
787 void SensorDeviceImpl::onTrackerMessage(TrackerMessage* message)
788 {
789 if (message->Type != TrackerMessage_Sensors)
790 return;
792 const float timeUnit = (1.0f / 1000.f);
793 TrackerSensors& s = message->Sensors;
796 // Call OnMessage() within a lock to avoid conflicts with handlers.
797 Lock::Locker scopeLock(HandlerRef.GetLock());
800 if (SequenceValid)
801 {
802 unsigned timestampDelta;
804 if (s.Timestamp < LastTimestamp)
805 timestampDelta = ((((int)s.Timestamp) + 0x10000) - (int)LastTimestamp);
806 else
807 timestampDelta = (s.Timestamp - LastTimestamp);
809 // If we missed a small number of samples, replicate the last sample.
810 if ((timestampDelta > LastSampleCount) && (timestampDelta <= 254))
811 {
812 if (HandlerRef.GetHandler())
813 {
814 MessageBodyFrame sensors(this);
815 sensors.TimeDelta = (timestampDelta - LastSampleCount) * timeUnit;
816 sensors.Acceleration = LastAcceleration;
817 sensors.RotationRate = LastRotationRate;
818 sensors.MagneticField = LastMagneticField;
819 sensors.Temperature = LastTemperature;
821 HandlerRef.GetHandler()->OnMessage(sensors);
822 }
823 }
824 }
825 else
826 {
827 LastAcceleration = Vector3f(0);
828 LastRotationRate = Vector3f(0);
829 LastMagneticField= Vector3f(0);
830 LastTemperature = 0;
831 SequenceValid = true;
832 }
834 LastSampleCount = s.SampleCount;
835 LastTimestamp = s.Timestamp;
837 bool convertHMDToSensor = (Coordinates == Coord_Sensor) && (HWCoordinates == Coord_HMD);
839 if (HandlerRef.GetHandler())
840 {
841 MessageBodyFrame sensors(this);
842 UByte iterations = s.SampleCount;
844 if (s.SampleCount > 3)
845 {
846 iterations = 3;
847 sensors.TimeDelta = (s.SampleCount - 2) * timeUnit;
848 }
849 else
850 {
851 sensors.TimeDelta = timeUnit;
852 }
854 for (UByte i = 0; i < iterations; i++)
855 {
856 sensors.Acceleration = AccelFromBodyFrameUpdate(s, i, convertHMDToSensor);
857 sensors.RotationRate = EulerFromBodyFrameUpdate(s, i, convertHMDToSensor);
858 sensors.MagneticField= MagFromBodyFrameUpdate(s, convertHMDToSensor);
859 sensors.Temperature = s.Temperature * 0.01f;
860 HandlerRef.GetHandler()->OnMessage(sensors);
861 // TimeDelta for the last two sample is always fixed.
862 sensors.TimeDelta = timeUnit;
863 }
865 LastAcceleration = sensors.Acceleration;
866 LastRotationRate = sensors.RotationRate;
867 LastMagneticField= sensors.MagneticField;
868 LastTemperature = sensors.Temperature;
869 }
870 else
871 {
872 UByte i = (s.SampleCount > 3) ? 2 : (s.SampleCount - 1);
873 LastAcceleration = AccelFromBodyFrameUpdate(s, i, convertHMDToSensor);
874 LastRotationRate = EulerFromBodyFrameUpdate(s, i, convertHMDToSensor);
875 LastMagneticField = MagFromBodyFrameUpdate(s, convertHMDToSensor);
876 LastTemperature = s.Temperature * 0.01f;
877 }
878 }
880 } // namespace OVR