OVR_SensorFusion.h
Go to the documentation of this file.
00001 /************************************************************************************
00002 
00003 PublicHeader:   OVR.h
00004 Filename    :   OVR_SensorFusion.h
00005 Content     :   Methods that determine head orientation from sensor data over time
00006 Created     :   October 9, 2012
00007 Authors     :   Michael Antonov, Steve LaValle
00008 
00009 Copyright   :   Copyright 2012 Oculus VR, Inc. All Rights reserved.
00010 
00011 Use of this software is subject to the terms of the Oculus license
00012 agreement provided at the time of installation or download, or which
00013 otherwise accompanies this software in either electronic or hard copy form.
00014 
00015 *************************************************************************************/
00016 
00017 #ifndef OVR_SensorFusion_h
00018 #define OVR_SensorFusion_h
00019 
00020 #include "OVR_Device.h"
00021 #include "OVR_SensorFilter.h"
00022 
00023 namespace OVR {
00024 
00025 //-------------------------------------------------------------------------------------
00026 // ***** SensorFusion
00027 
00028 // SensorFusion class accumulates Sensor notification messages to keep track of
00029 // orientation, which involves integrating the gyro and doing correction with gravity.
00030 // Orientation is reported as a quaternion, from which users can obtain either the
00031 // rotation matrix or Euler angles.
00032 //
00033 // The class can operate in two ways:
00034 //  - By user manually passing MessageBodyFrame messages to the OnMessage() function. 
00035 //  - By attaching SensorFusion to a SensorDevice, in which case it will
00036 //    automatically handle notifications from that device.
00037 
00038 class SensorFusion : public NewOverrideBase
00039 {
00040     enum
00041     {
00042         MagMaxReferences = 80
00043     };
00044 
00045 public:
00046     SensorFusion(SensorDevice* sensor = 0);
00047     ~SensorFusion();
00048     
00049     // Attaches this SensorFusion to a sensor device, from which it will receive
00050     // notification messages. If a sensor is attached, manual message notification
00051     // is not necessary. Calling this function also resets SensorFusion state.
00052     bool        AttachToSensor(SensorDevice* sensor);
00053 
00054     // Returns true if this Sensor fusion object is attached to a sensor.
00055     bool        IsAttachedToSensor() const              { return Handler.IsHandlerInstalled(); }
00056 
00057     void        SetGravityEnabled(bool enableGravity)   { EnableGravity = enableGravity; }
00058    
00059     bool        IsGravityEnabled() const                { return EnableGravity;}
00060 
00061         void        SetYawCorrectionEnabled(bool enableYawCorrection) { EnableYawCorrection = enableYawCorrection; }
00062    
00063     // Yaw correction is set up to work
00064     bool        IsYawCorrectionEnabled() const          { return EnableYawCorrection;}
00065 
00066     // Yaw correction is currently working (forcing a corrective yaw rotation)
00067     bool        IsYawCorrectionInProgress() const       { return YawCorrectionInProgress;}
00068 
00069     // Store the calibration matrix for the magnetometer
00070     void        SetMagCalibration(const Matrix4f& m)
00071     {
00072         MagCalibrationMatrix = m;
00073         MagCalibrated = true;
00074     }
00075 
00076     // True only if the mag has calibration values stored
00077     bool        HasMagCalibration() const        { return MagCalibrated;}
00078   
00079     // Force the mag into the uncalibrated state
00080     void        ClearMagCalibration()            { MagCalibrated = false; }
00081 
00082         // These refer to reference points that associate mag readings with orientations
00083         void        ClearMagReferences()             { MagNumReferences = 0; }
00084     void        SetMagRefDistance(const float d) { MagRefDistance = d; }
00085 
00086     // Notifies SensorFusion object about a new BodyFrame message from a sensor.
00087     // Should be called by user if not attaching to a sensor.
00088     void        OnMessage(const MessageBodyFrame& msg)
00089     {
00090         OVR_ASSERT(!IsAttachedToSensor());
00091         handleMessage(msg);
00092     }
00093 
00094     // Obtain the current accumulated orientation.
00095     Quatf       GetOrientation() const
00096     {
00097         Lock::Locker lockScope(Handler.GetHandlerLock());
00098         return Q;
00099     }    
00100 
00101     // Use a predictive filter to estimate the future orientation
00102         Quatf       GetPredictedOrientation(float pdt); // Specify lookahead time in ms
00103         Quatf       GetPredictedOrientation() { return GetPredictedOrientation(PredictionDT); }
00104 
00105     // Obtain the last absolute acceleration reading, in m/s^2.
00106     Vector3f    GetAcceleration() const
00107     {
00108         Lock::Locker lockScope(Handler.GetHandlerLock());
00109         return A;
00110     }
00111     
00112     // Obtain the last angular velocity reading, in rad/s.
00113     Vector3f    GetAngularVelocity() const
00114     {
00115         Lock::Locker lockScope(Handler.GetHandlerLock());
00116         return AngV;
00117     }
00118     // Obtain the last magnetometer reading, in Gauss
00119     Vector3f    GetMagnetometer() const
00120     {
00121         Lock::Locker lockScope(Handler.GetHandlerLock());
00122         return RawMag;
00123     }
00124     // Obtain the filtered magnetometer reading, in Gauss
00125     Vector3f    GetFilteredMagnetometer() const
00126     {
00127         Lock::Locker lockScope(Handler.GetHandlerLock());
00128         return FRawMag.Mean();
00129     }
00130     // Obtain the calibrated magnetometer reading (direction and field strength)
00131     Vector3f    GetCalibratedMagnetometer() const
00132     {
00133         OVR_ASSERT(MagCalibrated);
00134         Lock::Locker lockScope(Handler.GetHandlerLock());
00135         return CalMag;
00136     }
00137 
00138     Vector3f    GetCalibratedMagValue(const Vector3f& rawMag) const;
00139 
00140     float       GetMagRefYaw() const
00141     {
00142         return MagRefYaw;
00143     }
00144 
00145         float       GetYawErrorAngle() const
00146         {
00147                 return YawErrorAngle;
00148         }
00149     // For later
00150     //Vector3f    GetGravity() const;
00151 
00152     // Resets the current orientation
00153     void        Reset();
00154 
00155     // Configuration
00156 
00157     // Gain used to correct gyro with accel. Default value is appropriate for typical use.
00158     float       GetAccelGain() const   { return Gain; }
00159     void        SetAccelGain(float ag) { Gain = ag; }
00160 
00161     // Multiplier for yaw rotation (turning); setting this higher than 1 (the default) can allow the game
00162     // to be played without auxillary rotation controls, possibly making it more immersive. Whether this is more
00163     // or less likely to cause motion sickness is unknown.
00164     float       GetYawMultiplier() const  { return YawMult; }
00165     void        SetYawMultiplier(float y) { YawMult = y; }
00166 
00167     void        SetDelegateMessageHandler(MessageHandler* handler)
00168     { pDelegate = handler; }
00169 
00170         // Prediction functions.
00171     // Prediction delta specifes how much prediction should be applied in seconds; it should in
00172     // general be under the average rendering latency. Call GetPredictedOrientation() to get
00173     // predicted orientation.
00174     float       GetPredictionDelta() const                  { return PredictionDT; }
00175     void        SetPrediction(float dt, bool enable = true) { PredictionDT = dt; EnablePrediction = enable; }
00176         void            SetPredictionEnabled(bool enable = true)    { EnablePrediction = enable; }    
00177         bool            IsPredictionEnabled()                       { return EnablePrediction; }
00178 
00179 private:
00180     SensorFusion* getThis()  { return this; }
00181 
00182     // Internal handler for messages; bypasses error checking.
00183     void handleMessage(const MessageBodyFrame& msg);
00184 
00185     // Set the magnetometer's reference orientation for use in yaw correction
00186     // The supplied mag is an uncalibrated value
00187     void        SetMagReference(const Quatf& q, const Vector3f& rawMag);
00188     // Default to current HMD orientation
00189     void        SetMagReference()                { SetMagReference(Q, RawMag); }
00190 
00191         class BodyFrameHandler : public MessageHandler
00192     {
00193         SensorFusion* pFusion;
00194     public:
00195         BodyFrameHandler(SensorFusion* fusion) : pFusion(fusion) { }
00196         ~BodyFrameHandler();
00197 
00198         virtual void OnMessage(const Message& msg);
00199         virtual bool SupportsMessageType(MessageType type) const;
00200     };   
00201 
00202     Quatf             Q;
00203         Quatf                     QUncorrected;
00204     Vector3f          A;    
00205     Vector3f          AngV;
00206     Vector3f          CalMag;
00207     Vector3f          RawMag;
00208     unsigned int      Stage;
00209         float             RunningTime;
00210         float             DeltaT;
00211     BodyFrameHandler  Handler;
00212     MessageHandler*   pDelegate;
00213     float             Gain;
00214     float             YawMult;
00215     volatile bool     EnableGravity;
00216 
00217     bool              EnablePrediction;
00218     float             PredictionDT;
00219         float             PredictionTimeIncrement;
00220 
00221     SensorFilter      FRawMag;
00222     SensorFilter      FAccW;
00223     SensorFilter      FAngV;
00224 
00225     int               TiltCondCount;
00226     float             TiltErrorAngle;
00227     Vector3f          TiltErrorAxis;
00228 
00229     bool              EnableYawCorrection;
00230     Matrix4f          MagCalibrationMatrix;
00231     bool              MagCalibrated;
00232     int               MagCondCount;
00233     float             MagRefDistance;
00234     Quatf             MagRefQ;
00235     Vector3f          MagRefM;
00236     float             MagRefYaw;
00237     bool              MagHasNearbyReference;
00238     Quatf             MagRefTableQ[MagMaxReferences];
00239     Vector3f          MagRefTableM[MagMaxReferences];
00240     float             MagRefTableYaw[MagMaxReferences];
00241     int               MagNumReferences;
00242     float             YawErrorAngle;
00243     int               YawErrorCount;
00244     bool              YawCorrectionInProgress;
00245         bool                      YawCorrectionActivated;
00246 
00247 };
00248 
00249 
00250 } // namespace OVR
00251 
00252 #endif


oculus_sdk
Author(s):
autogenerated on Fri Aug 28 2015 11:53:11