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