1

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.

Kostas Letros
  • 53
  • 1
  • 11
  • Could you add your Unity code? Also maybe some pictures of expected vs actual result would be helpful here – derHugo Apr 30 '21 at 14:35
  • I edited my question to add more details. – Kostas Letros Apr 30 '21 at 20:15
  • Well the issue sounds actually (at least partially) to be on the sender side .. if that gyroscope just uses the orientation the moment it is turned on as the identity (zero) rotation .. then it sounds like there is not much you can do about that. I don't know the library you are using there but in general the way you apply the rotation in Untiy looks odd to me. I know you sometimes switch out and invert axis values and invert `w` (e.g. when going from right to left handed coordinate system) but I have never heard of putting the `w` component somewhere else tbh ... – derHugo Apr 30 '21 at 20:43
  • Thanks for replying. Yeah it seems odd to me too. But for some reason sometimes it works. So when the initial orientation is proper the gun is rotated properly around each of the axes. I am also using an Oculus Rift S to play the game and I was wondering how can it be that the oculus ovr controller never looses orientation even after it's off. Furthermore, I have found an [algorithm]( https://github.com/kriswiner/MPU9250/blob/master/quaternionFilters.ino) that seems promising to me but I haven't tested it yet. What do you think – Kostas Letros May 01 '21 at 04:52
  • As said I think this is hardware specific .. the oculus controllers are tracked by the trackers and the oculus so the system all the time knows their relative orientations between each other (see [Oculus Controller Tracking](https://developer.oculus.com/documentation/native/pc/dg-input-touch-poses))... your gun though doesn't know any other reference system than its own – derHugo May 01 '21 at 08:34
  • So what do I need to buy instead, to make it work? – Kostas Letros May 01 '21 at 09:24

0 Answers0