Merge branch 'master' into stable

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);