ANNOUNCING: navX MXP Robotics Navigation Sensor

[slibert]

Originally Posted by ayeckley

Update: I've removed the 32 bit JRE and installed the 64 bit JRE 1.8. Everything seems to be working properly.

I'm still digesting the actual navX code. It's not clear to me yet which calculations are being performed in Scott's code vs. what is performed by the DMP. Definitely one of the most complex embedded projects I've encountered; most don't even do floating point math.

The DMP is only used to read the quaternions each time a data ready interrupt occurs, calculated using proprietary Invensense algorithms. So the DMP (this is all performed on the Invensense chip) does the gyro-auto cal, and all the gyro/accel data acquisition, filtering and fusion - and puts into a FIFO the calibrated gyro data, the raw accelerometer data, and the quaternion.

Due to several issues found in the invensense "motion processing library" (MPL), the rest of the work is performed on the very capable STM32F4 processor by Kauai Labs code.

After acquiring a quaternion (line 596), this code in MPU9250/mpucontroller.cpp:

- transforms the quaternion to yaw/pitch/roll
- calculates the gravity vector from the quaternion, and subtracts that from the accelerometer data to yield linear acceleration.
- acquires periodic magnetometer samples, time-averaging to reduce noise
- applies magnetometer calibration to the raw magnetometer data to get a compass heading
- tilt-corrects the compass heading based on the pitch/roll
- calculates the "fused (9-axis) heading" from the yaw and the tilt-corrected, calibrated compass heading

Then, in the navx-mxp/navx-mxp.cpp nav10_main(), it:

- maintains statistics of linear acceleration to detect motion/no-motion
- maintains a yaw history, to detect if rotating
- if in startup calibration period, and not moving and not rotating, calculates a yaw offset, which is removed from subsequent yaw values
- sends out updates, updates registers, and processes inbound requests

You'll also come across some work-in-progress code for building and applying a temperature-compensation gyro bias which should someday completely eliminate the automatic gyro calibration after sufficient temp/bias values have been acquired and stored to flash memory.