mirror of
https://github.com/mii443/rust-openvr.git
synced 2025-08-24 17:19:25 +00:00
1166 lines
39 KiB
C++
1166 lines
39 KiB
C++
/************************************************************************************
|
|
|
|
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
|