新地磁校准算法

Assets/BowArrow/Scripts/Bluetooth/AimHandler.cs

@@ -4,6 +4,8 @@ using System.Collections.Generic;
 using System.Linq;
 using UnityEngine.UI;
 using Newtonsoft.Json;
+using o0._9Axis;
+
 /* 瞄准处理器 */
 public class AimHandler : MonoBehaviour
 {
@@ -42,7 +44,8 @@ public class AimHandler : MonoBehaviour
     Vector3 cMaxVector = new Vector3(0,0,0);
     Vector3 cMinVector = new Vector3(0, 0, 0);
 
-    public o0MagneticCalibraterEllipsoidFitting MagCalibrater;
+    [o0.Serialize]
+    public MagnetometerAutoCalibrater MagCalibrater;
     o0GyrCalibrater GyrCalibrater;
 
     //陀螺仪校准进度记录
@@ -80,24 +83,25 @@ public class AimHandler : MonoBehaviour
             SetIdentityButton.onClick.AddListener(DoIdentity);
         }
         
-        try 
-        {  
-            string magDataStr = PlayerPrefs.GetString("o0MagneticCalibrater");
-            MagCalibrater = JsonConvert.DeserializeObject<o0MagneticCalibraterEllipsoidFitting>(magDataStr);
-        } 
-        catch(Exception) 
-        {
-            MagCalibrater = null;
-        }
+        // try 
+        // {  
+        //     string magDataStr = PlayerPrefs.GetString("o0MagneticCalibrater");
+        //     MagCalibrater = JsonConvert.DeserializeObject<o0MagneticCalibraterEllipsoidFitting>(magDataStr);
+        // } 
+        // catch(Exception) 
+        // {
+        //     MagCalibrater = null;
+        // }
 
         if (MagCalibrater == null) 
         {
-            MagCalibrater = new o0MagneticCalibraterEllipsoidFitting();
+            // MagCalibrater = new o0MagneticCalibraterEllipsoidFitting();
+            MagCalibrater = new MagnetometerAutoCalibrater();
         }
         if (MagCalibrationButton)
         {
             MagCalibrationButton.onClick.AddListener(delegate {
-                CalibrateMag(!MagCalibrater.Calibration);
+                // CalibrateMag(!MagCalibrater.Calibration);
             });
         }
 
@@ -137,93 +141,93 @@ public class AimHandler : MonoBehaviour
 
     [NonSerialized] public bool isCalibrateMagPerfect = false;
      public void CalibrateMag(bool calibration) {
-        try {
-            if (calibration)
-            {
-                PointCorrector.ins = new PointCorrector(this.ellipseScript, this.MagCalibrater);
-                MagCalibrater.Calibration = calibration;
-                MagCalibrationButton.GetComponentInChildren<Text>().text = "停止地磁计校准";
-                this.cMaxVector = new Vector3(float.MinValue, float.MinValue, float.MinValue);
-                this.cMinVector = new Vector3(float.MaxValue, float.MaxValue, float.MaxValue);
-                this.ellipseScript.ellipseTran.gameObject.SetActive(false);
-            }
-            else
-            {
-                PointCorrector.ins = null;
-                List<Vector3> list = MagCalibrater.getRecords();
-
-                //停止校准时候，看看数组值
-                float maxDistance = 0f,ratio = 1f;
-                Vector3 maxVector3 = new Vector3(0,0,0);
-                List<Vector3> endRecords = new List<Vector3>();
-                foreach (Vector3 i in list)
-                {
-                    Vector3 v = i - MagCalibrater._Center;
-                    if (Math.Abs(v.magnitude) > maxDistance)
-                    {
-                        maxVector3 = v;
-                        maxDistance = Math.Abs(v.magnitude);
-                        if(Math.Abs(v.magnitude) < Math.Abs(MagCalibrater._Radius.magnitude))
-                            ratio = Math.Abs(v.magnitude) / Math.Abs(MagCalibrater._Radius.magnitude);
-                        else
-                            ratio = Math.Abs(MagCalibrater._Radius.magnitude) / Math.Abs(v.magnitude);
-                    }
-                }
-                Debug.LogWarning(maxDistance + " == " + Math.Abs(MagCalibrater._Radius.magnitude) + " == " + MagCalibrater._Radius + " = " + maxVector3);
-                //如果比例效果不理想。可以设置为ratio=0.5f 
-                foreach (Vector3 i in list)
-                {
-                    //- MagCalibrater._Center
-                    Vector3 v = i ;
-                    v *= ratio;
-                    if(endRecords.Count>3000)
-                    {
-                        endRecords.RemoveAt(0);
-                    }
-                    endRecords.Add(v);
-                }
-                this.ellipseScript.ClearAndUpdatePointArray();
-                // this.ellipseScript.DrawPointCloud(endRecords);
-
-                MagCalibrater.Calibration = calibration;
-
-                this.ellipseScript.ellipseTran.gameObject.SetActive(true);
-                //绘制椭圆形
-                if (MagCalibrater._Radius != this.ellipseScript.ellipseTran.localScale)
-                {
-                    this.ellipseScript.setEllipseLocalScaleAndCenter(MagCalibrater._Radius, MagCalibrater._Center * ratio);
-                    //设置绘制图像相机的对应位置
-                    this.ellipseScript.setCameraPos(MagCalibrater._Center * ratio);
-                    this.ellipseScript.setCameraSize(MagCalibrater._Radius);
-                }
-
-                MagCalibrationButton.GetComponentInChildren<Text>().text = "开始地磁计校准";
-                PlayerPrefs.SetString("o0MagneticCalibrater", JsonConvert.SerializeObject(MagCalibrater));
-
-                #region 看校准是否完美，求Records方差
-                if (list == null || list.Count == 0) isCalibrateMagPerfect = false;
-                else
-                {
-                    double sumV = 0;
-                    double[] listArray = new double[list.Count];
-                    for (int i = 0; i < list.Count; i++)
-                    {
-                        listArray[i] = MagCalibrater.Update(list[i]).magnitude;
-                        sumV += listArray[i];
-                    }
-                    double avarageV = sumV / listArray.Length;
-                    double varianceV = 0;
-                    foreach (double vvv in listArray)
-                    {
-                        varianceV += Math.Pow(vvv - avarageV, 2);
-                    }
-                    varianceV /= listArray.Length;
-                    isCalibrateMagPerfect = varianceV < 0.001;
-                    PopupMgr.ins.ShowTip(TextAutoLanguage2.GetTextByKey("tip_mag-calibrate-variance-equal") + varianceV);
-                }
-                #endregion
-            }
-        } catch (Exception e) { Debug.LogError(e.Message); }
+        // try {
+        //     if (calibration)
+        //     {
+        //         PointCorrector.ins = new PointCorrector(this.ellipseScript, this.MagCalibrater);
+        //         MagCalibrater.Calibration = calibration;
+        //         MagCalibrationButton.GetComponentInChildren<Text>().text = "停止地磁计校准";
+        //         this.cMaxVector = new Vector3(float.MinValue, float.MinValue, float.MinValue);
+        //         this.cMinVector = new Vector3(float.MaxValue, float.MaxValue, float.MaxValue);
+        //         this.ellipseScript.ellipseTran.gameObject.SetActive(false);
+        //     }
+        //     else
+        //     {
+        //         PointCorrector.ins = null;
+        //         List<Vector3> list = MagCalibrater.getRecords();
+
+        //         //停止校准时候，看看数组值
+        //         float maxDistance = 0f,ratio = 1f;
+        //         Vector3 maxVector3 = new Vector3(0,0,0);
+        //         List<Vector3> endRecords = new List<Vector3>();
+        //         foreach (Vector3 i in list)
+        //         {
+        //             Vector3 v = i - MagCalibrater._Center;
+        //             if (Math.Abs(v.magnitude) > maxDistance)
+        //             {
+        //                 maxVector3 = v;
+        //                 maxDistance = Math.Abs(v.magnitude);
+        //                 if(Math.Abs(v.magnitude) < Math.Abs(MagCalibrater._Radius.magnitude))
+        //                     ratio = Math.Abs(v.magnitude) / Math.Abs(MagCalibrater._Radius.magnitude);
+        //                 else
+        //                     ratio = Math.Abs(MagCalibrater._Radius.magnitude) / Math.Abs(v.magnitude);
+        //             }
+        //         }
+        //         Debug.LogWarning(maxDistance + " == " + Math.Abs(MagCalibrater._Radius.magnitude) + " == " + MagCalibrater._Radius + " = " + maxVector3);
+        //         //如果比例效果不理想。可以设置为ratio=0.5f 
+        //         foreach (Vector3 i in list)
+        //         {
+        //             //- MagCalibrater._Center
+        //             Vector3 v = i ;
+        //             v *= ratio;
+        //             if(endRecords.Count>3000)
+        //             {
+        //                 endRecords.RemoveAt(0);
+        //             }
+        //             endRecords.Add(v);
+        //         }
+        //         this.ellipseScript.ClearAndUpdatePointArray();
+        //         // this.ellipseScript.DrawPointCloud(endRecords);
+
+        //         MagCalibrater.Calibration = calibration;
+
+        //         this.ellipseScript.ellipseTran.gameObject.SetActive(true);
+        //         //绘制椭圆形
+        //         if (MagCalibrater._Radius != this.ellipseScript.ellipseTran.localScale)
+        //         {
+        //             this.ellipseScript.setEllipseLocalScaleAndCenter(MagCalibrater._Radius, MagCalibrater._Center * ratio);
+        //             //设置绘制图像相机的对应位置
+        //             this.ellipseScript.setCameraPos(MagCalibrater._Center * ratio);
+        //             this.ellipseScript.setCameraSize(MagCalibrater._Radius);
+        //         }
+
+        //         MagCalibrationButton.GetComponentInChildren<Text>().text = "开始地磁计校准";
+        //         PlayerPrefs.SetString("o0MagneticCalibrater", JsonConvert.SerializeObject(MagCalibrater));
+
+        //         #region 看校准是否完美，求Records方差
+        //         if (list == null || list.Count == 0) isCalibrateMagPerfect = false;
+        //         else
+        //         {
+        //             double sumV = 0;
+        //             double[] listArray = new double[list.Count];
+        //             for (int i = 0; i < list.Count; i++)
+        //             {
+        //                 listArray[i] = MagCalibrater.Update(list[i]).magnitude;
+        //                 sumV += listArray[i];
+        //             }
+        //             double avarageV = sumV / listArray.Length;
+        //             double varianceV = 0;
+        //             foreach (double vvv in listArray)
+        //             {
+        //                 varianceV += Math.Pow(vvv - avarageV, 2);
+        //             }
+        //             varianceV /= listArray.Length;
+        //             isCalibrateMagPerfect = varianceV < 0.001;
+        //             PopupMgr.ins.ShowTip(TextAutoLanguage2.GetTextByKey("tip_mag-calibrate-variance-equal") + varianceV);
+        //         }
+        //         #endregion
+        //     }
+        // } catch (Exception e) { Debug.LogError(e.Message); }
     }
 
     //转换读取的数据，无符号->有符号
@@ -313,6 +317,14 @@ public class AimHandler : MonoBehaviour
             Acc = new Vector3(-az, ay, -ax) / 32768 * 16;
             Gyr = new Vector3(yaw, -pitch, roll) / 32768 * 2;
             Mag = new Vector3(-z, y, x) / 32768 * 256; //旧版
+            mag0o = UnityVectorTo0o(Mag);
+            if (!MagCalibrater.Update(mag0o)) {
+                isCalibrateMagPerfect = false;
+                return;
+            }
+            isCalibrateMagPerfect = true;
+            mag0o = MagCalibrater.EllipsoidFitting.Map(mag0o);
+            Mag = o0VectorToUnity(mag0o);
         } else if (CommonConfig.devicePlan == 1) {
             Acc = new Vector3(ax, ay, az) / 32768 * 16;
             Gyr = new Vector3(roll, pitch, yaw) / 32768 * 2;
@@ -338,36 +350,36 @@ public class AimHandler : MonoBehaviour
             GyrScaleText.text = "" + (_9Axis.getGyrOld() * 1000000).ToString();
         }
 
-        if(Mag.x > -128 && Mag.y > -128 && Mag.z > -128 && Mag.x < 128 && Mag.y < 128 && Mag.z < 128)
-        {   
-            //绘制地磁计点
-            if (MagCalibrater.Calibration)
-            {
-                this.ellipseScript.AddAndUpdatePointArray(Mag);
-
-                if (Mag.x > cMaxVector.x) cMaxVector.x = Mag.x;
-                if (Mag.y > cMaxVector.y) cMaxVector.y = Mag.y;
-                if (Mag.z > cMaxVector.z) cMaxVector.z = Mag.z;
-                if (Mag.x < cMinVector.x) cMinVector.x = Mag.x;
-                if (Mag.y < cMinVector.y) cMinVector.y = Mag.y;
-                if (Mag.z < cMinVector.z) cMinVector.z = Mag.z;
-
-                Vector3 centerPoint = (this.cMaxVector + this.cMinVector) / 2;
-
-                //设置绘制图像相机的对应位置
-                this.ellipseScript.setCameraPos(centerPoint);
-                this.ellipseScript.setCameraSize(this.cMaxVector - this.cMinVector);
-
-                PointCorrector.ins?.Update(centerPoint);
-            }
-            Mag = MagCalibrater.Update(Mag);
-
-            if (MagScaleText)
-            {
-                MagScaleText.text = MagCalibrater._Radius.ToString();
-            }
+        // if(Mag.x > -128 && Mag.y > -128 && Mag.z > -128 && Mag.x < 128 && Mag.y < 128 && Mag.z < 128)
+        // {   
+        //     //绘制地磁计点
+        //     if (MagCalibrater.Calibration)
+        //     {
+        //         this.ellipseScript.AddAndUpdatePointArray(Mag);
+
+        //         if (Mag.x > cMaxVector.x) cMaxVector.x = Mag.x;
+        //         if (Mag.y > cMaxVector.y) cMaxVector.y = Mag.y;
+        //         if (Mag.z > cMaxVector.z) cMaxVector.z = Mag.z;
+        //         if (Mag.x < cMinVector.x) cMinVector.x = Mag.x;
+        //         if (Mag.y < cMinVector.y) cMinVector.y = Mag.y;
+        //         if (Mag.z < cMinVector.z) cMinVector.z = Mag.z;
+
+        //         Vector3 centerPoint = (this.cMaxVector + this.cMinVector) / 2;
+
+        //         //设置绘制图像相机的对应位置
+        //         this.ellipseScript.setCameraPos(centerPoint);
+        //         this.ellipseScript.setCameraSize(this.cMaxVector - this.cMinVector);
+
+        //         PointCorrector.ins?.Update(centerPoint);
+        //     }
+        //     Mag = MagCalibrater.Update(Mag);
+
+        //     if (MagScaleText)
+        //     {
+        //         MagScaleText.text = MagCalibrater._Radius.ToString();
+        //     }
            
-        }
+        // }
         MagObj.transform.GetChild(0).localPosition = Mag;
 
         var ms = (((long)bytes[1]) *60 + bytes[2])*1000 + (long)TwoByteToFloat(bytes[3], bytes[4]);
@@ -382,7 +394,6 @@ public class AimHandler : MonoBehaviour
         GapMs = TimeGap;
         gyr0o = UnityVectorTo0o(Gyr);
         acc0o = UnityVectorTo0o(Acc);
-        mag0o = UnityVectorTo0o(Mag);
         
         distanceToAxis.Update(gyr0o, acc0o, mag0o, GapMs);
         acc0o = distanceToAxis.AccCorrection(gyr0o, acc0o, GapMs);/**///轴心偏离矫正

Assets/Plugins/o0Lib/o0MagnetometerCalibrater.dll.meta

@@ -0,0 +1,33 @@
+fileFormatVersion: 2
+guid: 0969ff03e522b4b4f99563701bef1b38
+PluginImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  iconMap: {}
+  executionOrder: {}
+  defineConstraints: []
+  isPreloaded: 0
+  isOverridable: 0
+  isExplicitlyReferenced: 0
+  validateReferences: 1
+  platformData:
+  - first:
+      Any: 
+    second:
+      enabled: 1
+      settings: {}
+  - first:
+      Editor: Editor
+    second:
+      enabled: 0
+      settings:
+        DefaultValueInitialized: true
+  - first:
+      Windows Store Apps: WindowsStoreApps
+    second:
+      enabled: 0
+      settings:
+        CPU: AnyCPU
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: