oculus1
view 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 source
1 /************************************************************************************
3 PublicHeader: OVR.h
4 Filename : OVR_SensorFusion.h
5 Content : Methods that determine head orientation from sensor data over time
6 Created : October 9, 2012
7 Authors : Michael Antonov, Steve LaValle
9 Copyright : Copyright 2012 Oculus VR, Inc. All Rights reserved.
11 Use of this software is subject to the terms of the Oculus license
12 agreement provided at the time of installation or download, or which
13 otherwise accompanies this software in either electronic or hard copy form.
15 *************************************************************************************/
17 #ifndef OVR_SensorFusion_h
18 #define OVR_SensorFusion_h
20 #include "OVR_Device.h"
21 #include "OVR_SensorFilter.h"
22 #include <time.h>
24 namespace OVR {
26 //-------------------------------------------------------------------------------------
27 // ***** SensorFusion
29 // SensorFusion class accumulates Sensor notification messages to keep track of
30 // orientation, which involves integrating the gyro and doing correction with gravity.
31 // Magnetometer based yaw drift correction is also supported; it is usually enabled
32 // automatically based on loaded magnetometer configuration.
33 // Orientation is reported as a quaternion, from which users can obtain either the
34 // rotation matrix or Euler angles.
35 //
36 // The class can operate in two ways:
37 // - By user manually passing MessageBodyFrame messages to the OnMessage() function.
38 // - By attaching SensorFusion to a SensorDevice, in which case it will
39 // automatically handle notifications from that device.
42 class SensorFusion : public NewOverrideBase
43 {
44 enum
45 {
46 MagMaxReferences = 80
47 };
49 public:
50 SensorFusion(SensorDevice* sensor = 0);
51 ~SensorFusion();
54 // *** Setup
56 // Attaches this SensorFusion to a sensor device, from which it will receive
57 // notification messages. If a sensor is attached, manual message notification
58 // is not necessary. Calling this function also resets SensorFusion state.
59 bool AttachToSensor(SensorDevice* sensor);
61 // Returns true if this Sensor fusion object is attached to a sensor.
62 bool IsAttachedToSensor() const { return Handler.IsHandlerInstalled(); }
66 // *** State Query
68 // Obtain the current accumulated orientation. Many apps will want to use GetPredictedOrientation
69 // instead to reduce latency.
70 Quatf GetOrientation() const { return lockedGet(&Q); }
72 // Get predicted orientaion in the near future; predictDt is lookahead amount in seconds.
73 Quatf GetPredictedOrientation(float predictDt);
74 Quatf GetPredictedOrientation() { return GetPredictedOrientation(PredictionDT); }
76 // Obtain the last absolute acceleration reading, in m/s^2.
77 Vector3f GetAcceleration() const { return lockedGet(&A); }
78 // Obtain the last angular velocity reading, in rad/s.
79 Vector3f GetAngularVelocity() const { return lockedGet(&AngV); }
81 // Obtain the last raw magnetometer reading, in Gauss
82 Vector3f GetMagnetometer() const { return lockedGet(&RawMag); }
83 // Obtain the calibrated magnetometer reading (direction and field strength)
84 Vector3f GetCalibratedMagnetometer() const { OVR_ASSERT(MagCalibrated); return lockedGet(&CalMag); }
87 // Resets the current orientation.
88 void Reset();
92 // *** Configuration
94 void EnableMotionTracking(bool enable = true) { MotionTrackingEnabled = enable; }
95 bool IsMotionTrackingEnabled() const { return MotionTrackingEnabled; }
97 // Multiplier for yaw rotation (turning); setting this higher than 1 (the default) can allow the game
98 // to be played without auxillary rotation controls, possibly making it more immersive.
99 // Whether this is more or less likely to cause motion sickness is unknown.
100 float GetYawMultiplier() const { return YawMult; }
101 void SetYawMultiplier(float y) { YawMult = y; }
104 // *** Prediction Control
106 // Prediction functions.
107 // Prediction delta specifes how much prediction should be applied in seconds; it should in
108 // general be under the average rendering latency. Call GetPredictedOrientation() to get
109 // predicted orientation.
110 float GetPredictionDelta() const { return PredictionDT; }
111 void SetPrediction(float dt, bool enable = true) { PredictionDT = dt; EnablePrediction = enable; }
112 void SetPredictionEnabled(bool enable = true) { EnablePrediction = enable; }
113 bool IsPredictionEnabled() { return EnablePrediction; }
116 // *** Accelerometer/Gravity Correction Control
118 // Enables/disables gravity correction (on by default).
119 void SetGravityEnabled(bool enableGravity) { EnableGravity = enableGravity; }
120 bool IsGravityEnabled() const { return EnableGravity;}
122 // Gain used to correct gyro with accel. Default value is appropriate for typical use.
123 float GetAccelGain() const { return Gain; }
124 void SetAccelGain(float ag) { Gain = ag; }
128 // *** Magnetometer and Yaw Drift Correction Control
130 // Methods to load and save a mag calibration. Calibrations can optionally
131 // be specified by name to differentiate multiple calibrations under different conditions
132 // If LoadMagCalibration succeeds, it will override YawCorrectionEnabled based on
133 // saved calibration setting.
134 bool SaveMagCalibration(const char* calibrationName = NULL) const;
135 bool LoadMagCalibration(const char* calibrationName = NULL);
137 // Enables/disables magnetometer based yaw drift correction. Must also have mag calibration
138 // data for this correction to work.
139 void SetYawCorrectionEnabled(bool enable) { EnableYawCorrection = enable; }
140 // Determines if yaw correction is enabled.
141 bool IsYawCorrectionEnabled() const { return EnableYawCorrection;}
143 // Yaw correction is currently working (forcing a corrective yaw rotation)
144 bool IsYawCorrectionInProgress() const { return YawCorrectionInProgress;}
146 // Store the calibration matrix for the magnetometer
147 void SetMagCalibration(const Matrix4f& m)
148 {
149 MagCalibrationMatrix = m;
150 time(&MagCalibrationTime); // time stamp the calibration
151 MagCalibrated = true;
152 }
154 // Retrieves the magnetometer calibration matrix
155 Matrix4f GetMagCalibration() const { return MagCalibrationMatrix; }
156 // Retrieve the time of the calibration
157 time_t GetMagCalibrationTime() const { return MagCalibrationTime; }
159 // True only if the mag has calibration values stored
160 bool HasMagCalibration() const { return MagCalibrated;}
161 // Force the mag into the uncalibrated state
162 void ClearMagCalibration() { MagCalibrated = false; }
164 // These refer to reference points that associate mag readings with orientations
165 void ClearMagReferences() { MagNumReferences = 0; }
166 void SetMagRefDistance(const float d) { MagRefDistance = d; }
169 Vector3f GetCalibratedMagValue(const Vector3f& rawMag) const;
171 float GetMagRefYaw() const { return MagRefYaw; }
172 float GetYawErrorAngle() const { return YawErrorAngle; }
176 // *** Message Handler Logic
178 // Notifies SensorFusion object about a new BodyFrame message from a sensor.
179 // Should be called by user if not attaching to a sensor.
180 void OnMessage(const MessageBodyFrame& msg)
181 {
182 OVR_ASSERT(!IsAttachedToSensor());
183 handleMessage(msg);
184 }
186 void SetDelegateMessageHandler(MessageHandler* handler)
187 { pDelegate = handler; }
191 private:
193 SensorFusion* getThis() { return this; }
195 // Helper used to read and return value within a Lock.
196 template<class C>
197 C lockedGet(const C* p) const
198 {
199 Lock::Locker lockScope(Handler.GetHandlerLock());
200 return *p;
201 }
203 // Internal handler for messages; bypasses error checking.
204 void handleMessage(const MessageBodyFrame& msg);
206 // Set the magnetometer's reference orientation for use in yaw correction
207 // The supplied mag is an uncalibrated value
208 void setMagReference(const Quatf& q, const Vector3f& rawMag);
209 // Default to current HMD orientation
210 void setMagReference() { setMagReference(Q, RawMag); }
212 class BodyFrameHandler : public MessageHandler
213 {
214 SensorFusion* pFusion;
215 public:
216 BodyFrameHandler(SensorFusion* fusion) : pFusion(fusion) { }
217 ~BodyFrameHandler();
219 virtual void OnMessage(const Message& msg);
220 virtual bool SupportsMessageType(MessageType type) const;
221 };
223 SensorInfo CachedSensorInfo;
225 Quatf Q;
226 Quatf QUncorrected;
227 Vector3f A;
228 Vector3f AngV;
229 Vector3f CalMag;
230 Vector3f RawMag;
231 unsigned int Stage;
232 float RunningTime;
233 float DeltaT;
234 BodyFrameHandler Handler;
235 MessageHandler* pDelegate;
236 float Gain;
237 float YawMult;
238 volatile bool EnableGravity;
240 bool EnablePrediction;
241 float PredictionDT;
242 float PredictionTimeIncrement;
244 SensorFilter FRawMag;
245 SensorFilter FAccW;
246 SensorFilter FAngV;
248 int TiltCondCount;
249 float TiltErrorAngle;
250 Vector3f TiltErrorAxis;
252 bool EnableYawCorrection;
253 Matrix4f MagCalibrationMatrix;
254 time_t MagCalibrationTime;
255 bool MagCalibrated;
256 int MagCondCount;
257 float MagRefDistance;
258 Quatf MagRefQ;
259 Vector3f MagRefM;
260 float MagRefYaw;
261 bool MagHasNearbyReference;
262 Quatf MagRefTableQ[MagMaxReferences];
263 Vector3f MagRefTableM[MagMaxReferences];
264 float MagRefTableYaw[MagMaxReferences];
265 int MagNumReferences;
266 float YawErrorAngle;
267 int YawErrorCount;
268 bool YawCorrectionInProgress;
269 bool YawCorrectionActivated;
271 bool MotionTrackingEnabled;
272 };
275 } // namespace OVR
277 #endif