diff options
Diffstat (limited to 'LibOVR/Src/Util/Util_MagCalibration.cpp')
| -rw-r--r-- | LibOVR/Src/Util/Util_MagCalibration.cpp | 57 |
1 files changed, 52 insertions, 5 deletions
diff --git a/LibOVR/Src/Util/Util_MagCalibration.cpp b/LibOVR/Src/Util/Util_MagCalibration.cpp index c537154..58b8c45 100644 --- a/LibOVR/Src/Util/Util_MagCalibration.cpp +++ b/LibOVR/Src/Util/Util_MagCalibration.cpp @@ -23,6 +23,12 @@ void MagCalibration::BeginAutoCalibration(SensorFusion& sf) // This is a "hard" reset of the mag, so need to clear stored values sf.ClearMagCalibration(); SampleCount = 0; + + // reset the statistics + MinMagValues = Vector3f(10000.0f,10000.0f,10000.0f); + MaxMagValues = Vector3f(-10000.0f,-10000.0f,-10000.0f); + MinQuatValues = Quatf(1.0f,1.0f,1.0f,1.0f); + MaxQuatValues = Quatf(0.0f,0.0f,0.0f,0.0f); } unsigned MagCalibration::UpdateAutoCalibration(SensorFusion& sf) @@ -36,7 +42,11 @@ unsigned MagCalibration::UpdateAutoCalibration(SensorFusion& sf) InsertIfAcceptable(q, m); if ((SampleCount == 4) && (Stat == Mag_AutoCalibrating)) + { + //LogText("Magnetometer Output Spread: %f %f %f\n",MagSpread.x,MagSpread.y,MagSpread.z); + //LogText("Quaternion Spread: %f %f %f %f\n",QuatSpread.x,QuatSpread.y,QuatSpread.z,QuatSpread.w); SetCalibration(sf); + } return Stat; @@ -83,7 +93,39 @@ bool MagCalibration::IsAcceptableSample(const Quatf& q, const Vector3f& m) bool MagCalibration::InsertIfAcceptable(const Quatf& q, const Vector3f& m) { - if (IsAcceptableSample(q, m)) + // Update some statistics + if (m.x < MinMagValues.x) + MinMagValues.x = m.x; + if (m.y < MinMagValues.y) + MinMagValues.y = m.y; + if (m.z < MinMagValues.z) + MinMagValues.z = m.z; + if (m.x > MaxMagValues.x) + MaxMagValues.x = m.x; + if (m.y > MaxMagValues.y) + MaxMagValues.y = m.y; + if (m.z > MaxMagValues.z) + MaxMagValues.z = m.z; + if (q.x < MinQuatValues.x) + MinQuatValues.x = q.x; + if (q.y < MinQuatValues.y) + MinQuatValues.y = q.y; + if (q.z < MinQuatValues.z) + MinQuatValues.z = q.z; + if (q.w < MinQuatValues.w) + MinQuatValues.w = q.w; + if (q.x > MaxQuatValues.x) + MaxQuatValues.x = q.x; + if (q.y > MaxQuatValues.y) + MaxQuatValues.y = q.y; + if (q.z > MaxQuatValues.z) + MaxQuatValues.z = q.z; + if (q.w > MaxQuatValues.w) + MaxQuatValues.w = q.w; + MagSpread = MaxMagValues - MinMagValues; + QuatSpread = MaxQuatValues - MinQuatValues; + + if (IsAcceptableSample(q, m)) { MagSamples[SampleCount] = m; QuatSamples[SampleCount] = q; @@ -94,6 +136,14 @@ bool MagCalibration::InsertIfAcceptable(const Quatf& q, const Vector3f& m) return false; } +Matrix4f MagCalibration::GetMagCalibration() const +{ + Matrix4f calMat = Matrix4f(); + calMat.M[0][3] = -MagCenter.x; + calMat.M[1][3] = -MagCenter.y; + calMat.M[2][3] = -MagCenter.z; + return calMat; +} bool MagCalibration::SetCalibration(SensorFusion& sf) { @@ -101,10 +151,7 @@ bool MagCalibration::SetCalibration(SensorFusion& sf) return false; MagCenter = CalculateSphereCenter(MagSamples[0],MagSamples[1],MagSamples[2],MagSamples[3]); - Matrix4f calMat = Matrix4f(); - calMat.M[0][3] = -MagCenter.x; - calMat.M[1][3] = -MagCenter.y; - calMat.M[2][3] = -MagCenter.z; + Matrix4f calMat = GetMagCalibration(); sf.SetMagCalibration(calMat); Stat = Mag_Calibrated; //LogText("MagCenter: %f %f %f\n",MagCenter.x,MagCenter.y,MagCenter.z); |