diff options
Diffstat (limited to 'LibOVR/Src/OVR_SensorImpl.cpp')
| -rw-r--r-- | LibOVR/Src/OVR_SensorImpl.cpp | 1165 |
1 files changed, 1165 insertions, 0 deletions
diff --git a/LibOVR/Src/OVR_SensorImpl.cpp b/LibOVR/Src/OVR_SensorImpl.cpp new file mode 100644 index 0000000..91ae7e0 --- /dev/null +++ b/LibOVR/Src/OVR_SensorImpl.cpp @@ -0,0 +1,1165 @@ +/************************************************************************************ + +Filename : OVR_SensorImpl.cpp +Content : Oculus Sensor device implementation. +Created : March 7, 2013 +Authors : Lee Cooper, Dov Katz + +Copyright : Copyright 2014 Oculus VR, Inc. All Rights reserved. + +Licensed under the Oculus VR Rift SDK License Version 3.1 (the "License"); +you may not use the Oculus VR Rift SDK except in compliance with the License, +which is provided at the time of installation or download, or which +otherwise accompanies this software in either electronic or hard copy form. + +You may obtain a copy of the License at + +http://www.oculusvr.com/licenses/LICENSE-3.1 + +Unless required by applicable law or agreed to in writing, the Oculus VR SDK +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. + +*************************************************************************************/ + +#include "OVR_SensorImpl.h" +#include "OVR_Sensor2Impl.h" +#include "OVR_SensorImpl_Common.h" +#include "OVR_JSON.h" +#include "OVR_Profile.h" +#include "Kernel/OVR_Alg.h" +#include <time.h> + +// HMDDeviceDesc can be created/updated through Sensor carrying DisplayInfo. + +#include "Kernel/OVR_Timer.h" + +//extern FILE *SF_LOG_fp; + +namespace OVR { + +using namespace Alg; + +//------------------------------------------------------------------------------------- +// ***** Oculus Sensor-specific packet data structures + +enum { + Sensor_VendorId = Oculus_VendorId, + Sensor_Tracker_ProductId = Device_Tracker_ProductId, + Sensor_Tracker2_ProductId = Device_Tracker2_ProductId, + Sensor_KTracker_ProductId = Device_KTracker_ProductId, + + Sensor_BootLoader = 0x1001, + + Sensor_DefaultReportRate = 500, // Hz + Sensor_MaxReportRate = 1000 // Hz +}; + + +// Messages we care for +enum TrackerMessageType +{ + TrackerMessage_None = 0, + TrackerMessage_Sensors = 1, + TrackerMessage_Unknown = 0x100, + TrackerMessage_SizeError = 0x101, +}; + + +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 + int iterationCount = (SampleCount > 2) ? 3 : SampleCount; + + for (int 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; +}; + + +//------------------------------------------------------------------------------------- +// ***** 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); + +#if 0 + // These are not well-measured on most devices - probably best to ignore them. + OutsideLensSurfaceToScreen[0] = DecodeUInt32(Buffer+24) * (1/1000000.f); + OutsideLensSurfaceToScreen[1] = DecodeUInt32(Buffer+28) * (1/1000000.f); + // TODO: add spline-based distortion. + // TODO: currently these values are all zeros in the HMD itself. + 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); +#else + // The above are either measured poorly, or don't have values at all. + // To remove the temptation to use them, set them to junk. + OutsideLensSurfaceToScreen[0] = -1.0f; + OutsideLensSurfaceToScreen[1] = -1.0f; + DistortionK[0] = -1.0f; + DistortionK[1] = -1.0f; + DistortionK[2] = -1.0f; + DistortionK[3] = -1.0f; + DistortionK[4] = -1.0f; + DistortionK[5] = -1.0f; +#endif +} + + +//------------------------------------------------------------------------------------- +// ***** SensorDeviceFactory + +SensorDeviceFactory &SensorDeviceFactory::GetInstance() +{ + static SensorDeviceFactory instance; + return 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) + { + + if (desc.ProductId == Sensor_BootLoader) + { // If we find a sensor in boot loader mode then notify the app + // about the existence of the device, but don't allow the app + // to create or access the device + BootLoaderDeviceCreateDesc createDesc(pFactory, desc); + ExternalVisitor.Visit(createDesc); + return; + } + + 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_Tracker_ProductId)) || + ((vendorId == Sensor_VendorId) && (productId == Sensor_Tracker2_ProductId)) || + ((vendorId == Sensor_VendorId) && (productId == Sensor_KTracker_ProductId)); +} + +bool SensorDeviceFactory::DetectHIDDevice(DeviceManager* pdevMgr, const HIDDeviceDesc& desc) +{ + if (MatchVendorProduct(desc.VendorId, desc.ProductId)) + { + if (desc.ProductId == Sensor_BootLoader) + { // If we find a sensor in boot loader mode then notify the app + // about the existence of the device, but don't allow them + // to create or access the device + BootLoaderDeviceCreateDesc createDesc(this, desc); + pdevMgr->AddDevice_NeedsLock(createDesc); + return false; // return false to allow upstream boot loader factories to catch the device + } + else + { + SensorDeviceCreateDesc createDesc(this, desc); + return pdevMgr->AddDevice_NeedsLock(createDesc).GetPtr() != NULL; + } + } + return false; +} + +//------------------------------------------------------------------------------------- +// ***** SensorDeviceCreateDesc + +DeviceBase* SensorDeviceCreateDesc::NewDeviceInstance() +{ + if (HIDDesc.ProductId == Sensor_Tracker2_ProductId) + { + return new Sensor2DeviceImpl(this); + } + + return new SensorDeviceImpl(this); +} + +bool SensorDeviceCreateDesc::GetDeviceInfo(DeviceInfo* info) const +{ + if ((info->InfoClassType != Device_Sensor) && + (info->InfoClassType != Device_None)) + return false; + + info->Type = Device_Sensor; + info->ProductName = HIDDesc.Product; + info->Manufacturer = HIDDesc.Manufacturer; + info->Version = HIDDesc.VersionNumber; + + if (info->InfoClassType == Device_Sensor) + { + SensorInfo* sinfo = (SensorInfo*)info; + sinfo->VendorId = HIDDesc.VendorId; + sinfo->ProductId = HIDDesc.ProductId; + sinfo->MaxRanges = SensorRangeImpl::GetMaxSensorRange(); + sinfo->SerialNumber = HIDDesc.SerialNumber; + } + 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. + NextKeepAliveTickSeconds(0), + FullTimestamp(0), + MaxValidRange(SensorRangeImpl::GetMaxSensorRange()), + magCalibrated(false) +{ + SequenceValid = false; + LastSampleCount = 0; + LastTimestamp = 0; + + OldCommandId = 0; + + PrevAbsoluteTime = 0.0; + +#ifdef OVR_OS_ANDROID + pPhoneSensors = PhoneSensors::Create(); +#endif +} + +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(); + 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); + // Increase the magnetometer range, since the default value is not enough in practice + CurrentRange.MaxMagneticField = 2.5f; + setRange(CurrentRange); + } + + // Read the currently configured calibration from sensor. + SensorFactoryCalibrationImpl sc; + if (GetInternalDevice()->GetFeatureReport(sc.Buffer, SensorFactoryCalibrationImpl::PacketSize)) + { + sc.Unpack(); + AccelCalibrationOffset = sc.AccelOffset; + GyroCalibrationOffset = sc.GyroOffset; + AccelCalibrationMatrix = sc.AccelMatrix; + GyroCalibrationMatrix = sc.GyroMatrix; + CalibrationTemperature = sc.Temperature; + } + + // 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); + + // Load mag calibration + MagCalibrationReport report; + bool res = GetMagCalibrationReport(&report); + if (res && report.Version > 0) + { + magCalibration = report.Calibration; + magCalibrated = true; + } +} + +void SensorDeviceImpl::closeDeviceOnError() +{ + LogText("OVR::SensorDevice - Lost connection to '%s'\n", getHIDDesc()->Path.ToCStr()); + NextKeepAliveTickSeconds = 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); + } + } +} + +double SensorDeviceImpl::OnTicks(double tickSeconds) +{ + if (tickSeconds >= NextKeepAliveTickSeconds) + { + // Use 3-seconds keep alive by default. + double keepAliveDelta = 3.0; + + // 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. + NextKeepAliveTickSeconds = tickSeconds + keepAliveDelta; + } + return NextKeepAliveTickSeconds - tickSeconds; +} + +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::GetFactoryCalibration(Vector3f* AccelOffset, Vector3f* GyroOffset, + Matrix4f* AccelMatrix, Matrix4f* GyroMatrix, + float* Temperature) +{ + *AccelOffset = AccelCalibrationOffset; + *GyroOffset = GyroCalibrationOffset; + *AccelMatrix = AccelCalibrationMatrix; + *GyroMatrix = GyroCalibrationMatrix; + *Temperature = CalibrationTemperature; +} + +bool SensorDeviceImpl::IsMagCalibrated() +{ + return magCalibrated; +} + +void SensorDeviceImpl::SetOnboardCalibrationEnabled(bool enabled) +{ + // Push call with wait. + GetManagerImpl()->GetThreadQueue()-> + PushCall(this, &SensorDeviceImpl::setOnboardCalibrationEnabled, enabled, true); +} + +Void SensorDeviceImpl::setOnboardCalibrationEnabled(bool enabled) +{ + // Read the original configuration + SensorConfigImpl scfg; + if (GetInternalDevice()->GetFeatureReport(scfg.Buffer, SensorConfigImpl::PacketSize)) + { + scfg.Unpack(); + } + + if (enabled) + scfg.Flags |= (SensorConfigImpl::Flag_AutoCalibration | SensorConfigImpl::Flag_UseCalibration); + else + scfg.Flags &= ~(SensorConfigImpl::Flag_AutoCalibration | SensorConfigImpl::Flag_UseCalibration); + + scfg.Pack(); + + GetInternalDevice()->SetFeatureReport(scfg.Buffer, SensorConfigImpl::PacketSize); + return 0; +} + +void SensorDeviceImpl::AddMessageHandler(MessageHandler* handler) +{ + if (handler) + SequenceValid = false; + DeviceBase::AddMessageHandler(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, + Matrix4f magCalibration, + bool convertHMDToSensor = false) +{ + float mx = (float)update.MagX; + float my = (float)update.MagY; + float mz = (float)update.MagZ; + // 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. + Vector3f mag = convertHMDToSensor ? Vector3f(mx, my, -mz) : Vector3f(mx, mz, my); + mag *= 0.0001f; + // Apply calibration + return magCalibration.Transform(mag); +} + +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; +} + +bool SensorDeviceImpl::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); +} + +void SensorDeviceImpl::onTrackerMessage(TrackerMessage* message) +{ + if (message->Type != TrackerMessage_Sensors) + return; + + const double timeUnit = (1.0 / 1000.0); + double scaledTimeUnit = timeUnit; + TrackerSensors& s = message->Sensors; + // DK1 timestamps the first sample, so the actual device time will be later + // by the time we get the message if there are multiple samples. + int timestampAdjust = (s.SampleCount > 0) ? s.SampleCount-1 : 0; + + const double now = Timer::GetSeconds(); + double absoluteTimeSeconds = 0.0; + + + if (SequenceValid) + { + unsigned timestampDelta; + + if (s.Timestamp < LastTimestamp) + { + // The timestamp rolled around the 16 bit counter, so FullTimeStamp + // needs a high word increment. + FullTimestamp += 0x10000; + timestampDelta = ((((int)s.Timestamp) + 0x10000) - (int)LastTimestamp); + } + else + { + timestampDelta = (s.Timestamp - LastTimestamp); + } + // Update the low word of FullTimeStamp + FullTimestamp = ( FullTimestamp & ~0xffff ) | s.Timestamp; + + double deviceTime = (FullTimestamp + timestampAdjust) * timeUnit; + absoluteTimeSeconds = TimeFilter.SampleToSystemTime(deviceTime, now, PrevAbsoluteTime); + scaledTimeUnit = TimeFilter.ScaleTimeUnit(timeUnit); + PrevAbsoluteTime = absoluteTimeSeconds; + + // If we missed a small number of samples, generate the sample that would have immediately + // proceeded the current one. Re-use the IMU values from the last processed sample. + if ((timestampDelta > LastSampleCount) && (timestampDelta <= 254)) + { + if (HandlerRef.HasHandlers()) + { + MessageBodyFrame sensors(this); + + sensors.AbsoluteTimeSeconds = absoluteTimeSeconds - s.SampleCount * scaledTimeUnit; + sensors.TimeDelta = (float)((timestampDelta - LastSampleCount) * scaledTimeUnit); + sensors.Acceleration = LastAcceleration; + sensors.RotationRate = LastRotationRate; + sensors.MagneticField = LastMagneticField; + sensors.Temperature = LastTemperature; + + HandlerRef.Call(sensors); + } + } + } + else + { + LastAcceleration = Vector3f(0); + LastRotationRate = Vector3f(0); + LastMagneticField= Vector3f(0); + LastTemperature = 0; + SequenceValid = true; + + // This is our baseline sensor to host time delta, + // it will be adjusted with each new message. + FullTimestamp = s.Timestamp; + + double deviceTime = (FullTimestamp + timestampAdjust) * timeUnit; + absoluteTimeSeconds = TimeFilter.SampleToSystemTime(deviceTime, now, PrevAbsoluteTime); + scaledTimeUnit = TimeFilter.ScaleTimeUnit(timeUnit); + PrevAbsoluteTime = absoluteTimeSeconds; + } + + LastSampleCount = s.SampleCount; + LastTimestamp = s.Timestamp; + + bool convertHMDToSensor = (Coordinates == Coord_Sensor) && (HWCoordinates == Coord_HMD); + +#ifdef OVR_OS_ANDROID + // LDC - Normally we get the coordinate system from the tracker. + // Since KTracker doesn't store it we'll always assume HMD coordinate system. + convertHMDToSensor = false; +#endif + + if (HandlerRef.HasHandlers()) + { + MessageBodyFrame sensors(this); + UByte iterations = s.SampleCount; + + if (s.SampleCount > 3) + { + iterations = 3; + sensors.TimeDelta = (float)((s.SampleCount - 2) * scaledTimeUnit); + } + else + { + sensors.TimeDelta = (float)scaledTimeUnit; + } + + for (UByte i = 0; i < iterations; i++) + { + sensors.AbsoluteTimeSeconds = absoluteTimeSeconds - ( iterations - 1 - i ) * scaledTimeUnit; + sensors.Acceleration = AccelFromBodyFrameUpdate(s, i, convertHMDToSensor); + sensors.RotationRate = EulerFromBodyFrameUpdate(s, i, convertHMDToSensor); + sensors.MagneticField = MagFromBodyFrameUpdate(s, magCalibration, convertHMDToSensor); + +#ifdef OVR_OS_ANDROID + replaceWithPhoneMag(&(sensors.MagneticField)); +#endif + sensors.Temperature = s.Temperature * 0.01f; + HandlerRef.Call(sensors); + // TimeDelta for the last two sample is always fixed. + sensors.TimeDelta = (float)scaledTimeUnit; + } + + 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, magCalibration, convertHMDToSensor); + +#ifdef OVR_OS_ANDROID + replaceWithPhoneMag(&LastMagneticField); +#endif + LastTemperature = s.Temperature * 0.01f; + } +} + + +#ifdef OVR_OS_ANDROID + +void SensorDeviceImpl::replaceWithPhoneMag(Vector3f* val) +{ + + // Native calibrated. + pPhoneSensors->SetMagSource(PhoneSensors::MagnetometerSource_Native); + + Vector3f magPhone; + pPhoneSensors->GetLatestMagValue(&magPhone); + + // Phone value is in micro-Tesla. Convert it to Gauss and flip axes. + magPhone *= 10000.0f/1000000.0f; + + Vector3f res; + res.x = -magPhone.y; + res.y = magPhone.x; + res.z = magPhone.z; + + *val = res; +} +#endif + +const int MAX_DEVICE_PROFILE_MAJOR_VERSION = 1; + +// Writes the current calibration for a particular device to a device profile file +bool SensorDeviceImpl::SetMagCalibrationReport(const MagCalibrationReport &data) +{ + // Get device info + SensorInfo sinfo; + GetDeviceInfo(&sinfo); + + // A named calibration may be specified for calibration in different + // environments, otherwise the default calibration is used + const char* calibrationName = "default"; + + // Generate a mag calibration event + JSON* calibration = JSON::CreateObject(); + // (hardcoded for now) the measurement and representation method + calibration->AddStringItem("Version", "2.0"); + calibration->AddStringItem("Name", "default"); + + // time stamp the calibration + char time_str[64]; + +#ifdef OVR_OS_WIN32 + struct tm caltime; + time_t now = time(0); + localtime_s(&caltime, &now); + strftime(time_str, 64, "%Y-%m-%d %H:%M:%S", &caltime); +#else + struct tm* caltime; + time_t now = time(0); + caltime = localtime(&now); + strftime(time_str, 64, "%Y-%m-%d %H:%M:%S", caltime); +#endif + + calibration->AddStringItem("Time", time_str); + + // write the full calibration matrix + char matrix[256]; + data.Calibration.ToString(matrix, 256); + calibration->AddStringItem("CalibrationMatrix", matrix); + // save just the offset, for backwards compatibility + // this can be removed when we don't want to support 0.2.4 anymore + Vector3f center(data.Calibration.M[0][3], data.Calibration.M[1][3], data.Calibration.M[2][3]); + Matrix4f tmp = data.Calibration; tmp.M[0][3] = tmp.M[1][3] = tmp.M[2][3] = 0; tmp.M[3][3] = 1; + center = tmp.Inverted().Transform(center); + Matrix4f oldcalmat; oldcalmat.M[0][3] = center.x; oldcalmat.M[1][3] = center.y; oldcalmat.M[2][3] = center.z; + oldcalmat.ToString(matrix, 256); + calibration->AddStringItem("Calibration", matrix); + + String path = GetBaseOVRPath(true); + path += "/Devices.json"; + + // Look for a preexisting device file to edit + Ptr<JSON> root = *JSON::Load(path); + if (root) + { // Quick sanity check of the file type and format before we parse it + JSON* version = root->GetFirstItem(); + if (version && version->Name == "Oculus Device Profile Version") + { + int major = atoi(version->Value.ToCStr()); + if (major > MAX_DEVICE_PROFILE_MAJOR_VERSION) + { + // don't use the file on unsupported major version number + root->Release(); + root = NULL; + } + } + else + { + root->Release(); + root = NULL; + } + } + + JSON* device = NULL; + if (root) + { + device = root->GetFirstItem(); // skip the header + device = root->GetNextItem(device); + while (device) + { // Search for a previous calibration with the same name for this device + // and remove it before adding the new one + if (device->Name == "Device") + { + JSON* item = device->GetItemByName("Serial"); + if (item && item->Value == sinfo.SerialNumber) + { // found an entry for this device + item = device->GetNextItem(item); + while (item) + { + if (item->Name == "MagCalibration") + { + JSON* name = item->GetItemByName("Name"); + if (name && name->Value == calibrationName) + { // found a calibration of the same name + item->RemoveNode(); + item->Release(); + break; + } + } + item = device->GetNextItem(item); + } + + + /* + this is removed temporarily, since this is a sensor fusion setting, not sensor itself + should be moved to the correct place when Brant has finished the user profile implementation + // update the auto-mag flag + item = device->GetItemByName("EnableYawCorrection"); + if (item) + item->dValue = (double)EnableYawCorrection; + else + device->AddBoolItem("EnableYawCorrection", EnableYawCorrection);*/ + + break; + } + } + + device = root->GetNextItem(device); + } + } + else + { // Create a new device root + root = *JSON::CreateObject(); + root->AddStringItem("Oculus Device Profile Version", "1.0"); + } + + if (device == NULL) + { + device = JSON::CreateObject(); + device->AddStringItem("Product", sinfo.ProductName); + device->AddNumberItem("ProductID", sinfo.ProductId); + device->AddStringItem("Serial", sinfo.SerialNumber); + // removed temporarily, see above + //device->AddBoolItem("EnableYawCorrection", EnableYawCorrection); + + root->AddItem("Device", device); + } + + // Create and the add the new calibration event to the device + device->AddItem("MagCalibration", calibration); + return root->Save(path); +} + +// Loads a saved calibration for the specified device from the device profile file +bool SensorDeviceImpl::GetMagCalibrationReport(MagCalibrationReport* data) +{ + data->Version = 0; + data->Calibration.SetIdentity(); + + // Get device info + SensorInfo sinfo; + GetDeviceInfo(&sinfo); + + // A named calibration may be specified for calibration in different + // environments, otherwise the default calibration is used + const char* calibrationName = "default"; + + String path = GetBaseOVRPath(true); + path += "/Devices.json"; + + // Load the device profiles + Ptr<JSON> root = *JSON::Load(path); + if (root == NULL) + return false; + + // Quick sanity check of the file type and format before we parse it + JSON* version = root->GetFirstItem(); + if (version && version->Name == "Oculus Device Profile Version") + { + int major = atoi(version->Value.ToCStr()); + if (major > MAX_DEVICE_PROFILE_MAJOR_VERSION) + return false; // don't parse the file on unsupported major version number + } + else + { + return false; + } + + JSON* device = root->GetNextItem(version); + while (device) + { // Search for a previous calibration with the same name for this device + // and remove it before adding the new one + if (device->Name == "Device") + { + JSON* item = device->GetItemByName("Serial"); + if (item && item->Value == sinfo.SerialNumber) + { // found an entry for this device + + JSON* autoyaw = device->GetItemByName("EnableYawCorrection"); + // as a temporary HACK, return no calibration if EnableYawCorrection is off + // this will force disable yaw correction in SensorFusion + // proper solution would load the value in the Profile, which SensorFusion can access + if (autoyaw && autoyaw->dValue == 0) + return true; + + item = device->GetNextItem(item); + while (item) + { + if (item->Name == "MagCalibration") + { + JSON* calibration = item; + JSON* name = calibration->GetItemByName("Name"); + if (name && name->Value == calibrationName) + { // found a calibration with this name + + int major = 0; + JSON* version = calibration->GetItemByName("Version"); + if (version) + major = atoi(version->Value.ToCStr()); + + if (major > data->Version && major <= 2) + { + time_t now; + time(&now); + + // parse the calibration time + //time_t calibration_time = now; + JSON* caltime = calibration->GetItemByName("Time"); + if (caltime) + { + const char* caltime_str = caltime->Value.ToCStr(); + + tm ct; + memset(&ct, 0, sizeof(tm)); + +#ifdef OVR_OS_WIN32 + struct tm nowtime; + localtime_s(&nowtime, &now); + ct.tm_isdst = nowtime.tm_isdst; + sscanf_s(caltime_str, "%d-%d-%d %d:%d:%d", + &ct.tm_year, &ct.tm_mon, &ct.tm_mday, + &ct.tm_hour, &ct.tm_min, &ct.tm_sec); +#else + struct tm* nowtime = localtime(&now); + ct.tm_isdst = nowtime->tm_isdst; + sscanf(caltime_str, "%d-%d-%d %d:%d:%d", + &ct.tm_year, &ct.tm_mon, &ct.tm_mday, + &ct.tm_hour, &ct.tm_min, &ct.tm_sec); +#endif + ct.tm_year -= 1900; + ct.tm_mon--; + //calibration_time = mktime(&ct); + } + + // parse the calibration matrix + JSON* cal = calibration->GetItemByName("CalibrationMatrix"); + if (!cal) + cal = calibration->GetItemByName("Calibration"); + if (cal) + { + data->Calibration = Matrix4f::FromString(cal->Value.ToCStr()); + data->Version = (UByte)major; + } + } + } + } + item = device->GetNextItem(item); + } + + return true; + } + } + + device = root->GetNextItem(device); + } + + return true; +} + + +bool SensorDeviceImpl::SetSerialReport(const SerialReport& data) +{ + bool result; + if (!GetManagerImpl()->GetThreadQueue()-> + PushCallAndWaitResult(this, &Sensor2DeviceImpl::setSerialReport, &result, data)) + { + return false; + } + + return result; +} + +bool SensorDeviceImpl::setSerialReport(const SerialReport& data) +{ + SerialImpl di(data); + return GetInternalDevice()->SetFeatureReport(di.Buffer, SerialImpl::PacketSize); +} + +bool SensorDeviceImpl::GetSerialReport(SerialReport* data) +{ + bool result; + if (!GetManagerImpl()->GetThreadQueue()-> + PushCallAndWaitResult(this, &Sensor2DeviceImpl::getSerialReport, &result, data)) + { + return false; + } + + return result; +} + +bool SensorDeviceImpl::getSerialReport(SerialReport* data) +{ + SerialImpl di; + if (GetInternalDevice()->GetFeatureReport(di.Buffer, SerialImpl::PacketSize)) + { + di.Unpack(); + *data = di.Settings; + return true; + } + + return false; +} + +} // namespace OVR |