nuclear@1: /************************************************************************************
nuclear@1: 
nuclear@1: PublicHeader:   OVR.h
nuclear@1: Filename    :   OVR_SensorFusion.h
nuclear@1: Content     :   Methods that determine head orientation from sensor data over time
nuclear@1: Created     :   October 9, 2012
nuclear@1: Authors     :   Michael Antonov, Steve LaValle
nuclear@1: 
nuclear@1: Copyright   :   Copyright 2012 Oculus VR, Inc. All Rights reserved.
nuclear@1: 
nuclear@1: Use of this software is subject to the terms of the Oculus license
nuclear@1: agreement provided at the time of installation or download, or which
nuclear@1: otherwise accompanies this software in either electronic or hard copy form.
nuclear@1: 
nuclear@1: *************************************************************************************/
nuclear@1: 
nuclear@1: #ifndef OVR_SensorFusion_h
nuclear@1: #define OVR_SensorFusion_h
nuclear@1: 
nuclear@1: #include "OVR_Device.h"
nuclear@1: #include "OVR_SensorFilter.h"
nuclear@1: #include <time.h>
nuclear@1: 
nuclear@1: namespace OVR {
nuclear@1: 
nuclear@1: //-------------------------------------------------------------------------------------
nuclear@1: // ***** SensorFusion
nuclear@1: 
nuclear@1: // SensorFusion class accumulates Sensor notification messages to keep track of
nuclear@1: // orientation, which involves integrating the gyro and doing correction with gravity.
nuclear@1: // Magnetometer based yaw drift correction is also supported; it is usually enabled
nuclear@1: // automatically based on loaded magnetometer configuration.
nuclear@1: // Orientation is reported as a quaternion, from which users can obtain either the
nuclear@1: // rotation matrix or Euler angles.
nuclear@1: //
nuclear@1: // The class can operate in two ways:
nuclear@1: //  - By user manually passing MessageBodyFrame messages to the OnMessage() function. 
nuclear@1: //  - By attaching SensorFusion to a SensorDevice, in which case it will
nuclear@1: //    automatically handle notifications from that device.
nuclear@1: 
nuclear@1: 
nuclear@1: class SensorFusion : public NewOverrideBase
nuclear@1: {
nuclear@1:     enum
nuclear@1:     {
nuclear@1:         MagMaxReferences = 80
nuclear@1:     };        
nuclear@1: 
nuclear@1: public:
nuclear@1:     SensorFusion(SensorDevice* sensor = 0);
nuclear@1:     ~SensorFusion();
nuclear@1: 
nuclear@1: 
nuclear@1:     // *** Setup
nuclear@1:     
nuclear@1:     // Attaches this SensorFusion to a sensor device, from which it will receive
nuclear@1:     // notification messages. If a sensor is attached, manual message notification
nuclear@1:     // is not necessary. Calling this function also resets SensorFusion state.
nuclear@1:     bool        AttachToSensor(SensorDevice* sensor);
nuclear@1: 
nuclear@1:     // Returns true if this Sensor fusion object is attached to a sensor.
nuclear@1:     bool        IsAttachedToSensor() const  { return Handler.IsHandlerInstalled(); }
nuclear@1: 
nuclear@1: 
nuclear@1: 
nuclear@1:     // *** State Query
nuclear@1: 
nuclear@1:     // Obtain the current accumulated orientation. Many apps will want to use GetPredictedOrientation
nuclear@1:     // instead to reduce latency.
nuclear@1:     Quatf       GetOrientation() const      { return lockedGet(&Q); }
nuclear@1: 
nuclear@1:     // Get predicted orientaion in the near future; predictDt is lookahead amount in seconds.
nuclear@1:     Quatf       GetPredictedOrientation(float predictDt);
nuclear@1:     Quatf       GetPredictedOrientation()   { return GetPredictedOrientation(PredictionDT); }
nuclear@1: 
nuclear@1:     // Obtain the last absolute acceleration reading, in m/s^2.
nuclear@1:     Vector3f    GetAcceleration() const     { return lockedGet(&A); }
nuclear@1:     // Obtain the last angular velocity reading, in rad/s.
nuclear@1:     Vector3f    GetAngularVelocity() const  { return lockedGet(&AngV); }
nuclear@1: 
nuclear@1:     // Obtain the last raw magnetometer reading, in Gauss
nuclear@1:     Vector3f    GetMagnetometer() const     { return lockedGet(&RawMag); }   
nuclear@1:     // Obtain the calibrated magnetometer reading (direction and field strength)
nuclear@1:     Vector3f    GetCalibratedMagnetometer() const  { OVR_ASSERT(MagCalibrated); return lockedGet(&CalMag); }
nuclear@1: 
nuclear@1: 
nuclear@1:     // Resets the current orientation.
nuclear@1:     void        Reset();
nuclear@1: 
nuclear@1: 
nuclear@1: 
nuclear@1:     // *** Configuration
nuclear@1: 
nuclear@1:     void        EnableMotionTracking(bool enable = true)    { MotionTrackingEnabled = enable; }
nuclear@1:     bool        IsMotionTrackingEnabled() const             { return MotionTrackingEnabled;   }
nuclear@1: 
nuclear@1:     // Multiplier for yaw rotation (turning); setting this higher than 1 (the default) can allow the game
nuclear@1:     // to be played without auxillary rotation controls, possibly making it more immersive.
nuclear@1:     // Whether this is more or less likely to cause motion sickness is unknown.
nuclear@1:     float       GetYawMultiplier() const  { return YawMult; }
nuclear@1:     void        SetYawMultiplier(float y) { YawMult = y; }
nuclear@1: 
nuclear@1: 
nuclear@1:     // *** Prediction Control
nuclear@1: 
nuclear@1:     // Prediction functions.
nuclear@1:     // Prediction delta specifes how much prediction should be applied in seconds; it should in
nuclear@1:     // general be under the average rendering latency. Call GetPredictedOrientation() to get
nuclear@1:     // predicted orientation.
nuclear@1:     float       GetPredictionDelta() const                  { return PredictionDT; }
nuclear@1:     void        SetPrediction(float dt, bool enable = true) { PredictionDT = dt; EnablePrediction = enable; }
nuclear@1:     void		SetPredictionEnabled(bool enable = true)    { EnablePrediction = enable; }    
nuclear@1:     bool		IsPredictionEnabled()                       { return EnablePrediction; }
nuclear@1: 
nuclear@1: 
nuclear@1:     // *** Accelerometer/Gravity Correction Control
nuclear@1: 
nuclear@1:     // Enables/disables gravity correction (on by default).
nuclear@1:     void        SetGravityEnabled(bool enableGravity)       { EnableGravity = enableGravity; }   
nuclear@1:     bool        IsGravityEnabled() const                    { return EnableGravity;}
nuclear@1: 
nuclear@1:     // Gain used to correct gyro with accel. Default value is appropriate for typical use.
nuclear@1:     float       GetAccelGain() const                        { return Gain; }
nuclear@1:     void        SetAccelGain(float ag)                      { Gain = ag; }
nuclear@1: 
nuclear@1: 
nuclear@1: 
nuclear@1:     // *** Magnetometer and Yaw Drift Correction Control
nuclear@1: 
nuclear@1:     // Methods to load and save a mag calibration.  Calibrations can optionally
nuclear@1:     // be specified by name to differentiate multiple calibrations under different conditions
nuclear@1:     // If LoadMagCalibration succeeds, it will override YawCorrectionEnabled based on
nuclear@1:     // saved calibration setting.
nuclear@1:     bool        SaveMagCalibration(const char* calibrationName = NULL) const;
nuclear@1:     bool        LoadMagCalibration(const char* calibrationName = NULL);
nuclear@1: 
nuclear@1:     // Enables/disables magnetometer based yaw drift correction. Must also have mag calibration
nuclear@1:     // data for this correction to work.
nuclear@1: 	void        SetYawCorrectionEnabled(bool enable)    { EnableYawCorrection = enable; }
nuclear@1:     // Determines if yaw correction is enabled.
nuclear@1:     bool        IsYawCorrectionEnabled() const          { return EnableYawCorrection;}
nuclear@1: 
nuclear@1:     // Yaw correction is currently working (forcing a corrective yaw rotation)
nuclear@1:     bool        IsYawCorrectionInProgress() const       { return YawCorrectionInProgress;}
nuclear@1: 
nuclear@1:     // Store the calibration matrix for the magnetometer
nuclear@1:     void        SetMagCalibration(const Matrix4f& m)
nuclear@1:     {
nuclear@1:         MagCalibrationMatrix = m;
nuclear@1:         time(&MagCalibrationTime);   // time stamp the calibration
nuclear@1:         MagCalibrated = true;
nuclear@1:     }
nuclear@1: 
nuclear@1:     // Retrieves the magnetometer calibration matrix
nuclear@1:     Matrix4f    GetMagCalibration() const        { return MagCalibrationMatrix; }
nuclear@1:     // Retrieve the time of the calibration
nuclear@1:     time_t      GetMagCalibrationTime() const    { return MagCalibrationTime; }
nuclear@1: 
nuclear@1:     // True only if the mag has calibration values stored
nuclear@1:     bool        HasMagCalibration() const        { return MagCalibrated;}  
nuclear@1:     // Force the mag into the uncalibrated state
nuclear@1:     void        ClearMagCalibration()            { MagCalibrated = false; }
nuclear@1: 
nuclear@1: 	// These refer to reference points that associate mag readings with orientations
nuclear@1: 	void        ClearMagReferences()             { MagNumReferences = 0; }
nuclear@1:     void        SetMagRefDistance(const float d) { MagRefDistance = d; }
nuclear@1: 
nuclear@1: 
nuclear@1:     Vector3f    GetCalibratedMagValue(const Vector3f& rawMag) const;
nuclear@1: 
nuclear@1:     float       GetMagRefYaw() const             { return MagRefYaw; }
nuclear@1:     float       GetYawErrorAngle() const         { return YawErrorAngle; }
nuclear@1: 
nuclear@1: 
nuclear@1: 
nuclear@1:     // *** Message Handler Logic
nuclear@1: 
nuclear@1:     // Notifies SensorFusion object about a new BodyFrame message from a sensor.
nuclear@1:     // Should be called by user if not attaching to a sensor.
nuclear@1:     void        OnMessage(const MessageBodyFrame& msg)
nuclear@1:     {
nuclear@1:         OVR_ASSERT(!IsAttachedToSensor());
nuclear@1:         handleMessage(msg);
nuclear@1:     }
nuclear@1: 
nuclear@1:     void        SetDelegateMessageHandler(MessageHandler* handler)
nuclear@1:     { pDelegate = handler; }
nuclear@1: 
nuclear@1: 
nuclear@1: 
nuclear@1: private:
nuclear@1: 
nuclear@1:     SensorFusion* getThis()  { return this; }
nuclear@1: 
nuclear@1:     // Helper used to read and return value within a Lock.
nuclear@1:     template<class C>
nuclear@1:     C lockedGet(const C* p) const
nuclear@1:     {
nuclear@1:         Lock::Locker lockScope(Handler.GetHandlerLock());
nuclear@1:         return *p;
nuclear@1:     }
nuclear@1: 
nuclear@1:     // Internal handler for messages; bypasses error checking.
nuclear@1:     void        handleMessage(const MessageBodyFrame& msg);
nuclear@1: 
nuclear@1:     // Set the magnetometer's reference orientation for use in yaw correction
nuclear@1:     // The supplied mag is an uncalibrated value
nuclear@1:     void        setMagReference(const Quatf& q, const Vector3f& rawMag);
nuclear@1:     // Default to current HMD orientation
nuclear@1:     void        setMagReference()  { setMagReference(Q, RawMag); }
nuclear@1: 
nuclear@1: 	class BodyFrameHandler : public MessageHandler
nuclear@1:     {
nuclear@1:         SensorFusion* pFusion;
nuclear@1:     public:
nuclear@1:         BodyFrameHandler(SensorFusion* fusion) : pFusion(fusion) { }
nuclear@1:         ~BodyFrameHandler();
nuclear@1: 
nuclear@1:         virtual void OnMessage(const Message& msg);
nuclear@1:         virtual bool SupportsMessageType(MessageType type) const;
nuclear@1:     };   
nuclear@1: 
nuclear@1:     SensorInfo        CachedSensorInfo;
nuclear@1:     
nuclear@1:     Quatf             Q;
nuclear@1: 	Quatf			  QUncorrected;
nuclear@1:     Vector3f          A;    
nuclear@1:     Vector3f          AngV;
nuclear@1:     Vector3f          CalMag;
nuclear@1:     Vector3f          RawMag;
nuclear@1:     unsigned int      Stage;
nuclear@1: 	float             RunningTime;
nuclear@1: 	float             DeltaT;
nuclear@1:     BodyFrameHandler  Handler;
nuclear@1:     MessageHandler*   pDelegate;
nuclear@1:     float             Gain;
nuclear@1:     float             YawMult;
nuclear@1:     volatile bool     EnableGravity;
nuclear@1: 
nuclear@1:     bool              EnablePrediction;
nuclear@1:     float             PredictionDT;
nuclear@1: 	float             PredictionTimeIncrement;
nuclear@1: 
nuclear@1:     SensorFilter      FRawMag;
nuclear@1:     SensorFilter      FAccW;
nuclear@1:     SensorFilter      FAngV;
nuclear@1: 
nuclear@1:     int               TiltCondCount;
nuclear@1:     float             TiltErrorAngle;
nuclear@1:     Vector3f          TiltErrorAxis;
nuclear@1: 
nuclear@1:     bool              EnableYawCorrection;
nuclear@1:     Matrix4f          MagCalibrationMatrix;
nuclear@1:     time_t            MagCalibrationTime;    
nuclear@1:     bool              MagCalibrated;
nuclear@1:     int               MagCondCount;
nuclear@1:     float             MagRefDistance;
nuclear@1:     Quatf             MagRefQ;
nuclear@1:     Vector3f          MagRefM;
nuclear@1:     float             MagRefYaw;
nuclear@1:     bool              MagHasNearbyReference;
nuclear@1:     Quatf             MagRefTableQ[MagMaxReferences];
nuclear@1:     Vector3f          MagRefTableM[MagMaxReferences];
nuclear@1:     float             MagRefTableYaw[MagMaxReferences];
nuclear@1:     int               MagNumReferences;
nuclear@1:     float             YawErrorAngle;
nuclear@1:     int               YawErrorCount;
nuclear@1:     bool              YawCorrectionInProgress;
nuclear@1: 	bool			  YawCorrectionActivated;
nuclear@1: 
nuclear@1:     bool              MotionTrackingEnabled;
nuclear@1: };
nuclear@1: 
nuclear@1: 
nuclear@1: } // namespace OVR
nuclear@1: 
nuclear@1: #endif