Tuesday, March 4, 2014

The "Foculus Rift" part 2: Reverse engineering the motion tracker

The function of the motion tracker is to estimate the current roll, pitch and yaw angles of the head mounted display (HMD), which allows the 3D game to know what you are looking at and render the scene accordingly.

The original Rift uses the following configuration (as nicely shown by ifixIt):
  • Invensense MPU-6000, gyroscope and accelerometer sensor
  • Honeywell HMC5983, magnetic field sensor / compass
  • ST  32F103C8, ARM 32 bit microcontroller (with USB connection)



The Oculus Rift Tracker board (Picture from oculusvr.com)

The ARM reads out the three sensors over an SPI and I2C interface and sends the data to the host PC over USB. The host runs the Rift SDK, which performs the sensor fusion and estimates the head position. New data is sent at a rate of ~ 1 kHz to minimize latency.

Now the interesting bit: the company ST is aggressively marketing their STM32 microcontrollers. You can get the STM32F3DISCOVERY board for as little as 15 Euro. This is an evaluation board, containing the three types of sensors mentioned above and comes with a programmable USB connection. It is an excellent hardware platform to emulate the Rift tracker.

The STM32F3 discovery kit. (Picture from ST.com)
The masterplan is the following:
  1. Reverse engineer the Rift USB communication protocol. Understand which messages are exchanged and how the raw sensor values are encoded.
  2. Write a firmware for the STM board to appear just like a Rift to the Host PC. This includes setting the USB descriptors (including Vendor ID, Product ID and the String descriptors) to identical values.
  3. Write a firmware which periodically reads out the sensors and sends the data to the host PC in Rift compatible format.

Reverse Engineering

Much information can be found on the web. For example the Vendor and Product ID can be found in many logfiles posted by Rift users in various forums. Google is your friend.

Furthermore an USB packet sniffer (such as Wireshark) can be used to get an idea of the USB traffic and what kind of binary data is sent. This is an important debugging tool.

Perhaps one of the most insightful options is to have a close look at the source code of the Rift SDK. A lot of interesting stuff can be found in the file: OVR_SensorImpl.cpp
I found it extremely useful to modify the SDK code slightly to print me debug messages, showing what is sent over USB. Specifically I modified OVR_Linux_HIDDevice.cpp on line 630:


bool HIDDevice::SetFeatureReport(UByte* data, UInt32 length){
    if (DeviceHandle < 0)
        return false;
    UByte reportID = data[0];
    if (reportID == 0) {
        // Not using reports so remove from data packet.
        data++;
        length--;
    }
    LogText("HACK::SetFeature, length = %d, data[0-end] = ", length);
    int temp;
    for (temp=0; temp < length; temp++){
        LogText("%02x ", data[temp]);
    }
    LogText("\n");
    int r = ioctl(DeviceHandle, HIDIOCSFEATURE(length), data);
 return (r >= 0);
}

bool HIDDevice::GetFeatureReport(UByte* data, UInt32 length) {
    if (DeviceHandle < 0)
        return false;
    int r = ioctl(DeviceHandle, HIDIOCGFEATURE(length), data);
    LogText("HACK::GetFeature, length = %d, data[0-end] = ", length);
    int temp;
    for (temp=0; temp < length; temp++){
        LogText("%02x ", data[temp]);
    }
    LogText("\n");
    return (r >= 0);
}

With this change I got the following output, running the OculusWorld demo (with the fully functioning tracker):


Configuration data exchange between Oculus Rift SDK and Tracker

At the start of a game, the SDK will read several configuration parameters from the tracker, which contain information about the screen, the lenses and the tracker. This is done by exchanging HID "feature reports" over the USB endpoint 0. Here is a snippet showing some of the parameters describing the screen:


Snippet from the Rift SDK, which decodes the 56 byte long configuration packet, containing information about the display.

To get an idea what kind of configuration packets are exchanged and what is encoded by their binary values, have a look at the foculusTypes.h file.

Next, the host PC will send periodically (every 3 s) a "keep alive" packet to the tracker, telling it to keep sending sensor data. After the first received keep alive packet, the tracker will start to transmit the sensor data to the PC, typically every 1 ms. This is done in an "interrupt IN transfer" on USB endpoint 1.

The sensor data is arranged in a 62 byte long USB frame in the following way:

uint8_t messageType;    // = 1 indicates a sensor data frame
uint8_t sampleCount;    // How many sensorData slots are used (1, 2 or 3)
uint16_t timeStamp;     // Incremented for each packet
uint16_t commandID;     // Not used yet
int16_t temp;           // Temperature
uint8_t sensorData1[16];// Sensor data packet, 16 bytes each.
uint8_t sensorData2[16];// Sensor data packet, 16 bytes each.
uint8_t sensorData3[16];// Sensor data packet, 16 bytes each.
int16_t mX;             // 16 bit Magnetometer data
int16_t mY;
int16_t mZ;


The magnetometer data is directly encoded in three 16 bit signed integers (mX, mY and mZ).
The data from the gyroscope and accelerometer is formated as 21 bit signed integers and stored in the sensorData blocks.
The three 16 byte arrays contain up to three readings of the gyroscope and accelerometer. 

Let's show an example of how the data is packed in these blocks.
We start with the data, read out from the gyroscope, formated as 21 bit signed integers:
x - axis:  x20 x19 x18 ... x3  x2  x1  x0   (21 bits)
y - axis:  y20 y19 y18 ... y3  y2  y1  y0
z - axis:  z20 z19 z18 ... z3  z2  z1  z0

This is aranged in the first 8 bytes of the sensorData block as such:
sensorData[0] = x20 x19 x18 x17 x16 x15 x14 x13
sensorData[1] = x12 x11 x10 x9  x8  x7  x6  x5
sensorData[2] = x4  x3  x2  x1  x0  y20 y19 y18
sensorData[3] = y17 y16 y15 y14 y13 y12 y11 y10
sensorData[4] = y9  y8  y7  y6  y5  y4  y3  y2
sensorData[5] = y1  y0  z20 z19 z18 z17 z16 z15
sensorData[6] = z14 z13 z12 z11 z10 z9  z8  z7
sensorData[7] = z6  z5  z4  z3  z2  z1  z0  0

The last 8 byte in the sensordata block contain the X, Y and Z values of the accelerometer in the same format.


Developing a Rift compatible tracker


After reverse engineering the communication protocol, it was possible to fully emulate the Rift tracker. I might post more details about the development process soon.

Until then, you can have a look at how the "Foculus" tracker has been implemented in the attached source code, which is heavily annotated with comments. I have uploaded the entire eclipse project folder:

Sourcecode on GitHub:



If you have a STM32F3DISCOVERY board, you might want to try out the code.
Some hints on how to set up a full development environment can be found here.

If you just want to flash the binary (.elf) file, have a look at how_to_flash.txt in the zip file.

This is pretty much a work in progress, so feedback is much appreciated. 
Let me know if you find bugs, if it works or doesn't work for you or if you have decided to continue development and came up with some new features to the firmware.

Known Issues, open questions and things to do

  • The SDK can set the following flags: Flag_CalibrationTest, Flag_UseCalibration, Flag_AutoCalibration. I have no idea how the SDK expects the Tracker to behave after it set these flags. Currently they are not evaluated at all.
  • Another thing is that I can not use the Oculus Config Utility to calibrate the Magnetometer, as it checks for a connected HMD. Without the calibration, there is no Yaw drift correction in most of the games. I hope to fix this problem with a EDID hack.


In part 3 I want to have a look on how to spoof the EDID, to make the LCD display panel look like an actual Oculus Rift monitor to the SDK.



Update 20.3.14

I wrote a small script in python, to receive the data from the fake rift and plot it. It turned out there were glitches in the Magnetometer data. I found this while recording data for 60 s while rotating the HMD randomly in all directions. The data as read from the magnetometer has been plotted on a 3D plane (left) and over time (right)







I think that these glitches actually come from the LSM303 magnetometer sensor. First I thought it is a synchronization issue. But it turned out, the only way to get rid of them was to reduce the output data rate of the sensor from 220 Hz to 75 Hz:




By the way, the left plot illustrates nicely that each one of the magnetometer readings should theoretically be a point on an ellipsoid. The Oculus Rift calibration procedure determines the parameters (its diameters and center point) of this ellipsoid.



Update 29.3.14

Several bugs have been fixed and new features were added.

Scaling of sensor values

In the previous releases, I was assuming the tracker does not actually do any rescaling and just transmits the raw numbers as they are read from the sensors. But as it turns out, this assumption is wrong. The Oculus SDK expects the sensor values to be scaled to 10^-4 [rad/s], 10^-4 [m/s^2] and 10^-4 [Gauss], independent on which measurement range the sensor is set to. In the current version of the Foculus Tracker I have accounted for that.


Gyroscope zero level error correction

Place the tracker on a table and make sure it absolutely does not move. Then press the blue "User button" on the STM32F3 DISCOVERY board. The LED pattern will blink a few times, to show that the calibration will now take place. Then 3 LEDs will turn on in sequence, showing which sensor range is calibrated at the moment. Then the IDLE LED pattern will be displayed again and the Tracker is calibrated and ready to use. Please note that the calibration values will be lost as soon as the Tracker is unplugged from USB.

The calibration significantly reduces gyroscope zero-level error and hence gyro drift, as shown by the following plots:

Before calibration. A bias error is clearly visible. This will be constantly integrated and lead to significant drift of tracker orientation.

After calibration. The zero level error has been reduced.



Further updates (Changelog)


// Changelog
//--------------------------------
// 09.03.2014: Fixed bug in handleConfigPacketEvent() by adding break; statements (data rate was always 1 ms before)
// 10.03.2014:  Changed wMaxPacketSize from 62 to 64
// 19.03.2014:  Changed I2C Bus speed to 400 kHz, which allows to read all 3 sensors in 0.65 ms  (before it was > 2 ms)
// 20.03.2014:  Now evaluating the "new data ready" pins of all 3 sensors (improves timing a lot, reduces jitter)
//              Enabled FIFO in Streaming mode of Accelerometer and Gyro (no samples will be lost!)
//              Fixed Glitches in Magnetometer output by setting it to 75 Hz measurement rate (was 220 Hz before)
// 23.03.2014:  Fixed a problem with the USB interrupt and atomic access, not allowing the tracker to change sensor scale
//    Changed sensor scaling to floating point numbers and scaled to values as expected from the SDK
// 29.03.2014:  Added gyroscope "set to zero" calibration routine (Press the user button on the STM board and keep it very still)
//    Added temperature readout from gyro
//    Added some nice LED animations for IDLE mode, Tracker running mode and Calibration mode






43 comments:

  1. Very interesting. I plan to build oculus rift.
    I have AT070TN90 LCD.
    I need to buy a LCD driver and headtracker (from this store will be good? Http://www.kamami.pl/index.php?productID=196963)
    I have a magnifying glass. I need something else?
    For STM32F3DISCOVERY I need to buy a USB ST-LINK connector (has a high price, compared to the rest)?

    ReplyDelete
    Replies
    1. Hi, you should give it a try. The STM32F3DISCOVERY has the USB ST-LINK on board, so no need to buy it extra. Cheers!

      Delete
  2. Thx. When you add new part?

    ReplyDelete
  3. Cool man. I myself am trying to develop a mipi controller for 5.5 FHD to collect himself Oculus, the tracker has not yet tried to do. I look forward to continue your work. Thank you for your efforts.

    ReplyDelete
  4. Very interesting project. Be sure to buy STM32F3DISCOVERY and take part in the testing.

    ReplyDelete
  5. This your project work in Windows ?

    ReplyDelete
  6. Worked for me on Windows 7 (although don't have Oculus dev kit installed yet). Need a few adjustments, see the issue report on GitHub page.

    Awesome work. Thanks,
    Simon

    ReplyDelete
  7. Is it possible to flash other IMUs ? Like the chinese air-mouse T31 which works wirelessly ?

    ReplyDelete
  8. Foculus in action...
    https://www.youtube.com/watch?v=ffoFpm5pfEM

    Cheers,
    Simon.

    ReplyDelete
  9. Wow, I'm glad to see the code works for you. Seems to be a nice open source racing-sim you are working on. I also really like you "project box" for the STM board, hahahhah ;)
    Any troubles with yaw drift?

    Cheers YFL

    ReplyDelete
  10. Amazing!!!
    I'm planning to build a VR headset with the STM32F3, so your work really helps a lot!

    Just a small problem...
    I'm using Atollic TrueSTUDIO for ARM Lite. And since your codes are written in eclipse, I use the Demo Project provided by STM32F3-Discovery Board Firmware Applications Package and import your files in inc and src folders. It successfully compiled and reprogrammed the firmware of the board. And I can play Oculus' games.
    But the LEDs blink very slowly and when I press the USER button to enter calibration routine, it stuck with the LD5 on. After reconnect it, windows cannot recognize it...

    Also, I use the STM32 ST-LINK Utility to flash your stm32f3_HID_for_real.hex file to the board. It works really fine!

    Is that any extra setting with the clock or something? Because I notice "SysTick_Config(RCC_Clocks.HCLK_Frequency / 1000);" in main and I change it to "SysTick_Config(RCC_Clocks.HCLK_Frequency / 100);" but nothing happened...

    Cheers,
    Kailang

    ReplyDelete
  11. Amazing work!
    I have my STM32F3discovery yesterday,after I flashed the file, I connect the STM32F3 borad to PC(windows 7), It firstly recognized as"tracker DK",but when I check them in Device Manager,it is be shown as"usb hid device",I don't know the reason,could you help me figure out this question?
    thanks a lot!

    ReplyDelete
    Replies
    1. That's OK... You can still play Oculus based game.

      Delete
    2. Thanks a lot for your help. I have on more question to ask.
      Do you noticed that the angle of Z axis is reverse? I changed " gyroBuffer[ cgZ ]" to" -gyroBuffer[ cgZ ]". but i get new question now. When I move the board around the Z axis to about 45 degree,and then keep it still,after about 0.5s, the angle changes itself. Do you know how to solve the problem?

      Delete
  12. HI,I noted that games are moving around slowly,would you check this problem? thank you.

    ReplyDelete
  13. Hi,

    Do you know how to send the data from the tracker to emulate the mouse for games that are not supported by the Rift? I was thinking of using FreePIE but not sure if that will work.

    Cheers

    ReplyDelete
  14. Hi there, is there any chance you would take a look at this tracker working agains rift SDK 0.4.x? Thanks in advance. Lukas T

    ReplyDelete
  15. I just flashed my stm32f3 and successfully used it on a roller coaster game.
    But every other game I try doesn't work with it. Am I missing something?
    Anyone else have this problem?

    ReplyDelete
  16. Hi, I am sucesfully using this with 0.3.x SDK software, demos and games with EDID from Oculus. With 0.4.x SDK (and games based on that) it does not even detect as oculus so it doesnt work.

    ReplyDelete
  17. the project is discontinued ?

    ReplyDelete
  18. Please do consider continuing work on this for the new SDK... I bet you could get some people to donate to the cause if it turns out to be a lot of work (myself included).

    ReplyDelete
  19. Same here, I would donate without a blink of the eye to make this work with 0.4 sdk.

    ReplyDelete
  20. Hi

    I have STM32F4 Discovery but this codes don't work on it.
    How I can use on my F4 Disco.?

    Thanks

    ReplyDelete
    Replies
    1. STM32F4 Discovery only have 3 axis accelerometer but no giroscope chip on it. This firmware working only for F3 Discovery!

      Delete
    2. Thanks, my bad. Escaped my attention.

      Delete
  21. I think the SDK 0.4 problem is not a firmware problem so Yetifrisstlama can't solve it.
    My opinion is the SDK 0.4 check the serialnumbers of the HMD and work only with original oculus.
    So rather we nedd a patch or a crack for the SDK 0.4.x.

    ReplyDelete
  22. I was able to get a Naze32 10DOF multicopter flight control board reprogrammed using the real DK1 tracker firmware, and both the new and old SDKs detect the Naze32 10DOF as a real tracker, however there is no tracking, I believe there is no tracking because the sensors on the Naze32 10DOF are connected to the to the STM32F103 CPU in a different way than the real DK1 tracker.

    BTW I had to add a second USB port to the Naze32 10DOF, wired in the same way as the real DK1 tracker, before the computer recognized it has a DK1 tracker.

    ReplyDelete
  23. hi everyone I find this really interesting project this is a moment I am trying to emulate a rift suddenly I hurry to buy a stm32f3discovery only being veiled in windows I use the STM32 ST-Link Utility on flash goes well but when I pass on usb user nothing happens apart put the LED flashes red com that someone could help me please

    ReplyDelete
    Replies
    1. Try to playing with the jumper settings.

      Delete
    2. there are a certain manipulation rider to do during and after the flash memory?

      Delete
    3. after purchase of a new card code works however only the native games and not all works.
      is that its pe edid come from?

      Delete
  24. Nobody knows it and everybody is waiting for yetifrissrlama but it seems like he died or moved to a Tibetian church for 7 years where the internet connection is not avaiable yet. :)

    ReplyDelete
  25. Hi, maybe I can flash on windows8, or only can flash on linux?

    ReplyDelete
  26. Hi to all, we did DK2 compatible motion tracker based on similar components. The tracker recognized as DK2 in Oculus Utils and it works under Oculus Runtime 0.4.4 - 0.8. We are from Russia, therefore the project's site on Russian language http://vrdevice.ru, sorry.
    This video how tracker works in Fallout4 https://youtu.be/-k8cKAdbWZM with latest VorpX version under Runtime 0.8.

    ReplyDelete
  27. A todos que partilham e trabalham sob estas mesmas convicções e princípios Friv Friv 360 Friv4school 2020 Senhor Deputado Cashman, agradeço-lhe a informação. Juegos De Roblox Juegos De Zoxy Mais uma vez, obrigada ao Parlamento por comungar da visão que informa a nova política dos consumidores Juegos Kizi 2017 Juegos Yepi 2017 Twizl 3 Zoxy 2 assente no mercado - a visão de um mercado de consumidores informados e capacitados que procuram e usufruem, com confianca.

    ReplyDelete
  28. Wir danken Ihnen Friv4school 2018 Gry Friv 2 Gry Friv 5 dass Sie diese Hoffnung mit uns teilen und diesen Schritt auf dem Pilgerweg des Vertrauens Gry Friv Juegos Friv 100 Juegos Friv 1000 mit uns gegangen sind. Juegos Friv 5 Juegos De Friv 2 Juegos Friv 250 wir danken Ihnen für das Interesse an unseren Produkten und hoffe

    ReplyDelete
  29. Caro leitor, no Friv Jogos você encontra uma imensa variedade de jogos on-line sem pagar nada! Temos muitos jogos divertidos e gratuitos, com novas inserções e atualizações diárias! Você encontrará JOGOS FRIV 2, FRIV 3, FRIV 4 e FRIV 1000 e FRIV 360. Aproveite e boa diversão!

    ReplyDelete
  30. Really absorbing post. Thanks for your great post.I like what your blog stands. You can play Friv Unblocked , Friv Unblocked, Kizi Unblocked ,Kizi Unblocked in my website.

    ReplyDelete

  31. Great set of tips from the master himself. Excellent ideas. Thanks for Awesome tips Keep it up
    vorpx-keygen
    movavi-video-editor-activation-key
    smadav-pro-crack
    bandicam-crack

    ReplyDelete