Motion Driver From Invensense

Motion Driver is an embedded software stack of the sensor driver layer that easily configures and leverages many of the features of the InvenSense motion tracking solutions. The motion devices supported are MPU6050/MPU6500/MPU9150/MPU9250. Many of the features of the hardware and the onboard Digital Motion Processor (DMP) are encapsulated into modular APIs which can be used and referenced.

Motion Driver is designed as a solution which can be easily ported to most MCUs. This document highlights the fundamental procedure and choices you will encounter when starting to develop an embedded project using the Motion Driver as the reference. We will go into some of the more details topics like programming the DMP, calibration, and self-test.

Motion Driver

Motion Driver From Invensense

The DMP features

The DMP is a fast, low power, programmable, the embedded lightweight processor in the MPU devices. It is designed to offload functionality, like sensor fusion and gesture recognition, from the MCU to save overall power in the system.

The DMP has many features which can be dynamically turned off and on at run-time. The individual feature can also be disabled while leaving others running. All DMP data is outputted to the FIFO except for pedometer. The DMP can also be programmed to generate an interrupt via gesture or if data ready.

3 Axis Low Power Quaternions – gyro only quaternions. This feature when enabled will integrate the gyro data at 200Hz while outputting the sensor fusion data to the FIFO at the user requested rate. The 200Hz integration will allow for more accurate sensor fusion data. In MD6, if this feature is enabled, the driver will push the 3-axis quaternion into the MPL library and the MPL will handle the accel and compass integrations.
6 Axis Low Power Quaternions – gyro and accel quaternions. Similar to the 3-axis LPQ integrates the accel and gyro at 200Hz sampling rates will outputting to the FIFO at the user requested rates. The 3-axis LPQ and 6-axis LPQ are mutually exclusive and should not be run concurrently. If enabled the 6-axis quaternions can be pushed into the MPL library and the MPL will handle the compass integration for 9-axis.
Orientation Gesture Recognition – Uses sensor data to detect is there is a change in device orientation from portrait, landscape, reverse portrait, and reverse orientation. Very much dependent on the orientation matrix.
Tap Gesture Recognition - multi-directional tap detection on the device. This feature will let users know on which axis position or negative a tap is detected on. It can detect multi-taps up to 4. APIs exist to configure this feature’s threshold, dead time, and tap counts.
Pedometer Gesture Recognition – simple pedometer providing step count and timestamp. This feature is automatically enabled but is not triggered until there is 5 seconds of continuous steps detected. After 5 seconds the count and timestamp will start and the data can be read out from the DMP memory.
DMP Interrupt – interrupts can be configured to generate either when there is sensor data ready which is the FIFO output rate, or when a tap or orientation gesture is detected.

The MPL

The Motion Driver contains a binary library which contains algorithms for sensor fusion and dynamic calibration. The motion driver pushes the sensor data into the MPL and the MPL will handle the 9-axis sensor fusion including the compass integration. MPL features are configured before enabling the MPL library. They can be dynamically turned off and on through API calls into the MPL.

Algorhithms

Gyro Calibration (Fast No Motion):

Run-time calibration routine. Once a no motion state is detected the gyro calibration will trigger. Calibration will complete within .5 seconds of no motion state detection.

Gyro Temperature Compensation:

After each gyro calibration, the MPL will record the internal temperature. After several data points the MPL will be able to build a multi-point temperature slope for the gyro and apply it along with the calibration biases. This will compensate for gyro drift due to temperature.

Compass Calibration:

Run-time hard iron compass calibration for MPU9150 and MPU9250. MPL reads and builds the magnetic field environment around the device. Once enough data is present the compass offset can be applied and 9-axis quaternions can be generated. If you are in an environment with unstable magnetic field the compass will not get calibrated. If compass is not calibrated the quaternions will only use 6-axis.

Mag Disturbance Rejection:

After calibration the MPL library will keep track of the magnetic field and if there is an anomy detected, the MPL library will reject the compass data and switch back to 6-axis fusion. After a magnetic disturbance is detected, the MPL library will continue to check the compass data every 5 seconds. At each check if the disturbance is no longer there, it will switch back to 9-axis fusion…otherwise it will continue to reject the data.

Fusion 3 Axis:

Gyro angle quaternions

Fusion 6 Axis:

Gyro and accel quaternions

Fusion 9 Axis:

Gyro, accel, and compass quaternions

Sensor Data

Many other types of data can be derived from the quaternion data. These conversions to the other data types outside the 3 main sensors are provided in the Motion Driver, users will be able to get the following data

Compass – magnetic field data in micro tesla on each axis
Gyro – X, Y, Z axis rotational acceleration data in degrees per second
Accel – X, Y, Z axis linear acceleration data in Gees
Heading – 360 degrees from North with Y+ axis as the pointer
Rotational Matrix – linear math 9 element matrix representation
Euler Angles – Pitch, roll, yaw based in degrees with frame reference
Quaternions – sensor fused w, x, y, z rotational angles
Linear Acceleration – linear acceleration in body frame coordinates.
Gravity Vector – Which access gravity effects

MPU Hardware Features

The Motion Driver has some specific algorithms for the hardware of the MPU devices.

Factory Calibration – example of how to calibrate the accel and gyroscope at the factory line. It is highly recommended that the accel is calibrated. Before calibration you must orient your device in a specific orientation. This particular algorithm requires that the device be placed still with the Z+ axis upwards against gravity. Once triggered the biases are obtained and can be applied either to the Hardware Offset registers or the MPL library. Biases need to be saved in flash memory so can be reapplied after on/off.
Factory Self-Test – Factory Tool based on InvenSense Hardware Self-Test algorithm to provide go/no go test of the MEMS sensors.
Saving and Loading Sensor States – APIs on saving the sensor states to flash memory. This includes the calibration data for each sensor and also the temperature compensation data. Users can use this as an example of how to save the sensor data into their flash or EEPROM.
Low Power Accel Mode – Only for MPU6500 and MPU9250. This is an accel only duty cycle mode for lower power. There are multiple samplings users can select from 1Hz up to 640Hz.
Low Power Motion Interrupt Mode – Only for MPU6500 and MPU9250. Sets the MPU device into a low power accel mode in which if a motion is detected it will generate an interrupt to the MCU for the MCU to wake up and continue processing. The threshold for motion is configurable.
Register dump – a dump of all register values

Motion Driver Features

This is a quick overview on the MD features.

DMP Features

3/6 Axis Low Power Quaternions

Tap, Orientation, and Pedometer Gesture Detections

MPL Algorithms

Run Time Gyro Calibration
Run Time Gyro Temperature Compensation
Run Time Compass Calibration
Run Time Magnetic Disturbance Rejection
3/6/9 Axis Sensor Fusion

Hardware Features

Factory Calibration
Factory Self Test
Saving and loading Sensor states
Low power Accel mode
Low Power Motion Interrupt Mode
Register Dump

With every embedded system, the functionality and performance is dependent on the MCU selected. Cost, low power, speed, tool chains, and processing are all factors to consider. For the MPU device if you are planning to use the InvenSense Motion Driver software here are some things to consider.

Flash and RAM Size: Flash and RAM size is dependent on code optimization, compilers, what features you want to use, and what other components are in your system. In general, though, MD requires you can need reserve the following amount of Flash and RAM. Keep in mind this will only be for the motion driver and not for other possible functionalities.

16-bit MCU – 128K and 12K

32 bit MCU - 68k and 10k(No optimization, 64k and 8k with optimization)

Again since depending size is very dependent on compiler and compiler settings, customers should spend time to determine what size flash and RAM is needed for their applications.

Long long math support : The MPL library requires support for long long (64-bit) math. You will need to make sure if you are using the MPL library your tool chains can support this. Usually,8051 MCUs cannot support such mathematical calculations. If the tool chain does not support long long math, you can still use the DMP to get 6-axis fusion.

Interrupts : The MPU device can provide an interrupt for various functions from low power gesture recognition or data ready interrupts. While not required for the system to use the MPU interrupt, if you are planning to use it, then you must reserve a GPIO pin which has wakeup capabilities.

Sampling Rate : Sensor fusion requires large computational power from the MCU. This plays into how much processing is possible per sample and limits your sampling rate. For example the TI 6-bit MSP430 with the Motion Driver should be limited to 100Hz sampling rate if the MCU is doing the full 9-axis fusion. Anything over 100Hz sampling rate the MSP430 start missing data. Higher end 32-bit MCUs can usually achieve 200Hz sensor fusion if there are no other large computational functionality required in the system. This sample rate can be increased if you offload the processing onto the DMP.

Selecting the MCU

Connecting the Hardware

After selecting the MCU most likely you will have a MCU evaluation kit or your own PCB board. To connect the MPU device to the MCU board for evaluation, you can obtain an InvenSense MPU evaluation board through InvenSense.com. MPU6050, MPU6500, MPU9150, and MPU9250 are all available.

You will need to connect the following pins from the evaluation board to the MCU board.

VDD and VDD_IO: Depending on the MPU device this would be a 3V or a 1.8V
SDA and SCL: I2C pins
GND: Connect to ground
INT: Connect to GPIO for interrupts

To confirm your hardware setup and also the basic I2C functionality, start by reading the MPU device’s whoami register and confirm you are getting the correct device ID. For the MPU series, the I2C address is 0x68 while the device ID is all different for the different parts so please check the specs. After hardware confirmation, we can continue with the software integration.

Motion Driver 6.12 Firmware Package

The Motion Driver release firmware contains the following folders.

core\driver : This folder contains the InvenSense drivers layer for the MPU devices as well as the MCU specific drivers.
main.c: The main function and main loop for the project application. Customers can use this code as a reference to integrate the driver functions into their project.
core\mllite : This folder contains the MPL data processing functions that store the received sensor data and processes the data
core\mpl : Contains the InvenSense Proprietary MPL library – a library containing advanced algorithms for sensor fusion and run-time calibrations.
core\eMPL-hal : This folder contains the files that provide same the sensor data conversion such as euler angles.

Integrating Motion Driver

Embedded Motion Driver consists of the following components that are required to be integrated on the target hardware platform:

Driver
Motion Processing Library
Sample HAL

The diagram below shows the architecture of Motion Driver 6.12 and the tasks to be performed to port the Motion Driver successfully to the target platform

The Invensense MD6.12 Driver Layer (core\driver\eMPL) consists of these files

inv_mpu.c - the driver which can be easily ported to different embedded platforms.
inv_mpu.h - contains the structures and prototypes for the InvenSense driver.
inv_mpu_dmp_motion_driver.c - the driver for containing the dmp image and the APIs to load and configure the DMP.
inv_mpu_dmp_motion_driver.h – contains the prototypes and defines for the DMP features
dmpKey.h - Contains the defines for DMP memory locations for DMP features
dmpmap.h - Contains the defines for DMP memory locations

The user would need to provide the following APIs to support I2C read/write functionality, system clock access, hardware interrupts callbacks and logging corresponding to the platform on which the Motion Driver is to be ported. These functions need to be defined in inv_mpu.c and inv_mpu_dmp_motion_driver.c.

i2c_write and i2c_read : this will need to be linked to the i2c drivers. This functions will take in 4 parameters then perform the i2c transactions

unsigned char slave_addr
unsigned char reg_addr
unsigned char *data
unsigned char length

delay_ms : this function will take in one unsigned long parameter and it will act as a delay in milliseconds for the system.

get_ms : get_ms is mainly used to get the current timestamp. Timestamp are usually an unsigned long and in milliseconds. This function will mainly be used for the compass scheduler and also additional information for sensor fusion data.

log_i and log_e : MPL messaging system in which it can log informational or error messages. Current implementation packets the message and sends it out through the USB or UART for the pythonclient to receive. The logging code is located in the file log_msp430.c or log_stm32l.c

MPL Library

This is the core of the proprietary InvenSense Motion Apps algorithms and consists of Mllite and mpl directory. There is no porting required for MPL. You may need to include system specific header files to support the memcpy, memset, … etc., function calls in the mllite package. The Motion Driver package will include pre-compiled MPL libraries, one for the TI MSP430 platform and the other for ARM core platforms. The ARM library has a generic library which can be linked to any ARM MCU. It also have some pre-compiled for M3 and M4 for better optimizations.

The eMPL-HAL directory contains the APIs to get various data from the MPL library.

The data you can obtain are from the following APIs

int inv_get_sensor_type_accel(long *data, int8_t *accuracy, inv_time_t *timestamp);
int inv_get_sensor_type_gyro(long *data, int8_t *accuracy, inv_time_t *timestamp);
int inv_get_sensor_type_compass(long *data, int8_t *accuracy, inv_time_t *timestamp);
int inv_get_sensor_type_quat(long *data, int8_t *accuracy, inv_time_t *timestamp);
int inv_get_sensor_type_euler(long *data, int8_t *accuracy, inv_time_t *timestamp);
int inv_get_sensor_type_rot_mat(long *data, int8_t *accuracy, inv_time_t *timestamp);
int inv_get_sensor_type_heading(long *data, int8_t *accuracy, inv_time_t *timestamp);
int inv_get_sensor_type_linear_acceleration(float *values, int8_t *accuracy, inv_time_t *timestamp)

Initialization APIs:

At power on the MPU device will provide sensor data in it default state. The inv_mpu.c provides a reference API on how to initialize the MPU device with some basic configurations such as powering on the sensors and setting the scale range and sampling rate

int mpu_init(struct int_param_s *int_param)
int mpu_set_gyro_fsr(unsigned short fsr)
int mpu_set_accel_fsr(unsigned char fsr)
int mpu_set_lpf(unsigned short lpf)
int mpu_set_sample_rate(unsigned short rate)
int mpu_set_compass_sample_rate(unsigned short rate)
int mpu_configure_fifo(unsigned char sensors)
int mpu_set_sensors(unsigned char sensors)

The Orientation Matrix

The application also needs to define the orientation matrix for the MPU device and the 3rd party compass if present on the platform. The orientation matrix will reconfigure the physical hardware sensor axis to the to the device coordinates. A wrong configuration will get you inaccurate results from the sensors data. For more information on how the matrix should be defined please reference the Orientation Matrix Transformation chart.pdf. The matrixes will be pushed into both the MPL library and DMP for fusion calculations. Sample code of Orientation matrix

             struct platform_data_s {
                    signed char orientation[9];
                    };
                        /* The sensors can be mounted onto the board in any orientation. The mounting
                        * matrix seen below tells the MPL how to rotate the raw data from the
                        * driver(s).
                       */
                    static struct platform_data_s gyro_pdata = {
       .orientation = {-1, 0, 0,
                                                0,-1, 0,
                                                0, 0, 1}
                    };
                    static struct platform_data_s compass_pdata = {
                    #ifdef MPU9150_IS_ACTUALLY_AN_MPU6050_WITH_AK8975_ON_SECONDARY
                       .orientation = {-1, 0, 0,
                                                0, 1, 0,
                                                0, 0,-1}
                    #else
                         .orientation = { 0, 1, 0,
                                              1, 0, 0,
                                                 0, 0,-1}
                    #endif
                    }

Interrupts Handling

The MPU device has one interrupt output pin. The interrupt can be programmed to be generated either at

FIFO output rate
DMP generated

Usually we generate an interrupt when there is new sensor data readily available in the FIFO. The DMP can also be programmed to generate an interrupt either when a gesture is detected.

If you are using the MD, reference example, when a sensor data ready interrupt is generated, the interrupt routine sets a global flag new_gyro to 1. In the main loop it will know that there is a new set of sensor data to process. Here is a list of APIs related to interrupts

int dmp_set_interrupt_mode(unsigned char mode)
static int set_int_enable(unsigned char enable)

DMP Initialization

The DMP firmware code is 3kB image found in the structure

static const unsigned char dmp_memory[DMP_CODE_SIZE]

This image needs to be downloaded into the DMP memory banks. After downloading a starting address needs to be provided then the DMP state needs to be turned on. APIs related to DMP initialization are the following

int dmp_load_motion_driver_firmware(void)
int dmp_load_motion_driver_firmware(void)
int dmp_set_fifo_rate(unsigned short rate)
int mpu_set_dmp_state(unsigned char enable)

The Motion Driver example on DMP initialization can be found in the main function right before the main loop.

DMP Features

The DMP features many functions as detailed in the Features Guide. These functions can be dynamically enabled and disabled. The main API is

int dmp_enable_feature(unsigned char mask);

This function takes the mask and indexes into the correct memory address in the DMP firmware to enabled and disable the feature. Features are

#define DMP_FEATURE_TAP (0x001)
#define DMP_FEATURE_ANDROID_ORIENT (0x002)
#define DMP_FEATURE_LP_QUAT (0x004)
#define DMP_FEATURE_PEDOMETER (0x008)
#define DMP_FEATURE_6X_LP_QUAT (0x010)
#define DMP_FEATURE_GYRO_CAL (0x020)
#define DMP_FEATURE_SEND_RAW_ACCEL (0x040)
#define DMP_FEATURE_SEND_RAW_GYRO (0x080)
#define DMP_FEATURE_SEND_CAL_GYRO (0x100)

For Tap and Orientation data parsing, the MD6.12 drivers define 2 call back functions which will handle the parsing and log it to the python client.

int dmp_register_tap_cb(void (*func)(unsigned char, unsigned char))
int dmp_register_android_orient_cb(void (*func)(unsigned char))
static int decode_gesture(unsigned char *gesture)
static void tap_cb(unsigned char direction, unsigned char count)
static void android_orient_cb(unsigned char orientation)

There are also some configurable settings for Tap such as threshold. The APIs are available in the inv_mpu_dmp_motion_driver.

DMP FIFO Output

DMP only writes to the FIFO when specific features are enabled such as tap or sensor data. The MD6.12 driver will wait for the DMP to generate an interrupt, and then read the contents of the FIFO. FIFO format is dependent on which DMP features are enabled.The DMP FIFO output format can be seen in the API function.

int dmp_read_fifo(short *gyro, short *accel, long *quat, unsigned long *timestamp, short *sensors, unsigned char *more);

InvenSense Hardware Self-test

The Hardware Self-Test is an optional factory line test customers can use as a go/no-go test on their production line. The HWST algorithm will test the MEMS sensor and confirm working functionality by internally moving and measuring the movement and comparing the output to the Invensense data saved in its registers. For more detailed information please look in the product spec. The Motion Driver code provides a sample code on how the HWST can be ran and its output. The hardware selftest can be run without any interaction with the MPL since it's completely localized in the driver. The API which runs the complete self test is:

static inline void run_self_test(void)

The Motiion Driver, bundles the self test and factory calibration together since the sensor offsets are calculated through the normal self test routine. However customers can separate calibration and self test if they wish.

The MPU6050/MPU9150 has a different self test algorithm compared to the MPU6500/MPU9250. The API returns the status of the each axis of the sensor and the accel and gyro bias for calibration

int mpu_run_6500_self_test(long *gyro, long *accel, unsigned char debug)
int mpu_run_self_test(long *gyro, long *accel)

Calibration Data and Storage

Calibration data contains information describing the inherent biases and temperature dependent behavior of the MPU gyro, accelerometer, and compass. This data is used during MPL execution to improve the accuracy of the results returned by the MPL. Calibration data may change slowly over time, temperature, and environment so Invensense provides several in-use sensor calibration algorithms which will constantly calibrate the sensors throughout its lifetime. Details are described in the Features Guide. It is recommended that the MPU sensor accel and gyro be calibrated at the factory line and if using the MPL library to turn on the in-use algorithms.

Integrating the MPL Library

The MPL library is a precompiled library containing the sensor fusion engine. When porting Motion Driver the library needs to be compatible with integrators system. The Motion Driver comes with 2 libraries.

MSP430 - compiled using Code Composer. Should be compatible with all MSP430 product line
ARM– The Motion Driver comes with many precompiled libraries for ARM. There are specific IAR, Keil and GNU 4.9.3 compiled libraries. Each compiler compiles the library using specific settings for M0, M0+, M3, M4, and M4F.

After the library is linked the code will need to enable the library and it’s features. The library initialization can be found in the main function before the main loop. The features are described in the Feature Guide. Here are the associated APIs

inv_error_t inv_init_mpl(void)
inv_error_t inv_enable_quaternion(void) //enable 6-axis
inv_error_t inv_enable_9x_sensor_fusion(void) //enable 9-axis fusion
inv_error_t inv_enable_fast_nomot(void) //gyro in-use calibration
inv_error_t inv_enable_gyro_tc(void) //gyro temperature compensation
inv_error_t inv_enable_vector_compass_cal(void) //compass calibration
inv_error_t inv_enable_magnetic_disturbance(void) //magnetic disturbance
inv_error_t inv_enable_eMPL_outputs(void)
inv_error_t inv_start_mpl(void)

Compiler-Specific Setting

To compile for the different parts (MPU6050, MPU9150, MPU6500, and MPU9250), you will need to set the compiler flag

The default symbols needed are

MPL_LOG_NDEBUG=1
MPU9150 or MPU6050 or MPU6500 or MPU9250
EMPL
USE_DMP
EMPL_TARGET_MSP430 or its equivalent

Once the part is set the compiler will compile for that specific part and its features.

For downloading the source code and source document, please check the GitHub.

IMU

Sensor Fusion

Implementation of the Project

IMU With ARM Processor

GitHub

Video