Sunday, June 10, 2018

Inertial Head Tracker

Summary

I'm a long time fan of flight simulators on PC. I'm looking forward to the day when VR headsets are able to provide a high-quality, immersive experience. However, VR is expensive and tough on computing power. In the past, I used an open source infrared headtracking system called FreeTrack. This provided so-so performance compared to the 1st party solution TrackIR, but at a fraction of the cost. Unfortunately, the FreeTrack setup had a lot of quality-of-life issues that really negated the benefits it brought to simulation. I propose a better way.

To solve the shortcomings of IR solutions and maintain low costs, I developed an inertial head tracker using easily available open source hardware and software. For a cost of approximately $40 USD, I created a highly accurate 3 degree of freedom (pitch, roll, yaw) inertial head tracker. Typical inertial motion solutions are plagued by accelerometer and gyro drift. Luckily, the Freescale FXOS8700CQ and FXAS21002C coupled with the "Mahony algorithm" developed by Robert Mahony et al. and library implementation by Paul Stoffregen have entirely solved this issue.

The result is a high-quality motion tracker with very smooth and predictable behavior at a modest cost. My immense thanks to all of the open source contributions that enabled this project--it was truly a slam dunk weekend project and I can't overstate how impressed I am with the result.

OpenTrack can also easily map head movements to mouse movements, which might make this project great for helping people with physical disabilities operate a computer. 



Implementation

Hardware

Software

Tools

  • Soldering iron, solder, wire/header

Instructions

  1. Solder the Teensy prop shield directly to the Teensy 3.2. Headers work great for this, since you can stack the two boards to make a small package without risking accidental shorts. I recommend soldering the prop shield under the Teensy controller, so you can access the reset button on the Teensy.
  2. Mount the Teensy stack to a hat, headset, or similar piece of headgear. It's important to complete the mechanical mounting of the Teensy stack before magnetometer calibration, since any nearby metals, magnets, or cables may impact the calibration.
  3. Install the required software and libraries.
  4. Connect the Teensy stack to your PC via USB and use the Arduino IDE to flash the "CalibrateSensors" example from the NXPMotionSense library. Ensure the board is set to "Teensy 3.2/3.1".
  5. Run MotionCal, and select the COM port for the Teensy in the drop down menu. Rotate the Teensy and headwear in complete circles until the gaps, variance, wobble, and fit error are low (5% or lower is a good goal). The calibration GUI should show a sphere or ellipsoid, and the "Send Cal" button will no longer be grayed out when the calibration is acceptable. Click "Send Cal" to save the calibration to EEPROM. It is critical that calibration be completed with the final mechanical setup. If using a headset, you may need to wear it and/or hold the speaker transducers apart as if you were hearing it to create a proper calibration.
  6. Flash the "MahonyIMU" example from the NXPMotionSense library. Change the USB type from "Serial" to "Flight Sim Controls + Joystick".
  7. Put your headset on and open the Arduino Serial Monitor. You will see the current values of heading, pitch, and roll in order. The values shown will vary with each setup depending on the mounting orientation of your Teensy and the direction your computer desk is facing. Note the heading value when you are looking straight ahead after about 1 minute has elapsed. The 1 minute wait is necessary for the algorithm to reduce the error with the magnetometer.
  8. Copy the below code to replace MahonyIMU example. Put the heading value in line 13 for the "headingcenter" variable. I coded "wraparound" handling if your center heading and range overlap 0. There is no such protection or calculation included for pitch and roll, which may be necessary if you mount your PCB upside down.
  9. Save the updated code to a safe place and flash the Teensy. It is now functioning as a USB joystick. You can view the output in the Arduino Serial Monitor for debugging if necessary, or launch the Windows "Set up USB Game Controllers" application to see windows receiving the data.
  10. Launch OpenTrack and adjust the input to use the Teensy. Depending on your luck, you may need to invert axes to match your movement. You can also adjust sensitivity, smoothing, and define curves in OpenTrack. For the output settings, set the interface option as "Use TrackIR, hide FreeTrack" to be compatible with most TrackIR compatible applications. I highly recommend setting a bind key for "Center" under "Options" to have the software adjust the resting center. This is to account for small differences in your positioning as every time you use the head tracker, you will have a slightly different resting position.

IMU HeadTracker Code

// Inertial Monitoring Unit (IMU) using Mahony filter.
//
// To view this data, use the Arduino Serial Monitor to watch the
// scrolling angles, or run the OrientationVisualiser example in Processing.

#include <NXPMotionSense.h>
#include <MahonyAHRS.h>
#include <Wire.h>
#include <EEPROM.h>

NXPMotionSense imu;
Mahony filter;
const int headingcenter = 0; // ADJUST THIS TO YOUR "RESTING" HEADING. MUST BE POSITIVE VALUE BETWEEN 0 AND 359.
const int headingrange = 70; // APPROXIMATE RANGE OF MOTION IN DEGREES +/- OF HEADING CENTER. MOVING OUTSIDE THIS CENTERS THE VIEW FOR RECOVERY.
int headingminimum;
int headingmaximum;
int joyheading;
int joypitch;
int joyroll;

void setup() {
  Serial.begin(9600);
  imu.begin();
  filter.begin(100); // 100 measurements per second
  if((headingcenter-headingrange)<0 bound="" calculate="" degrees="" else="" heading="" headingcenter="" headingminimum="headingcenter-headingrange;" headingrange="" if="" in="" lower="">360) // Calculate heading upper bound in degrees
  {
    headingmaximum=headingcenter+headingrange-360;
  }
  else
  {
    headingmaximum=headingcenter+headingrange;
  }
}

void loop() {
  float ax, ay, az;
  float gx, gy, gz;
  float mx, my, mz;
  float roll, pitch, heading;

  if (imu.available()) {
    // Read the motion sensors
    imu.readMotionSensor(ax, ay, az, gx, gy, gz, mx, my, mz);

    // Update the Mahony filter
    filter.update(gx, gy, gz, ax, ay, az, mx, my, mz);

    // print the heading, pitch and roll
    roll = filter.getRoll();
    pitch = filter.getPitch();
    heading = filter.getYaw();
    Serial.print("Orientation: ");
    Serial.print(heading);
    Serial.print(" ");
    Serial.print(pitch);
    Serial.print(" ");
    Serial.println(roll);


// Heading calculation code. Includes zero-wraparound handling if heading range overlaps 0/360 boundary.
    if(headingminimum>headingmaximum)
    {
      if((heading>headingmaximum)&&(heading(headingmaximum)))
    {
      joyheading = 512;
    }
    else
    {
      joyheading = (heading-(headingminimum))/((headingmaximum)-(headingminimum))*(1024-0)+0;
    }

    if(pitch==0)
    {
      joypitch = 512;
    }
    else
    {
      joypitch = (pitch-(-180))/(180-(-180))*(1024-0)+0;
    }
    
    if(roll==0)
    {
      joyroll = 512;
    }
    else
    {
      joyroll = (roll-(-180))/(180-(-180))*(1024-0)+0;
    }    
    Joystick.X(joypitch);
    Joystick.Y(joyroll);
    Joystick.Z(joyheading);

    Serial.print("Joyvalue: ");
    Serial.print(joyheading);
    Serial.print(" ");
    Serial.print(joypitch);
    Serial.print(" ");
    Serial.println(joyroll);
  }
}