From f29e505a7df820e1376d97b41515bdc89d472499 Mon Sep 17 00:00:00 2001 From: Brad Davis Date: Fri, 16 Aug 2013 10:21:36 -0700 Subject: Official SDK version 0.24 --- LibOVR/Src/Kernel/OVR_List.h | 2 +- LibOVR/Src/Kernel/OVR_Math.h | 2 +- LibOVR/Src/Kernel/OVR_Types.h | 2 +- LibOVR/Src/OVR_DeviceImpl.h | 2 +- LibOVR/Src/OVR_Linux_HIDDevice.cpp | 2 +- LibOVR/Src/OVR_Linux_HMDDevice.cpp | 2 +- LibOVR/Src/OVR_Profile.cpp | 43 ++-- LibOVR/Src/OVR_Profile.h | 3 + LibOVR/Src/OVR_SensorFusion.cpp | 334 ++++++++++++++++++++++++++++---- LibOVR/Src/OVR_SensorFusion.h | 213 +++++++++++--------- LibOVR/Src/Util/Util_MagCalibration.cpp | 57 +++++- LibOVR/Src/Util/Util_MagCalibration.h | 19 +- LibOVR/Src/Util/Util_Render_Stereo.cpp | 4 +- 13 files changed, 522 insertions(+), 163 deletions(-) (limited to 'LibOVR/Src') diff --git a/LibOVR/Src/Kernel/OVR_List.h b/LibOVR/Src/Kernel/OVR_List.h index 4b32f49..6ad471d 100644 --- a/LibOVR/Src/Kernel/OVR_List.h +++ b/LibOVR/Src/Kernel/OVR_List.h @@ -115,7 +115,7 @@ struct ListNode // } // -// List<> represents a doubly-linked list if T, where each T must derive +// List<> represents a doubly-linked list of T, where each T must derive // from ListNode. B specifies the base class that was directly // derived from ListNode, and is only necessary if there is an intermediate // inheritance chain. diff --git a/LibOVR/Src/Kernel/OVR_Math.h b/LibOVR/Src/Kernel/OVR_Math.h index 2b255c7..567ea9c 100644 --- a/LibOVR/Src/Kernel/OVR_Math.h +++ b/LibOVR/Src/Kernel/OVR_Math.h @@ -879,7 +879,7 @@ public: } // Sets this quaternion to the one rotates in the opposite direction. - void Invert() const + void Invert() { *this = Quat(-x, -y, -z, w); } diff --git a/LibOVR/Src/Kernel/OVR_Types.h b/LibOVR/Src/Kernel/OVR_Types.h index 75493aa..db024f1 100644 --- a/LibOVR/Src/Kernel/OVR_Types.h +++ b/LibOVR/Src/Kernel/OVR_Types.h @@ -291,7 +291,7 @@ namespace BaseTypes #if defined(OVR_CC_MSVC) # define OVR_FORCE_INLINE __forceinline #elif defined(OVR_CC_GNU) -# define OVR_FORCE_INLINE __attribute__((always_inline)) +# define OVR_FORCE_INLINE __attribute__((always_inline)) inline #else # define OVR_FORCE_INLINE inline #endif // OVR_CC_MSVC diff --git a/LibOVR/Src/OVR_DeviceImpl.h b/LibOVR/Src/OVR_DeviceImpl.h index 2273d3d..e61e37b 100644 --- a/LibOVR/Src/OVR_DeviceImpl.h +++ b/LibOVR/Src/OVR_DeviceImpl.h @@ -350,7 +350,7 @@ public: virtual void Shutdown(); - // Every DeviceManager has an associated profile manager, which us used to store + // Every DeviceManager has an associated profile manager, which is used to store // user settings that may affect device behavior. virtual ProfileManager* GetProfileManager() const { return pProfileManager.GetPtr(); } diff --git a/LibOVR/Src/OVR_Linux_HIDDevice.cpp b/LibOVR/Src/OVR_Linux_HIDDevice.cpp index 6656e9a..062d566 100644 --- a/LibOVR/Src/OVR_Linux_HIDDevice.cpp +++ b/LibOVR/Src/OVR_Linux_HIDDevice.cpp @@ -258,7 +258,7 @@ bool HIDDeviceManager::Enumerate(HIDEnumerateVisitor* enumVisitor) udev_device_unref(hid); entry = udev_list_entry_get_next(entry); } - } + } // Free the enumerator and udev objects udev_enumerate_unref(devices); diff --git a/LibOVR/Src/OVR_Linux_HMDDevice.cpp b/LibOVR/Src/OVR_Linux_HMDDevice.cpp index 7b26479..dc94851 100644 --- a/LibOVR/Src/OVR_Linux_HMDDevice.cpp +++ b/LibOVR/Src/OVR_Linux_HMDDevice.cpp @@ -176,7 +176,7 @@ void HMDDeviceFactory::EnumerateDevices(EnumerateVisitor& visitor) bool foundHMD = false; Display* display = XOpenDisplay(NULL); - if (display) + if (display && XineramaIsActive(display)) { int numberOfScreens; XineramaScreenInfo* screens = XineramaQueryScreens(display, &numberOfScreens); diff --git a/LibOVR/Src/OVR_Profile.cpp b/LibOVR/Src/OVR_Profile.cpp index 7554a28..83eade3 100644 --- a/LibOVR/Src/OVR_Profile.cpp +++ b/LibOVR/Src/OVR_Profile.cpp @@ -46,16 +46,15 @@ namespace OVR { //----------------------------------------------------------------------------- // Returns the pathname of the JSON file containing the stored profiles -String GetProfilePath(bool create_dir) +String GetBaseOVRPath(bool create_dir) { String path; #if defined(OVR_OS_WIN32) - - PWSTR data_path = NULL; - SHGetKnownFolderPath(FOLDERID_LocalAppData, 0, NULL, &data_path); + + TCHAR data_path[MAX_PATH]; + SHGetFolderPath(0, CSIDL_LOCAL_APPDATA, NULL, 0, data_path); path = String(data_path); - CoTaskMemFree(data_path); path += "/Oculus"; @@ -113,10 +112,15 @@ String GetProfilePath(bool create_dir) #endif - path += "/Profiles.json"; return path; } +String GetProfilePath(bool create_dir) +{ + String path = GetBaseOVRPath(create_dir); + path += "/Profiles.json"; + return path; +} //----------------------------------------------------------------------------- // ***** ProfileManager @@ -232,22 +236,25 @@ void ProfileManager::LoadCache(ProfileType device) Ptr profile = *CreateProfileObject(profileName, device, &deviceName); // Read the base profile fields. - while (item = profileItem->GetNextItem(item), item) + if (profile) { - if (item->Type != JSON_Object) + while (item = profileItem->GetNextItem(item), item) { - profile->ParseProperty(item->Name, item->Value); - } - else - { // Search for the matching device to get device specific fields - if (!deviceFound && OVR_strcmp(item->Name, deviceName) == 0) + if (item->Type != JSON_Object) { - deviceFound = true; - - for (JSON* deviceItem = item->GetFirstItem(); deviceItem; - deviceItem = item->GetNextItem(deviceItem)) + profile->ParseProperty(item->Name, item->Value); + } + else + { // Search for the matching device to get device specific fields + if (!deviceFound && OVR_strcmp(item->Name, deviceName) == 0) { - profile->ParseProperty(deviceItem->Name, deviceItem->Value); + deviceFound = true; + + for (JSON* deviceItem = item->GetFirstItem(); deviceItem; + deviceItem = item->GetNextItem(deviceItem)) + { + profile->ParseProperty(deviceItem->Name, deviceItem->Value); + } } } } diff --git a/LibOVR/Src/OVR_Profile.h b/LibOVR/Src/OVR_Profile.h index bfd8ea9..7184aae 100644 --- a/LibOVR/Src/OVR_Profile.h +++ b/LibOVR/Src/OVR_Profile.h @@ -181,6 +181,9 @@ protected: friend class ProfileManager; }; + +String GetBaseOVRPath(bool create_dir); + } #endif // OVR_Profile_h \ No newline at end of file diff --git a/LibOVR/Src/OVR_SensorFusion.cpp b/LibOVR/Src/OVR_SensorFusion.cpp index a4c5809..ba913c1 100644 --- a/LibOVR/Src/OVR_SensorFusion.cpp +++ b/LibOVR/Src/OVR_SensorFusion.cpp @@ -16,6 +16,8 @@ otherwise accompanies this software in either electronic or hard copy form. #include "OVR_SensorFusion.h" #include "Kernel/OVR_Log.h" #include "Kernel/OVR_System.h" +#include "OVR_JSON.h" +#include "OVR_Profile.h" namespace OVR { @@ -32,7 +34,8 @@ SensorFusion::SensorFusion(SensorDevice* sensor) MagCondCount(0), MagCalibrated(false), MagRefQ(0, 0, 0, 1), MagRefM(0), MagRefYaw(0), YawErrorAngle(0), MagRefDistance(0.5f), YawErrorCount(0), YawCorrectionActivated(false), YawCorrectionInProgress(false), - EnableYawCorrection(false), MagNumReferences(0), MagHasNearbyReference(false) + EnableYawCorrection(false), MagNumReferences(0), MagHasNearbyReference(false), + MotionTrackingEnabled(true) { if (sensor) AttachToSensor(sensor); @@ -47,6 +50,7 @@ SensorFusion::~SensorFusion() bool SensorFusion::AttachToSensor(SensorDevice* sensor) { + pSensor = sensor; if (sensor != NULL) { MessageHandler* pCurrentHandler = sensor->GetMessageHandler(); @@ -63,6 +67,9 @@ bool SensorFusion::AttachToSensor(SensorDevice* sensor) ("SensorFusion::AttachToSensor failed - sensor %p already has handler", sensor)); return false; } + + // Automatically load the default mag calibration for this sensor + LoadMagCalibration(); } if (Handler.IsHandlerInstalled()) @@ -95,7 +102,7 @@ void SensorFusion::Reset() void SensorFusion::handleMessage(const MessageBodyFrame& msg) { - if (msg.Type != Message_BodyFrame) + if (msg.Type != Message_BodyFrame || !IsMotionTrackingEnabled()) return; // Put the sensor readings into convenient local variables @@ -251,7 +258,7 @@ void SensorFusion::handleMessage(const MessageBodyFrame& msg) { if (MagNumReferences == 0) { - SetMagReference(); // Use the current direction + setMagReference(); // Use the current direction } else if (Q.Distance(MagRefQ) > MagRefDistance) { @@ -278,7 +285,7 @@ void SensorFusion::handleMessage(const MessageBodyFrame& msg) //LogText("Using reference %d\n",bestNdx); } else if (MagNumReferences < MagMaxReferences) - SetMagReference(); + setMagReference(); } } @@ -322,9 +329,7 @@ void SensorFusion::handleMessage(const MessageBodyFrame& msg) } -// Simple predictive filters based on extrapolating the smoothed, current angular velocity -// or using smooth time derivative information. The argument is the amount of time into -// the future to predict. +// A predictive filter based on extrapolating the smoothed, current angular velocity Quatf SensorFusion::GetPredictedOrientation(float pdt) { Lock::Locker lockScope(Handler.GetHandlerLock()); @@ -332,45 +337,32 @@ Quatf SensorFusion::GetPredictedOrientation(float pdt) if (EnablePrediction) { -#if 1 // This method assumes a constant angular velocity Vector3f angVelF = FAngV.SavitzkyGolaySmooth8(); float angVelFL = angVelF.Length(); - + + // Force back to raw measurement + angVelF = AngV; + angVelFL = AngV.Length(); + + // Dynamic prediction interval: Based on angular velocity to reduce vibration + const float minPdt = 0.001f; + const float slopePdt = 0.1f; + float newpdt = pdt; + float tpdt = minPdt + slopePdt * angVelFL; + if (tpdt < pdt) + newpdt = tpdt; + //LogText("PredictonDTs: %d\n",(int)(newpdt / PredictionTimeIncrement + 0.5f)); + if (angVelFL > 0.001f) { Vector3f rotAxisP = angVelF / angVelFL; - float halfRotAngleP = angVelFL * pdt * 0.5f; + float halfRotAngleP = angVelFL * newpdt * 0.5f; float sinaHRAP = sin(halfRotAngleP); Quatf deltaQP(rotAxisP.x*sinaHRAP, rotAxisP.y*sinaHRAP, rotAxisP.z*sinaHRAP, cos(halfRotAngleP)); qP = QUncorrected * deltaQP; - } -#else - // This method estimates angular acceleration, conservatively - OVR_ASSERT(pdt >= 0); - int predictionStages = (int)(pdt / PredictionTimeIncrement + 0.5f); - Quatd qpd = Quatd(Q.x,Q.y,Q.z,Q.w); - Vector3f aa = FAngV.SavitzkyGolayDerivative12(); - Vector3d aad = Vector3d(aa.x,aa.y,aa.z); - Vector3f angVelF = FAngV.SavitzkyGolaySmooth8(); - Vector3d avkd = Vector3d(angVelF.x,angVelF.y,angVelF.z); - Vector3d rotAxisd = Vector3d(0,1,0); - for (int i = 0; i < predictionStages; i++) - { - double angVelLengthd = avkd.Length(); - if (angVelLengthd > 0) - rotAxisd = avkd / angVelLengthd; - double halfRotAngled = angVelLengthd * PredictionTimeIncrement * 0.5; - double sinHRAd = sin(halfRotAngled); - Quatd deltaQd = Quatd(rotAxisd.x*sinHRAd, rotAxisd.y*sinHRAd, rotAxisd.z*sinHRAd, - cos(halfRotAngled)); - qpd = qpd * deltaQd; - // Update angular velocity by using the angular acceleration estimate - avkd += aad; - } - qP = Quatf((float)qpd.x,(float)qpd.y,(float)qpd.z,(float)qpd.w); -#endif + } } return qP; } @@ -387,7 +379,7 @@ Vector3f SensorFusion::GetCalibratedMagValue(const Vector3f& rawMag) const } -void SensorFusion::SetMagReference(const Quatf& q, const Vector3f& rawMag) +void SensorFusion::setMagReference(const Quatf& q, const Vector3f& rawMag) { if (MagNumReferences < MagMaxReferences) { @@ -430,6 +422,274 @@ bool SensorFusion::BodyFrameHandler::SupportsMessageType(MessageType type) const return (type == Message_BodyFrame); } +// Writes the current calibration for a particular device to a device profile file +// sensor - the sensor that was calibrated +// cal_name - an optional name for the calibration or default if cal_name == NULL +bool SensorFusion::SaveMagCalibration(const char* calibrationName) const +{ + if (pSensor == NULL || !HasMagCalibration()) + return false; + + // A named calibration may be specified for calibration in different + // environments, otherwise the default calibration is used + if (calibrationName == NULL) + calibrationName = "default"; + + SensorInfo sinfo; + pSensor->GetDeviceInfo(&sinfo); + + // Generate a mag calibration event + JSON* calibration = JSON::CreateObject(); + // (hardcoded for now) the measurement and representation method + calibration->AddStringItem("Version", "1.0"); + calibration->AddStringItem("Name", "default"); + + // time stamp the calibration + char time_str[64]; + +#ifdef OVR_OS_WIN32 + struct tm caltime; + localtime_s(&caltime, &MagCalibrationTime); + strftime(time_str, 64, "%Y-%m-%d %H:%M:%S", &caltime); +#else + struct tm* caltime; + caltime = localtime(&MagCalibrationTime); + strftime(time_str, 64, "%Y-%m-%d %H:%M:%S", caltime); +#endif + + calibration->AddStringItem("Time", time_str); + + // write the full calibration matrix + Matrix4f calmat = GetMagCalibration(); + char matrix[128]; + int pos = 0; + for (int r=0; r<4; r++) + { + for (int c=0; c<4; c++) + { + pos += (int)OVR_sprintf(matrix+pos, 128, "%g ", calmat.M[r][c]); + } + } + calibration->AddStringItem("Calibration", matrix); + + + String path = GetBaseOVRPath(true); + path += "/Devices.json"; + + // Look for a prexisting device file to edit + Ptr 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") + { // In the future I may need to check versioning to determine parse method + } + 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); + } + + // 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); + 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 +// sensor - the sensor that the calibration was saved for +// cal_name - an optional name for the calibration or the default if cal_name == NULL +bool SensorFusion::LoadMagCalibration(const char* calibrationName) +{ + if (pSensor == NULL) + return false; + + // A named calibration may be specified for calibration in different + // environments, otherwise the default calibration is used + if (calibrationName == NULL) + calibrationName = "default"; + + SensorInfo sinfo; + pSensor->GetDeviceInfo(&sinfo); + + String path = GetBaseOVRPath(true); + path += "/Devices.json"; + + // Load the device profiles + Ptr 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") + { // In the future I may need to check versioning to determine parse method + } + else + { + return false; + } + + bool autoEnableCorrection = 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"); + if (autoyaw) + autoEnableCorrection = (autoyaw->dValue != 0); + + 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 + + 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("Calibration"); + if (cal) + { + const char* data_str = cal->Value.ToCStr(); + Matrix4f calmat; + for (int r=0; r<4; r++) + { + for (int c=0; c<4; c++) + { + calmat.M[r][c] = (float)atof(data_str); + while (data_str && *data_str != ' ') + data_str++; + + if (data_str) + data_str++; + } + } + + SetMagCalibration(calmat); + MagCalibrationTime = calibration_time; + EnableYawCorrection = autoEnableCorrection; + + return true; + } + } + } + item = device->GetNextItem(item); + } + + break; + } + } + + device = root->GetNextItem(device); + } + + return false; +} + + } // namespace OVR diff --git a/LibOVR/Src/OVR_SensorFusion.h b/LibOVR/Src/OVR_SensorFusion.h index adfe780..c660a86 100644 --- a/LibOVR/Src/OVR_SensorFusion.h +++ b/LibOVR/Src/OVR_SensorFusion.h @@ -19,6 +19,7 @@ otherwise accompanies this software in either electronic or hard copy form. #include "OVR_Device.h" #include "OVR_SensorFilter.h" +#include namespace OVR { @@ -27,6 +28,8 @@ namespace OVR { // SensorFusion class accumulates Sensor notification messages to keep track of // orientation, which involves integrating the gyro and doing correction with gravity. +// Magnetometer based yaw drift correction is also supported; it is usually enabled +// automatically based on loaded magnetometer configuration. // Orientation is reported as a quaternion, from which users can obtain either the // rotation matrix or Euler angles. // @@ -35,16 +38,20 @@ namespace OVR { // - By attaching SensorFusion to a SensorDevice, in which case it will // automatically handle notifications from that device. + class SensorFusion : public NewOverrideBase { enum { MagMaxReferences = 80 - }; + }; public: SensorFusion(SensorDevice* sensor = 0); ~SensorFusion(); + + + // *** Setup // Attaches this SensorFusion to a sensor device, from which it will receive // notification messages. If a sensor is attached, manual message notification @@ -52,15 +59,85 @@ public: bool AttachToSensor(SensorDevice* sensor); // Returns true if this Sensor fusion object is attached to a sensor. - bool IsAttachedToSensor() const { return Handler.IsHandlerInstalled(); } + bool IsAttachedToSensor() const { return Handler.IsHandlerInstalled(); } + + + + // *** State Query + + // Obtain the current accumulated orientation. Many apps will want to use GetPredictedOrientation + // instead to reduce latency. + Quatf GetOrientation() const { return lockedGet(&Q); } + + // Get predicted orientaion in the near future; predictDt is lookahead amount in seconds. + Quatf GetPredictedOrientation(float predictDt); + Quatf GetPredictedOrientation() { return GetPredictedOrientation(PredictionDT); } + + // Obtain the last absolute acceleration reading, in m/s^2. + Vector3f GetAcceleration() const { return lockedGet(&A); } + // Obtain the last angular velocity reading, in rad/s. + Vector3f GetAngularVelocity() const { return lockedGet(&AngV); } + + // Obtain the last raw magnetometer reading, in Gauss + Vector3f GetMagnetometer() const { return lockedGet(&RawMag); } + // Obtain the calibrated magnetometer reading (direction and field strength) + Vector3f GetCalibratedMagnetometer() const { OVR_ASSERT(MagCalibrated); return lockedGet(&CalMag); } + + + // Resets the current orientation. + void Reset(); + + + + // *** Configuration + + void EnableMotionTracking(bool enable = true) { MotionTrackingEnabled = enable; } + bool IsMotionTrackingEnabled() const { return MotionTrackingEnabled; } + + // Multiplier for yaw rotation (turning); setting this higher than 1 (the default) can allow the game + // to be played without auxillary rotation controls, possibly making it more immersive. + // Whether this is more or less likely to cause motion sickness is unknown. + float GetYawMultiplier() const { return YawMult; } + void SetYawMultiplier(float y) { YawMult = y; } + - void SetGravityEnabled(bool enableGravity) { EnableGravity = enableGravity; } - - bool IsGravityEnabled() const { return EnableGravity;} + // *** Prediction Control - void SetYawCorrectionEnabled(bool enableYawCorrection) { EnableYawCorrection = enableYawCorrection; } - - // Yaw correction is set up to work + // Prediction functions. + // Prediction delta specifes how much prediction should be applied in seconds; it should in + // general be under the average rendering latency. Call GetPredictedOrientation() to get + // predicted orientation. + float GetPredictionDelta() const { return PredictionDT; } + void SetPrediction(float dt, bool enable = true) { PredictionDT = dt; EnablePrediction = enable; } + void SetPredictionEnabled(bool enable = true) { EnablePrediction = enable; } + bool IsPredictionEnabled() { return EnablePrediction; } + + + // *** Accelerometer/Gravity Correction Control + + // Enables/disables gravity correction (on by default). + void SetGravityEnabled(bool enableGravity) { EnableGravity = enableGravity; } + bool IsGravityEnabled() const { return EnableGravity;} + + // Gain used to correct gyro with accel. Default value is appropriate for typical use. + float GetAccelGain() const { return Gain; } + void SetAccelGain(float ag) { Gain = ag; } + + + + // *** Magnetometer and Yaw Drift Correction Control + + // Methods to load and save a mag calibration. Calibrations can optionally + // be specified by name to differentiate multiple calibrations under different conditions + // If LoadMagCalibration succeeds, it will override YawCorrectionEnabled based on + // saved calibration setting. + bool SaveMagCalibration(const char* calibrationName = NULL) const; + bool LoadMagCalibration(const char* calibrationName = NULL); + + // Enables/disables magnetometer based yaw drift correction. Must also have mag calibration + // data for this correction to work. + void SetYawCorrectionEnabled(bool enable) { EnableYawCorrection = enable; } + // Determines if yaw correction is enabled. bool IsYawCorrectionEnabled() const { return EnableYawCorrection;} // Yaw correction is currently working (forcing a corrective yaw rotation) @@ -70,12 +147,17 @@ public: void SetMagCalibration(const Matrix4f& m) { MagCalibrationMatrix = m; + time(&MagCalibrationTime); // time stamp the calibration MagCalibrated = true; } + // Retrieves the magnetometer calibration matrix + Matrix4f GetMagCalibration() const { return MagCalibrationMatrix; } + // Retrieve the time of the calibration + time_t GetMagCalibrationTime() const { return MagCalibrationTime; } + // True only if the mag has calibration values stored - bool HasMagCalibration() const { return MagCalibrated;} - + bool HasMagCalibration() const { return MagCalibrated;} // Force the mag into the uncalibrated state void ClearMagCalibration() { MagCalibrated = false; } @@ -83,110 +165,49 @@ public: void ClearMagReferences() { MagNumReferences = 0; } void SetMagRefDistance(const float d) { MagRefDistance = d; } - // Notifies SensorFusion object about a new BodyFrame message from a sensor. - // Should be called by user if not attaching to a sensor. - void OnMessage(const MessageBodyFrame& msg) - { - OVR_ASSERT(!IsAttachedToSensor()); - handleMessage(msg); - } - - // Obtain the current accumulated orientation. - Quatf GetOrientation() const - { - Lock::Locker lockScope(Handler.GetHandlerLock()); - return Q; - } - - // Use a predictive filter to estimate the future orientation - Quatf GetPredictedOrientation(float pdt); // Specify lookahead time in ms - Quatf GetPredictedOrientation() { return GetPredictedOrientation(PredictionDT); } - - // Obtain the last absolute acceleration reading, in m/s^2. - Vector3f GetAcceleration() const - { - Lock::Locker lockScope(Handler.GetHandlerLock()); - return A; - } - - // Obtain the last angular velocity reading, in rad/s. - Vector3f GetAngularVelocity() const - { - Lock::Locker lockScope(Handler.GetHandlerLock()); - return AngV; - } - // Obtain the last magnetometer reading, in Gauss - Vector3f GetMagnetometer() const - { - Lock::Locker lockScope(Handler.GetHandlerLock()); - return RawMag; - } - // Obtain the filtered magnetometer reading, in Gauss - Vector3f GetFilteredMagnetometer() const - { - Lock::Locker lockScope(Handler.GetHandlerLock()); - return FRawMag.Mean(); - } - // Obtain the calibrated magnetometer reading (direction and field strength) - Vector3f GetCalibratedMagnetometer() const - { - OVR_ASSERT(MagCalibrated); - Lock::Locker lockScope(Handler.GetHandlerLock()); - return CalMag; - } Vector3f GetCalibratedMagValue(const Vector3f& rawMag) const; - float GetMagRefYaw() const - { - return MagRefYaw; - } + float GetMagRefYaw() const { return MagRefYaw; } + float GetYawErrorAngle() const { return YawErrorAngle; } - float GetYawErrorAngle() const - { - return YawErrorAngle; - } - // For later - //Vector3f GetGravity() const; - // Resets the current orientation - void Reset(); - // Configuration + // *** Message Handler Logic - // Gain used to correct gyro with accel. Default value is appropriate for typical use. - float GetAccelGain() const { return Gain; } - void SetAccelGain(float ag) { Gain = ag; } - - // Multiplier for yaw rotation (turning); setting this higher than 1 (the default) can allow the game - // to be played without auxillary rotation controls, possibly making it more immersive. Whether this is more - // or less likely to cause motion sickness is unknown. - float GetYawMultiplier() const { return YawMult; } - void SetYawMultiplier(float y) { YawMult = y; } + // Notifies SensorFusion object about a new BodyFrame message from a sensor. + // Should be called by user if not attaching to a sensor. + void OnMessage(const MessageBodyFrame& msg) + { + OVR_ASSERT(!IsAttachedToSensor()); + handleMessage(msg); + } void SetDelegateMessageHandler(MessageHandler* handler) { pDelegate = handler; } - // Prediction functions. - // Prediction delta specifes how much prediction should be applied in seconds; it should in - // general be under the average rendering latency. Call GetPredictedOrientation() to get - // predicted orientation. - float GetPredictionDelta() const { return PredictionDT; } - void SetPrediction(float dt, bool enable = true) { PredictionDT = dt; EnablePrediction = enable; } - void SetPredictionEnabled(bool enable = true) { EnablePrediction = enable; } - bool IsPredictionEnabled() { return EnablePrediction; } + private: + SensorFusion* getThis() { return this; } + // Helper used to read and return value within a Lock. + template + C lockedGet(const C* p) const + { + Lock::Locker lockScope(Handler.GetHandlerLock()); + return *p; + } + // Internal handler for messages; bypasses error checking. - void handleMessage(const MessageBodyFrame& msg); + void handleMessage(const MessageBodyFrame& msg); // Set the magnetometer's reference orientation for use in yaw correction // The supplied mag is an uncalibrated value - void SetMagReference(const Quatf& q, const Vector3f& rawMag); + void setMagReference(const Quatf& q, const Vector3f& rawMag); // Default to current HMD orientation - void SetMagReference() { SetMagReference(Q, RawMag); } + void setMagReference() { setMagReference(Q, RawMag); } class BodyFrameHandler : public MessageHandler { @@ -199,6 +220,8 @@ private: virtual bool SupportsMessageType(MessageType type) const; }; + Ptr pSensor; + Quatf Q; Quatf QUncorrected; Vector3f A; @@ -228,6 +251,7 @@ private: bool EnableYawCorrection; Matrix4f MagCalibrationMatrix; + time_t MagCalibrationTime; bool MagCalibrated; int MagCondCount; float MagRefDistance; @@ -244,6 +268,7 @@ private: bool YawCorrectionInProgress; bool YawCorrectionActivated; + bool MotionTrackingEnabled; }; diff --git a/LibOVR/Src/Util/Util_MagCalibration.cpp b/LibOVR/Src/Util/Util_MagCalibration.cpp index c537154..58b8c45 100644 --- a/LibOVR/Src/Util/Util_MagCalibration.cpp +++ b/LibOVR/Src/Util/Util_MagCalibration.cpp @@ -23,6 +23,12 @@ void MagCalibration::BeginAutoCalibration(SensorFusion& sf) // This is a "hard" reset of the mag, so need to clear stored values sf.ClearMagCalibration(); SampleCount = 0; + + // reset the statistics + MinMagValues = Vector3f(10000.0f,10000.0f,10000.0f); + MaxMagValues = Vector3f(-10000.0f,-10000.0f,-10000.0f); + MinQuatValues = Quatf(1.0f,1.0f,1.0f,1.0f); + MaxQuatValues = Quatf(0.0f,0.0f,0.0f,0.0f); } unsigned MagCalibration::UpdateAutoCalibration(SensorFusion& sf) @@ -36,7 +42,11 @@ unsigned MagCalibration::UpdateAutoCalibration(SensorFusion& sf) InsertIfAcceptable(q, m); if ((SampleCount == 4) && (Stat == Mag_AutoCalibrating)) + { + //LogText("Magnetometer Output Spread: %f %f %f\n",MagSpread.x,MagSpread.y,MagSpread.z); + //LogText("Quaternion Spread: %f %f %f %f\n",QuatSpread.x,QuatSpread.y,QuatSpread.z,QuatSpread.w); SetCalibration(sf); + } return Stat; @@ -83,7 +93,39 @@ bool MagCalibration::IsAcceptableSample(const Quatf& q, const Vector3f& m) bool MagCalibration::InsertIfAcceptable(const Quatf& q, const Vector3f& m) { - if (IsAcceptableSample(q, m)) + // Update some statistics + if (m.x < MinMagValues.x) + MinMagValues.x = m.x; + if (m.y < MinMagValues.y) + MinMagValues.y = m.y; + if (m.z < MinMagValues.z) + MinMagValues.z = m.z; + if (m.x > MaxMagValues.x) + MaxMagValues.x = m.x; + if (m.y > MaxMagValues.y) + MaxMagValues.y = m.y; + if (m.z > MaxMagValues.z) + MaxMagValues.z = m.z; + if (q.x < MinQuatValues.x) + MinQuatValues.x = q.x; + if (q.y < MinQuatValues.y) + MinQuatValues.y = q.y; + if (q.z < MinQuatValues.z) + MinQuatValues.z = q.z; + if (q.w < MinQuatValues.w) + MinQuatValues.w = q.w; + if (q.x > MaxQuatValues.x) + MaxQuatValues.x = q.x; + if (q.y > MaxQuatValues.y) + MaxQuatValues.y = q.y; + if (q.z > MaxQuatValues.z) + MaxQuatValues.z = q.z; + if (q.w > MaxQuatValues.w) + MaxQuatValues.w = q.w; + MagSpread = MaxMagValues - MinMagValues; + QuatSpread = MaxQuatValues - MinQuatValues; + + if (IsAcceptableSample(q, m)) { MagSamples[SampleCount] = m; QuatSamples[SampleCount] = q; @@ -94,6 +136,14 @@ bool MagCalibration::InsertIfAcceptable(const Quatf& q, const Vector3f& m) return false; } +Matrix4f MagCalibration::GetMagCalibration() const +{ + Matrix4f calMat = Matrix4f(); + calMat.M[0][3] = -MagCenter.x; + calMat.M[1][3] = -MagCenter.y; + calMat.M[2][3] = -MagCenter.z; + return calMat; +} bool MagCalibration::SetCalibration(SensorFusion& sf) { @@ -101,10 +151,7 @@ bool MagCalibration::SetCalibration(SensorFusion& sf) return false; MagCenter = CalculateSphereCenter(MagSamples[0],MagSamples[1],MagSamples[2],MagSamples[3]); - Matrix4f calMat = Matrix4f(); - calMat.M[0][3] = -MagCenter.x; - calMat.M[1][3] = -MagCenter.y; - calMat.M[2][3] = -MagCenter.z; + Matrix4f calMat = GetMagCalibration(); sf.SetMagCalibration(calMat); Stat = Mag_Calibrated; //LogText("MagCenter: %f %f %f\n",MagCenter.x,MagCenter.y,MagCenter.z); diff --git a/LibOVR/Src/Util/Util_MagCalibration.h b/LibOVR/Src/Util/Util_MagCalibration.h index fd26d22..1f8e8cb 100644 --- a/LibOVR/Src/Util/Util_MagCalibration.h +++ b/LibOVR/Src/Util/Util_MagCalibration.h @@ -41,7 +41,11 @@ public: { MinMagDistanceSq = MinMagDistance * MinMagDistance; MinQuatDistanceSq = MinQuatDistance * MinQuatDistance; - } + MinMagValues = Vector3f(10000.0f,10000.0f,10000.0f); + MaxMagValues = Vector3f(-10000.0f,-10000.0f,-10000.0f); + MinQuatValues = Quatf(1.0f,1.0f,1.0f,1.0f); + MaxQuatValues = Quatf(0.0f,0.0f,0.0f,0.0f); + } // Methods that are useful for either auto or manual calibration bool IsUnitialized() const { return Stat == Mag_Uninitialized; } @@ -93,6 +97,12 @@ public: // A result of the calibration, which is the center of a sphere that // roughly approximates the magnetometer data. Vector3f GetMagCenter() const { return MagCenter; } + // Retrieves the full magnetometer calibration matrix + Matrix4f GetMagCalibration() const; + // Retrieves the range of each quaternion term during calibration + Quatf GetCalibrationQuatSpread() const { return QuatSpread; } + // Retrieves the range of each magnetometer term during calibration + Vector3f GetCalibrationMagSpread() const { return MagSpread; } private: // Determine the unique sphere through 4 non-coplanar points @@ -109,6 +119,13 @@ private: float MinQuatDistance; float MinMagDistanceSq; float MinQuatDistanceSq; + // For gathering statistics during calibration + Vector3f MinMagValues; + Vector3f MaxMagValues; + Vector3f MagSpread; + Quatf MinQuatValues; + Quatf MaxQuatValues; + Quatf QuatSpread; unsigned SampleCount; Vector3f MagSamples[4]; diff --git a/LibOVR/Src/Util/Util_Render_Stereo.cpp b/LibOVR/Src/Util/Util_Render_Stereo.cpp index b16cfb5..e2600a9 100644 --- a/LibOVR/Src/Util/Util_Render_Stereo.cpp +++ b/LibOVR/Src/Util/Util_Render_Stereo.cpp @@ -247,7 +247,7 @@ void StereoConfig::update2D() // eye, where hmd screen projection surface is at 0.05m distance. // This introduces an extra off-center pixel projection shift based on eye distance. // This offCenterShift is the pixel offset of the other camera's center - // in your reference camera based on surface distance. + // in your reference camera based on surface distance. float metersToPixels = (HMD.HResolution / HMD.HScreenSize); float lensDistanceScreenPixels= metersToPixels * HMD.LensSeparationDistance; float eyeDistanceScreenPixels = metersToPixels * InterpupillaryDistance; @@ -278,7 +278,7 @@ void StereoConfig::update2D() void StereoConfig::updateEyeParams() { // Projection matrix for the center eye, which the left/right matrices are based on. - Matrix4f projCenter = Matrix4f::PerspectiveRH(YFov, Aspect, 0.01f, 1000.0f); + Matrix4f projCenter = Matrix4f::PerspectiveRH(YFov, Aspect, 0.01f, 2000.0f); switch(Mode) { -- cgit v1.2.3