y

This guide shows how to use a BerryIMU and a small TFT to create a digital compass, similar to those that can be found on smartphones.

• The TFT used in this guide is a PiScreen
• The IMU is a BerryIMU – Magnetometer,  gyroscope, accelerometer and pressure sensor
• SDL is used to display the output to the TFT
• Tilt compensations is used
• A low pass filter is used to reduce noise
• Compass calibration is needed

If you don’t a small TFT like the PiScreen, you can still use this guide to display the output to a monitor via HDMI.

Git repository here
The code can be pulled down to your Raspberry Pi with;

The code for this guide can be found under the compass_tutorial04_graphical_output directory.

Prerequisites for this guide;

• A working TFT (not covered in this guide)
• A working magnetometer (compass) with tilt compensation. A guide can be found here
• Understand how to perform Hard Iron calibration. A guide can be found here

We will be covering some basic SDL which will be used to produce our graphics.

The IMU used in this guide is the BerryIMU. However, other IMUs or accelerometers and gyroscopes can be used.. Eg Pololu MinIMU, Adafruit IMU and Sparkfun IMUs

Installing SDL

If you want to test to see if SDL installed correctly, you can create a file called test.c and copy in the code below;

#include <SDL/SDL.h>
int main(int argc, char** argv) {
        SDL_Init(SDL_INIT_EVERYTHING);
        SDL_Surface *screen;
        screen = SDL_SetVideoMode( 480, 320, 16, SDL_SWSURFACE );
        SDL_Rect rect;
        rect.x = 100;
        rect.y = 100;
        rect.w = 200;
        rect.h = 200;
        Uint32 color = SDL_MapRGB(screen->format, 0xff,0xff,0xff);
              SDL_FillRect(screen, &rect, color);
        SDL_Flip(screen);
        getchar();
        SDL_Quit();
        return 0;
}

The above code can be compiled with;

And to run the program;

You should see your screen go white or a white square should be shown.

Setup SDL and Load Images

We will be adding to the code which was already created in the Guide to interfacing a Gyro and Accelerometer with a Raspberry Pi guide.

As we are using SDL, we will need to include the SDL header files in our program;

#include "SDL.h"
#include "SDL/SDL_image.h"

When using SDL, you first have to initialize SDL and setup your surfaces before you can start displaying information on the screen. The next section shows how this is done.

The two lines below initialize SDL and sets it to not display a cursor;

        SDL_Init(SDL_INIT_VIDEO);
        SDL_ShowCursor(SDL_DISABLE);

We then use the SDL_GetVideoInfo() function to get information about our display. E.g. Resolution and bits per pixel,etc. This is stored in the structure ‘videoinfo’. To view what height was returned, we would use ‘videoInfo->current_h’

We then use this information to create our screen, using the SDL_SetVideoMode() function. This function needs these parameters, X Resolution, Y resolution, bits per pixel and display surface type. In the code below we are using a software surface for the display type.

        videoInfo = SDL_GetVideoInfo();
        screen = SDL_SetVideoMode(videoInfo->current_w, videoInfo->current_h, videoInfo->vfmt->BitsPerPixel, SDL_SWSURFACE );
        if ( screen == NULL ) {
                fprintf(stderr, "Unable to setvideo: %s\n", SDL_GetError());
                exit(1);
        }

Once we have setup SDL, we can start loading our images.

        outerRing =  IMG_Load("OuterRing.png");
        compassNeedle =  IMG_Load("CompassNeedle.png");
        if (outerRing == NULL || compassNeedle == NULL) printf("error\n");

After we load the image, which is a PNG, we need to convert it to the correct format so it keeps its transparency

        compatibleOuterRing = SDL_DisplayFormatAlpha( outerRing );
        compatibleCompassNeedle = SDL_DisplayFormatAlpha( compassNeedle);

Rotate Needle Based on Compass Heading

We will create a function to rotate our needle,  this will need to be done every time the main program loop is processed.  We will pass the heading to the new function, the heading will be used to rotate the needle.

int graphics(float heading)

The first task we want to do in the function is erase all information on the current surface. This can be done by using SDL_FillRect() and the value 0x000000, which is black.

        SDL_FillRect(screen,NULL,0x000000);

The next section of code converts the heading value from a float into a  string and then renders the text into the SDL surface ‘textSurface’. The third line rotates the text by 90 degrees

        snprintf(headingString, 8, "%7.3f", heading);
        textSurface = TTF_RenderText_Solid(font, headingString, colorWhite);
        currentDegressRotated =  rotozoomSurface(textSurface, TEXTANGLE, 1.0, 0);

When then specify the position of the outer ring  and needle image.

        outerRingposition.x = (SCREEN_WIDTH - compatibleOuterRing->w)/2;
        outerRingposition.y = (SCREEN_HEIGHT - compatibleOuterRing->h)/2;
        compassNeedlePosition.x = (SCREEN_WIDTH - compatibleCompassNeedle->w)/2;
        compassNeedlePosition.y = 20;

We can now rotate our needle based on the angle from the IMU. To do this we will use the rotozoomSurface() function. We will pass to it our compatible needle image, heading, the zoom factor(1.0 for no zoom) and smoothness (0 for no smooth). We add 90 to heading so that the needle is in the correct orientation when the IMU is attached to the Raspberry Pi, this value can be removed or increase.

                rotation = rotozoomSurface(compatibleCompassNeedle, heading+90, 1.0, 0);
        if (rotation == NULL) printf("error rotating needle\n");

After we rotate the image, we need to recenter the pivot point.

                compassNeedlePosition.x -= rotation->w/2-compatibleCompassNeedle->w/2;
        compassNeedlePosition.y -= rotation->h/2-compatibleCompassNeedle->h/2;

We then blit all the surfaces to the screen surface

                        SDL_BlitSurface(currentDegressRotated, NULL, screen, &topLine1);
        SDL_BlitSurface(compatibleOuterRing, NULL, screen, &outerRingposition);
        SDL_BlitSurface(rotation, NULL, screen, &compassNeedlePosition);

We can now flip our surface so we can see it on our display

        SDL_Flip(screen);

And finally, we need to free our surfaces for the next time the graphic() function is called. This prevents a memory leak.

        SDL_FreeSurface(screen);
        SDL_FreeSurface(rotationInclinometerJeepFront);
        SDL_FreeSurface(rotationInclinometerJeepSide);

Low Pass Filter

We use a low pass filter to reduce noise. This will stop the needle from bouncing around, however it will add a little delay in the movement.   The lower the low pass filter factor (LPF_FACT0R) the larger the delay.

magRaw[0] =  magRaw[0]  * MAG_LPF_FACTOR + oldXMagRawValue*(1 - MAG_LPF_FACTOR);
magRaw[1] =  magRaw[1]  * MAG_LPF_FACTOR + oldYMagRawValue*(1 - MAG_LPF_FACTOR);
magRaw[2] =  magRaw[2]  * MAG_LPF_FACTOR + oldZMagRawValue*(1 - MAG_LPF_FACTOR);
accRaw[0] =  accRaw[0]  * ACC_LPF_FACTOR + oldXAccRawValue*(1 - ACC_LPF_FACTOR);
accRaw[1] =  accRaw[1]  * ACC_LPF_FACTOR + oldYAccRawValue*(1 - ACC_LPF_FACTOR);
accRaw[2] =  accRaw[2]  * ACC_LPF_FACTOR + oldZAccRawValue*(1 - ACC_LPF_FACTOR);
oldXMagRawValue = magRaw[0];
oldYMagRawValue = magRaw[1];
oldZMagRawValue = magRaw[2];
oldXAccRawValue = accRaw[0];
oldYAccRawValue = accRaw[1];
oldZAccRawValue = accRaw[2];

Update Main Loop

You can now call the graphics() function from your main loop and pass the current heading

  graphics(heading);

Compile and Run

As there are a number of libraries we are using to create this program, we have to include them when compiling. You can use this to compile;

To run;

Displaying the Output on a Monitor

If you are using a small TFT connected to your Raspberry Pi, you can have the output shown on this TFT just like in the video above.

To do this, you fist need to specify /dev/fb1 as the framebuffer device.  /dev/fb0 will force the output to HMDI.

putenv("SDL_FBDEV=/dev/fb1");

The above command needs to be place just before SDL_Init();

 BerryIMU Placement

We used an elastic band to keep th BerryIMU in place.
The IMU needs to be in the correct orientation to match of with the needle on the display.  The image below shows the orientation needed for our code in github.

Calibration

Once the BerryIMU is attached to the Raspberry Pi, the magnetometer will need to be calibration as the Raspberry Pi will distort (hard iron)  the magnetic field around the IMU and the readings from the magnetometer will be incorrect.

You can follow this guide to calibration the magnetometer to remove the hard iron distortion.