MPU6050 IMU

With all this rage about using and programming drones and other robots, the word “IMU” has become to be understood by many persons. An IMU or Inertial Measurement Unit is an important component of robotic systems. The IMU is essentially uses and “fuses” together data from other sensors which is then used to track things such as position, orientation and velocity. In robotic systems and frameworks such as ROS, the data used to measure these things is in the form of quaternion data. Also any device that tracks movement can benefit from using quaternions to determine orientation.

As such a low cost IMU such as the MPU6050 can be used for prototyping and low cost robotics. The MPU6050 is very low cost and readily available from a variety of sources. In this post we look at how we can use the MPU6050 to generate the quaternion data needed for accurate orientation measurement for your robots and devices. Skipping all the complex theory, I will explain what quaternions are and how we can use the MPU6050 to generate quaternion output with the STM32F103C8T6. The principles in this post will apply to any IMU as we only use the MPU 6050 for it’s accelerometer and gyroscope readings and do not use the on board Digital Motion Processing Unit (DMP) calculations.

Understanding Accelerometers, Magnetometers and Gyroscopes

I despise persons who try to explain everything with giving an explanation of how it works, it’s like teaching someone to drive a car and teaching them about when to apply brakes by first covering fluid mechanics and material science. That is not fun. All people need to know is that the gas pedal makes the car go and the brake pedal makes the car stop, however if they still insist you explain how a braking system works, you can do so without resorting to the fluid mechanics explanation.

In that frame of mind let’s begin our discussion on quaternions by looking at the sensors responsible for generating the data, which are accelerometers, gyroscopes and magnetometers.

Gyroscopes

In one line one can say that gyroscopes measure rotation. Stepping it up a notch, we can say that gyroscopes measure angular velocity. What this means is that gyros measure the rate of change of the angle of a body which is spinning. So gyroscopes essentially measure the rotation of something relative to 3 axes, the X, Y and Z axes. The image below best explains this.

Gyro X, Y, Z

The gyroscope will measure the rate at which something spins about the particular axis in degrees or radians per second. Let me explain this more simply. If you have a wheel that makes one turn every second, you  can say that the wheel turns at a rate of one turn per second, or 360 degrees per seconds or 6.28319 radians per second.

It was common to have gyroscopes that measure rotation about one, two or three axes. However in the MPU6050, the gyroscope can measure rotation about each of the 3 axes.

Accelerometer

The accelerometer in simple terms measures linear movement. We can also say that accelerometers measure the rate at which the velocity of an object changes, which is usually measured in G-forces or meters per second per second (or m/s squared) which is essentially the force acting on a body. Gravity is essentially a “static” force which means that it remains the same regardless of the point of time at which you take the measurement. I use static within quotations, because it is important to remember that  gravity can change from place to place, and nothing is ever truly static. Though in some cases the changes are so minute, they can be ignored (non-standard analysis with the infinitesimal method of differentiation anyone!?). Anyway enough math, back to accelerometers, they not only measure static forces but also dynamic forces such as movement or vibration.

Accelerometer X, Y, Z

As is illustrated in the image above, they can measure acceleration relative to each of the axes above, X, Y or Z. Some applications may require only one axes and others may use all three. Since an accelerometer can measure G’s it can be used for fall detection as the G forces acting on each axis will be zero during a free fall. They can also be used to measure the distance traveled by an object by using an accelerometer.

Magnetometer

Though we will not utilize magnetometers in our application they are an important component that must be understood as they are found in some IMU’s. A magnetometer measures magnetic flux density or magnetic fields. We must remember that the earth has a magnetic field, so the magnetometer can be used as a compass, however I refrain from using them because despite being very accurate and having no drift, as they can be affected by other magnetic fields (such as motors and speakers) which requires more calculations to compensate for the presence of these fields, for robotic systems, unless you absolutely need magnetometers avoid them. IF you do need an IMU with magnetometer readings, you can use the LSM9DS1 which is very reliable, has a lot of libraries and provides accurate readings.


Sensor Fusion, Filters, Eular angles and Gimbals

Sensor Fusion and Filters

So ignoring the magnetometer, we have accelerometers that measure linear movement and gyroscopes that measure rotation about axes. Using measurements from each sensor has it’s faults. When tracking and measuring orientation, gyroscopes are accurate but the values tend to drift over time, the accelerometer is less accurate for orientation measurement because whilst it does not drift over time, it does suffer from the problem of processing the tiniest movement and gives readings that are very very jittery.

This problem is overcome by taking the value of these two values and “fusing” the data together by means of a special “algorithm” known as a digital filter (there are also analog electronic filters, but that’s another talk). For all you non-DSP guys a filter is essentially a mathematical operation performed on sampling some data over points in time. In our case the sensor data from the gyroscope and accelerometer to give some desired result. There are many methods of doing this filtering, including complimentary filters, Kalman filters and Madgwick filter. Each filter has it’s strength and weaknesses, which you can decide on based on your application.

Eular Angles and Gimbals

Eular Angles

Now math can be boring sometimes, so let me explain this section as simply as possible. A very smart guy named Eular came up with the idea of a way of representing rotation using angles, and he essentially said that we can represent any orientation of a body by using three basic types of rotations about three axes as pictured above. These are rotations represented about the axis associated each degree of freedom of the device. Now there can be a lot of complex mathematics to describe what is illustrated above, or I could use this picture below as it relates to robotics.

Roll. Pitch. Yaw

Instead of thinking of Eular angles in terms of alpha, beta and gamma on a coordinate plane,  lets think of it in terms of “roll”, “pitch” and “yaw” of a missile!. So the roll, pitch and yaw can be thought of as illustrated above, with rotation about each of the axis, controlling the orientation of the missile. Keep this in mind and look at the image below.

Roll Pitch Yaw Gimbals

Now that you understand roll, pitch and yaw, we can look think of it in terms of gimbals, if we imagine the green, orange and blue semi-circles illustrated above as full-circles or each circle as a torus controlling the yellow, red and purple rods. Now each circles can be thought of as an “object” called a gimbal which is a support that allows a body to turn about a particular axis, think of a gimbal like being mounted to the object like shown above. If we turn a gimbal, we turn a rod and since all the rods are mounted together, we get rotation of the object about each of the gimbal axis. Assuming all the gimbals are initially set to zero, if we know how much we “turn” a gimbal, we can precisely determine the orientation of the object with respect to rotation about each axis.

 

Rotating Gimbals

 

Picture the rotating gimbals above and visualize what is happening. Now when two of the gimbals are turned in such a way that they a parallel to each other, then essentially you can think of the two gimbals as “becoming one” (this is really an over-simplification only used for visualization purposes), now since these two have become one, you are left with only two total gimbals. For example if the pitch gimbal rotates parallel to the roll gimbal so that it becomes parallel to the yaw gimbal,  then if you rotate the yaw gimbal, it would have no effect on the object attempting to be rotated. This is known as gimbal lock and causes a lot of real world problems when you are trying to measure orientation and essentially occurs when any two axes line up.

 


 

Quaternions

Since gimbal lock causes problems when you are working on problems that involve orientation, a quaternion or for our case more specifically since we focus on spatial rotation a unit quaternion uses representation of complex numbers and builds on top of complex numbers adding quaternion units into the mix, without going into the complexities, just know that while Euler angles (roll, pitch, yaw), is related to orientation around an axis, unit quaternion values are related to rotation in relation to two quaternion. Using unit quaternions to determine spatial orientation adds an additional rotational axis to the mix and are actually easier to work with as they give more accurate results.

So essentially a unit quaternion for the purpose of spatial rotation can be thought of as a 4 dimensional numbers used to represent 3-dimensional rotation which for our purposes are used to eliminate the problems associated with gimbal lock. They are usually specified as 4 values q0, q1, q2 and q3 or sometimes X, Y, Z, W. If’s hard to picture a 4th spatial dimension, unless we look at it in relation to matrices.

If you think this was too brief, then have a read here.

Now that we have the gist of what quarternions are lets get to the fun part, getting it from the IMU.


Selecting a Filter Algorithm

Now as I explained earlier there are many different filters that we can use to fuse the sensor data of the gyroscope and accelerometer to obtain the type of data we are interested in whether it be Eular angles or unit quaternion values, we can use a complementary filter, a Kalman filter or a Madgwick filter. I personally prefer the Madgwick filter because it is very simple to work with and has been personally tested by me across a lot of C compilers and I am yet to come across one that will not compile the code. It also compiles with the MikroC  compiler for ARM without any errors and works out of the box, should you require more information on the Madgwick filter you could read about it here.


C Program

 

/*
 * File: MPU6050.c
 * Author: Armstrong Subero
 * uC: STM32F103C8T6 w/PLL OSC @ 72 MHz, 3.3v
 * Program: I22_MPU6050
 * Compiler: MikroC Pro v6.0.0
 * Program Version: 1.0
 *
 * Program Description: This Program Allows STM32F103C8T6 to be interfaced with
 *                      the MPU6050 and reads the IMU and performs calculations
 *                      using the Madgwick filter to produce unit quaternion
 *                      values that can be used to determine spatial orientation
 *
 * Hardware Description: A MPU6050 is connected to the I2C bus and a CP2104 is
 *                       connected to UART1
 *
 * Created April 13th, 2018, 1:27 AM
 */

 /******************************************************************************
 * Includes and Defines
 ******************************************************************************/
#include "stdbool.h"
#include <built_in.h>
#include <stdio.h>
#include <stdlib.h>
#include "MPU6050.h"
#include "MadgwickAHRS.h"

// Volatile Variables
volatile int   GYRO_XOUT, GYRO_YOUT, GYRO_ZOUT;
volatile int   ACCEL_XOUT, ACCEL_YOUT, ACCEL_ZOUT;

// Global variables
double acc_x;
double acc_y;
double acc_z;
double gyro_x;
double gyro_y;
double gyro_z;

int info;
char buf[25];
int cal_var = 0;

double   gyro_x_offset;
double   gyro_y_offset;
double   gyro_z_offset;

char gyro_x_out[10];
char gyro_y_out[10];
char gyro_z_out[10];
char accel_x_out[10];
char accel_y_out[10];
char accel_z_out[10];

// quaternion values
char q0_t[10];
char q1_t[10];
char q2_t[10];
char q3_t[10];


// Writes to the IMU via I2C1
void MPU6050_Write(unsigned char addr, int raddr, int rdata)
{
  buf[0] = raddr;                          // register address
  buf[1] = rdata;                          // register data
  I2C1_Start();                            // issue I2C start signal
  I2C1_Write(addr,buf,2,END_MODE_STOP);    // write data via I2C bus
}

// Test to ensure MP6050 is connected
int MPU6050_Test(void)
{
    buf[0] =  MPU6050_RA_WHO_AM_I;
    I2C1_Start();
    I2C1_Write(MPU6050_ADDRESS, buf, 1, END_MODE_RESTART);
    I2C1_Read(MPU6050_ADDRESS, buf, 1, END_MODE_STOP);

    if(buf[0] == 0x68)
    {
        return 1;
    }
    else
    {
        return 0;
    }
}

// Read  the IMU via I2C1
int MPU6050_Read(unsigned short addr, int raddr)
{
    buf[0] = raddr;
    I2C1_Start();
    I2C1_Write(addr,  buf, 1, END_MODE_RESTART);
    I2C1_Read (addr,  buf, 1, END_MODE_STOP);
    
    return buf[0];
}

// Initializes the IMU
void MPU6050_Init(void)
{
    // Wakeup from sleep
    MPU6050_Write(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_1, 0x00);
    
    // sample rate = 8kHz / 1 + 109 = 72.7Hz
    MPU6050_Write(MPU6050_ADDRESS,  MPU6050_RA_SMPLRT_DIV, 109);

    // Write Accelerometer to +\- 4g
   MPU6050_Write(MPU6050_ADDRESS, MPU6050_RA_ACCEL_CONFIG, 0x00);

   // gyro scale of 2000 degrees/s
   MPU6050_Write(MPU6050_ADDRESS, MPU6050_RA_GYRO_CONFIG, 0x18);

   // Disable Frame Sync, Set Digital Low pass filter to 256Hz DLPF
   MPU6050_Write(MPU6050_ADDRESS, MPU6050_RA_CONFIG, 0x00);

    // Disable sensor output to FIFO buffer
   MPU6050_Write(MPU6050_ADDRESS, MPU6050_RA_FIFO_EN, 0x00);

    // AUX I2C setup, single master control
   MPU6050_Write(MPU6050_ADDRESS, MPU6050_RA_I2C_MST_CTRL, 0x00);
   
    // Setup INT pin and AUX I2C pass through
   MPU6050_Write(MPU6050_ADDRESS, MPU6050_RA_INT_PIN_CFG, 0x00);

    // Enable data ready interrupt
   MPU6050_Write(MPU6050_ADDRESS, MPU6050_RA_INT_ENABLE, 0x00);
}

// Read the raw accel vlaue from the IMU
int MPU6050_Accel_Raw(char axis)
{
    //Variables for high and low accel values
    int   ACCEL_XOUT_H, ACCEL_XOUT_L, ACCEL_YOUT_H;
    int   ACCEL_YOUT_L, ACCEL_ZOUT_H, ACCEL_ZOUT_L;

    // Read accelerometer values out from H and L registers
    ACCEL_XOUT_H = MPU6050_Read(MPU6050_ADDRESS, MPU6050_RA_ACCEL_XOUT_H);
    ACCEL_XOUT_L = MPU6050_Read(MPU6050_ADDRESS, MPU6050_RA_ACCEL_XOUT_L);
    ACCEL_YOUT_H = MPU6050_Read(MPU6050_ADDRESS, MPU6050_RA_ACCEL_YOUT_H);
    ACCEL_YOUT_L = MPU6050_Read(MPU6050_ADDRESS, MPU6050_RA_ACCEL_YOUT_L);
    ACCEL_ZOUT_H = MPU6050_Read(MPU6050_ADDRESS, MPU6050_RA_ACCEL_ZOUT_H);
    ACCEL_ZOUT_L = MPU6050_Read(MPU6050_ADDRESS, MPU6050_RA_ACCEL_ZOUT_L);
    
    // Convert received data
    ACCEL_XOUT = ( ACCEL_XOUT_H<<8 | ACCEL_XOUT_L );
    ACCEL_YOUT = ( ACCEL_YOUT_H<<8 | ACCEL_YOUT_L );
    ACCEL_ZOUT = ( ACCEL_ZOUT_H<<8 | ACCEL_ZOUT_L );
    

    // Return value to user based on reqiested axis
    switch (axis)
    {
        case 'X':
            return ACCEL_XOUT;    //  Accel X Axis
            break;
        case 'Y':
            return ACCEL_YOUT;    // Accel Y Axis
            break;
        case 'Z':
            return ACCEL_ZOUT;    //  Accel Z Axis
            break;
        default:
            return 0;
    }
}


// Read the accel value and convert to G's
double MPU6050_Read_Accel(char axis)
{
    // Variables for high and low accel values
    int  ACCEL_XOUT_H, ACCEL_XOUT_L, ACCEL_YOUT_H;
    int  ACCEL_YOUT_L, ACCEL_ZOUT_H, ACCEL_ZOUT_L;
    
    float  GFORCEX;
    float  GFORCEY;
    float  GFORCEZ;

    // Read accelerometer values out from H and L registers
    ACCEL_XOUT_H = MPU6050_Read(MPU6050_ADDRESS, MPU6050_RA_ACCEL_XOUT_H);
    ACCEL_XOUT_L = MPU6050_Read(MPU6050_ADDRESS, MPU6050_RA_ACCEL_XOUT_L);
    ACCEL_YOUT_H = MPU6050_Read(MPU6050_ADDRESS, MPU6050_RA_ACCEL_YOUT_H);
    ACCEL_YOUT_L = MPU6050_Read(MPU6050_ADDRESS, MPU6050_RA_ACCEL_YOUT_L);
    ACCEL_ZOUT_H = MPU6050_Read(MPU6050_ADDRESS, MPU6050_RA_ACCEL_ZOUT_H);
    ACCEL_ZOUT_L = MPU6050_Read(MPU6050_ADDRESS, MPU6050_RA_ACCEL_ZOUT_L);

    // Convert received data
    ACCEL_XOUT = ( ACCEL_XOUT_H<<8 | ACCEL_XOUT_L );
    ACCEL_YOUT = ( ACCEL_YOUT_H<<8 | ACCEL_YOUT_L );
    ACCEL_ZOUT = ( ACCEL_ZOUT_H<<8 | ACCEL_ZOUT_L );
    
    // Calculate G-Force Data
    GFORCEX = (ACCEL_XOUT  / G_CALC_VAL);
    GFORCEY = (ACCEL_YOUT  / G_CALC_VAL);
    GFORCEZ = (ACCEL_ZOUT  / G_CALC_VAL);

    // Return G's value to user
    switch (axis)
    {
        case 'X':
            return GFORCEX;    //  Gforce X Axis
            break;
        case 'Y':
            return GFORCEY;    // Gforce Y Axis
            break;
        case 'Z':
            return GFORCEZ;    //  Gforce Z Axis
            break;
        default:
            return 0;
    }
}

// Read the raw gyro data from the IMU
int MPU6050_Gyro_Raw(char axis)
{
    int  GYRO_XOUT_H, GYRO_XOUT_L;
    int  GYRO_YOUT_H, GYRO_YOUT_L;
    int  GYRO_ZOUT_H ,GYRO_ZOUT_L;

    /* Read data from gyroscope */
    GYRO_XOUT_H = MPU6050_Read(MPU6050_ADDRESS, MPU6050_RA_GYRO_XOUT_H);
    GYRO_XOUT_L = MPU6050_Read(MPU6050_ADDRESS, MPU6050_RA_GYRO_XOUT_L);

    GYRO_YOUT_H = MPU6050_Read(MPU6050_ADDRESS, MPU6050_RA_GYRO_YOUT_H);
    GYRO_YOUT_L = MPU6050_Read(MPU6050_ADDRESS, MPU6050_RA_GYRO_YOUT_L);
    
    GYRO_ZOUT_H = MPU6050_Read(MPU6050_ADDRESS, MPU6050_RA_GYRO_ZOUT_H);
    GYRO_ZOUT_L = MPU6050_Read(MPU6050_ADDRESS, MPU6050_RA_GYRO_ZOUT_L);

    // Convert received data
    GYRO_XOUT = ( GYRO_XOUT_H << 8 | GYRO_XOUT_L );
    GYRO_YOUT = ( GYRO_YOUT_H << 8 | GYRO_YOUT_L );
    GYRO_ZOUT = ( GYRO_ZOUT_H << 8 | GYRO_ZOUT_L );

    // Return value to user
    switch (axis)
    {
        case 'X':
            return GYRO_XOUT;    // Gyro X Axis
            break;
        case 'Y':
            return GYRO_YOUT;    //  Gyro Y Axis
            break;
        case 'Z':
            return GYRO_ZOUT;    //  Gyro Z Axis
            break;
        default:
            return 0;
    }
}


// Main
void main() {

    // Initialize the module at 230400 baud with no parity bit, one stop bit at UART1
    UART1_Init(230400);
    // Wait for UART module to stabilize
    Delay_ms(100);

    //Initialize the MPU6050 module with 400Kbps speed on PORTB
    I2C1_Init_Advanced(400000, &_GPIO_MODULE_I2C1_PB67);
    Delay_ms(1000);
    
    // Let us know everything is OK
    UART1_Write_Text("Pre Init Okay");
    UART1_Write(13);
    
    // Initialize the MPU6050 module
    MPU6050_Init();
    
    info = MPU6050_Test();
      
    if (info == 1)
    {
        UART1_Write_Text("MPU6050 OK");
        UART1_Write(0x0A); //Line Feed LF
        UART1_Write(0x0D); //Return Car CR
    }

     else
     {
        UART1_Write_Text("MPU6050 Failed");
        UART1_Write(0x0A); //Line Feed LF
        UART1_Write(0x0D); //Return Car CR
     }
      
      // give sensor a second to stabilize
      Delay_ms(1000);
      
      // Calculate gyro offset
      for (cal_var = 0; cal_var <= 64; cal_var++)
      {
        gyro_x_offset += MPU6050_Gyro_Raw('X');
        gyro_y_offset += MPU6050_Gyro_Raw('Y');
        gyro_z_offset += MPU6050_Gyro_Raw('Z');
        
        Delay_ms(1);
      }
      
      // Determine gyro offset for 64 read values
      gyro_x_offset /= 64;
      gyro_y_offset /= 64;
      gyro_z_offset /= 64;
      
    while(1)
    {
       // Read gyro values and convert to Radians per second
       gyro_x  =  (MPU6050_Gyro_Raw('X')  - gyro_x_offset)  / 939.650784f;
       gyro_y  =  (MPU6050_Gyro_Raw('Y')  - gyro_y_offset)  / 939.650784f;
       gyro_z  =  (MPU6050_Gyro_Raw('Z')  - gyro_z_offset)  / 939.650784f;
       
       // Read accel values  and convert relative to 1G
       acc_x = MPU6050_Read_Accel('X') / 2.0f;
       acc_y = MPU6050_Read_Accel('Y') / 2.0f;
       acc_z = MPU6050_Read_Accel('Z') / 2.0f;

       //////////////////////////////
       // Display Accel Values
       ///////////////////////////////
       UART1_Write_Text("  AccelX: ");
       sprintf(accel_x_out, "%7.2f", acc_x);
       UART1_Write_Text(accel_x_out);

       UART1_Write_Text("  AccelY: ");
       sprintf(accel_y_out, "%7.2f", acc_y);
       UART1_Write_Text(accel_y_out);

       UART1_Write_Text("  AccelZ: ");
       sprintf(accel_z_out, "%7.2f", acc_z);
       UART1_Write_Text(accel_z_out);

       UART1_Write(0x0A); //Line Feed LF
       UART1_Write(0x0D); //Return Car CR
       
       /////////////////////////////////
       // Display Gyro Values
       /////////////////////////////////
       UART1_Write_Text("  GyroX: ");
       sprintf(gyro_x_out, "%7.2f", gyro_x);
       UART1_Write_Text(gyro_x_out);
       
       UART1_Write_Text("  GyroY: ");
       sprintf(gyro_y_out, "%7.2f", gyro_y);
       UART1_Write_Text(gyro_y_out);
       
       UART1_Write_Text("  GyroZ: ");
       sprintf(gyro_z_out, "%7.2f", gyro_z);
       UART1_Write_Text(gyro_z_out);
       
       UART1_Write(0x0A); //Line Feed LF
       UART1_Write(0x0D); //Return Car CR
       
       /////////////////////////////////////
       // Apply Madgwick filter on values
       ////////////////////////////////////
       MadgwickAHRSupdateIMU(gyro_x, gyro_y, gyro_z, acc_x, acc_y, acc_z);
       
       ///////////////////////////////////
       // Send unit quaternion values
       //////////////////////////////////
       UART1_Write_Text("  Q0: ");
       sprintf(q0_t, "%7.2f", q0);
       UART1_Write_Text(q0_t);
       
       UART1_Write_Text("  Q1: ");
       sprintf(q1_t, "%7.2f", q1);
       UART1_Write_Text(q1_t);
       
       UART1_Write_Text("  Q2: ");
       sprintf(q2_t, "%7.2f", q2);
       UART1_Write_Text(q2_t);
       
       UART1_Write_Text("  Q3: ");
       sprintf(q3_t, "%7.2f", q3);
       UART1_Write_Text(q3_t);
       
       UART1_Write(0x0A); //Line Feed LF
       UART1_Write(0x0D); //Return Car CR
       
       Delay_ms(1);
   }
    
}

Results

Sample Accelerometer, Gyro and Quaternion Output Values

 

Test Circuit

 

Conclusion

The MPU6050 is a low cost sensor that provides relatively accurate gyroscope and accelerometer readings and when a proper filtering algorithm is used, you can obtain accurate data for tracking spatial orientation with very little hardware.

You can download the code from libstock for Mikroelektronika here

Leave a Reply

Your email address will not be published. Required fields are marked *