ESP32 and MPU quaternion values are different - Unity3d shows different initial orentation of object

623 Views Asked by At

I am trying to build a 3D FPS game in unity. A friend of mine bought a (replica) gun and modified it to add an ESP32 and an MPU-9250 gyroscope/accelerometer in it to track and send the rotation of the gun (using quaternions) to unity. The problem is that each time I "power on" the gun and start the game the initial rotation of the weapon in the game is different. (I don't want to use euler angles because of the gimbal lock problem.) Any ideas where the problem might be and how to fix it?

I am currently using the MPU-9250 DMP Arduino Library as in here.

I have started thinking that the problem lies on the fact that each time I turn on the power of the gun, the gyroscope axes are reinitialized. According to another library - Calibration - accel/gyro/mag offsets are NOT stored to register if the MPU has powered off. You need to set all offsets at every bootup by yourself (or calibrate at every bootup). I do not want to do that every time the ESP32 is restarted. If only I could use a fixed coordinate system no matter what the position of the gun is, when the ESP32 reboots.

Here is the code I have written so far:

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

In Unity I receive the data and parse it as 4 floats. Then I set the gun's rotation in the game as (y,w,-x,z) because the coordinate system of the gun is different from the one that Unity uses. So the code is like:

// 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,w,-x,z);

----------------Edit--------------------

I read about Madgwick filter (alternatively Kalman filter and Mahony filter) which is said to be able to produce a quaternion-based estimate of absolute device orientation (based on the earth's reference system ie. North etc.) If I understood correctly . But I am not really sure if that solves my problem.

0

There are 0 best solutions below