oculus1
diff libovr/Src/OVR_SensorFusion.h @ 1:e2f9e4603129
added LibOVR and started a simple vr wrapper.
author | John Tsiombikas <nuclear@member.fsf.org> |
---|---|
date | Sat, 14 Sep 2013 16:14:59 +0300 |
parents | |
children |
line diff
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