IMU Sampling using the Raspberry Pi

In this post, I’ll describe the lessons learnt from trying to sample IMU sensors to obtain raw gyroscope and accelerometer data as input to sensor fusion algorithms using the Raspberry Pi.

Pose (orientation) computation by combining angular velocity data collected from gyroscopes and acceleration data from accelerometers is an important first step in a variety of AR/VR applications. The sensor fusion is typically done using complimentary filters or kalman filters. High speed, constant rate sampling of the sensor data is important for optimal performance of the sensor fusion algorithms.

In our experiments, we took an MPU6050 and MPU9250 sensor from Invensense and connected them to a Raspberry Pi 3 as shown in the figure below. The MPU 9250 was selected as it hosts a magnetometer in addition to the accelerometer and gyroscope.

The second IMU (MPU 6050) is also connected to the SCL and SDA lines. It is possible to connect multiple I2C devices on the same lines as long as they have different addresses. The default address of the MPU 6050/9250 is 0x68. This can be changed to 0x69 by connecting ADO pin to VCC. The interrupt pin of the second IMU is connected to GPIO 27 (Pin 13).

To verify that the IMUs are registered properly, run i2cdetect -y 1 on the Pi console. You should see the two devices at 0x68 and 0x69.

Software Architecture

To set my interrupt service routines and receive sensor data over the I2C bus, I use the WiringPi library by Gord)on (http://wiringpi.com/). The library provides convenient read/write functions that allow for read/writes from/to specific registers on I2C devices connected to the Pi and registering interrupt service routines to receive callbacks when interrupts arrive at a GPIO pin.

For reading data from the MPU9250, I took the corresponding code for the MPU6050 developed by Jeff Rowberg and re implemented the functions that I needed such as setting the gyro/accel low pass filter settings and sampling rates, obtaining the raw sensor values by reading the appropriate registers and setting various interrupt flags. Note that while most of architecture (registers, bit locations, bit lengths etc) is the same for the MPU6050/9250, there are some differences. For example, the MPU9250 allows for setting a low pass filter on the accelerometer and the gyroscope independently. The bandwidth and delay values for the various low pass filter settings are also slightly different between the two MPU’s.

The implementation of some of the functions is shown below. The variable fd shown in the code below refers to the descriptor returned after calling wiringPiI2CSetup(addr). The variable addr is the address of the I2C device (0x68 or 0x69).

Jeff Rowberg’s implementation of the MPU6050 driver is intended for the Arduino hardware and uses the Arduino I2CDev library for register read/write operations. The I2CDev library in turn uses the Arduino Wire library for the low level I2C operations. Since my implementation of the MPU9250 driver is intended to run on the Raspberry Pi, I reimplemented portions of the I2CDev library for the Pi and rerouted the low level register access functions through the WiringPi library. Some relevant code samples are shown below.

Setting up Interrupts

Interrupts provide a convenient mechanism to receive a notification when data from the sensors is ready to be read and thereby eliminates the need to constantly poll the sensors for data, freeing up your program to do other things while waiting for a fresh batch of sensor data. Setting up interrupts require setting the correct configuration for the sensor that will be sending the interrupt and for the host processor that will be receiving the interrupts.

Interrupt Settings for the MPU9250

Data ready interrupts can be configured on the MPU9250 as follows. This should be done in your initialization function for the MPU9250.

Receiving Interrupts on the Pi

The WiringPi library makes it simple to set up a callback function (interrupt service routine) that will be called when an interrupt arrives. This can be configured as shown below.

Note that you must read the interrupt register by calling getIntStatus() to clear the interrupts. Without this, the interrupts will not occur again.

Changing the default I2C Speed

The default I2C baudrate on the Pi 3 is 100Kbps (kilo bits (not bytes) per second). At this speed, clearing the interrupt register and reading the IMU data (14 bytes; 3 16 bit gyros, 1 16 bit temperature, 3 16 bit accels) takes about 6.5ms, which is unacceptably slow. Since the data is read while processing the ISR, the slow speed of the data read operation imposes an upper bound of 150Hz on the interrupt frequency. It is possible to increase the default I2C baudrate by modifying the /boot/config.txt file as shown below.

myscreen

You need to reboot the pi for the setting to take effect.

The ISR processing time for three different values of the I2C baudrates is shown in the table below.

Baud Rate (bits/sec) Processing Time (milliseconds)
32000 19.5
100000 (default) 6.5
400000 1.7

An interrupt service routine processing time of 1.7ms enables us to achieve an interrupt frequency of around 500Hz, which is adequate for most high speed sampling applications. It is interesting to note that the processing times are much slower than what a simple data rate calculation given the baudrate would suggest. For example, at a baudrate of 100K, reading 14 bytes should take 14*8*1000/100000 = 1.12ms. However the actual processing time is almost 6 times higher. I’m not sure why this is so. If anyone knows, please let me know!

Interrupt Timing Issues

For efficient implementation and optimal performance of sensor fusion algorithms, it is important to be able to achieve constant rate sampling i.e., achieve a constant time interval between two samples. Since we are using IMU interrupts as our sampling mechanism, the constant rate sampling requirement means being able to receive interrupts at a constant rate. Receiving events at a constant rate can be problematic on non realtime operating systems such as Raspbian where multiple processes are running concurrently and the scheduler can schedule out the process hosting the interrupt service routine at will. To investigate these timing issues, we logged the time interval between two interrupts for different interrupt frequencies. The results are shown below. The interrupt frequencies can be set by setting the appropriate sample rate divider using the setRate function in the MPU6050 driver. For example, a 100 Hz sampling frequency can be achieved by calling setRate(9).

As can be seen, the sampling rate generally stays constant with small fluctuations, but suffers from occasional large deviations.

To verify that the fluctuations are due to the non-real time nature of the Rasbian operating system and not due to some other hardware/software issue, I connected a MPU6050 IMU to an Arduino Mega and configured it to send interrupts at 250Hz. The graph of the time interval between interrupts and the mean and variance of the time interval data is shown below.

Mean Variance
4006.9 18.73 (0.47%)

So what can be done to achieve constant rate interrupts on the Pi?

Note that even at a sampling rate of 50Hz, there are large spikes in the inter-interrupt time. Thus, even if one performs high speed sampling plus simple data pre-processing such as low pass filtering on a dedicated microcontroller such as a Teensy or Arduino and send the averaged data to the Pi at a lower rate (say 50Hz), the problem of receiving this data at a constant rate remains. People have proposed applying the RT_PREEMPT patch and recompiling the Raspbian kernel to help achieve more deterministic timing. I have not tried this yet.

Visual GDB: A Great Visual Studio Plugin for working on the Pi

There is a never-ending debate about what is the most efficient way to develop on the Pi. An easy to use IDE with good editing and debugging support can have a dramatic impact on your productivity and help reduce frustration. When I started working on the Pi, I was developing directly on the Pi using codeblocks. While codeblocks is a usable IDE, it pales in comparison to the stability and features offered by Visual Studio. Furthermore, as fast as the Raspberry Pi 3 is with a 1.2GHz processor and 1GB Ram, working directly on the Pi can be frustrating as programs often freeze and occasionally crash, particularly if one is running multiple processor/memory heavy programs such as an internet browser. Wouldn’t it be nice if you could use Visual Studio running on your PC to develop, build and debug programs for the Pi?

It turns out that there is a great Visual Studio plugin called Visual GDB developed by a German company named Sysprogs (https://sysprogs.com/about/). Visual GDB allows you to develop your applications using the familiar and convenient environment of visual studio and build your application using either a locally installed ARM toolchain or directly on the Pi. The best feature of Visual GDB is a fantastic debugger integration, enabling you to set breakpoints and watch the value of program variables using the standard visual studio debugger. Indeed, one can often forget the program is actually executing on another computer! Visual GDB also offers a great integration with the visual studio output window, letting you see the output of your printf statements conveniently in the visual studio output window.

Visual GDB has a great set of tutorials on the website; if you would like to learn more, you should check out their tutorials and download a trial version. In the remainder of this post, I’ll touch upon a few issues that took me some time to figure out.

Visual GDB keeps the source files on your PC synced with the files on the Pi. If your source files are located in C:\MyProject\ on your PC, Visual GDB will copy these files to /tmp/VisualGDB/C/MyProject on the Pi. Any project dependencies such as include paths, shared lib names/paths can be specified in the appropriate cmake directives. For example, my project relies upon the WiringPi library. I installed the source for the WiringPi lib and built the binaries at /home/pi/3rdParty/wiringPi. I can specify the path for the header/lib files for the wiringPi library in my CMakeLists.txt as follows:

include_directories(/home/pi/3rdParty/wiringPi/devLib)
target_link_libraries(test_wiringPi -L”/home/pi/3rdParty/wiringPi/devLib” -lwiringPi)

If you are using the remote toolchain to build your program, you don’t need these dependencies installed on your PC.

Debugging your Program

When your are debugging your program, GDB will need to know the paths for any shared libraries (the Linux analogue of dlls) used by your program. These paths are set using the LD_LIBRARY_PATH variable. You can set this variable in Visual GDB Project Properties->Debug Settings. However, setting the LD_LIBRARY_PATH this way only works if you execute your program in the Visual GDB environment (for example by hitting F5 or Ctrl+F5) in Visual Studio. If you try to run the generated executable directly (coped by default to /tmp), you’ll get “can’t find .so file” error. You can set the path for your shared libraries by editing the /home/pi/.bashrc and add a export LD_LIBRARY_PATH line at the end. For example:

export LD_LIBRARY_PATH=/home/pi/3rdParty/wiringPi/devLib:LD_LIBRARY_PATH

Doing so makes the LIB_LIBRARY_PATH setting available for both GDB and standalone execution of your program. So you don’t need to set it separately in the Visual GDB Project Properties anymore.

5 Comments

  1. Hi, very good article. Are you making your code available? I came across this article searching how best / easiest way to access the magnetometer.

    • Sure, when I have some time, I’ll zip it up and post it. I’m not actively working on IMUs anymore, mostly working on deep learning now.

Leave a Reply to ankur6ue Cancel reply

Your email address will not be published.


*