1.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
     1.2 +++ b/libovr/Src/OVR_SensorFusion.h	Sat Sep 14 16:14:59 2013 +0300
     1.3 @@ -0,0 +1,277 @@
     1.4 +/************************************************************************************
     1.5 +
     1.6 +PublicHeader:   OVR.h
     1.7 +Filename    :   OVR_SensorFusion.h
     1.8 +Content     :   Methods that determine head orientation from sensor data over time
     1.9 +Created     :   October 9, 2012
    1.10 +Authors     :   Michael Antonov, Steve LaValle
    1.11 +
    1.12 +Copyright   :   Copyright 2012 Oculus VR, Inc. All Rights reserved.
    1.13 +
    1.14 +Use of this software is subject to the terms of the Oculus license
    1.15 +agreement provided at the time of installation or download, or which
    1.16 +otherwise accompanies this software in either electronic or hard copy form.
    1.17 +
    1.18 +*************************************************************************************/
    1.19 +
    1.20 +#ifndef OVR_SensorFusion_h
    1.21 +#define OVR_SensorFusion_h
    1.22 +
    1.23 +#include "OVR_Device.h"
    1.24 +#include "OVR_SensorFilter.h"
    1.25 +#include <time.h>
    1.26 +
    1.27 +namespace OVR {
    1.28 +
    1.29 +//-------------------------------------------------------------------------------------
    1.30 +// ***** SensorFusion
    1.31 +
    1.32 +// SensorFusion class accumulates Sensor notification messages to keep track of
    1.33 +// orientation, which involves integrating the gyro and doing correction with gravity.
    1.34 +// Magnetometer based yaw drift correction is also supported; it is usually enabled
    1.35 +// automatically based on loaded magnetometer configuration.
    1.36 +// Orientation is reported as a quaternion, from which users can obtain either the
    1.37 +// rotation matrix or Euler angles.
    1.38 +//
    1.39 +// The class can operate in two ways:
    1.40 +//  - By user manually passing MessageBodyFrame messages to the OnMessage() function. 
    1.41 +//  - By attaching SensorFusion to a SensorDevice, in which case it will
    1.42 +//    automatically handle notifications from that device.
    1.43 +
    1.44 +
    1.45 +class SensorFusion : public NewOverrideBase
    1.46 +{
    1.47 +    enum
    1.48 +    {
    1.49 +        MagMaxReferences = 80
    1.50 +    };        
    1.51 +
    1.52 +public:
    1.53 +    SensorFusion(SensorDevice* sensor = 0);
    1.54 +    ~SensorFusion();
    1.55 +
    1.56 +
    1.57 +    // *** Setup
    1.58 +    
    1.59 +    // Attaches this SensorFusion to a sensor device, from which it will receive
    1.60 +    // notification messages. If a sensor is attached, manual message notification
    1.61 +    // is not necessary. Calling this function also resets SensorFusion state.
    1.62 +    bool        AttachToSensor(SensorDevice* sensor);
    1.63 +
    1.64 +    // Returns true if this Sensor fusion object is attached to a sensor.
    1.65 +    bool        IsAttachedToSensor() const  { return Handler.IsHandlerInstalled(); }
    1.66 +
    1.67 +
    1.68 +
    1.69 +    // *** State Query
    1.70 +
    1.71 +    // Obtain the current accumulated orientation. Many apps will want to use GetPredictedOrientation
    1.72 +    // instead to reduce latency.
    1.73 +    Quatf       GetOrientation() const      { return lockedGet(&Q); }
    1.74 +
    1.75 +    // Get predicted orientaion in the near future; predictDt is lookahead amount in seconds.
    1.76 +    Quatf       GetPredictedOrientation(float predictDt);
    1.77 +    Quatf       GetPredictedOrientation()   { return GetPredictedOrientation(PredictionDT); }
    1.78 +
    1.79 +    // Obtain the last absolute acceleration reading, in m/s^2.
    1.80 +    Vector3f    GetAcceleration() const     { return lockedGet(&A); }
    1.81 +    // Obtain the last angular velocity reading, in rad/s.
    1.82 +    Vector3f    GetAngularVelocity() const  { return lockedGet(&AngV); }
    1.83 +
    1.84 +    // Obtain the last raw magnetometer reading, in Gauss
    1.85 +    Vector3f    GetMagnetometer() const     { return lockedGet(&RawMag); }   
    1.86 +    // Obtain the calibrated magnetometer reading (direction and field strength)
    1.87 +    Vector3f    GetCalibratedMagnetometer() const  { OVR_ASSERT(MagCalibrated); return lockedGet(&CalMag); }
    1.88 +
    1.89 +
    1.90 +    // Resets the current orientation.
    1.91 +    void        Reset();
    1.92 +
    1.93 +
    1.94 +
    1.95 +    // *** Configuration
    1.96 +
    1.97 +    void        EnableMotionTracking(bool enable = true)    { MotionTrackingEnabled = enable; }
    1.98 +    bool        IsMotionTrackingEnabled() const             { return MotionTrackingEnabled;   }
    1.99 +
   1.100 +    // Multiplier for yaw rotation (turning); setting this higher than 1 (the default) can allow the game
   1.101 +    // to be played without auxillary rotation controls, possibly making it more immersive.
   1.102 +    // Whether this is more or less likely to cause motion sickness is unknown.
   1.103 +    float       GetYawMultiplier() const  { return YawMult; }
   1.104 +    void        SetYawMultiplier(float y) { YawMult = y; }
   1.105 +
   1.106 +
   1.107 +    // *** Prediction Control
   1.108 +
   1.109 +    // Prediction functions.
   1.110 +    // Prediction delta specifes how much prediction should be applied in seconds; it should in
   1.111 +    // general be under the average rendering latency. Call GetPredictedOrientation() to get
   1.112 +    // predicted orientation.
   1.113 +    float       GetPredictionDelta() const                  { return PredictionDT; }
   1.114 +    void        SetPrediction(float dt, bool enable = true) { PredictionDT = dt; EnablePrediction = enable; }
   1.115 +    void		SetPredictionEnabled(bool enable = true)    { EnablePrediction = enable; }    
   1.116 +    bool		IsPredictionEnabled()                       { return EnablePrediction; }
   1.117 +
   1.118 +
   1.119 +    // *** Accelerometer/Gravity Correction Control
   1.120 +
   1.121 +    // Enables/disables gravity correction (on by default).
   1.122 +    void        SetGravityEnabled(bool enableGravity)       { EnableGravity = enableGravity; }   
   1.123 +    bool        IsGravityEnabled() const                    { return EnableGravity;}
   1.124 +
   1.125 +    // Gain used to correct gyro with accel. Default value is appropriate for typical use.
   1.126 +    float       GetAccelGain() const                        { return Gain; }
   1.127 +    void        SetAccelGain(float ag)                      { Gain = ag; }
   1.128 +
   1.129 +
   1.130 +
   1.131 +    // *** Magnetometer and Yaw Drift Correction Control
   1.132 +
   1.133 +    // Methods to load and save a mag calibration.  Calibrations can optionally
   1.134 +    // be specified by name to differentiate multiple calibrations under different conditions
   1.135 +    // If LoadMagCalibration succeeds, it will override YawCorrectionEnabled based on
   1.136 +    // saved calibration setting.
   1.137 +    bool        SaveMagCalibration(const char* calibrationName = NULL) const;
   1.138 +    bool        LoadMagCalibration(const char* calibrationName = NULL);
   1.139 +
   1.140 +    // Enables/disables magnetometer based yaw drift correction. Must also have mag calibration
   1.141 +    // data for this correction to work.
   1.142 +	void        SetYawCorrectionEnabled(bool enable)    { EnableYawCorrection = enable; }
   1.143 +    // Determines if yaw correction is enabled.
   1.144 +    bool        IsYawCorrectionEnabled() const          { return EnableYawCorrection;}
   1.145 +
   1.146 +    // Yaw correction is currently working (forcing a corrective yaw rotation)
   1.147 +    bool        IsYawCorrectionInProgress() const       { return YawCorrectionInProgress;}
   1.148 +
   1.149 +    // Store the calibration matrix for the magnetometer
   1.150 +    void        SetMagCalibration(const Matrix4f& m)
   1.151 +    {
   1.152 +        MagCalibrationMatrix = m;
   1.153 +        time(&MagCalibrationTime);   // time stamp the calibration
   1.154 +        MagCalibrated = true;
   1.155 +    }
   1.156 +
   1.157 +    // Retrieves the magnetometer calibration matrix
   1.158 +    Matrix4f    GetMagCalibration() const        { return MagCalibrationMatrix; }
   1.159 +    // Retrieve the time of the calibration
   1.160 +    time_t      GetMagCalibrationTime() const    { return MagCalibrationTime; }
   1.161 +
   1.162 +    // True only if the mag has calibration values stored
   1.163 +    bool        HasMagCalibration() const        { return MagCalibrated;}  
   1.164 +    // Force the mag into the uncalibrated state
   1.165 +    void        ClearMagCalibration()            { MagCalibrated = false; }
   1.166 +
   1.167 +	// These refer to reference points that associate mag readings with orientations
   1.168 +	void        ClearMagReferences()             { MagNumReferences = 0; }
   1.169 +    void        SetMagRefDistance(const float d) { MagRefDistance = d; }
   1.170 +
   1.171 +
   1.172 +    Vector3f    GetCalibratedMagValue(const Vector3f& rawMag) const;
   1.173 +
   1.174 +    float       GetMagRefYaw() const             { return MagRefYaw; }
   1.175 +    float       GetYawErrorAngle() const         { return YawErrorAngle; }
   1.176 +
   1.177 +
   1.178 +
   1.179 +    // *** Message Handler Logic
   1.180 +
   1.181 +    // Notifies SensorFusion object about a new BodyFrame message from a sensor.
   1.182 +    // Should be called by user if not attaching to a sensor.
   1.183 +    void        OnMessage(const MessageBodyFrame& msg)
   1.184 +    {
   1.185 +        OVR_ASSERT(!IsAttachedToSensor());
   1.186 +        handleMessage(msg);
   1.187 +    }
   1.188 +
   1.189 +    void        SetDelegateMessageHandler(MessageHandler* handler)
   1.190 +    { pDelegate = handler; }
   1.191 +
   1.192 +
   1.193 +
   1.194 +private:
   1.195 +
   1.196 +    SensorFusion* getThis()  { return this; }
   1.197 +
   1.198 +    // Helper used to read and return value within a Lock.
   1.199 +    template<class C>
   1.200 +    C lockedGet(const C* p) const
   1.201 +    {
   1.202 +        Lock::Locker lockScope(Handler.GetHandlerLock());
   1.203 +        return *p;
   1.204 +    }
   1.205 +
   1.206 +    // Internal handler for messages; bypasses error checking.
   1.207 +    void        handleMessage(const MessageBodyFrame& msg);
   1.208 +
   1.209 +    // Set the magnetometer's reference orientation for use in yaw correction
   1.210 +    // The supplied mag is an uncalibrated value
   1.211 +    void        setMagReference(const Quatf& q, const Vector3f& rawMag);
   1.212 +    // Default to current HMD orientation
   1.213 +    void        setMagReference()  { setMagReference(Q, RawMag); }
   1.214 +
   1.215 +	class BodyFrameHandler : public MessageHandler
   1.216 +    {
   1.217 +        SensorFusion* pFusion;
   1.218 +    public:
   1.219 +        BodyFrameHandler(SensorFusion* fusion) : pFusion(fusion) { }
   1.220 +        ~BodyFrameHandler();
   1.221 +
   1.222 +        virtual void OnMessage(const Message& msg);
   1.223 +        virtual bool SupportsMessageType(MessageType type) const;
   1.224 +    };   
   1.225 +
   1.226 +    SensorInfo        CachedSensorInfo;
   1.227 +    
   1.228 +    Quatf             Q;
   1.229 +	Quatf			  QUncorrected;
   1.230 +    Vector3f          A;    
   1.231 +    Vector3f          AngV;
   1.232 +    Vector3f          CalMag;
   1.233 +    Vector3f          RawMag;
   1.234 +    unsigned int      Stage;
   1.235 +	float             RunningTime;
   1.236 +	float             DeltaT;
   1.237 +    BodyFrameHandler  Handler;
   1.238 +    MessageHandler*   pDelegate;
   1.239 +    float             Gain;
   1.240 +    float             YawMult;
   1.241 +    volatile bool     EnableGravity;
   1.242 +
   1.243 +    bool              EnablePrediction;
   1.244 +    float             PredictionDT;
   1.245 +	float             PredictionTimeIncrement;
   1.246 +
   1.247 +    SensorFilter      FRawMag;
   1.248 +    SensorFilter      FAccW;
   1.249 +    SensorFilter      FAngV;
   1.250 +
   1.251 +    int               TiltCondCount;
   1.252 +    float             TiltErrorAngle;
   1.253 +    Vector3f          TiltErrorAxis;
   1.254 +
   1.255 +    bool              EnableYawCorrection;
   1.256 +    Matrix4f          MagCalibrationMatrix;
   1.257 +    time_t            MagCalibrationTime;    
   1.258 +    bool              MagCalibrated;
   1.259 +    int               MagCondCount;
   1.260 +    float             MagRefDistance;
   1.261 +    Quatf             MagRefQ;
   1.262 +    Vector3f          MagRefM;
   1.263 +    float             MagRefYaw;
   1.264 +    bool              MagHasNearbyReference;
   1.265 +    Quatf             MagRefTableQ[MagMaxReferences];
   1.266 +    Vector3f          MagRefTableM[MagMaxReferences];
   1.267 +    float             MagRefTableYaw[MagMaxReferences];
   1.268 +    int               MagNumReferences;
   1.269 +    float             YawErrorAngle;
   1.270 +    int               YawErrorCount;
   1.271 +    bool              YawCorrectionInProgress;
   1.272 +	bool			  YawCorrectionActivated;
   1.273 +
   1.274 +    bool              MotionTrackingEnabled;
   1.275 +};
   1.276 +
   1.277 +
   1.278 +} // namespace OVR
   1.279 +
   1.280 +#endif