InvenSense MPU-9255/MPU-9250/MPU-6500 gyroscope accelerometer and compass

EDIT: Arduino version of here.

The BBC micro:bit does not have a builtin gyroscope and the self-balancing robot requires one.

Among the many options, the MPU-6500/MPU-9250/MPU-9255 based breakout boards are probably among the cheapest motion sensors available. The MPU-6500 costs SGD9.90 from a local retail shop and is available online for much less. The differences between the models are:

• MPU-6500: gyroscope and accelerometer, 6 axis.
• MPU-9250: gyroscope, accelerometer and compass, 9 axis
• MPU-9255: same as MPU-9250 (almost)

The gyroscope and accelerometer are identical across all three models. Each axis has 16 bits of resolution. The gyroscope measures up to +- 2000 degrees per second. The accelerometer measures up to +-16g. The builtin accelerometer on the micro:bit only has 11 bits of resolution and a range of +-4g.

There are no drivers or library for these chips for the micro:bit. InvenSense provides product specification and register map documents, enough to figure out how to interface with the chips. I can ignore the  advanced features like the FIFO buffer, interrupts and the digital motion processor. I just need to configure the chips, read the gyroscope and accelerometer values.

So, if you need a cheap gyroscope and accelerometer that works with micro:bit, here's a working library you can add to your code.

Hardware instructions

• Connect VCC and GND to the 3V and GND pins on the micro:bit respectively.
• Connect SCL and SDA to pin 19 and 20 on the micro:bit respectively. These are the default I2C pins.
• Connect AD0 to GND to select the default I2C address.

Software instructions

• Create a new project on MakeCode.
• Click on the arrow at the bottom left to show the simulator.
• Click "{} Javascript" to switch to Javascript mode.
• Click on "Explorer" to expand the Explorer tree.
• Click on the "+" on the Explorer bar to add a custom.ts script.
• Click "Go ahead!" on the "Add custom blocks?" prompt.
• Now, it should be showing the custom.ts script. Delete all the default code.

If your project already has a custom.ts script, ignore the above instructions. Just switch to that script.

Now copy and append the code below to the end of your custom.ts.

Update: reset_mpu() now takes two parameters gyro_lpf and accel_lpf to control the digital low pass filter.

/**
 * Public domain. Use at your own risk!
 * Simplified interface for InvenSense MPU-6500, MPU-9250, MPU-9255
 */
//% weight=90 color=#0040A0
namespace InvMPU {
    export const MPU_6500_ID = 0x70;
    export const MPU_9250_ID = 0x71;
    export const MPU_9255_ID = 0x73;

    const MPU_ADDR = 0x68;
    const WHO_AM_I = 0x75;
    const REG_PWR_MGMT_1 = 0x6b;
    const REG_SIGNAL_PATH_RESET = 0x68;
    const REG_USER_CTRL = 0x6a;
    const REG_ACCEL_XOUT_H = 0x3b;
    const REG_ACCEL_YOUT_H = 0x3d;
    const REG_ACCEL_ZOUT_H = 0x3f;
    const REG_GYRO_XOUT_H = 0x43;
    const REG_GYRO_YOUT_H = 0x45;
    const REG_GYRO_ZOUT_H = 0x47;
    const REG_CONFIG = 0x1a;
    const REG_GYRO_CONFIG = 0x1b;
    const REG_ACCEL_CONFIG = 0x1c;
    const REG_ACCEL_CONFIG2 = 0x1d;
    const XG_OFFSET_H = 0x13;
    const YG_OFFSET_H = 0x15;
    const ZG_OFFSET_H = 0x17;

    function mpu_read(reg: number): number {
        pins.i2cWriteNumber(MPU_ADDR, reg, NumberFormat.UInt8BE, true);
        return pins.i2cReadNumber(MPU_ADDR, NumberFormat.UInt8BE, false);
    }

    function mpu_read_int16(reg: number): number {
        pins.i2cWriteNumber(MPU_ADDR, reg, NumberFormat.UInt8BE, true);
        return pins.i2cReadNumber(MPU_ADDR, NumberFormat.Int16BE, false);
    }

    function mpu_write(reg: number, data: number) {
        pins.i2cWriteNumber(MPU_ADDR, reg << 8 | (data & 0xff), NumberFormat.UInt16BE);
    }

    function mpu_write_int16(reg: number, data: number) {
        mpu_write(reg, (data >> 8) & 0xff);
        mpu_write(reg + 1, data & 0xff);
    }

    /**
     * Contains one of MPU_6500_ID, MPU_9250_ID, MPU_9255_ID or zero.
     */
    //% block
    export let sensor_id = 0;

    /**
     * Look for a MPU-6500, MPU-9250 or MPU-9255.
     * Returns true if the MPU was found.
     * The MPU id is in sensor_id.
     */
    //% block
    //% weight=99
    export function find_mpu(): boolean {
        sensor_id = mpu_read(WHO_AM_I);
        return sensor_id == MPU_9255_ID || sensor_id == MPU_9250_ID || sensor_id == MPU_6500_ID;
    }

    /**
     * Reset the MPU and configure the gyroscope to +- 2000 degrees/second and the accelerometer to +-16g.
     * The low pass filter settings control how sensitive the sensors are to quick changes.
     * In order of increasing sensitivity: 6, 5, 4, 3, 2, 1, 0, 7
     * Info from MPU-9250 register map document.
     * @param gyro_lpf Gyroscope low pass filter setting, eg: 0
     *   7: 8kHz sampling rate, 36001Hz bandwidth, 0.17ms delay.
     *   0: 8kHz sampling rate, 250Hz bandwidth, 0.97ms delay.
     *   1: 1kHz sampling rate, 184Hz bandwidth, 2.9ms delay.
     *   2: 1kHz sampling rate, 92Hz bandwidth, 3.9ms delay.
     *   3: 1kHz sampling rate, 41Hz bandwidth, 5.9ms delay.
     *   4: 1kHz sampling rate, 20Hz bandwidth, 9.9ms delay.
     *   5: 1kHz sampling rate, 10Hz bandwidth, 17.85ms delay.
     *   6: 1kHz sampling rate, 5Hz bandwidth, 33.48ms delay.
     * @param accel_lpf Accelerometer low pass filter setting, eg: 0
     *   7: 1kHz sampling rate, 420Hz 3dB bandwidth, 1.38ms delay.
     *   0: 1kHz sampling rate, 218.1Hz 3dB bandwidth, 1.88ms delay.
     *   1: 1kHz sampling rate, 218.1Hz 3dB bandwidth, 1.88ms delay.
     *   2: 1kHz sampling rate, 99Hz 3dB bandwidth, 2.88ms delay.
     *   3: 1kHz sampling rate, 44.8Hz 3dB bandwidth, 4.88ms delay.
     *   4: 1kHz sampling rate, 21.2Hz 3dB bandwidth, 8.87ms delay.
     *   5: 1kHz sampling rate, 10.2Hz 3dB bandwidth, 16.83ms delay.
     *   6: 1kHz sampling rate, 5.05Hz 3dB bandwidth, 32.48ms delay.
     */
    //% block
    //% weight=98
    export function reset_mpu(gyro_lpf=0, accel_lpf=0) {
        control.assert(gyro_lpf >= 0 || gyro_lpf <= 7, "gyro_lpf must be between 0 and 7");
        control.assert(accel_lpf >= 0 || accel_lpf <= 7, "accel_lpf must be between 0 and 7");
        mpu_write(REG_PWR_MGMT_1, 0x80); // H_RESET, internal 20MHz clock
        mpu_write(REG_SIGNAL_PATH_RESET, 0x7); // GYRO_RST | ACCEL_RST | TEMP_RST
        mpu_write(REG_USER_CTRL, 0x1); // SIG_COND_RST
        mpu_write(REG_CONFIG, gyro_lpf); 
        mpu_write(REG_GYRO_CONFIG, 0x18); // GYRO_FS_SEL = 3, +-2000 dps, DLPF on
        mpu_write(REG_ACCEL_CONFIG, 0x18); // ACCEL_FS_SEL = 3, +-16g
        mpu_write(REG_ACCEL_CONFIG2, accel_lpf); // DLPF on
    }

    /**
     * Gyro X axis value after calling read_gyro().
     * Value from -32768 to 32767.
     */
    //% block
    export let gyro_x = 0;

    /**
     * Gyro Y axis value after calling read_gyro().
     * Value from -32768 to 32767.
     */
    //% block
    export let gyro_y = 0;

    /**
     * Gyro Z axis value after calling read_gyro().
     * Value from -32768 to 32767.
     */
    //% block
    export let gyro_z = 0;

    /**
     * Read all three gyroscope axis values and store them in gyro_x, gyro_y and gyro_z.
     */
    //% block
    //% weight=95
    export function read_gyro() {
        gyro_x = mpu_read_int16(REG_GYRO_XOUT_H);
        gyro_y = mpu_read_int16(REG_GYRO_YOUT_H);
        gyro_z = mpu_read_int16(REG_GYRO_ZOUT_H);
    }

    /**
     * Accelerometer X axis value after calling read_accel().
     * Value from -32768 to 32767.
     */
    //% block
    export let accel_x = 0;

    /**
     * Accelerometer Y axis value after calling read_accel().
     * Value from -32768 to 32767.
     */
    //% block
    export let accel_y = 0;

    /**
     * Accelerometer Z axis value after calling read_accel().
     * Value from -32768 to 32767.
     */
    //% block
    export let accel_z = 0;

    /**
     * Read all three accelerometer values and store them in accel_x, accel_y and accel_z.
     */
    //% block
    //% weight=94
    export function read_accel() {
        accel_x = mpu_read_int16(REG_ACCEL_XOUT_H);
        accel_y = mpu_read_int16(REG_ACCEL_YOUT_H);
        accel_z = mpu_read_int16(REG_ACCEL_ZOUT_H);
    }

    export let var_x = 0;
    export let var_y = 0;
    export let var_z = 0;

    /**
     * Gyro X axis bias after calling get_gyro_bias().
     */
    //% block
    export let gyro_x_bias = 0;

    /**
     * Gyro Y axis bias after calling get_gyro_bias().
     */
    //% block
    export let gyro_y_bias = 0;

    /**
     * Gyro Z axis bias after calling get_gyro_bias().
     */
    //% block
    export let gyro_z_bias = 0;

    /**
     * Compute the gyro bias for all three axis. The bias for each axis is the average of 100 samples.
     * Returns true if the sensor is steady enough to calculate the bias.
     * The bias values are store in gyro_x_bias, gyro_y_bias and gyro_z_bias.
     */
    //% block
    //% weight=97
    export function compute_gyro_bias(): boolean {
        mpu_write_int16(XG_OFFSET_H, 0);
        mpu_write_int16(YG_OFFSET_H, 0);
        mpu_write_int16(ZG_OFFSET_H, 0);
        const N = 100;
        const MAX_VAR = 40;
        let sum_x = 0, sum_y = 0, sum_z = 0;
        let xs: number[] = [], ys: number[] = [], zs: number[] = [];
        for (let i = 0; i < N; i++) {
            let x = mpu_read_int16(REG_GYRO_XOUT_H);
            let y = mpu_read_int16(REG_GYRO_YOUT_H);
            let z = mpu_read_int16(REG_GYRO_ZOUT_H);
            sum_x += x;
            sum_y += y;
            sum_z += z;
            xs.push(x);
            ys.push(y);
            zs.push(z);
            basic.pause(1);
        }
        let mean_x = sum_x / N;
        let mean_y = sum_y / N;
        let mean_z = sum_z / N;
        var_x = 0;
        var_y = 0;
        var_z = 0;
        for (let i = 0; i < N; i++) {
            let dx = xs[i] - mean_x;
            var_x = var_x + dx * dx;
            let dy = ys[i] - mean_y;
            var_y = var_y + dy * dy;
            let dz = zs[i] - mean_z;
            var_z = var_z + dz * dz;
        }
        var_x = var_x / N;
        var_y = var_y / N;
        var_z = var_z / N;
        if (var_x > MAX_VAR || var_y > MAX_VAR || var_z > MAX_VAR) {
            return false;
        }
        gyro_x_bias = mean_x;
        gyro_y_bias = mean_y;
        gyro_z_bias = mean_z;
        return true;
    }

    /**
     * Set the gyro bias. The bias can be calculated by calling get_gyro_bias().
     * @param x_bias Bias in the X direction, eg: 0
     * @param y_bias Bias in the Y direction, eg: 0
     * @param z_bias Bias in the Z direction, eg: 0
     */
    //% block
    //% weight=96
    export function set_gyro_bias(x_bias: number, y_bias: number, z_bias: number) {
        mpu_write_int16(XG_OFFSET_H, -2 * x_bias);
        mpu_write_int16(YG_OFFSET_H, -2 * y_bias);
        mpu_write_int16(ZG_OFFSET_H, -2 * z_bias);
    }
}

Switch back to main.ts and reload your project. Your main code can now access blocks and javascript that talks to the motion sensor chips.

The InvMPU package contains these functions and they are also available as blocks from the blocks interface of MakeCode:

• find_mpu(): find one of the three MPUs on the I2C bus and returns true if found.
• reset_mpu(): reset the MPU and configure it to +-16g and 2000 degrees per second.
• compute_gyro_bias(): compute the gyroscope bias. The sensor must be stationary.
• set_gyro_bias(): set the gyroscope bias to values computed in previous calls in get_gyro_bias().
• read_gyro(): read all three gyroscope axis values.
• read_accel(): read all three accelerometer values.

One more thing, we really want to know the angle the sensor or robot is tilted from vertical. The axis parallel to the wheels is fixed. We can calculate the angle from the other two axes of the accelerometer using the inverse tangent function atan2(). Unfortunately, the micro:bit does not provide this function or support floating point numbers.

No worries. Here my little approximation of atan2() that takes two signed 16 bits values from the accelerometer and returns 100 times the angle in degrees. For example, if Trig.atan2() returns 2050, it means the angle is 20.5 degrees.

Copy and append the Trig module for micro:bit to the end of your custom.ts.

Update Jan 2021: MakeCode v3 now supports trigonometry functions. Instead of Trig.atan2, use Math.atan2(), convert to degrees and round the result to an integer.

Here's sample code that finds the mpu, resets it, computes the gyro bias, sets the gyro bias and prints values from the gyroscope and accelerometer and the tilt angle on the serial line.

BTW, the bias values are reasonably stable for each hardware breakout board. I use the bias values calculated by the sample program in the balancing code so that I don't have to recompute the bias each time I turn the robot on.

let angle = 0
basic.showIcon(IconNames.Happy)
while (true) {
    while (!(input.buttonIsPressed(Button.A))) {
        basic.pause(100)
    }
    basic.showIcon(IconNames.Diamond)
    if (!(InvMPU.find_mpu())) {
        if (InvMPU.sensor_id != 0) {
            serial.writeLine("Unknown sensor id " + InvMPU.sensor_id)
        }
        serial.writeLine("Cannot find MPU-6500, MPU-6500 or MPU-9255")
        basic.showIcon(IconNames.No)
    } else {
        if (InvMPU.sensor_id == InvMPU.MPU_6500_ID) {
            serial.writeLine("MPU-6500")
        }
        if (InvMPU.sensor_id == InvMPU.MPU_9250_ID) {
            serial.writeLine("MPU-9250")
        }
        if (InvMPU.sensor_id == InvMPU.MPU_9255_ID) {
            serial.writeLine("MPU-9255")
        }
        InvMPU.reset_mpu()
        basic.pause(2000)
        basic.clearScreen()
        if (InvMPU.compute_gyro_bias()) {
            serial.writeLine("X variance " + InvMPU.var_x)
            serial.writeLine("Y variance " + InvMPU.var_y)
            serial.writeLine("Z variance " + InvMPU.var_z)
            serial.writeLine("X bias " + InvMPU.gyro_x_bias)
            serial.writeLine("Y bias " + InvMPU.gyro_y_bias)
            serial.writeLine("Z bias " + InvMPU.gyro_z_bias)
            InvMPU.set_gyro_bias(InvMPU.gyro_x_bias, InvMPU.gyro_y_bias, InvMPU.gyro_z_bias)
            break;
        } else {
            basic.showIcon(IconNames.Angry)
        }
    }
}
basic.showIcon(IconNames.Yes)
while (true) {
    InvMPU.read_gyro()
    InvMPU.read_accel()
    angle = Trig.atan2(InvMPU.accel_z, 0 - InvMPU.accel_x)
    serial.writeLine("Gyro: " + InvMPU.gyro_x + " " + InvMPU.gyro_y + " " + InvMPU.gyro_z + " Angle: " + angle + " Accel: " + InvMPU.accel_x + " " + InvMPU.accel_y + " " + InvMPU.accel_z)
    basic.pause(100)
}

Coming up, code for the self-balancing robot.

Public domain. 

Use at your own risk!