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