oculus1

annotate libovr/Src/OVR_SensorImpl.cpp @ 24:8419d8a13cee

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