diff options
Diffstat (limited to 'LibOVR/Src/OVR_SensorImpl.cpp')
| -rw-r--r-- | LibOVR/Src/OVR_SensorImpl.cpp | 1764 |
1 files changed, 882 insertions, 882 deletions
diff --git a/LibOVR/Src/OVR_SensorImpl.cpp b/LibOVR/Src/OVR_SensorImpl.cpp index fb322df..ced6541 100644 --- a/LibOVR/Src/OVR_SensorImpl.cpp +++ b/LibOVR/Src/OVR_SensorImpl.cpp @@ -1,882 +1,882 @@ -/************************************************************************************
-
-Filename : OVR_SensorImpl.cpp
-Content : Oculus Sensor device implementation.
-Created : March 7, 2013
-Authors : Lee Cooper
-
-Copyright : Copyright 2013 Oculus VR, Inc. All Rights reserved.
-
-Use of this software is subject to the terms of the Oculus license
-agreement provided at the time of installation or download, or which
-otherwise accompanies this software in either electronic or hard copy form.
-
-*************************************************************************************/
-
-#include "OVR_SensorImpl.h"
-
-// HMDDeviceDesc can be created/updated through Sensor carrying DisplayInfo.
-
-#include "Kernel/OVR_Timer.h"
-
-namespace OVR {
-
-//-------------------------------------------------------------------------------------
-// ***** Oculus Sensor-specific packet data structures
-
-enum {
- Sensor_VendorId = Oculus_VendorId,
- Sensor_ProductId = 0x0001,
-
- // ST's VID used originally; should be removed in the future
- Sensor_OldVendorId = 0x0483,
- Sensor_OldProductId = 0x5750,
-
- Sensor_DefaultReportRate = 500, // Hz
- Sensor_MaxReportRate = 1000 // Hz
-};
-
-// Reported data is little-endian now
-static UInt16 DecodeUInt16(const UByte* buffer)
-{
- return (UInt16(buffer[1]) << 8) | UInt16(buffer[0]);
-}
-
-static SInt16 DecodeSInt16(const UByte* buffer)
-{
- return (SInt16(buffer[1]) << 8) | SInt16(buffer[0]);
-}
-
-static UInt32 DecodeUInt32(const UByte* buffer)
-{
- return (buffer[0]) | UInt32(buffer[1] << 8) | UInt32(buffer[2] << 16) | UInt32(buffer[3] << 24);
-}
-
-static float DecodeFloat(const UByte* buffer)
-{
- union {
- UInt32 U;
- float F;
- };
-
- U = DecodeUInt32(buffer);
- return F;
-}
-
-
-static void UnpackSensor(const UByte* buffer, SInt32* x, SInt32* y, SInt32* z)
-{
- // Sign extending trick
- // from http://graphics.stanford.edu/~seander/bithacks.html#FixedSignExtend
- struct {SInt32 x:21;} s;
-
- *x = s.x = (buffer[0] << 13) | (buffer[1] << 5) | ((buffer[2] & 0xF8) >> 3);
- *y = s.x = ((buffer[2] & 0x07) << 18) | (buffer[3] << 10) | (buffer[4] << 2) |
- ((buffer[5] & 0xC0) >> 6);
- *z = s.x = ((buffer[5] & 0x3F) << 15) | (buffer[6] << 7) | (buffer[7] >> 1);
-}
-
-// Messages we care for
-enum TrackerMessageType
-{
- TrackerMessage_None = 0,
- TrackerMessage_Sensors = 1,
- TrackerMessage_Unknown = 0x100,
- TrackerMessage_SizeError = 0x101,
-};
-
-struct TrackerSample
-{
- SInt32 AccelX, AccelY, AccelZ;
- SInt32 GyroX, GyroY, GyroZ;
-};
-
-
-struct TrackerSensors
-{
- UByte SampleCount;
- UInt16 Timestamp;
- UInt16 LastCommandID;
- SInt16 Temperature;
-
- TrackerSample Samples[3];
-
- SInt16 MagX, MagY, MagZ;
-
- TrackerMessageType Decode(const UByte* buffer, int size)
- {
- if (size < 62)
- return TrackerMessage_SizeError;
-
- SampleCount = buffer[1];
- Timestamp = DecodeUInt16(buffer + 2);
- LastCommandID = DecodeUInt16(buffer + 4);
- Temperature = DecodeSInt16(buffer + 6);
-
- //if (SampleCount > 2)
- // OVR_DEBUG_LOG_TEXT(("TackerSensor::Decode SampleCount=%d\n", SampleCount));
-
- // Only unpack as many samples as there actually are
- UByte iterationCount = (SampleCount > 2) ? 3 : SampleCount;
-
- for (UByte i = 0; i < iterationCount; i++)
- {
- UnpackSensor(buffer + 8 + 16 * i, &Samples[i].AccelX, &Samples[i].AccelY, &Samples[i].AccelZ);
- UnpackSensor(buffer + 16 + 16 * i, &Samples[i].GyroX, &Samples[i].GyroY, &Samples[i].GyroZ);
- }
-
- MagX = DecodeSInt16(buffer + 56);
- MagY = DecodeSInt16(buffer + 58);
- MagZ = DecodeSInt16(buffer + 60);
-
- return TrackerMessage_Sensors;
- }
-};
-
-struct TrackerMessage
-{
- TrackerMessageType Type;
- TrackerSensors Sensors;
-};
-
-bool DecodeTrackerMessage(TrackerMessage* message, UByte* buffer, int size)
-{
- memset(message, 0, sizeof(TrackerMessage));
-
- if (size < 4)
- {
- message->Type = TrackerMessage_SizeError;
- return false;
- }
-
- switch (buffer[0])
- {
- case TrackerMessage_Sensors:
- message->Type = message->Sensors.Decode(buffer, size);
- break;
-
- default:
- message->Type = TrackerMessage_Unknown;
- break;
- }
-
- return (message->Type < TrackerMessage_Unknown) && (message->Type != TrackerMessage_None);
-}
-
-
-// ***** SensorRangeImpl Implementation
-
-// Sensor HW only accepts specific maximum range values, used to maximize
-// the 16-bit sensor outputs. Use these ramps to specify and report appropriate values.
-static const UInt16 AccelRangeRamp[] = { 2, 4, 8, 16 };
-static const UInt16 GyroRangeRamp[] = { 250, 500, 1000, 2000 };
-static const UInt16 MagRangeRamp[] = { 880, 1300, 1900, 2500 };
-
-static UInt16 SelectSensorRampValue(const UInt16* ramp, unsigned count,
- float val, float factor, const char* label)
-{
- UInt16 threshold = (UInt16)(val * factor);
-
- for (unsigned i = 0; i<count; i++)
- {
- if (ramp[i] >= threshold)
- return ramp[i];
- }
- OVR_DEBUG_LOG(("SensorDevice::SetRange - %s clamped to %0.4f",
- label, float(ramp[count-1]) / factor));
- OVR_UNUSED2(factor, label);
- return ramp[count-1];
-}
-
-// SensorScaleImpl provides buffer packing logic for the Sensor Range
-// record that can be applied to DK1 sensor through Get/SetFeature. We expose this
-// through SensorRange class, which has different units.
-struct SensorRangeImpl
-{
- enum { PacketSize = 8 };
- UByte Buffer[PacketSize];
-
- UInt16 CommandId;
- UInt16 AccelScale;
- UInt16 GyroScale;
- UInt16 MagScale;
-
- SensorRangeImpl(const SensorRange& r, UInt16 commandId = 0)
- {
- SetSensorRange(r, commandId);
- }
-
- void SetSensorRange(const SensorRange& r, UInt16 commandId = 0)
- {
- CommandId = commandId;
- AccelScale = SelectSensorRampValue(AccelRangeRamp, sizeof(AccelRangeRamp)/sizeof(AccelRangeRamp[0]),
- r.MaxAcceleration, (1.0f / 9.81f), "MaxAcceleration");
- GyroScale = SelectSensorRampValue(GyroRangeRamp, sizeof(GyroRangeRamp)/sizeof(GyroRangeRamp[0]),
- r.MaxRotationRate, Math<float>::RadToDegreeFactor, "MaxRotationRate");
- MagScale = SelectSensorRampValue(MagRangeRamp, sizeof(MagRangeRamp)/sizeof(MagRangeRamp[0]),
- r.MaxMagneticField, 1000.0f, "MaxMagneticField");
- Pack();
- }
-
- void GetSensorRange(SensorRange* r)
- {
- r->MaxAcceleration = AccelScale * 9.81f;
- r->MaxRotationRate = DegreeToRad((float)GyroScale);
- r->MaxMagneticField= MagScale * 0.001f;
- }
-
- static SensorRange GetMaxSensorRange()
- {
- return SensorRange(AccelRangeRamp[sizeof(AccelRangeRamp)/sizeof(AccelRangeRamp[0]) - 1] * 9.81f,
- GyroRangeRamp[sizeof(GyroRangeRamp)/sizeof(GyroRangeRamp[0]) - 1] *
- Math<float>::DegreeToRadFactor,
- MagRangeRamp[sizeof(MagRangeRamp)/sizeof(MagRangeRamp[0]) - 1] * 0.001f);
- }
-
- void Pack()
- {
- Buffer[0] = 4;
- Buffer[1] = UByte(CommandId & 0xFF);
- Buffer[2] = UByte(CommandId >> 8);
- Buffer[3] = UByte(AccelScale);
- Buffer[4] = UByte(GyroScale & 0xFF);
- Buffer[5] = UByte(GyroScale >> 8);
- Buffer[6] = UByte(MagScale & 0xFF);
- Buffer[7] = UByte(MagScale >> 8);
- }
-
- void Unpack()
- {
- CommandId = Buffer[1] | (UInt16(Buffer[2]) << 8);
- AccelScale= Buffer[3];
- GyroScale = Buffer[4] | (UInt16(Buffer[5]) << 8);
- MagScale = Buffer[6] | (UInt16(Buffer[7]) << 8);
- }
-};
-
-
-// Sensor configuration command, ReportId == 2.
-
-struct SensorConfigImpl
-{
- enum { PacketSize = 7 };
- UByte Buffer[PacketSize];
-
- // Flag values for Flags.
- enum {
- Flag_RawMode = 0x01,
- Flag_CallibrationTest = 0x02, // Internal test mode
- Flag_UseCallibration = 0x04,
- Flag_AutoCallibration = 0x08,
- Flag_MotionKeepAlive = 0x10,
- Flag_CommandKeepAlive = 0x20,
- Flag_SensorCoordinates = 0x40
- };
-
- UInt16 CommandId;
- UByte Flags;
- UInt16 PacketInterval;
- UInt16 KeepAliveIntervalMs;
-
- SensorConfigImpl() : CommandId(0), Flags(0), PacketInterval(0), KeepAliveIntervalMs(0)
- {
- memset(Buffer, 0, PacketSize);
- Buffer[0] = 2;
- }
-
- void SetSensorCoordinates(bool sensorCoordinates)
- { Flags = (Flags & ~Flag_SensorCoordinates) | (sensorCoordinates ? Flag_SensorCoordinates : 0); }
- bool IsUsingSensorCoordinates() const
- { return (Flags & Flag_SensorCoordinates) != 0; }
-
- void Pack()
- {
- Buffer[0] = 2;
- Buffer[1] = UByte(CommandId & 0xFF);
- Buffer[2] = UByte(CommandId >> 8);
- Buffer[3] = Flags;
- Buffer[4] = UByte(PacketInterval);
- Buffer[5] = UByte(KeepAliveIntervalMs & 0xFF);
- Buffer[6] = UByte(KeepAliveIntervalMs >> 8);
- }
-
- void Unpack()
- {
- CommandId = Buffer[1] | (UInt16(Buffer[2]) << 8);
- Flags = Buffer[3];
- PacketInterval = Buffer[4];
- KeepAliveIntervalMs= Buffer[5] | (UInt16(Buffer[6]) << 8);
- }
-
-};
-
-
-// SensorKeepAlive - feature report that needs to be sent at regular intervals for sensor
-// to receive commands.
-struct SensorKeepAliveImpl
-{
- enum { PacketSize = 5 };
- UByte Buffer[PacketSize];
-
- UInt16 CommandId;
- UInt16 KeepAliveIntervalMs;
-
- SensorKeepAliveImpl(UInt16 interval = 0, UInt16 commandId = 0)
- : CommandId(commandId), KeepAliveIntervalMs(interval)
- {
- Pack();
- }
-
- void Pack()
- {
- Buffer[0] = 8;
- Buffer[1] = UByte(CommandId & 0xFF);
- Buffer[2] = UByte(CommandId >> 8);
- Buffer[3] = UByte(KeepAliveIntervalMs & 0xFF);
- Buffer[4] = UByte(KeepAliveIntervalMs >> 8);
- }
-
- void Unpack()
- {
- CommandId = Buffer[1] | (UInt16(Buffer[2]) << 8);
- KeepAliveIntervalMs= Buffer[3] | (UInt16(Buffer[4]) << 8);
- }
-};
-
-
-//-------------------------------------------------------------------------------------
-// ***** SensorDisplayInfoImpl
-SensorDisplayInfoImpl::SensorDisplayInfoImpl()
- : CommandId(0), DistortionType(Base_None)
-{
- memset(Buffer, 0, PacketSize);
- Buffer[0] = 9;
-}
-
-void SensorDisplayInfoImpl::Unpack()
-{
- CommandId = Buffer[1] | (UInt16(Buffer[2]) << 8);
- DistortionType = Buffer[3];
- HResolution = DecodeUInt16(Buffer+4);
- VResolution = DecodeUInt16(Buffer+6);
- HScreenSize = DecodeUInt32(Buffer+8) * (1/1000000.f);
- VScreenSize = DecodeUInt32(Buffer+12) * (1/1000000.f);
- VCenter = DecodeUInt32(Buffer+16) * (1/1000000.f);
- LensSeparation = DecodeUInt32(Buffer+20) * (1/1000000.f);
- EyeToScreenDistance[0] = DecodeUInt32(Buffer+24) * (1/1000000.f);
- EyeToScreenDistance[1] = DecodeUInt32(Buffer+28) * (1/1000000.f);
- DistortionK[0] = DecodeFloat(Buffer+32);
- DistortionK[1] = DecodeFloat(Buffer+36);
- DistortionK[2] = DecodeFloat(Buffer+40);
- DistortionK[3] = DecodeFloat(Buffer+44);
- DistortionK[4] = DecodeFloat(Buffer+48);
- DistortionK[5] = DecodeFloat(Buffer+52);
-}
-
-
-//-------------------------------------------------------------------------------------
-// ***** SensorDeviceFactory
-
-SensorDeviceFactory SensorDeviceFactory::Instance;
-
-void SensorDeviceFactory::EnumerateDevices(EnumerateVisitor& visitor)
-{
-
- class SensorEnumerator : public HIDEnumerateVisitor
- {
- // Assign not supported; suppress MSVC warning.
- void operator = (const SensorEnumerator&) { }
-
- DeviceFactory* pFactory;
- EnumerateVisitor& ExternalVisitor;
- public:
- SensorEnumerator(DeviceFactory* factory, EnumerateVisitor& externalVisitor)
- : pFactory(factory), ExternalVisitor(externalVisitor) { }
-
- virtual bool MatchVendorProduct(UInt16 vendorId, UInt16 productId)
- {
- return pFactory->MatchVendorProduct(vendorId, productId);
- }
-
- virtual void Visit(HIDDevice& device, const HIDDeviceDesc& desc)
- {
- SensorDeviceCreateDesc createDesc(pFactory, desc);
- ExternalVisitor.Visit(createDesc);
-
- // Check if the sensor returns DisplayInfo. If so, try to use it to override potentially
- // mismatching monitor information (in case wrong EDID is reported by splitter),
- // or to create a new "virtualized" HMD Device.
-
- SensorDisplayInfoImpl displayInfo;
-
- if (device.GetFeatureReport(displayInfo.Buffer, SensorDisplayInfoImpl::PacketSize))
- {
- displayInfo.Unpack();
-
- // If we got display info, try to match / create HMDDevice as well
- // so that sensor settings give preference.
- if (displayInfo.DistortionType & SensorDisplayInfoImpl::Mask_BaseFmt)
- {
- SensorDeviceImpl::EnumerateHMDFromSensorDisplayInfo(displayInfo, ExternalVisitor);
- }
- }
- }
- };
-
- //double start = Timer::GetProfileSeconds();
-
- SensorEnumerator sensorEnumerator(this, visitor);
- GetManagerImpl()->GetHIDDeviceManager()->Enumerate(&sensorEnumerator);
-
- //double totalSeconds = Timer::GetProfileSeconds() - start;
-}
-
-bool SensorDeviceFactory::MatchVendorProduct(UInt16 vendorId, UInt16 productId) const
-{
- return ((vendorId == Sensor_VendorId) && (productId == Sensor_ProductId)) ||
- ((vendorId == Sensor_OldVendorId) && (productId == Sensor_OldProductId));
-}
-
-bool SensorDeviceFactory::DetectHIDDevice(DeviceManager* pdevMgr, const HIDDeviceDesc& desc)
-{
- if (MatchVendorProduct(desc.VendorId, desc.ProductId))
- {
- SensorDeviceCreateDesc createDesc(this, desc);
- return pdevMgr->AddDevice_NeedsLock(createDesc).GetPtr() != NULL;
- }
- return false;
-}
-
-//-------------------------------------------------------------------------------------
-// ***** SensorDeviceCreateDesc
-
-DeviceBase* SensorDeviceCreateDesc::NewDeviceInstance()
-{
- return new SensorDeviceImpl(this);
-}
-
-bool SensorDeviceCreateDesc::GetDeviceInfo(DeviceInfo* info) const
-{
- if ((info->InfoClassType != Device_Sensor) &&
- (info->InfoClassType != Device_None))
- return false;
-
- OVR_strcpy(info->ProductName, DeviceInfo::MaxNameLength, HIDDesc.Product.ToCStr());
- OVR_strcpy(info->Manufacturer, DeviceInfo::MaxNameLength, HIDDesc.Manufacturer.ToCStr());
- info->Type = Device_Sensor;
- info->Version = 0;
-
- if (info->InfoClassType == Device_Sensor)
- {
- SensorInfo* sinfo = (SensorInfo*)info;
- sinfo->VendorId = HIDDesc.VendorId;
- sinfo->ProductId = HIDDesc.ProductId;
- sinfo->MaxRanges = SensorRangeImpl::GetMaxSensorRange();
- OVR_strcpy(sinfo->SerialNumber, sizeof(sinfo->SerialNumber),HIDDesc.SerialNumber.ToCStr());
- }
- return true;
-}
-
-
-//-------------------------------------------------------------------------------------
-// ***** SensorDevice
-
-SensorDeviceImpl::SensorDeviceImpl(SensorDeviceCreateDesc* createDesc)
- : OVR::HIDDeviceImpl<OVR::SensorDevice>(createDesc, 0),
- Coordinates(SensorDevice::Coord_Sensor),
- HWCoordinates(SensorDevice::Coord_HMD), // HW reports HMD coordinates by default.
- NextKeepAliveTicks(0),
- MaxValidRange(SensorRangeImpl::GetMaxSensorRange())
-{
- SequenceValid = false;
- LastSampleCount= 0;
- LastTimestamp = 0;
-
- OldCommandId = 0;
-}
-
-SensorDeviceImpl::~SensorDeviceImpl()
-{
- // Check that Shutdown() was called.
- OVR_ASSERT(!pCreateDesc->pDevice);
-}
-
-// Internal creation APIs.
-bool SensorDeviceImpl::Initialize(DeviceBase* parent)
-{
- if (HIDDeviceImpl<OVR::SensorDevice>::Initialize(parent))
- {
- openDevice();
-
- LogText("OVR::SensorDevice initialized.\n");
-
- return true;
- }
-
- return false;
-}
-
-void SensorDeviceImpl::openDevice()
-{
-
- // Read the currently configured range from sensor.
- SensorRangeImpl sr(SensorRange(), 0);
-
- if (GetInternalDevice()->GetFeatureReport(sr.Buffer, SensorRangeImpl::PacketSize))
- {
- sr.Unpack();
- sr.GetSensorRange(&CurrentRange);
- }
-
-
- // If the sensor has "DisplayInfo" data, use HMD coordinate frame by default.
- SensorDisplayInfoImpl displayInfo;
- if (GetInternalDevice()->GetFeatureReport(displayInfo.Buffer, SensorDisplayInfoImpl::PacketSize))
- {
- displayInfo.Unpack();
- Coordinates = (displayInfo.DistortionType & SensorDisplayInfoImpl::Mask_BaseFmt) ?
- Coord_HMD : Coord_Sensor;
- }
-
- // Read/Apply sensor config.
- setCoordinateFrame(Coordinates);
- setReportRate(Sensor_DefaultReportRate);
-
- // Set Keep-alive at 10 seconds.
- SensorKeepAliveImpl skeepAlive(10 * 1000);
- GetInternalDevice()->SetFeatureReport(skeepAlive.Buffer, SensorKeepAliveImpl::PacketSize);
-}
-
-void SensorDeviceImpl::closeDeviceOnError()
-{
- LogText("OVR::SensorDevice - Lost connection to '%s'\n", getHIDDesc()->Path.ToCStr());
- NextKeepAliveTicks = 0;
-}
-
-void SensorDeviceImpl::Shutdown()
-{
- HIDDeviceImpl<OVR::SensorDevice>::Shutdown();
-
- LogText("OVR::SensorDevice - Closed '%s'\n", getHIDDesc()->Path.ToCStr());
-}
-
-
-void SensorDeviceImpl::OnInputReport(UByte* pData, UInt32 length)
-{
-
- bool processed = false;
- if (!processed)
- {
-
- TrackerMessage message;
- if (DecodeTrackerMessage(&message, pData, length))
- {
- processed = true;
- onTrackerMessage(&message);
- }
- }
-}
-
-UInt64 SensorDeviceImpl::OnTicks(UInt64 ticksMks)
-{
-
- if (ticksMks >= NextKeepAliveTicks)
- {
- // Use 3-seconds keep alive by default.
- UInt64 keepAliveDelta = Timer::MksPerSecond * 3;
-
- // Set Keep-alive at 10 seconds.
- SensorKeepAliveImpl skeepAlive(10 * 1000);
- // OnTicks is called from background thread so we don't need to add this to the command queue.
- GetInternalDevice()->SetFeatureReport(skeepAlive.Buffer, SensorKeepAliveImpl::PacketSize);
-
- // Emit keep-alive every few seconds.
- NextKeepAliveTicks = ticksMks + keepAliveDelta;
- }
- return NextKeepAliveTicks - ticksMks;
-}
-
-bool SensorDeviceImpl::SetRange(const SensorRange& range, bool waitFlag)
-{
- bool result = 0;
- ThreadCommandQueue * threadQueue = GetManagerImpl()->GetThreadQueue();
-
- if (!waitFlag)
- {
- return threadQueue->PushCall(this, &SensorDeviceImpl::setRange, range);
- }
-
- if (!threadQueue->PushCallAndWaitResult(this,
- &SensorDeviceImpl::setRange,
- &result,
- range))
- {
- return false;
- }
-
- return result;
-}
-
-void SensorDeviceImpl::GetRange(SensorRange* range) const
-{
- Lock::Locker lockScope(GetLock());
- *range = CurrentRange;
-}
-
-bool SensorDeviceImpl::setRange(const SensorRange& range)
-{
- SensorRangeImpl sr(range);
-
- if (GetInternalDevice()->SetFeatureReport(sr.Buffer, SensorRangeImpl::PacketSize))
- {
- Lock::Locker lockScope(GetLock());
- sr.GetSensorRange(&CurrentRange);
- return true;
- }
-
- return false;
-}
-
-void SensorDeviceImpl::SetCoordinateFrame(CoordinateFrame coordframe)
-{
- // Push call with wait.
- GetManagerImpl()->GetThreadQueue()->
- PushCall(this, &SensorDeviceImpl::setCoordinateFrame, coordframe, true);
-}
-
-SensorDevice::CoordinateFrame SensorDeviceImpl::GetCoordinateFrame() const
-{
- return Coordinates;
-}
-
-Void SensorDeviceImpl::setCoordinateFrame(CoordinateFrame coordframe)
-{
-
- Coordinates = coordframe;
-
- // Read the original coordinate frame, then try to change it.
- SensorConfigImpl scfg;
- if (GetInternalDevice()->GetFeatureReport(scfg.Buffer, SensorConfigImpl::PacketSize))
- {
- scfg.Unpack();
- }
-
- scfg.SetSensorCoordinates(coordframe == Coord_Sensor);
- scfg.Pack();
-
- GetInternalDevice()->SetFeatureReport(scfg.Buffer, SensorConfigImpl::PacketSize);
-
- // Re-read the state, in case of older firmware that doesn't support Sensor coordinates.
- if (GetInternalDevice()->GetFeatureReport(scfg.Buffer, SensorConfigImpl::PacketSize))
- {
- scfg.Unpack();
- HWCoordinates = scfg.IsUsingSensorCoordinates() ? Coord_Sensor : Coord_HMD;
- }
- else
- {
- HWCoordinates = Coord_HMD;
- }
- return 0;
-}
-
-void SensorDeviceImpl::SetReportRate(unsigned rateHz)
-{
- // Push call with wait.
- GetManagerImpl()->GetThreadQueue()->
- PushCall(this, &SensorDeviceImpl::setReportRate, rateHz, true);
-}
-
-unsigned SensorDeviceImpl::GetReportRate() const
-{
- // Read the original configuration
- SensorConfigImpl scfg;
- if (GetInternalDevice()->GetFeatureReport(scfg.Buffer, SensorConfigImpl::PacketSize))
- {
- scfg.Unpack();
- return Sensor_MaxReportRate / (scfg.PacketInterval + 1);
- }
- return 0; // error
-}
-
-Void SensorDeviceImpl::setReportRate(unsigned rateHz)
-{
- // Read the original configuration
- SensorConfigImpl scfg;
- if (GetInternalDevice()->GetFeatureReport(scfg.Buffer, SensorConfigImpl::PacketSize))
- {
- scfg.Unpack();
- }
-
- if (rateHz > Sensor_MaxReportRate)
- rateHz = Sensor_MaxReportRate;
- else if (rateHz == 0)
- rateHz = Sensor_DefaultReportRate;
-
- scfg.PacketInterval = UInt16((Sensor_MaxReportRate / rateHz) - 1);
-
- scfg.Pack();
-
- GetInternalDevice()->SetFeatureReport(scfg.Buffer, SensorConfigImpl::PacketSize);
- return 0;
-}
-
-void SensorDeviceImpl::SetMessageHandler(MessageHandler* handler)
-{
- if (handler)
- {
- SequenceValid = false;
- DeviceBase::SetMessageHandler(handler);
- }
- else
- {
- DeviceBase::SetMessageHandler(handler);
- }
-}
-
-// Sensor reports data in the following coordinate system:
-// Accelerometer: 10^-4 m/s^2; X forward, Y right, Z Down.
-// Gyro: 10^-4 rad/s; X positive roll right, Y positive pitch up; Z positive yaw right.
-
-
-// We need to convert it to the following RHS coordinate system:
-// X right, Y Up, Z Back (out of screen)
-//
-Vector3f AccelFromBodyFrameUpdate(const TrackerSensors& update, UByte sampleNumber,
- bool convertHMDToSensor = false)
-{
- const TrackerSample& sample = update.Samples[sampleNumber];
- float ax = (float)sample.AccelX;
- float ay = (float)sample.AccelY;
- float az = (float)sample.AccelZ;
-
- Vector3f val = convertHMDToSensor ? Vector3f(ax, az, -ay) : Vector3f(ax, ay, az);
- return val * 0.0001f;
-}
-
-
-Vector3f MagFromBodyFrameUpdate(const TrackerSensors& update,
- bool convertHMDToSensor = false)
-{
- // Note: Y and Z are swapped in comparison to the Accel.
- // This accounts for DK1 sensor firmware axis swap, which should be undone in future releases.
- if (!convertHMDToSensor)
- {
- return Vector3f( (float)update.MagX,
- (float)update.MagZ,
- (float)update.MagY) * 0.0001f;
- }
-
- return Vector3f( (float)update.MagX,
- (float)update.MagY,
- -(float)update.MagZ) * 0.0001f;
-}
-
-Vector3f EulerFromBodyFrameUpdate(const TrackerSensors& update, UByte sampleNumber,
- bool convertHMDToSensor = false)
-{
- const TrackerSample& sample = update.Samples[sampleNumber];
- float gx = (float)sample.GyroX;
- float gy = (float)sample.GyroY;
- float gz = (float)sample.GyroZ;
-
- Vector3f val = convertHMDToSensor ? Vector3f(gx, gz, -gy) : Vector3f(gx, gy, gz);
- return val * 0.0001f;
-}
-
-
-void SensorDeviceImpl::onTrackerMessage(TrackerMessage* message)
-{
- if (message->Type != TrackerMessage_Sensors)
- return;
-
- const float timeUnit = (1.0f / 1000.f);
- TrackerSensors& s = message->Sensors;
-
-
- // Call OnMessage() within a lock to avoid conflicts with handlers.
- Lock::Locker scopeLock(HandlerRef.GetLock());
-
-
- if (SequenceValid)
- {
- unsigned timestampDelta;
-
- if (s.Timestamp < LastTimestamp)
- timestampDelta = ((((int)s.Timestamp) + 0x10000) - (int)LastTimestamp);
- else
- timestampDelta = (s.Timestamp - LastTimestamp);
-
- // If we missed a small number of samples, replicate the last sample.
- if ((timestampDelta > LastSampleCount) && (timestampDelta <= 254))
- {
- if (HandlerRef.GetHandler())
- {
- MessageBodyFrame sensors(this);
- sensors.TimeDelta = (timestampDelta - LastSampleCount) * timeUnit;
- sensors.Acceleration = LastAcceleration;
- sensors.RotationRate = LastRotationRate;
- sensors.MagneticField = LastMagneticField;
- sensors.Temperature = LastTemperature;
-
- HandlerRef.GetHandler()->OnMessage(sensors);
- }
- }
- }
- else
- {
- LastAcceleration = Vector3f(0);
- LastRotationRate = Vector3f(0);
- LastMagneticField= Vector3f(0);
- LastTemperature = 0;
- SequenceValid = true;
- }
-
- LastSampleCount = s.SampleCount;
- LastTimestamp = s.Timestamp;
-
- bool convertHMDToSensor = (Coordinates == Coord_Sensor) && (HWCoordinates == Coord_HMD);
-
- if (HandlerRef.GetHandler())
- {
- MessageBodyFrame sensors(this);
- UByte iterations = s.SampleCount;
-
- if (s.SampleCount > 3)
- {
- iterations = 3;
- sensors.TimeDelta = (s.SampleCount - 2) * timeUnit;
- }
- else
- {
- sensors.TimeDelta = timeUnit;
- }
-
- for (UByte i = 0; i < iterations; i++)
- {
- sensors.Acceleration = AccelFromBodyFrameUpdate(s, i, convertHMDToSensor);
- sensors.RotationRate = EulerFromBodyFrameUpdate(s, i, convertHMDToSensor);
- sensors.MagneticField= MagFromBodyFrameUpdate(s, convertHMDToSensor);
- sensors.Temperature = s.Temperature * 0.01f;
- HandlerRef.GetHandler()->OnMessage(sensors);
- // TimeDelta for the last two sample is always fixed.
- sensors.TimeDelta = timeUnit;
- }
-
- LastAcceleration = sensors.Acceleration;
- LastRotationRate = sensors.RotationRate;
- LastMagneticField= sensors.MagneticField;
- LastTemperature = sensors.Temperature;
- }
- else
- {
- UByte i = (s.SampleCount > 3) ? 2 : (s.SampleCount - 1);
- LastAcceleration = AccelFromBodyFrameUpdate(s, i, convertHMDToSensor);
- LastRotationRate = EulerFromBodyFrameUpdate(s, i, convertHMDToSensor);
- LastMagneticField = MagFromBodyFrameUpdate(s, convertHMDToSensor);
- LastTemperature = s.Temperature * 0.01f;
- }
-}
-
-} // namespace OVR
-
-
+/************************************************************************************ + +Filename : OVR_SensorImpl.cpp +Content : Oculus Sensor device implementation. +Created : March 7, 2013 +Authors : Lee Cooper + +Copyright : Copyright 2013 Oculus VR, Inc. All Rights reserved. + +Use of this software is subject to the terms of the Oculus license +agreement provided at the time of installation or download, or which +otherwise accompanies this software in either electronic or hard copy form. + +*************************************************************************************/ + +#include "OVR_SensorImpl.h" + +// HMDDeviceDesc can be created/updated through Sensor carrying DisplayInfo. + +#include "Kernel/OVR_Timer.h" + +namespace OVR { + +//------------------------------------------------------------------------------------- +// ***** Oculus Sensor-specific packet data structures + +enum { + Sensor_VendorId = Oculus_VendorId, + Sensor_ProductId = 0x0001, + + // ST's VID used originally; should be removed in the future + Sensor_OldVendorId = 0x0483, + Sensor_OldProductId = 0x5750, + + Sensor_DefaultReportRate = 500, // Hz + Sensor_MaxReportRate = 1000 // Hz +}; + +// Reported data is little-endian now +static UInt16 DecodeUInt16(const UByte* buffer) +{ + return (UInt16(buffer[1]) << 8) | UInt16(buffer[0]); +} + +static SInt16 DecodeSInt16(const UByte* buffer) +{ + return (SInt16(buffer[1]) << 8) | SInt16(buffer[0]); +} + +static UInt32 DecodeUInt32(const UByte* buffer) +{ + return (buffer[0]) | UInt32(buffer[1] << 8) | UInt32(buffer[2] << 16) | UInt32(buffer[3] << 24); +} + +static float DecodeFloat(const UByte* buffer) +{ + union { + UInt32 U; + float F; + }; + + U = DecodeUInt32(buffer); + return F; +} + + +static void UnpackSensor(const UByte* buffer, SInt32* x, SInt32* y, SInt32* z) +{ + // Sign extending trick + // from http://graphics.stanford.edu/~seander/bithacks.html#FixedSignExtend + struct {SInt32 x:21;} s; + + *x = s.x = (buffer[0] << 13) | (buffer[1] << 5) | ((buffer[2] & 0xF8) >> 3); + *y = s.x = ((buffer[2] & 0x07) << 18) | (buffer[3] << 10) | (buffer[4] << 2) | + ((buffer[5] & 0xC0) >> 6); + *z = s.x = ((buffer[5] & 0x3F) << 15) | (buffer[6] << 7) | (buffer[7] >> 1); +} + +// Messages we care for +enum TrackerMessageType +{ + TrackerMessage_None = 0, + TrackerMessage_Sensors = 1, + TrackerMessage_Unknown = 0x100, + TrackerMessage_SizeError = 0x101, +}; + +struct TrackerSample +{ + SInt32 AccelX, AccelY, AccelZ; + SInt32 GyroX, GyroY, GyroZ; +}; + + +struct TrackerSensors +{ + UByte SampleCount; + UInt16 Timestamp; + UInt16 LastCommandID; + SInt16 Temperature; + + TrackerSample Samples[3]; + + SInt16 MagX, MagY, MagZ; + + TrackerMessageType Decode(const UByte* buffer, int size) + { + if (size < 62) + return TrackerMessage_SizeError; + + SampleCount = buffer[1]; + Timestamp = DecodeUInt16(buffer + 2); + LastCommandID = DecodeUInt16(buffer + 4); + Temperature = DecodeSInt16(buffer + 6); + + //if (SampleCount > 2) + // OVR_DEBUG_LOG_TEXT(("TackerSensor::Decode SampleCount=%d\n", SampleCount)); + + // Only unpack as many samples as there actually are + UByte iterationCount = (SampleCount > 2) ? 3 : SampleCount; + + for (UByte i = 0; i < iterationCount; i++) + { + UnpackSensor(buffer + 8 + 16 * i, &Samples[i].AccelX, &Samples[i].AccelY, &Samples[i].AccelZ); + UnpackSensor(buffer + 16 + 16 * i, &Samples[i].GyroX, &Samples[i].GyroY, &Samples[i].GyroZ); + } + + MagX = DecodeSInt16(buffer + 56); + MagY = DecodeSInt16(buffer + 58); + MagZ = DecodeSInt16(buffer + 60); + + return TrackerMessage_Sensors; + } +}; + +struct TrackerMessage +{ + TrackerMessageType Type; + TrackerSensors Sensors; +}; + +bool DecodeTrackerMessage(TrackerMessage* message, UByte* buffer, int size) +{ + memset(message, 0, sizeof(TrackerMessage)); + + if (size < 4) + { + message->Type = TrackerMessage_SizeError; + return false; + } + + switch (buffer[0]) + { + case TrackerMessage_Sensors: + message->Type = message->Sensors.Decode(buffer, size); + break; + + default: + message->Type = TrackerMessage_Unknown; + break; + } + + return (message->Type < TrackerMessage_Unknown) && (message->Type != TrackerMessage_None); +} + + +// ***** SensorRangeImpl Implementation + +// Sensor HW only accepts specific maximum range values, used to maximize +// the 16-bit sensor outputs. Use these ramps to specify and report appropriate values. +static const UInt16 AccelRangeRamp[] = { 2, 4, 8, 16 }; +static const UInt16 GyroRangeRamp[] = { 250, 500, 1000, 2000 }; +static const UInt16 MagRangeRamp[] = { 880, 1300, 1900, 2500 }; + +static UInt16 SelectSensorRampValue(const UInt16* ramp, unsigned count, + float val, float factor, const char* label) +{ + UInt16 threshold = (UInt16)(val * factor); + + for (unsigned i = 0; i<count; i++) + { + if (ramp[i] >= threshold) + return ramp[i]; + } + OVR_DEBUG_LOG(("SensorDevice::SetRange - %s clamped to %0.4f", + label, float(ramp[count-1]) / factor)); + OVR_UNUSED2(factor, label); + return ramp[count-1]; +} + +// SensorScaleImpl provides buffer packing logic for the Sensor Range +// record that can be applied to DK1 sensor through Get/SetFeature. We expose this +// through SensorRange class, which has different units. +struct SensorRangeImpl +{ + enum { PacketSize = 8 }; + UByte Buffer[PacketSize]; + + UInt16 CommandId; + UInt16 AccelScale; + UInt16 GyroScale; + UInt16 MagScale; + + SensorRangeImpl(const SensorRange& r, UInt16 commandId = 0) + { + SetSensorRange(r, commandId); + } + + void SetSensorRange(const SensorRange& r, UInt16 commandId = 0) + { + CommandId = commandId; + AccelScale = SelectSensorRampValue(AccelRangeRamp, sizeof(AccelRangeRamp)/sizeof(AccelRangeRamp[0]), + r.MaxAcceleration, (1.0f / 9.81f), "MaxAcceleration"); + GyroScale = SelectSensorRampValue(GyroRangeRamp, sizeof(GyroRangeRamp)/sizeof(GyroRangeRamp[0]), + r.MaxRotationRate, Math<float>::RadToDegreeFactor, "MaxRotationRate"); + MagScale = SelectSensorRampValue(MagRangeRamp, sizeof(MagRangeRamp)/sizeof(MagRangeRamp[0]), + r.MaxMagneticField, 1000.0f, "MaxMagneticField"); + Pack(); + } + + void GetSensorRange(SensorRange* r) + { + r->MaxAcceleration = AccelScale * 9.81f; + r->MaxRotationRate = DegreeToRad((float)GyroScale); + r->MaxMagneticField= MagScale * 0.001f; + } + + static SensorRange GetMaxSensorRange() + { + return SensorRange(AccelRangeRamp[sizeof(AccelRangeRamp)/sizeof(AccelRangeRamp[0]) - 1] * 9.81f, + GyroRangeRamp[sizeof(GyroRangeRamp)/sizeof(GyroRangeRamp[0]) - 1] * + Math<float>::DegreeToRadFactor, + MagRangeRamp[sizeof(MagRangeRamp)/sizeof(MagRangeRamp[0]) - 1] * 0.001f); + } + + void Pack() + { + Buffer[0] = 4; + Buffer[1] = UByte(CommandId & 0xFF); + Buffer[2] = UByte(CommandId >> 8); + Buffer[3] = UByte(AccelScale); + Buffer[4] = UByte(GyroScale & 0xFF); + Buffer[5] = UByte(GyroScale >> 8); + Buffer[6] = UByte(MagScale & 0xFF); + Buffer[7] = UByte(MagScale >> 8); + } + + void Unpack() + { + CommandId = Buffer[1] | (UInt16(Buffer[2]) << 8); + AccelScale= Buffer[3]; + GyroScale = Buffer[4] | (UInt16(Buffer[5]) << 8); + MagScale = Buffer[6] | (UInt16(Buffer[7]) << 8); + } +}; + + +// Sensor configuration command, ReportId == 2. + +struct SensorConfigImpl +{ + enum { PacketSize = 7 }; + UByte Buffer[PacketSize]; + + // Flag values for Flags. + enum { + Flag_RawMode = 0x01, + Flag_CallibrationTest = 0x02, // Internal test mode + Flag_UseCallibration = 0x04, + Flag_AutoCallibration = 0x08, + Flag_MotionKeepAlive = 0x10, + Flag_CommandKeepAlive = 0x20, + Flag_SensorCoordinates = 0x40 + }; + + UInt16 CommandId; + UByte Flags; + UInt16 PacketInterval; + UInt16 KeepAliveIntervalMs; + + SensorConfigImpl() : CommandId(0), Flags(0), PacketInterval(0), KeepAliveIntervalMs(0) + { + memset(Buffer, 0, PacketSize); + Buffer[0] = 2; + } + + void SetSensorCoordinates(bool sensorCoordinates) + { Flags = (Flags & ~Flag_SensorCoordinates) | (sensorCoordinates ? Flag_SensorCoordinates : 0); } + bool IsUsingSensorCoordinates() const + { return (Flags & Flag_SensorCoordinates) != 0; } + + void Pack() + { + Buffer[0] = 2; + Buffer[1] = UByte(CommandId & 0xFF); + Buffer[2] = UByte(CommandId >> 8); + Buffer[3] = Flags; + Buffer[4] = UByte(PacketInterval); + Buffer[5] = UByte(KeepAliveIntervalMs & 0xFF); + Buffer[6] = UByte(KeepAliveIntervalMs >> 8); + } + + void Unpack() + { + CommandId = Buffer[1] | (UInt16(Buffer[2]) << 8); + Flags = Buffer[3]; + PacketInterval = Buffer[4]; + KeepAliveIntervalMs= Buffer[5] | (UInt16(Buffer[6]) << 8); + } + +}; + + +// SensorKeepAlive - feature report that needs to be sent at regular intervals for sensor +// to receive commands. +struct SensorKeepAliveImpl +{ + enum { PacketSize = 5 }; + UByte Buffer[PacketSize]; + + UInt16 CommandId; + UInt16 KeepAliveIntervalMs; + + SensorKeepAliveImpl(UInt16 interval = 0, UInt16 commandId = 0) + : CommandId(commandId), KeepAliveIntervalMs(interval) + { + Pack(); + } + + void Pack() + { + Buffer[0] = 8; + Buffer[1] = UByte(CommandId & 0xFF); + Buffer[2] = UByte(CommandId >> 8); + Buffer[3] = UByte(KeepAliveIntervalMs & 0xFF); + Buffer[4] = UByte(KeepAliveIntervalMs >> 8); + } + + void Unpack() + { + CommandId = Buffer[1] | (UInt16(Buffer[2]) << 8); + KeepAliveIntervalMs= Buffer[3] | (UInt16(Buffer[4]) << 8); + } +}; + + +//------------------------------------------------------------------------------------- +// ***** SensorDisplayInfoImpl +SensorDisplayInfoImpl::SensorDisplayInfoImpl() + : CommandId(0), DistortionType(Base_None) +{ + memset(Buffer, 0, PacketSize); + Buffer[0] = 9; +} + +void SensorDisplayInfoImpl::Unpack() +{ + CommandId = Buffer[1] | (UInt16(Buffer[2]) << 8); + DistortionType = Buffer[3]; + HResolution = DecodeUInt16(Buffer+4); + VResolution = DecodeUInt16(Buffer+6); + HScreenSize = DecodeUInt32(Buffer+8) * (1/1000000.f); + VScreenSize = DecodeUInt32(Buffer+12) * (1/1000000.f); + VCenter = DecodeUInt32(Buffer+16) * (1/1000000.f); + LensSeparation = DecodeUInt32(Buffer+20) * (1/1000000.f); + EyeToScreenDistance[0] = DecodeUInt32(Buffer+24) * (1/1000000.f); + EyeToScreenDistance[1] = DecodeUInt32(Buffer+28) * (1/1000000.f); + DistortionK[0] = DecodeFloat(Buffer+32); + DistortionK[1] = DecodeFloat(Buffer+36); + DistortionK[2] = DecodeFloat(Buffer+40); + DistortionK[3] = DecodeFloat(Buffer+44); + DistortionK[4] = DecodeFloat(Buffer+48); + DistortionK[5] = DecodeFloat(Buffer+52); +} + + +//------------------------------------------------------------------------------------- +// ***** SensorDeviceFactory + +SensorDeviceFactory SensorDeviceFactory::Instance; + +void SensorDeviceFactory::EnumerateDevices(EnumerateVisitor& visitor) +{ + + class SensorEnumerator : public HIDEnumerateVisitor + { + // Assign not supported; suppress MSVC warning. + void operator = (const SensorEnumerator&) { } + + DeviceFactory* pFactory; + EnumerateVisitor& ExternalVisitor; + public: + SensorEnumerator(DeviceFactory* factory, EnumerateVisitor& externalVisitor) + : pFactory(factory), ExternalVisitor(externalVisitor) { } + + virtual bool MatchVendorProduct(UInt16 vendorId, UInt16 productId) + { + return pFactory->MatchVendorProduct(vendorId, productId); + } + + virtual void Visit(HIDDevice& device, const HIDDeviceDesc& desc) + { + SensorDeviceCreateDesc createDesc(pFactory, desc); + ExternalVisitor.Visit(createDesc); + + // Check if the sensor returns DisplayInfo. If so, try to use it to override potentially + // mismatching monitor information (in case wrong EDID is reported by splitter), + // or to create a new "virtualized" HMD Device. + + SensorDisplayInfoImpl displayInfo; + + if (device.GetFeatureReport(displayInfo.Buffer, SensorDisplayInfoImpl::PacketSize)) + { + displayInfo.Unpack(); + + // If we got display info, try to match / create HMDDevice as well + // so that sensor settings give preference. + if (displayInfo.DistortionType & SensorDisplayInfoImpl::Mask_BaseFmt) + { + SensorDeviceImpl::EnumerateHMDFromSensorDisplayInfo(displayInfo, ExternalVisitor); + } + } + } + }; + + //double start = Timer::GetProfileSeconds(); + + SensorEnumerator sensorEnumerator(this, visitor); + GetManagerImpl()->GetHIDDeviceManager()->Enumerate(&sensorEnumerator); + + //double totalSeconds = Timer::GetProfileSeconds() - start; +} + +bool SensorDeviceFactory::MatchVendorProduct(UInt16 vendorId, UInt16 productId) const +{ + return ((vendorId == Sensor_VendorId) && (productId == Sensor_ProductId)) || + ((vendorId == Sensor_OldVendorId) && (productId == Sensor_OldProductId)); +} + +bool SensorDeviceFactory::DetectHIDDevice(DeviceManager* pdevMgr, const HIDDeviceDesc& desc) +{ + if (MatchVendorProduct(desc.VendorId, desc.ProductId)) + { + SensorDeviceCreateDesc createDesc(this, desc); + return pdevMgr->AddDevice_NeedsLock(createDesc).GetPtr() != NULL; + } + return false; +} + +//------------------------------------------------------------------------------------- +// ***** SensorDeviceCreateDesc + +DeviceBase* SensorDeviceCreateDesc::NewDeviceInstance() +{ + return new SensorDeviceImpl(this); +} + +bool SensorDeviceCreateDesc::GetDeviceInfo(DeviceInfo* info) const +{ + if ((info->InfoClassType != Device_Sensor) && + (info->InfoClassType != Device_None)) + return false; + + OVR_strcpy(info->ProductName, DeviceInfo::MaxNameLength, HIDDesc.Product.ToCStr()); + OVR_strcpy(info->Manufacturer, DeviceInfo::MaxNameLength, HIDDesc.Manufacturer.ToCStr()); + info->Type = Device_Sensor; + info->Version = 0; + + if (info->InfoClassType == Device_Sensor) + { + SensorInfo* sinfo = (SensorInfo*)info; + sinfo->VendorId = HIDDesc.VendorId; + sinfo->ProductId = HIDDesc.ProductId; + sinfo->MaxRanges = SensorRangeImpl::GetMaxSensorRange(); + OVR_strcpy(sinfo->SerialNumber, sizeof(sinfo->SerialNumber),HIDDesc.SerialNumber.ToCStr()); + } + return true; +} + + +//------------------------------------------------------------------------------------- +// ***** SensorDevice + +SensorDeviceImpl::SensorDeviceImpl(SensorDeviceCreateDesc* createDesc) + : OVR::HIDDeviceImpl<OVR::SensorDevice>(createDesc, 0), + Coordinates(SensorDevice::Coord_Sensor), + HWCoordinates(SensorDevice::Coord_HMD), // HW reports HMD coordinates by default. + NextKeepAliveTicks(0), + MaxValidRange(SensorRangeImpl::GetMaxSensorRange()) +{ + SequenceValid = false; + LastSampleCount= 0; + LastTimestamp = 0; + + OldCommandId = 0; +} + +SensorDeviceImpl::~SensorDeviceImpl() +{ + // Check that Shutdown() was called. + OVR_ASSERT(!pCreateDesc->pDevice); +} + +// Internal creation APIs. +bool SensorDeviceImpl::Initialize(DeviceBase* parent) +{ + if (HIDDeviceImpl<OVR::SensorDevice>::Initialize(parent)) + { + openDevice(); + + LogText("OVR::SensorDevice initialized.\n"); + + return true; + } + + return false; +} + +void SensorDeviceImpl::openDevice() +{ + + // Read the currently configured range from sensor. + SensorRangeImpl sr(SensorRange(), 0); + + if (GetInternalDevice()->GetFeatureReport(sr.Buffer, SensorRangeImpl::PacketSize)) + { + sr.Unpack(); + sr.GetSensorRange(&CurrentRange); + } + + + // If the sensor has "DisplayInfo" data, use HMD coordinate frame by default. + SensorDisplayInfoImpl displayInfo; + if (GetInternalDevice()->GetFeatureReport(displayInfo.Buffer, SensorDisplayInfoImpl::PacketSize)) + { + displayInfo.Unpack(); + Coordinates = (displayInfo.DistortionType & SensorDisplayInfoImpl::Mask_BaseFmt) ? + Coord_HMD : Coord_Sensor; + } + + // Read/Apply sensor config. + setCoordinateFrame(Coordinates); + setReportRate(Sensor_DefaultReportRate); + + // Set Keep-alive at 10 seconds. + SensorKeepAliveImpl skeepAlive(10 * 1000); + GetInternalDevice()->SetFeatureReport(skeepAlive.Buffer, SensorKeepAliveImpl::PacketSize); +} + +void SensorDeviceImpl::closeDeviceOnError() +{ + LogText("OVR::SensorDevice - Lost connection to '%s'\n", getHIDDesc()->Path.ToCStr()); + NextKeepAliveTicks = 0; +} + +void SensorDeviceImpl::Shutdown() +{ + HIDDeviceImpl<OVR::SensorDevice>::Shutdown(); + + LogText("OVR::SensorDevice - Closed '%s'\n", getHIDDesc()->Path.ToCStr()); +} + + +void SensorDeviceImpl::OnInputReport(UByte* pData, UInt32 length) +{ + + bool processed = false; + if (!processed) + { + + TrackerMessage message; + if (DecodeTrackerMessage(&message, pData, length)) + { + processed = true; + onTrackerMessage(&message); + } + } +} + +UInt64 SensorDeviceImpl::OnTicks(UInt64 ticksMks) +{ + + if (ticksMks >= NextKeepAliveTicks) + { + // Use 3-seconds keep alive by default. + UInt64 keepAliveDelta = Timer::MksPerSecond * 3; + + // Set Keep-alive at 10 seconds. + SensorKeepAliveImpl skeepAlive(10 * 1000); + // OnTicks is called from background thread so we don't need to add this to the command queue. + GetInternalDevice()->SetFeatureReport(skeepAlive.Buffer, SensorKeepAliveImpl::PacketSize); + + // Emit keep-alive every few seconds. + NextKeepAliveTicks = ticksMks + keepAliveDelta; + } + return NextKeepAliveTicks - ticksMks; +} + +bool SensorDeviceImpl::SetRange(const SensorRange& range, bool waitFlag) +{ + bool result = 0; + ThreadCommandQueue * threadQueue = GetManagerImpl()->GetThreadQueue(); + + if (!waitFlag) + { + return threadQueue->PushCall(this, &SensorDeviceImpl::setRange, range); + } + + if (!threadQueue->PushCallAndWaitResult(this, + &SensorDeviceImpl::setRange, + &result, + range)) + { + return false; + } + + return result; +} + +void SensorDeviceImpl::GetRange(SensorRange* range) const +{ + Lock::Locker lockScope(GetLock()); + *range = CurrentRange; +} + +bool SensorDeviceImpl::setRange(const SensorRange& range) +{ + SensorRangeImpl sr(range); + + if (GetInternalDevice()->SetFeatureReport(sr.Buffer, SensorRangeImpl::PacketSize)) + { + Lock::Locker lockScope(GetLock()); + sr.GetSensorRange(&CurrentRange); + return true; + } + + return false; +} + +void SensorDeviceImpl::SetCoordinateFrame(CoordinateFrame coordframe) +{ + // Push call with wait. + GetManagerImpl()->GetThreadQueue()-> + PushCall(this, &SensorDeviceImpl::setCoordinateFrame, coordframe, true); +} + +SensorDevice::CoordinateFrame SensorDeviceImpl::GetCoordinateFrame() const +{ + return Coordinates; +} + +Void SensorDeviceImpl::setCoordinateFrame(CoordinateFrame coordframe) +{ + + Coordinates = coordframe; + + // Read the original coordinate frame, then try to change it. + SensorConfigImpl scfg; + if (GetInternalDevice()->GetFeatureReport(scfg.Buffer, SensorConfigImpl::PacketSize)) + { + scfg.Unpack(); + } + + scfg.SetSensorCoordinates(coordframe == Coord_Sensor); + scfg.Pack(); + + GetInternalDevice()->SetFeatureReport(scfg.Buffer, SensorConfigImpl::PacketSize); + + // Re-read the state, in case of older firmware that doesn't support Sensor coordinates. + if (GetInternalDevice()->GetFeatureReport(scfg.Buffer, SensorConfigImpl::PacketSize)) + { + scfg.Unpack(); + HWCoordinates = scfg.IsUsingSensorCoordinates() ? Coord_Sensor : Coord_HMD; + } + else + { + HWCoordinates = Coord_HMD; + } + return 0; +} + +void SensorDeviceImpl::SetReportRate(unsigned rateHz) +{ + // Push call with wait. + GetManagerImpl()->GetThreadQueue()-> + PushCall(this, &SensorDeviceImpl::setReportRate, rateHz, true); +} + +unsigned SensorDeviceImpl::GetReportRate() const +{ + // Read the original configuration + SensorConfigImpl scfg; + if (GetInternalDevice()->GetFeatureReport(scfg.Buffer, SensorConfigImpl::PacketSize)) + { + scfg.Unpack(); + return Sensor_MaxReportRate / (scfg.PacketInterval + 1); + } + return 0; // error +} + +Void SensorDeviceImpl::setReportRate(unsigned rateHz) +{ + // Read the original configuration + SensorConfigImpl scfg; + if (GetInternalDevice()->GetFeatureReport(scfg.Buffer, SensorConfigImpl::PacketSize)) + { + scfg.Unpack(); + } + + if (rateHz > Sensor_MaxReportRate) + rateHz = Sensor_MaxReportRate; + else if (rateHz == 0) + rateHz = Sensor_DefaultReportRate; + + scfg.PacketInterval = UInt16((Sensor_MaxReportRate / rateHz) - 1); + + scfg.Pack(); + + GetInternalDevice()->SetFeatureReport(scfg.Buffer, SensorConfigImpl::PacketSize); + return 0; +} + +void SensorDeviceImpl::SetMessageHandler(MessageHandler* handler) +{ + if (handler) + { + SequenceValid = false; + DeviceBase::SetMessageHandler(handler); + } + else + { + DeviceBase::SetMessageHandler(handler); + } +} + +// Sensor reports data in the following coordinate system: +// Accelerometer: 10^-4 m/s^2; X forward, Y right, Z Down. +// Gyro: 10^-4 rad/s; X positive roll right, Y positive pitch up; Z positive yaw right. + + +// We need to convert it to the following RHS coordinate system: +// X right, Y Up, Z Back (out of screen) +// +Vector3f AccelFromBodyFrameUpdate(const TrackerSensors& update, UByte sampleNumber, + bool convertHMDToSensor = false) +{ + const TrackerSample& sample = update.Samples[sampleNumber]; + float ax = (float)sample.AccelX; + float ay = (float)sample.AccelY; + float az = (float)sample.AccelZ; + + Vector3f val = convertHMDToSensor ? Vector3f(ax, az, -ay) : Vector3f(ax, ay, az); + return val * 0.0001f; +} + + +Vector3f MagFromBodyFrameUpdate(const TrackerSensors& update, + bool convertHMDToSensor = false) +{ + // Note: Y and Z are swapped in comparison to the Accel. + // This accounts for DK1 sensor firmware axis swap, which should be undone in future releases. + if (!convertHMDToSensor) + { + return Vector3f( (float)update.MagX, + (float)update.MagZ, + (float)update.MagY) * 0.0001f; + } + + return Vector3f( (float)update.MagX, + (float)update.MagY, + -(float)update.MagZ) * 0.0001f; +} + +Vector3f EulerFromBodyFrameUpdate(const TrackerSensors& update, UByte sampleNumber, + bool convertHMDToSensor = false) +{ + const TrackerSample& sample = update.Samples[sampleNumber]; + float gx = (float)sample.GyroX; + float gy = (float)sample.GyroY; + float gz = (float)sample.GyroZ; + + Vector3f val = convertHMDToSensor ? Vector3f(gx, gz, -gy) : Vector3f(gx, gy, gz); + return val * 0.0001f; +} + + +void SensorDeviceImpl::onTrackerMessage(TrackerMessage* message) +{ + if (message->Type != TrackerMessage_Sensors) + return; + + const float timeUnit = (1.0f / 1000.f); + TrackerSensors& s = message->Sensors; + + + // Call OnMessage() within a lock to avoid conflicts with handlers. + Lock::Locker scopeLock(HandlerRef.GetLock()); + + + if (SequenceValid) + { + unsigned timestampDelta; + + if (s.Timestamp < LastTimestamp) + timestampDelta = ((((int)s.Timestamp) + 0x10000) - (int)LastTimestamp); + else + timestampDelta = (s.Timestamp - LastTimestamp); + + // If we missed a small number of samples, replicate the last sample. + if ((timestampDelta > LastSampleCount) && (timestampDelta <= 254)) + { + if (HandlerRef.GetHandler()) + { + MessageBodyFrame sensors(this); + sensors.TimeDelta = (timestampDelta - LastSampleCount) * timeUnit; + sensors.Acceleration = LastAcceleration; + sensors.RotationRate = LastRotationRate; + sensors.MagneticField = LastMagneticField; + sensors.Temperature = LastTemperature; + + HandlerRef.GetHandler()->OnMessage(sensors); + } + } + } + else + { + LastAcceleration = Vector3f(0); + LastRotationRate = Vector3f(0); + LastMagneticField= Vector3f(0); + LastTemperature = 0; + SequenceValid = true; + } + + LastSampleCount = s.SampleCount; + LastTimestamp = s.Timestamp; + + bool convertHMDToSensor = (Coordinates == Coord_Sensor) && (HWCoordinates == Coord_HMD); + + if (HandlerRef.GetHandler()) + { + MessageBodyFrame sensors(this); + UByte iterations = s.SampleCount; + + if (s.SampleCount > 3) + { + iterations = 3; + sensors.TimeDelta = (s.SampleCount - 2) * timeUnit; + } + else + { + sensors.TimeDelta = timeUnit; + } + + for (UByte i = 0; i < iterations; i++) + { + sensors.Acceleration = AccelFromBodyFrameUpdate(s, i, convertHMDToSensor); + sensors.RotationRate = EulerFromBodyFrameUpdate(s, i, convertHMDToSensor); + sensors.MagneticField= MagFromBodyFrameUpdate(s, convertHMDToSensor); + sensors.Temperature = s.Temperature * 0.01f; + HandlerRef.GetHandler()->OnMessage(sensors); + // TimeDelta for the last two sample is always fixed. + sensors.TimeDelta = timeUnit; + } + + LastAcceleration = sensors.Acceleration; + LastRotationRate = sensors.RotationRate; + LastMagneticField= sensors.MagneticField; + LastTemperature = sensors.Temperature; + } + else + { + UByte i = (s.SampleCount > 3) ? 2 : (s.SampleCount - 1); + LastAcceleration = AccelFromBodyFrameUpdate(s, i, convertHMDToSensor); + LastRotationRate = EulerFromBodyFrameUpdate(s, i, convertHMDToSensor); + LastMagneticField = MagFromBodyFrameUpdate(s, convertHMDToSensor); + LastTemperature = s.Temperature * 0.01f; + } +} + +} // namespace OVR + + |