Vector3f SensorFusion::GetCalibratedMagValue(const Vector3f& rawMag) const
{
Vector3f mag = rawMag;
OVR_ASSERT(HasMagCalibration());
mag.x += MagCalibrationMatrix.M[0][3];
mag.y += MagCalibrationMatrix.M[1][3];
mag.z += MagCalibrationMatrix.M[2][3];
return mag;
}
void SensorFusion::handleMessage(const MessageBodyFrame& msg)
{
if (msg.Type != Message_BodyFrame)
return;
// Put the sensor readings into convenient local variables
Vector3f angVel = msg.RotationRate;
Vector3f rawAccel = msg.Acceleration;
Vector3f mag = msg.MagneticField;
// Set variables accessible through the class API
DeltaT = msg.TimeDelta;
AngV = msg.RotationRate;
AngV.y *= YawMult; // Warning: If YawMult != 1, then AngV is not true angular velocity
A = rawAccel;
// Allow external access to uncalibrated magnetometer values
RawMag = mag;
// Apply the calibration parameters to raw mag
if (HasMagCalibration())
{
mag.x += MagCalibrationMatrix.M[0][3];
mag.y += MagCalibrationMatrix.M[1][3];
mag.z += MagCalibrationMatrix.M[2][3];
}
// Provide external access to calibrated mag values
// (if the mag is not calibrated, then the raw value is returned)
CalMag = mag;
float angVelLength = angVel.Length();
float accLength = rawAccel.Length();
// Acceleration in the world frame (Q is current HMD orientation)
Vector3f accWorld = Q.Rotate(rawAccel);
// Keep track of time
Stage++;
float currentTime = Stage * DeltaT; // Assumes uniform time spacing
// Insert current sensor data into filter history
FRawMag.AddElement(RawMag);
FAccW.AddElement(accWorld);
FAngV.AddElement(angVel);
// Update orientation Q based on gyro outputs. This technique is
// based on direct properties of the angular velocity vector:
// Its direction is the current rotation axis, and its magnitude
// is the rotation rate (rad/sec) about that axis. Our sensor
// sampling rate is so fast that we need not worry about integral
// approximation error (not yet, anyway).
if (angVelLength > 0.0f)
{
Vector3f rotAxis = angVel / angVelLength;
float halfRotAngle = angVelLength * DeltaT * 0.5f;
float sinHRA = sin(halfRotAngle);
Quatf deltaQ(rotAxis.x*sinHRA, rotAxis.y*sinHRA, rotAxis.z*sinHRA, cos(halfRotAngle));
Q = Q * deltaQ;
}
// The quaternion magnitude may slowly drift due to numerical error,
// so it is periodically normalized.
if (Stage % 5000 == 0)
Q.Normalize();
// Maintain the uncorrected orientation for later use by predictive filtering
QUncorrected = Q;
// Perform tilt correction using the accelerometer data. This enables
// drift errors in pitch and roll to be corrected. Note that yaw cannot be corrected
// because the rotation axis is parallel to the gravity vector.
if (EnableGravity)
{
// Correcting for tilt error by using accelerometer data
const float gravityEpsilon = 0.4f;
const float angVelEpsilon = 0.1f; // Relatively slow rotation
const int tiltPeriod = 50; // Req'd time steps of stability
const float maxTiltError = 0.05f;
const float minTiltError = 0.01f;
// This condition estimates whether the only measured acceleration is due to gravity
// (the Rift is not linearly accelerating). It is often wrong, but tends to average
// out well over time.
if ((fabs(accLength - 9.81f) < gravityEpsilon) &&
(angVelLength < angVelEpsilon))
TiltCondCount++;
else
TiltCondCount = 0;
// After stable measurements have been taken over a sufficiently long period,
// estimate the amount of tilt error and calculate the tilt axis for later correction.
if (TiltCondCount >= tiltPeriod)
{ // Update TiltErrorEstimate
TiltCondCount = 0;
// Use an average value to reduce noice (could alternatively use an LPF)
Vector3f accWMean = FAccW.Mean();
// Project the acceleration vector into the XZ plane
Vector3f xzAcc = Vector3f(accWMean.x, 0.0f, accWMean.z);
// The unit normal of xzAcc will be the rotation axis for tilt correction
Vector3f tiltAxis = Vector3f(xzAcc.z, 0.0f, -xzAcc.x).Normalized();
Vector3f yUp = Vector3f(0.0f, 1.0f, 0.0f);
// This is the amount of rotation
float tiltAngle = yUp.Angle(accWMean);
// Record values if the tilt error is intolerable
if (tiltAngle > maxTiltError)
{
TiltErrorAngle = tiltAngle;
TiltErrorAxis = tiltAxis;
}
}
// This part performs the actual tilt correction as needed
if (TiltErrorAngle > minTiltError)
{
if ((TiltErrorAngle > 0.4f)&&(Stage < 8000))
{ // Tilt completely to correct orientation
Q = Quatf(TiltErrorAxis, -TiltErrorAngle) * Q;
TiltErrorAngle = 0.0f;
}
else
{
//LogText("Performing tilt correction - Angle: %f Axis: %f %f %f\n",
// TiltErrorAngle,TiltErrorAxis.x,TiltErrorAxis.y,TiltErrorAxis.z);
//float deltaTiltAngle = -Gain*TiltErrorAngle*0.005f;
// This uses agressive correction steps while your head is moving fast
float deltaTiltAngle = -Gain*TiltErrorAngle*0.005f*(5.0f*angVelLength+1.0f);
// Incrementally "untilt" by a small step size
Q = Quatf(TiltErrorAxis, deltaTiltAngle) * Q;
TiltErrorAngle += deltaTiltAngle;
}
}
}
// Yaw drift correction based on magnetometer data. This corrects the part of the drift
// that the accelerometer cannot handle.
// This will only work if the magnetometer has been enabled, calibrated, and a reference
// point has been set.
const float maxAngVelLength = 3.0f;
const int magWindow = 5;
const float yawErrorMax = 0.1f;
const float yawErrorMin = 0.01f;
const int yawErrorCountLimit = 50;
const float yawRotationStep = 0.00002f;
if (angVelLength < maxAngVelLength)
MagCondCount++;
else
MagCondCount = 0;
YawCorrectionInProgress = false;
if (EnableYawCorrection && MagReady && (currentTime > 2.0f) && (MagCondCount >= magWindow) &&
(Q.Distance(MagRefQ) < MagRefDistance))
{
// Use rotational invariance to bring reference mag value into global frame
Vector3f grefmag = MagRefQ.Rotate(GetCalibratedMagValue(MagRefM));
// Bring current (averaged) mag reading into global frame
Vector3f gmag = Q.Rotate(GetCalibratedMagValue(FRawMag.Mean()));
// Calculate the reference yaw in the global frame
float gryaw = atan2(grefmag.x,grefmag.z);
// Calculate the current yaw in the global frame
float gyaw = atan2(gmag.x,gmag.z);
//LogText("Yaw error estimate: %f\n",YawErrorAngle);
// The difference between reference and current yaws is the perceived error
YawErrorAngle = AngleDifference(gyaw,gryaw);
// If the perceived error is large, keep count
if ((fabs(YawErrorAngle) > yawErrorMax) && (!YawCorrectionActivated))
YawErrorCount++;
// After enough iterations of high perceived error, start the correction process
if (YawErrorCount > yawErrorCountLimit)
YawCorrectionActivated = true;
// If the perceived error becomes small, turn off the yaw correction
if ((fabs(YawErrorAngle) < yawErrorMin) && YawCorrectionActivated)
{
YawCorrectionActivated = false;
YawErrorCount = 0;
}
// Perform the actual yaw correction, due to previously detected, large yaw error
if (YawCorrectionActivated)
{
YawCorrectionInProgress = true;
int sign = (YawErrorAngle > 0.0f) ? 1 : -1;
// Incrementally "unyaw" by a small step size
Q = Quatf(Vector3f(0.0f,1.0f,0.0f), -yawRotationStep * sign) * Q;
}
}
}
// Writes the current calibration for a particular device to a device profile file
// sensor - the sensor that was calibrated
// cal_name - an optional name for the calibration or default if cal_name == NULL
bool SensorFusion::SaveMagCalibration(const char* calibrationName) const
{
if (CachedSensorInfo.SerialNumber[0] == 0 || !HasMagCalibration())
return false;
// A named calibration may be specified for calibration in different
// environments, otherwise the default calibration is used
if (calibrationName == NULL)
calibrationName = "default";
// Generate a mag calibration event
JSON* calibration = JSON::CreateObject();
// (hardcoded for now) the measurement and representation method
calibration->AddStringItem("Version", "2.0");
calibration->AddStringItem("Name", "default");
// time stamp the calibration
char time_str[64];
#if defined(OVR_OS_WIN32) and !defined(__MINGW32__)
struct tm caltime;
localtime_s(&caltime, &MagCalibrationTime);
strftime(time_str, 64, "%Y-%m-%d %H:%M:%S", &caltime);
#else
struct tm* caltime;
caltime = localtime(&MagCalibrationTime);
strftime(time_str, 64, "%Y-%m-%d %H:%M:%S", caltime);
#endif
calibration->AddStringItem("Time", time_str);
// write the full calibration matrix
char matrix[256];
Matrix4f calmat = GetMagCalibration();
calmat.ToString(matrix, 256);
calibration->AddStringItem("CalibrationMatrix", matrix);
// save just the offset, for backwards compatibility
// this can be removed when we don't want to support 0.2.4 anymore
Vector3f center(calmat.M[0][3], calmat.M[1][3], calmat.M[2][3]);
Matrix4f tmp = calmat; tmp.M[0][3] = tmp.M[1][3] = tmp.M[2][3] = 0; tmp.M[3][3] = 1;
center = tmp.Inverted().Transform(center);
Matrix4f oldcalmat; oldcalmat.M[0][3] = center.x; oldcalmat.M[1][3] = center.y; oldcalmat.M[2][3] = center.z;
oldcalmat.ToString(matrix, 256);
calibration->AddStringItem("Calibration", matrix);
String path = GetBaseOVRPath(true);
path += "/Devices.json";
// Look for a prexisting device file to edit
Ptr<JSON> root = *JSON::Load(path);
if (root)
{ // Quick sanity check of the file type and format before we parse it
JSON* version = root->GetFirstItem();
if (version && version->Name == "Oculus Device Profile Version")
{
int major = atoi(version->Value.ToCStr());
if (major > MAX_DEVICE_PROFILE_MAJOR_VERSION)
{
// don't use the file on unsupported major version number
root->Release();
root = NULL;
}
}
else
{
root->Release();
root = NULL;
}
}
JSON* device = NULL;
if (root)
{
device = root->GetFirstItem(); // skip the header
device = root->GetNextItem(device);
while (device)
{ // Search for a previous calibration with the same name for this device
// and remove it before adding the new one
if (device->Name == "Device")
{
JSON* item = device->GetItemByName("Serial");
if (item && item->Value == CachedSensorInfo.SerialNumber)
{ // found an entry for this device
item = device->GetNextItem(item);
while (item)
{
if (item->Name == "MagCalibration")
{
JSON* name = item->GetItemByName("Name");
if (name && name->Value == calibrationName)
{ // found a calibration of the same name
item->RemoveNode();
item->Release();
break;
}
}
item = device->GetNextItem(item);
}
// update the auto-mag flag
item = device->GetItemByName("EnableYawCorrection");
if (item)
item->dValue = (double)EnableYawCorrection;
else
device->AddBoolItem("EnableYawCorrection", EnableYawCorrection);
break;
}
}
device = root->GetNextItem(device);
}
}
else
{ // Create a new device root
root = *JSON::CreateObject();
root->AddStringItem("Oculus Device Profile Version", "1.0");
}
if (device == NULL)
{
device = JSON::CreateObject();
device->AddStringItem("Product", CachedSensorInfo.ProductName);
device->AddNumberItem("ProductID", CachedSensorInfo.ProductId);
device->AddStringItem("Serial", CachedSensorInfo.SerialNumber);
device->AddBoolItem("EnableYawCorrection", EnableYawCorrection);
root->AddItem("Device", device);
}
// Create and the add the new calibration event to the device
device->AddItem("MagCalibration", calibration);
return root->Save(path);
}
Vector3f SensorFusion::GetCalibratedMagValue(const Vector3f& rawMag) const
{
OVR_ASSERT(HasMagCalibration());
return MagCalibrationMatrix.Transform(rawMag);
}
