九轴设备方向算法

Assets/BowArrow/Scripts/Bluetooth/AimHandler.cs

@@ -243,25 +243,38 @@ public class AimHandler : MonoBehaviour
             return;
         if (bytes[19] == 0 && bytes[20] == 0 && bytes[21] == 0 && bytes[22] == 0 && bytes[23] == 0 && bytes[24] == 0)
             return;
-        
-        float ax = -TwoByteToFloat(bytes[7], bytes[8]);
-        float ay = -TwoByteToFloat(bytes[9], bytes[10]);
-        float az = -TwoByteToFloat(bytes[11], bytes[12]);
-        ax = ax / 32768 * 16;
-        ay = ay / 32768 * 16;
-        az = az / 32768 * 16;
-        Acc = new Vector3(ay, ax, az);
-        AccObj.transform.GetChild(0).localPosition = Acc;
 
+        float ax = TwoByteToFloat(bytes[7], bytes[8]);
+        float ay = TwoByteToFloat(bytes[9], bytes[10]);
+        float az = TwoByteToFloat(bytes[11], bytes[12]);
 
         float roll = TwoByteToFloat(bytes[13], bytes[14]);
         float pitch = TwoByteToFloat(bytes[15], bytes[16]);
         float yaw = TwoByteToFloat(bytes[17], bytes[18]);
-        roll = -roll / 32768 * 2000;
-        pitch = -pitch / 32768 * 2000;
-        yaw = -yaw / 32768 * 2000;
-        Gyr = new Vector3(pitch, roll, yaw) / 1000;
+
+        float x = TwoByteToFloat(bytes[19], bytes[20]);
+        float y = TwoByteToFloat(bytes[21], bytes[22]);
+        float z = TwoByteToFloat(bytes[23], bytes[24]);
+
+        float mxr = TwoByteToFloat(bytes[20], bytes[19]);
+        float myr = TwoByteToFloat(bytes[22], bytes[21]);
+        float mzr = TwoByteToFloat(bytes[24], bytes[23]);
+
+        Acc = new Vector3(-az, ay, -ax) / 32768 * 16;
+        Gyr = new Vector3(yaw, -pitch, roll) / 32768 * 2;
+        Mag = new Vector3(-z, y, x) / 32768 * 256; //旧版
+        //Acc = new Vector3(ax, ay, az) / 32768 * 16;
+        //Gyr = new Vector3(roll, pitch, yaw) / 32768 * 2;
+        //Mag = new Vector3(z, x, -y) / 32768 * 256;//第一个6+3硬件
+        //Acc = new Vector3(-az, ax, -ay) / 32768 * 16;
+        //Gyr = new Vector3(-yaw, roll, -pitch) / 32768 * 2;
+        //Mag = new Vector3(mzr, mxr, -myr) / 32768 * 256;//第二个6+3硬件
+
+
+        AccObj.transform.GetChild(0).localPosition = Acc;
+
         Gyr = GyrCalibrater.Update(Gyr);
+
         if (GyrCalibrater.Calibration) 
         {
             if (gyrCalibrateCompleteCount < gyrCalibrateTotalCount) {
@@ -270,18 +283,9 @@ public class AimHandler : MonoBehaviour
         }
         if (GyrScaleText && GyrCalibrater.Calibration) 
         {    
-            // GyrScaleText.text = GyrCalibrater._Average.x + "\n" + GyrCalibrater._Average.y + "\n" + GyrCalibrater._Average.z;
-            // GyrScaleText.text = "Gyr*1000,000:" + (_9Axis.GyrOld * 1000000).ToString();
             GyrScaleText.text = "" + (_9Axis.getGyrOld() * 1000000).ToString();
         }
 
-        float x = TwoByteToFloat(bytes[19], bytes[20]);
-        float y = TwoByteToFloat(bytes[21], bytes[22]);
-        float z = TwoByteToFloat(bytes[23], bytes[24]);
-        var mag = new Vector3(-y, x, -z);
-        Mag = mag / 32768 * 256;
-    
-
         if(Mag.x > -128 && Mag.y > -128 && Mag.z > -128 && Mag.x < 128 && Mag.y < 128 && Mag.z < 128)
         {   
             //绘制地磁计点
@@ -328,17 +332,7 @@ public class AimHandler : MonoBehaviour
         if (SB_EventSystem.ins && SB_EventSystem.ins.simulateMouseIsAwaked) {
             SB_EventSystem.ins.MoveSimulateMouse(newRotation);
         }
-        // 记录一些旋转角---start
-        // if (ArmBow.ins) {
-        //     for (int i = ArmBow.ins.recordRotations.Length - 1; i > 0 ; i--)
-        //     {
-        //         ArmBow.ins.recordRotations[i] = ArmBow.ins.recordRotations[i - 1];
-        //     }
-        //     ArmBow.ins.recordRotations[0] = newRotation;
-        //     ArmBow.ins.recordCount++;
-        // }
-        // 记录一些旋转角---end
-    }   
+    }
 
     [NonSerialized] public bool lerpForRotation = true;
     [NonSerialized] public float lerpTimeRate = 7;