diff options
Diffstat (limited to 'LibOVR/Src/OVR_SensorFusion.h')
| -rw-r--r-- | LibOVR/Src/OVR_SensorFusion.h | 213 |
1 files changed, 119 insertions, 94 deletions
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 <time.h> 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<class C> + 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<SensorDevice> 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; }; |