oculus1

annotate libovr/Src/OVR_SensorFusion.h @ 1:e2f9e4603129

find changesets by author, revision, files, or words in the commit message
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
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