Description

This project was completed as an early step towards creating an IMU-based motion capture suit. Readings from the accelerometer and gyroscope are used to calculate the IMU's current rotation quaternion by integrating over small time steps. The four quaternion values, stored as floating point numbers, and are sent over Bluetooth Low Energy to a PC. A program on the PC unpacks the quaternion components and sets the rotation of a cube displayed on the screen. I use node webkit to create a UI in HTML and the three.js library for hardware accelerated graphics. This project was completed both by using an mbed LPC1768, Bluefruit LE UART, and LSM9DS1 IMU as well as by using a custom nRF52832 board for BLE and another custom board with an MPU6050 IMU accessed over I2C. The performance of the two systems are compared.

Mbed System

The mbed LPC1768 begins by calibrating the IMU. It then quickly reads the accelerometer and gyroscope values, noting the elapsed time between successive reads. These values are then input to MadgwickQuaternionUpdate function which I adapted from here. This function estimates the quaternion's deriviative and applies it over the time slice. The magnetometer values were found to be too unreliable so I decided to use a six axis algorithm instead of a nine axis one. The quaternion values are sent over a serial connection to the UART as quickly as possible. Unfortunately, the Bluefruit LE UART performs horribly. I could only send about 2 packets (40 bytes) per second! Furthermore, the data was not always reliably transmitted and was often fragmented into multiple packets making it harder to parse. The rotation quaternion can be reset and the IMU recalibrated by sending the character 'r' from the computer to the board.

Video Demo

Custom System

Here, I use an MPU6050 IMU and send data over BLE with a nRF52832. The algorithm is very similar to the one described above. However, using a nRF52832 allowed me to ensure data was never fragmented across multiple packets and gave me better control over the packet interval. These two changes drastically increased throughput and allow me to send closer to 3000 bytes per second. Furthermore, the MPU6050 seems to give better results than the LSM9DS1.

Video Demo ( Better! )

Node Webkit Code

I use node webkit and three.js to create my user interface. In three.js, I set the camera so that the z-axis is up to match the IMU orientation and add some lights to the scene. I then create a cube that approximates the dimensions of the breadboard. I use the node.js library nrfuart, which works with both my custom system and the mbed system. Upon receiving data, I use a binary parser to unpack it and set the cube's quaternion to that which was received. Upon clicking the "Reset" button, an 'r' character is sent to the board so it knows to reset its rotation and recalibrate.

Here is a link to the project on github.

Import Code

#include "LSM9DS1.h"
#include "MadgwickUpdate.h"

Serial pc(USBTX, USBRX);

Timer t;
int lastPrint = 0;
// print to serial and send to BLE every 500 ms
int printInterval = 500;

RawSerial dev( p13, p14 );

int calibrate = 1;

void dev_recv()
{
    while(dev.readable()) {
        if ( dev.getc() == 'r' ){
                calibrate = 1;
        }
    }
}

int main() {
    
    LSM9DS1 lol(p28, p27, 0xD6, 0x3C);
    
    t.start();

    dev.baud(9600);
    dev.attach(&dev_recv, Serial::RxIrq);

    lol.begin();
    if (!lol.begin()) {
        pc.printf("Failed to communicate with LSM9DS1.\n");
    }

    while(1) {

        if( calibrate ){
            lol.calibrate();
            firstUpdate = t.read_us();
            lastUpdate = firstUpdate;
            q[0] = 1.0f; q[1] = 0.0f; q[2] = 0.0f; q[3] = 0.0f; 
            calibrate = 0;
        }
        
        if ( lol.accelAvailable() || lol.gyroAvailable() ) {
         
            lol.readAccel();
            lol.readGyro();
            
            Now = t.read_us();
            deltat = (float)((Now - lastUpdate)/1000000.0f);
            lastUpdate = Now;
            
            // convert from int16 to float
            float ax = lol.calcAccel(lol.ax);
            float ay = lol.calcAccel(lol.ay);
            float az = lol.calcAccel(lol.az);
            
            float gx = lol.calcGyro(lol.gx);
            float gy = lol.calcGyro(lol.gy);
            float gz = lol.calcGyro(lol.gz);
            
            // switch x and y to convert to right handed coordinate system. Convert gyro readings to radians.
            MadgwickQuaternionUpdate( ay, ax, az, gy*PI/180.0f, gx*PI/180.0f, gz*PI/180.0f );
            
            int nowMs = t.read_ms();
            if( nowMs - lastPrint > printInterval ){
                lastPrint = nowMs;
                pc.printf("gyro: %f %f %f\n\r", gx, gy, gz);
                pc.printf("accel: %f %f %f\n\r", ax, ay, az);
                pc.printf("quat: %f %f %f %f\n\r", q[0], q[1], q[2], q[3]);
                char* str = (char*) &q;
                int i = 0;
                while (i < 16){
                    dev.putc( str[i] );
                    i += 1;   
                }
            }
            
        }
    }
}

Node Webkit Code

<!DOCTYPE html>
<html lang="en">
    <head>
        <title>three.js webgl - geometry - cube</title>
        <meta charset="utf-8">
        <style>
          body {
            margin: 0px;
            background-color: #000000;
            overflow: hidden;
          }
        </style>
    </head>
    <body>

    <button type="button" onclick="send('r')" style="font-size: 150%;">Reset</button>

    <script src="three.min.js"></script>
    <script src="OrbitControls.js"></script>
    <script>

    var camera, scene, renderer, controls;
    var mesh;

    init();
    animate();

    function init() {

        camera = new THREE.PerspectiveCamera( 70, window.innerWidth / window.innerHeight, 1, 1000 );
        camera.position.x = 400; 
        camera.position.z = 50
        camera.up = new THREE.Vector3(0,0,1); // set z axis to up direction

        scene = new THREE.Scene();

        var geometry = new THREE.BoxGeometry( 200, 250, 50 );
        var material = new THREE.MeshLambertMaterial( { color: 0xFF0000 } );

        mesh = new THREE.Mesh( geometry, material );
        scene.add( mesh );

       // create dim white ambient light
        var light = new THREE.AmbientLight( 0x404040 );
        scene.add( light );

       // create light coming from front right
        var light = new THREE.DirectionalLight( 0xffffff, 3 );
        light.position.set( 0, 50, 140 ).multiplyScalar( 1.1 );
        scene.add( light );

        renderer = new THREE.WebGLRenderer();
        renderer.setPixelRatio( window.devicePixelRatio );
        renderer.setSize( window.innerWidth, window.innerHeight );
        document.body.appendChild( renderer.domElement );

        controls = new THREE.OrbitControls( camera, renderer.domElement );
        controls.enableDamping = true;
        controls.dampingFactor = 0.25;
        controls.enableZoom = false;

        window.addEventListener( 'resize', onWindowResize, false );

    }

    function onWindowResize() {

        camera.aspect = window.innerWidth / window.innerHeight;
        camera.updateProjectionMatrix();

        renderer.setSize( window.innerWidth, window.innerHeight );

    }


    function animate() {

        requestAnimationFrame( animate );

        controls.update();

        renderer.render( scene, camera );

    }


    var nrfuart = require('nrfuart');
    var Parser = require('binary-parser').Parser;

    var parser = new Parser().floatle('q1').floatle('q2').floatle('q3').floatle('q4');

    var send = function(){};

    nrfuart.discoverAll(function(ble_uart) {
        
       // notify disconnection 
        ble_uart.on('disconnect', function() {
            console.log('disconnected!');
        });
     
        // connect and setup 
        ble_uart.connectAndSetup(function() {
     
            send = function( data ) {
                ble_uart.write( data, function() {
                    console.log('data sent:', data);
                });
            }
     
            ble_uart.readDeviceName(function(devName) {
                console.log('Device name:', devName);
            });
            
            ble_uart.on('data', function(data) {
                console.log( data.toString().length );
                // need to check data length since Bluefruit fragments data
                if( data.toString().length >= 16 ){
                    var obj = parser.parse(data);
                    mesh.quaternion.set(obj.q2, obj.q3,obj.q4,obj.q1);   
                }
            });
        });
    });
    </script>

    </body>
</html>