ESP32 和 MPU 四元数值不同 - Unity3d 显示不同的对象初始方向

如何解决ESP32 和 MPU 四元数值不同 - Unity3d 显示不同的对象初始方向

我正在尝试统一构建 3D FPS 游戏。我的一个朋友买了一把（复制品）枪，并对其进行了修改，在其中添加了一个 ESP32 和一个 MPU-9250 陀螺仪/加速度计，以跟踪和发送枪的旋转（使用四元数）统一。问题是每次我“打开”枪并开始游戏时，游戏中武器的初始旋转是不同的。 （由于万向节锁定问题，我不想使用欧拉角。）有什么想法可能出在哪里以及如何解决？

我目前正在使用 MPU-9250 DMP Arduino 库，如 here。

我开始认为问题在于每次打开枪的电源时，陀螺仪轴都会重新初始化。根据 another library - 校准 - 如果 MPU 断电，则加速度/陀螺仪/磁力偏移不会存储到注册中。您需要在每次启动时自行设置所有偏移（或在每次启动时进行校准）。我不想每次重新启动 ESP32 时都这样做。如果ESP32重新启动时，无论枪在什么位置，我都可以使用固定坐标系。

这是我目前编写的代码：

#include "src\lib\SparkFunMPU9250-DMP.h"
#include <WiFi.h>
#include <WiFiUdp.h>

// -------------------------------------------------------------------------------------------

// Enter: WEAPON_* OR Handgun_*
const String gun = "Handgun_4";

char * udpAddress;
int udpPort;
int Trigger;
int Mag;
int Shutter;

// -------------------------------------------------------------------------------------------

// --- Trigger ---
//const int Trigger = 4;
int button_state_trigger = 0;
int button_state_mag = 0;
int button_state_shutter = 0;

// --- MPU9250 ---
MPU9250_DMP imu;



// --- WiFi ---
WiFiUDP udp;
// WIFI credentials
const char * networkName = "...";
const char * networkPswd = "...";
// IP address to send UDP data to:
// either use the ip address of the server or
// a network broadcast address
//const char * udpAddress = ...;
// UDP port
//const int udpPort = ...;
// payload to sent
String payload = "";
//Connection state?
boolean connected = false;

// Timers
unsigned long timer1,timer2;

// -------------------------------------------------------------------------------------------

void setup() {

  GunSelect(gun);

  // Initialize Serial Com
  Serial.begin(115200);

  // Initialize Trigger
  pinMode(Trigger,INPUT);
  pinMode(Mag,INPUT);
  pinMode(Shutter,INPUT);
 
  // Initialize MPU9250
  while (imu.begin() != INV_SUCCESS)  {
    Serial.println("Unable to communicate with MPU-9250");
    delay(2000);
  }
  imu.setSensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS);   // Enable all sensors
  imu.setGyroFSR(2000);           // Set Gyro dps,options are +/- 250,500,1000,or 2000 dps
  imu.setAccelFSR(16);            // Set Accel g,options are +/- 2,4,8,or 16 g
  imu.setSampleRate(100);        // Set sample rate of Accel/Gyro,range between 4Hz-1kHz
  imu.setLPF(5);                  // Set LPF corner frequency of Accel/Gyro,acceptable cvalues are 188,98,42,20,10,5 (Hz)
  imu.setCompassSampleRate(100);  // Set Mag rate,range between: 1-100Hz
  imu.dmpBegin(DMP_FEATURE_6X_LP_QUAT |   // 6-axis (accel/gyro) quaternion calculation
               DMP_FEATURE_GYRO_CAL,// Gyroscope calibration (0's out after 8 seconds of no motion)
               200);
  delay(10000);
  // Initialize WiFi
  connectToWiFi(networkName,networkPswd);
  
}
// -------------------------------------------------------------------------------------------

void loop() {
  // Timer Start
  // timer1 = micros();

  if ( imu.fifoAvailable() ) {
    if ( imu.dmpUpdateFifo() == INV_SUCCESS ) {
      // Switches Read
      button_state_trigger = buttonState(Trigger);
      if (gun == "WEAPON_1" || gun == "WEAPON_2" || gun == "WEAPON_3" || gun == "WEAPON_4" || gun == "WEAPON_5" || gun == "WEAPON_Test") {
        button_state_mag = buttonState(Mag);
        button_state_shutter = buttonState(Shutter);
      }
      
      double x   = imu.calcQuat(imu.qx);
      double y  = imu.calcQuat(imu.qy);
      double z    = imu.calcQuat(imu.qz);
      double w    = imu.calcQuat(imu.qw);

      
        Serial.println();
        Serial.println(button_state_trigger);
        Serial.println(button_state_mag);
        Serial.println(button_state_shutter);
        Serial.println("x: " + String(x));
        Serial.println("y: " + String(y));
        Serial.println("z: " + String(z));
        Serial.println("w: " + String(w));
        
        Serial.println();
      //*/

      // Send data via WiFi
     payload = "ACC|" + String(x,4) + "|" + String(y,4) + "|" + String(z,4) + "|" + String(w,4) + "|" + String(button_state_trigger) + "|" + String(button_state_mag) + "|" + String(button_state_shutter);
      sendData(payload);
    }
  }

  // Timer End
  // timer2 = micros();
  // Serial.println(timer2 - timer1);
}

在 Unity 中，我接收数据并将其解析为 4 个浮点数。然后我将游戏中枪的旋转设置为(y,w,-x,z)，因为枪的坐标系与Unity使用的坐标系不同。所以代码是这样的：

// Receiving data...


float x = float.Parse(data[1]);
float y = float.Parse(data[2]);
float z = float.Parse(data[3]);
float w = float.Parse(data[4]);

gun.rotation = new Quaternion(y,z);

----------------编辑--------------------

我读到了关于 Madgwick 滤波器（或者卡尔曼滤波器和 Mahony 滤波器），据说它能够产生基于四元数的绝对设备方向估计（基于地球的参考系统，即北等）如果我理解正确的话.但我不确定这是否能解决我的问题。