mirror of
https://github.com/mii443/rust-openvr.git
synced 2025-08-24 17:19:25 +00:00
355 lines
13 KiB
C++
355 lines
13 KiB
C++
/************************************************************************************
|
|
|
|
Filename : OVR_SensorCalibration.cpp
|
|
Content : Calibration data implementation for the IMU messages
|
|
Created : January 28, 2014
|
|
Authors : Max Katsev
|
|
|
|
Copyright : Copyright 2014 Oculus VR, Inc. All Rights reserved.
|
|
|
|
Licensed under the Oculus VR Rift SDK License Version 3.1 (the "License");
|
|
you may not use the Oculus VR Rift SDK except in compliance with the License,
|
|
which is provided at the time of installation or download, or which
|
|
otherwise accompanies this software in either electronic or hard copy form.
|
|
|
|
You may obtain a copy of the License at
|
|
|
|
http://www.oculusvr.com/licenses/LICENSE-3.1
|
|
|
|
Unless required by applicable law or agreed to in writing, the Oculus VR SDK
|
|
distributed under the License is distributed on an "AS IS" BASIS,
|
|
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
See the License for the specific language governing permissions and
|
|
limitations under the License.
|
|
|
|
*************************************************************************************/
|
|
|
|
#include "OVR_SensorCalibration.h"
|
|
#include "Kernel/OVR_Log.h"
|
|
#include "Kernel/OVR_Threads.h"
|
|
#include <time.h>
|
|
|
|
namespace OVR {
|
|
|
|
using namespace Alg;
|
|
|
|
const UByte VERSION = 2;
|
|
const UByte MAX_COMPAT_VERSION = 15;
|
|
|
|
SensorCalibration::SensorCalibration(SensorDevice* pSensor)
|
|
: MagCalibrated(false), GyroFilter(6000), GyroAutoTemperature(0)
|
|
{
|
|
this->pSensor = pSensor;
|
|
};
|
|
|
|
void SensorCalibration::Initialize()
|
|
{
|
|
// read factory calibration
|
|
pSensor->GetFactoryCalibration(&AccelOffset, &GyroAutoOffset, &AccelMatrix, &GyroMatrix, &GyroAutoTemperature);
|
|
|
|
// if the headset has an autocalibrated offset, prefer it over the factory defaults
|
|
GyroOffsetReport gyroReport;
|
|
bool result = pSensor->GetGyroOffsetReport(&gyroReport);
|
|
if (result && gyroReport.Version != GyroOffsetReport::Version_NoOffset)
|
|
{
|
|
GyroAutoOffset = (Vector3f) gyroReport.Offset;
|
|
GyroAutoTemperature = (float) gyroReport.Temperature;
|
|
}
|
|
|
|
// read the temperature tables and prepare the interpolation structures
|
|
result = pSensor->GetAllTemperatureReports(&TemperatureReports);
|
|
OVR_ASSERT(result);
|
|
for (int i = 0; i < 3; i++)
|
|
Interpolators[i].Initialize(TemperatureReports, i);
|
|
|
|
// read the mag calibration
|
|
MagCalibrationReport report;
|
|
result = pSensor->GetMagCalibrationReport(&report);
|
|
MagCalibrated = result && report.Version > 0;
|
|
MagMatrix = report.Calibration;
|
|
if (!MagCalibrated)
|
|
{
|
|
// OVR_ASSERT(false);
|
|
LogError("Magnetometer calibration not found!\n");
|
|
}
|
|
}
|
|
|
|
void SensorCalibration::DebugPrintLocalTemperatureTable()
|
|
{
|
|
LogText("TemperatureReports:\n");
|
|
for (int i = 0; i < (int)TemperatureReports.GetSize(); i++)
|
|
{
|
|
for (int j = 0; j < (int)TemperatureReports[i].GetSize(); j++)
|
|
{
|
|
TemperatureReport& tr = TemperatureReports[i][j];
|
|
|
|
LogText("[%d][%d]: Version=%3d, Bin=%d/%d, "
|
|
"Sample=%d/%d, TargetTemp=%3.1lf, "
|
|
"ActualTemp=%4.1lf, "
|
|
"Offset=(%7.2lf, %7.2lf, %7.2lf), "
|
|
"Time=%d\n", i, j, tr.Version,
|
|
tr.Bin, tr.NumBins,
|
|
tr.Sample, tr.NumSamples,
|
|
tr.TargetTemperature,
|
|
tr.ActualTemperature,
|
|
tr.Offset.x, tr.Offset.y, tr.Offset.z,
|
|
tr.Time);
|
|
}
|
|
}
|
|
}
|
|
|
|
void SensorCalibration::DebugClearHeadsetTemperatureReports()
|
|
{
|
|
OVR_ASSERT(pSensor != NULL);
|
|
|
|
bool result;
|
|
|
|
Array<Array<TemperatureReport> > temperatureReports;
|
|
pSensor->GetAllTemperatureReports(&temperatureReports);
|
|
|
|
OVR_ASSERT(temperatureReports.GetSize() > 0);
|
|
OVR_ASSERT(temperatureReports[0].GetSize() > 0);
|
|
|
|
TemperatureReport& tr = TemperatureReports[0][0];
|
|
|
|
tr.ActualTemperature = 0.0;
|
|
tr.Time = 0;
|
|
tr.Version = 0;
|
|
tr.Offset.x = tr.Offset.y = tr.Offset.z = 0.0;
|
|
|
|
for (UByte i = 0; i < tr.NumBins; i++)
|
|
{
|
|
tr.Bin = i;
|
|
|
|
for (UByte j = 0; j < tr.NumSamples; j++)
|
|
{
|
|
tr.Sample = j;
|
|
|
|
result = pSensor->SetTemperatureReport(tr);
|
|
OVR_ASSERT(result);
|
|
|
|
// Need to wait for the tracker board to finish writing to eeprom.
|
|
Thread::MSleep(50);
|
|
}
|
|
}
|
|
}
|
|
|
|
void SensorCalibration::Apply(MessageBodyFrame& msg)
|
|
{
|
|
AutocalibrateGyro(msg);
|
|
|
|
// compute the interpolated offset
|
|
Vector3f gyroOffset;
|
|
for (int i = 0; i < 3; i++)
|
|
gyroOffset[i] = (float) Interpolators[i].GetOffset(msg.Temperature, GyroAutoTemperature, GyroAutoOffset[i]);
|
|
|
|
// apply calibration
|
|
msg.RotationRate = GyroMatrix.Transform(msg.RotationRate - gyroOffset);
|
|
msg.Acceleration = AccelMatrix.Transform(msg.Acceleration - AccelOffset);
|
|
if (MagCalibrated)
|
|
msg.MagneticField = MagMatrix.Transform(msg.MagneticField);
|
|
}
|
|
|
|
void SensorCalibration::AutocalibrateGyro(MessageBodyFrame const& msg)
|
|
{
|
|
const float alpha = 0.4f;
|
|
// 1.25f is a scaling factor related to conversion from per-axis comparison to length comparison
|
|
const float absLimit = 1.25f * 0.349066f;
|
|
const float noiseLimit = 1.25f * 0.03f;
|
|
|
|
Vector3f gyro = msg.RotationRate;
|
|
// do a moving average to reject short term noise
|
|
Vector3f avg = (GyroFilter.IsEmpty()) ? gyro : gyro * alpha + GyroFilter.PeekBack() * (1 - alpha);
|
|
|
|
// Make sure the absolute value is below what is likely motion
|
|
// Make sure it is close enough to the current average that it is probably noise and not motion
|
|
if (avg.Length() >= absLimit || (avg - GyroFilter.Mean()).Length() >= noiseLimit)
|
|
GyroFilter.Clear();
|
|
GyroFilter.PushBack(avg);
|
|
|
|
// if had a reasonable number of samples already use it for the current offset
|
|
if (GyroFilter.GetSize() > GyroFilter.GetCapacity() / 2)
|
|
{
|
|
GyroAutoOffset = GyroFilter.Mean();
|
|
GyroAutoTemperature = msg.Temperature;
|
|
// After ~6 seconds of no motion, use the average as the new zero rate offset
|
|
if (GyroFilter.IsFull())
|
|
StoreAutoOffset();
|
|
}
|
|
}
|
|
|
|
void SensorCalibration::StoreAutoOffset()
|
|
{
|
|
const double maxDeltaT = 2.5;
|
|
const double minExtraDeltaT = 0.5;
|
|
const UInt32 minDelay = 24 * 3600; // 1 day in seconds
|
|
|
|
// find the best bin
|
|
UPInt binIdx = 0;
|
|
for (UPInt i = 1; i < TemperatureReports.GetSize(); i++)
|
|
if (Abs(GyroAutoTemperature - TemperatureReports[i][0].TargetTemperature) <
|
|
Abs(GyroAutoTemperature - TemperatureReports[binIdx][0].TargetTemperature))
|
|
binIdx = i;
|
|
|
|
// find the oldest and newest samples
|
|
// NB: uninitialized samples have Time == 0, so they will get picked as the oldest
|
|
UPInt newestIdx = 0, oldestIdx = 0;
|
|
for (UPInt i = 1; i < TemperatureReports[binIdx].GetSize(); i++)
|
|
{
|
|
// if the version is newer - do nothing
|
|
if (TemperatureReports[binIdx][i].Version > VERSION)
|
|
return;
|
|
if (TemperatureReports[binIdx][i].Time > TemperatureReports[binIdx][newestIdx].Time)
|
|
newestIdx = i;
|
|
if (TemperatureReports[binIdx][i].Time < TemperatureReports[binIdx][oldestIdx].Time)
|
|
oldestIdx = i;
|
|
}
|
|
TemperatureReport& oldestReport = TemperatureReports[binIdx][oldestIdx];
|
|
TemperatureReport& newestReport = TemperatureReports[binIdx][newestIdx];
|
|
OVR_ASSERT((oldestReport.Sample == 0 && newestReport.Sample == 0 && newestReport.Version == 0) ||
|
|
oldestReport.Sample == (newestReport.Sample + 1) % newestReport.NumSamples);
|
|
|
|
bool writeSuccess = false;
|
|
UInt32 now = (UInt32) time(0);
|
|
if (now - newestReport.Time > minDelay)
|
|
{
|
|
// only write a new sample if the temperature is close enough
|
|
if (Abs(GyroAutoTemperature - oldestReport.TargetTemperature) < maxDeltaT)
|
|
{
|
|
oldestReport.Time = now;
|
|
oldestReport.ActualTemperature = GyroAutoTemperature;
|
|
oldestReport.Offset = (Vector3d) GyroAutoOffset;
|
|
oldestReport.Version = VERSION;
|
|
writeSuccess = pSensor->SetTemperatureReport(oldestReport);
|
|
OVR_ASSERT(writeSuccess);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// if the newest sample is too recent - _update_ it if significantly closer to the target temp
|
|
if (Abs(GyroAutoTemperature - newestReport.TargetTemperature) + minExtraDeltaT
|
|
< Abs(newestReport.ActualTemperature - newestReport.TargetTemperature))
|
|
{
|
|
// (do not update the time!)
|
|
newestReport.ActualTemperature = GyroAutoTemperature;
|
|
newestReport.Offset = (Vector3d) GyroAutoOffset;
|
|
newestReport.Version = VERSION;
|
|
writeSuccess = pSensor->SetTemperatureReport(newestReport);
|
|
OVR_ASSERT(writeSuccess);
|
|
}
|
|
}
|
|
|
|
// update the interpolators with the new data
|
|
// this is not particularly expensive call and would only happen rarely
|
|
// but if performance is a problem, it's possible to only recompute the data that has changed
|
|
if (writeSuccess)
|
|
for (int i = 0; i < 3; i++)
|
|
Interpolators[i].Initialize(TemperatureReports, i);
|
|
}
|
|
|
|
const TemperatureReport& median(const Array<TemperatureReport>& temperatureReportsBin, int coord)
|
|
{
|
|
Array<double> values;
|
|
values.Reserve(temperatureReportsBin.GetSize());
|
|
for (unsigned i = 0; i < temperatureReportsBin.GetSize(); i++)
|
|
if (temperatureReportsBin[i].ActualTemperature != 0)
|
|
values.PushBack(temperatureReportsBin[i].Offset[coord]);
|
|
if (values.GetSize() > 0)
|
|
{
|
|
double med = Median(values);
|
|
// this is kind of a hack
|
|
for (unsigned i = 0; i < temperatureReportsBin.GetSize(); i++)
|
|
if (temperatureReportsBin[i].Offset[coord] == med)
|
|
return temperatureReportsBin[i];
|
|
// if we haven't found the median in the original array, something is wrong
|
|
OVR_DEBUG_BREAK;
|
|
}
|
|
return temperatureReportsBin[0];
|
|
}
|
|
|
|
void OffsetInterpolator::Initialize(Array<Array<TemperatureReport> > const& temperatureReports, int coord)
|
|
{
|
|
int bins = (int) temperatureReports.GetSize();
|
|
Temperatures.Clear();
|
|
Temperatures.Reserve(bins);
|
|
Values.Clear();
|
|
Values.Reserve(bins);
|
|
|
|
for (int bin = 0; bin < bins; bin++)
|
|
{
|
|
OVR_ASSERT(temperatureReports[bin].GetSize() == temperatureReports[0].GetSize());
|
|
const TemperatureReport& report = median(temperatureReports[bin], coord);
|
|
if (report.Version > 0 && report.Version <= MAX_COMPAT_VERSION)
|
|
{
|
|
Temperatures.PushBack(report.ActualTemperature);
|
|
Values.PushBack(report.Offset[coord]);
|
|
}
|
|
}
|
|
}
|
|
|
|
double OffsetInterpolator::GetOffset(double targetTemperature, double autoTemperature, double autoValue)
|
|
{
|
|
const double autoRangeExtra = 1.0;
|
|
const double minInterpolationDist = 0.5;
|
|
|
|
// difference between current and autocalibrated temperature adjusted for preference over historical data
|
|
const double adjustedDeltaT = Abs(autoTemperature - targetTemperature) - autoRangeExtra;
|
|
|
|
int count = (int) Temperatures.GetSize();
|
|
// handle special cases when we don't have enough data for proper interpolation
|
|
if (count == 0)
|
|
return autoValue;
|
|
if (count == 1)
|
|
{
|
|
if (adjustedDeltaT < Abs(Temperatures[0] - targetTemperature))
|
|
return autoValue;
|
|
else
|
|
return Values[0];
|
|
}
|
|
|
|
// first, find the interval that contains targetTemperature
|
|
// if all points are on the same side of targetTemperature, find the adjacent interval
|
|
int l;
|
|
if (targetTemperature < Temperatures[1])
|
|
l = 0;
|
|
else if (targetTemperature >= Temperatures[count - 2])
|
|
l = count - 2;
|
|
else
|
|
for (l = 1; l < count - 2; l++)
|
|
if (Temperatures[l] <= targetTemperature && targetTemperature < Temperatures[l+1])
|
|
break;
|
|
int u = l + 1;
|
|
|
|
// extend the interval if it's too small and the interpolation is unreliable
|
|
if (Temperatures[u] - Temperatures[l] < minInterpolationDist)
|
|
{
|
|
if (l > 0
|
|
&& (u == count - 1 || Temperatures[u] - Temperatures[l - 1] < Temperatures[u + 1] - Temperatures[l]))
|
|
l--;
|
|
else if (u < count - 1)
|
|
u++;
|
|
}
|
|
|
|
// verify correctness
|
|
OVR_ASSERT(l >= 0 && u < count);
|
|
OVR_ASSERT(l == 0 || Temperatures[l] <= targetTemperature);
|
|
OVR_ASSERT(u == count - 1 || targetTemperature < Temperatures[u]);
|
|
OVR_ASSERT((l == 0 && u == count - 1) || Temperatures[u] - Temperatures[l] > minInterpolationDist);
|
|
OVR_ASSERT(Temperatures[l] <= Temperatures[u]);
|
|
|
|
// perform the interpolation
|
|
double slope;
|
|
if (Temperatures[u] - Temperatures[l] >= minInterpolationDist)
|
|
slope = (Values[u] - Values[l]) / (Temperatures[u] - Temperatures[l]);
|
|
else
|
|
// avoid a badly conditioned problem
|
|
slope = 0;
|
|
if (adjustedDeltaT < Abs(Temperatures[u] - targetTemperature))
|
|
// use the autocalibrated value, if it's close
|
|
return autoValue + slope * (targetTemperature - autoTemperature);
|
|
else
|
|
return Values[u] + slope * (targetTemperature - Temperatures[u]);
|
|
}
|
|
|
|
} // namespace OVR
|