Low Level Programming of the Raspberry Pi in C

One of the things that disappointed me when I first got my Raspberry Pi was the fact that everybody was doing very high level programming with it. I wanted to program it like I used to do with microcontrollers, but it seemed like this was not as easy as I thought it would be. I believe however, that for embedded applications, you should be very cautious about dependencies, thus try to use as few libraries as possible. This article will show you how to program the Raspberry Pi in C code in a low level way. Tackling this can be a challenge, so let's get started!

Note that this tutorial will make use of the BCM2835 ARM Peripherals Manual. I will also refer to this as "datasheet", because it sound natural to me.

Getting Started

If you have any experience with low level programming of microcontrollers, then you know that everything is done by writing to the registers in the memory of the device. If you, for example, want to set a pin high, then you write a "1" to the register that corresponds to that certain pin. That means that, if we want to program our Raspberry Pi, we need to gain access to the memory of the BCM2835 first. This is by far the most complex part of programming the Raspberry Pi, so don't be discouraged!

First, we need a header file which defines some macro's.

RPI.h

#include <stdio.h>
 
#include <sys/mman.h>
#include <sys/types.h>
#include <sys/stat.h>
 
#include <unistd.h>
 
#define BCM2708_PERI_BASE       0x20000000
#define GPIO_BASE               (BCM2708_PERI_BASE + 0x200000)	// GPIO controller 
 
#define BLOCK_SIZE 		(4*1024)
 
// IO Acces
struct bcm2835_peripheral {
    unsigned long addr_p;
    int mem_fd;
    void *map;
    volatile unsigned int *addr;
};
 
struct bcm2835_peripheral gpio = {GPIO_BASE};
 
extern struct bcm2835_peripheral gpio;  // They have to be found somewhere, but can't be in the header

So, what is this? If we look at the Broadcom BCM2835 ARM Peripherals manual at page 6, we read:

Physical addresses range from 0x20000000 to 0x20FFFFFF for peripherals. The bus addresses for peripherals are set up to map onto the peripheral bus address range starting at 0x7E000000. Thus a peripheral advertised here at bus address 0x7Ennnnnn is available at physical address 0x20nnnnnn.
So the BCM2708_PERI_BASE macro contains the physical adress value at which the peripheral registers start. This is the adress we will need to use in our program. The virtual address value is 0x7E000000, and it are these virtual adresses that will be found in the datasheet.

There are a lot of different peripherals available on the BCM2835 (Timers, USB, GPIO, I2C, ...), and they will be defined by an offset to this virtual adress! For example, if we are interested in the GPIO peripheral (like in the example code above), we can find in the manual at page 90 that the virtual address is 0x7E200000. This means that the offset to the physical adress will be 0x200000 which explains the GPIO_BASE.

for every peripheral we define a struct of the type bcm2835_peripheral, which will contain the information about the location of the registers. Then we intialize it with the map_peripheral() function from the c file below. It is not so important to understand how this works in detail, as you can just copy paste the code. If you do want to know this, search for "accessing /dev/mem" on the internet. To release the peripheral, we can use unmap_peripheral()

RPI.c

#include "RPI.h"
 
struct bcm2835_peripheral gpio = {GPIO_BASE};
 
// Exposes the physical address defined in the passed structure using mmap on /dev/mem
int map_peripheral(struct bcm2835_peripheral *p)
{
   // Open /dev/mem
   if ((p->mem_fd = open("/dev/mem", O_RDWR|O_SYNC) ) < 0) {
      printf("Failed to open /dev/mem, try checking permissions.\n");
      return -1;
   }
 
   p->map = mmap(
      NULL,
      BLOCK_SIZE,
      PROT_READ|PROT_WRITE,
      MAP_SHARED,
      p->mem_fd,      // File descriptor to physical memory virtual file '/dev/mem'
      p->addr_p       // Address in physical map that we want this memory block to expose
   );
 
   if (p->map == MAP_FAILED) {
        perror("mmap");
        return -1;
   }
 
   p->addr = (volatile unsigned int *)p->map;
 
   return 0;
}
 
void unmap_peripheral(struct bcm2835_peripheral *p) {
 
    munmap(p->map, BLOCK_SIZE);
    close(p->mem_fd);
}

Now we have code to access the memory of the BCM2835, we can start to use the peripherals to do some badass projects! If you didn't fully grasp the above code, this is not a problem! Everything will start making sense after you see that code in action.

GPIO

GPIO stands for General Purpose In/Out, so this peripheral will allow us to set a pin either as in- or output and read or write (to) it. We can add the macro's below to our header file (RPI.h) to support this. I will explain how they work underneath.

// GPIO setup macros. Always use INP_GPIO(x) before using OUT_GPIO(x)
#define INP_GPIO(g)   *(gpio.addr + ((g)/10)) &= ~(7<<(((g)%10)*3))
#define OUT_GPIO(g)   *(gpio.addr + ((g)/10)) |=  (1<<(((g)%10)*3))
#define SET_GPIO_ALT(g,a) *(gpio.addr + (((g)/10))) |= (((a)<=3?(a) + 4:(a)==4?3:2)<<(((g)%10)*3))
 
#define GPIO_SET  *(gpio.addr + 7)  // sets   bits which are 1 ignores bits which are 0
#define GPIO_CLR  *(gpio.addr + 10) // clears bits which are 1 ignores bits which are 0
 
#define GPIO_READ(g)  *(gpio.addr + 13) &= (1<<(g))

At first sight, those macro's look terrifying! But actually they are quite easy to understand, so don't be scared!

gpio is a struct of the type bcm2835_peripheral that was initialized with map_peripheral() at the adress GPIO_BASE. This is what we discussed above. To be able to use this code, this need to be done first!

To understand properly how the macro's work, we need to take a look at the Broadcom BCM2835 ARM Peripherals manual at chapter 6 (page 89 etc.). We will go over all the macro's in the following text.

#define INP_GPIO(g) *(gpio.addr + ((g)/10)) &= ~(7<<(((g)%10)*3))

This macro sets pin "g" as an input. How?

In the datasheet at page 91 we find that the GPFSEL registers are organised per 10 pins. So one 32-bit register contains the setup bits for 10 pins. *gpio.addr + ((g))/10 is the register address that contains the GPFSEL bits of the pin "g" (and 9 other pins we are not interested in). Remember that the result of a division on integers in C does not contain the part behind the komma. There are three GPFSEL bits per pin (000: input, 001: output). The location of these three bits inside the GPFSEL register is given by ((g)%10)*3 (three times the remainder, remember the modulo % operator).

So to set pin "g" as an input, we need to set these bits to "000". This is done with a AND-operation between the GPSEL register and a string of which everything except those bits are 1. This string is created by bitshifting 7, which is binary 111, over ((g)%10)*3 places, and taking the inverse of that.

A little example: Say that we want to use gpio4 as an input.

  • The GPSEL address containing the function select bits for gpio4 is *gpio.addr + 4/10=*gpio.addr + 0
  • Inside this 32-bit register, the bits concerning gpio4 start from bit (4%10)*3=12
  • If we bitshift binary 111 over 12 positions we get: 00000000000000000111000000000000
  • We have to make those three bits zero in the GPFSEL register, so we have to invert that 32-bit string and perform an AND operation with the GPSEL register. (X = unknown)
  •      1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0 0 1 1 1 1 1 1 1 1 1 1 1 1   
         X X X X X X X X X X X X X X X X X X X X X X X X X X X X X X X X   (GPFSEL old)
    AND  ---------------------------------------------------------------
         X X X X X X X X X X X X X X X X X 0 0 0 X X X X X X X X X X X X   (GPFSEL new)      
    

#define OUT_GPIO(g)   *(gpio.addr + ((g)/10)) |=  (1<<(((g)%10)*3))

To set pin "g" as an output, we have to set its setup bits to 001. This is done in the same way as before, but now we bitshift a 1, and use a OR operation on the normal (not inverted) string.

This is also the reason that the comment says to "always use INP_GPIO(x) before using OUT_GPIO(x)". This way you are sure that the other 2 bits are 0, and justifies the use of a OR operation here. If you don't do that, you are not sure those bits will be zero and you might have given the pin "g" a different setup.

#define SET_GPIO_ALT(g,a) *(gpio.addr + (((g)/10))) |= (((a)<=3?(a) + 4:(a)==4?3:2)<<(((g)%10)*3))

As you can see in the picture above, input and output are not the only functionalities available for the pins. There are 6 "alternate function" possibilities for each pin. It are these alternate functions that you will want to use if you need to use other peripherals than the gpio, like I2C and SPI. The alternate function possibilities are on page 102 in section 6.2 of the manual. With SET_GPIO_ALT(g,a) you can set pin g to function 0,1,... In the part about I2C below, we will use this function to set GPIO0 and GPIO1 to SDA and SCL for use with I2C.

#define GPIO_SET  *(gpio.addr + 7)

In the datasheet on page 90, we seet that the GPSET register is located 7 32-bit registers further than the gpio base register. There is a small mistake on this page of the datasheet, as the first line is printed twice.

gpio.addr is defined as a pointer to an unsigned int, which is 32-bit on the Raspberry Pi. When you add an integer to a pointer, it will know that we are working with 32-bit values, so we don't need to multiply this with 4 (7*4 = 0x1C) to obtain the address from the datasheet.

So to use the macro GPIO_SET to set e.g. gpio 4 pin we can set the 4'th bit of this register to 1 using a bitshift:

GPIO_SET = 1 << 4;
It's important to note that writing a "0" to the GPIO_SET register will not reset that pin. You have to write a "1" to the GPIO_CLR register to reset a pin. This is very useful, as this means that we can use the above bitshift operation to write to all pins, without taking the risk of accidentally resetting a pin we forgot that was on. Awesome.

The gpio clear is ofcourse similar.

#define GPIO_READ(g)  *(gpio.addr + 13) &= (1<<(g))

To read a pin we simply perform a bitwise AND on the GPLEV register and the pin we are interested in (g). The result is the value of that pin.

Example - Blink a LED

Hopefully the above analysis helped you understanding how to use the GPIO functionality of the Raspberry Pi. It really is nothing more than writing 1's and 0's to the correct registers ... As a little example, here is the code to blink a led. Note that pin 7 corresponds to gpio 4 if you look a the pinout. To run this, make sure you added the GPIO macro's to your header file!

Don't try this at home (without a resistor in series with the led)!

main.c

#include "RPI.h"
 
int main(
{
  if(map_peripheral(&gpio) == -1) 
  {
    printf("Failed to map the physical GPIO registers into the virtual memory space.\n");
    return -1;
  }
 
  // Define pin 7 as output
  INP_GPIO(4);
  OUT_GPIO(4);
 
  while(1)
  {
    // Toggle pin 7 (blink a led!)
    GPIO_SET = 1 << 4;
    sleep(1);
 
    GPIO_CLR = 1 << 4;
    sleep(1);
  }
 
  return 0; 
}

I2C

The peripheral we will need to use the I2C functionality of the Raspberry Pi is the BSC (Broadcom Serial Controller). Information about this can be found in de datasheet chapter 3 (p. 28 and further). There are two BSC's: BSC0 and BSC1. We will use BSC0. First we map all the registers by adding the following macro's to our RPI.h file.

#define BSC0_BASE     (BCM2708_PERI_BASE + 0x205000)  // I2C controller 
 
extern struct bcm2835_peripheral bsc0;	
 
// I2C macros
#define BSC0_C          *(bsc0.addr + 0x00)
#define BSC0_S          *(bsc0.addr + 0x01)
#define BSC0_DLEN     *(bsc0.addr + 0x02)
#define BSC0_A          *(bsc0.addr + 0x03)
#define BSC0_FIFO     *(bsc0.addr + 0x04)
 
#define BSC_C_I2CEN     (1 << 15)
#define BSC_C_INTR      (1 << 10)
#define BSC_C_INTT      (1 << 9)
#define BSC_C_INTD      (1 << 8)
#define BSC_C_ST        (1 << 7)
#define BSC_C_CLEAR     (1 << 4)
#define BSC_C_READ      1
 
#define START_READ      BSC_C_I2CEN|BSC_C_ST|BSC_C_CLEAR|BSC_C_READ
#define START_WRITE     BSC_C_I2CEN|BSC_C_ST
 
#define BSC_S_CLKT  (1 << 9)
#define BSC_S_ERR     (1 << 8)
#define BSC_S_RXF     (1 << 7)
#define BSC_S_TXE     (1 << 6)
#define BSC_S_RXD     (1 << 5)
#define BSC_S_TXD     (1 << 4)
#define BSC_S_RXR     (1 << 3)
#define BSC_S_TXW     (1 << 2)
#define BSC_S_DONE    (1 << 1)
#define BSC_S_TA      1
 
#define CLEAR_STATUS    BSC_S_CLKT|BSC_S_ERR|BSC_S_DONE
 
// I2C Function Prototypes
void i2c_init();
void wait_i2c_done();

Not much special here, except maybe for the START_READ, START_WRITE and CLEAR_STATUS. Of course, these are just the combinations (OR) of bits in the status register that need to be set for that action to occur. It's just easier to do it like that than to set all the bits seperately every time.

Then we need to add the following functions to our RPI.C file.

struct bcm2835_peripheral bsc0 = {BSC0_BASE};
 
// Initialize I2C
void i2c_init()
{
    INP_GPIO(0);
    SET_GPIO_ALT(0, 0);
    INP_GPIO(1);
    SET_GPIO_ALT(1, 0);
}  
 
// Function to wait for the I2C transaction to complete
void wait_i2c_done() {
 
        int timeout = 50;
        while((!((BSC0_S) & BSC_S_DONE)) && --timeout) {
            usleep(1000);
        }
        if(timeout == 0)
            printf("Error: wait_i2c_done() timeout.\n");
}

Notice how we access the memory of the BSC0 peripheral with a struct of the type bcm2835_peripheral called bsc0. We initalize it so that the address points to BSC0_BASE.

The classic I2C delay is implemented with wait_i2c_done(). It constantly polls the "DONE" bit of the BSC0 S register (status). This bit becomes 1 when the transfer is completed. See datasheet p. 32.

Now we need to make sure that the GPIO pins are set up properly for I2C. For this we need one pin to behave as SDA, and one pin to behave as SCL. If we look at the pinout again, we see that GPIO0 and GPIO1 are the proper pins for this. They have to be enabled first though. For this we use the i2c_init() function. Inside you will find the SET_ALT_GPIO() macro used to set these pins to alternate functionality 0. The reason we set these to alternate functionality 0 is because the datasheet shows the following table at page 102:

Ok, so after initializing the I2C we are very close to start sending data over the I2C bus! To send or write some data, we need to take the following steps:

  • Write the I2C slave address of the device you want to communicate with (you can find this in the datasheet) to the BSC0_A adress (slave adress register).
  • Write the amount/quantity of bytes you want to send/receive to the BSC0_DLEN register (Data Length).
  • If you want to write to the slave, write the bytes you want to write in the BSC0_FIFO. Remember: FIFO = First In First Out!
  • Clear the status register: BSC0_S = CLEAR_STATUS
  • Start write or start read by writing to the BSC0 C register (Control): BSC0_C = START_WRITE or BSC0_C = START_READ

Are you totally confused? Me too! Let's take a look at some examples!

Example 1 - Real Time Clock (RTC)

In this example we will read a Real Time Clock (RTC) with a Raspberry Pi using I2C. The RTC we are using is the PCF8563, a very classic device. Normally you would want to be able to write the RTC as well for initializing, but I want to start with a simple example.

The data from the PCF8563 starts at register 2 (seconds) and the 6 sequential adresses contain minutes, hours, ... So in total there are 7 registers we need to read, starting from adress 2.

Reading data from the RTC is described in the PCF8563 datasheet and should be performed like this:

The first step, writing the slave address to the bus, will be done automatically for us if we write this address to the BSC0_A register. So we have to do this before starting communication. After this we need to write the register address of the RTC we want to start reading from to the bus. In our case, this is "2". After that, it will start sending the data, so we have to start reading data from the bus.

The code example below nicely illustrates how this is done on the Raspberry Pi.

#include "RPI.h"
#include <stdio.h>
#include <time.h>
#include <fcntl.h>
 
struct tm t;
 
/* Functions to do conversions between BCD and decimal */
unsigned int bcd_to_decimal(unsigned int bcd) {
    return ((bcd & 0xf0) >> 4) * 10 + (bcd & 0x0f);
}
unsigned int decimal_to_bcd(unsigned int d) {
    return ((d / 10) << 4) + (d % 10);
}
 
int main(int argc, char *argv[]) {
 
    /* Gain access to raspberry pi gpio and i2c peripherals */
    if(map_peripheral(&gpio) == -1) {
        printf("Failed to map the physical GPIO registers into the virtual memory space.\n");
        return -1;
    }
    if(map_peripheral(&bsc0) == -1) {
        printf("Failed to map the physical BSC0 (I2C) registers into the virtual memory space.\n");
        return -1;
    }
 
    /* BSC0 is on GPIO 0 & 1 */
    i2c_init();
 
    /* I2C Device Address 0x51 (See Datasheet) */
    BSC0_A = 0x51;
 
    /* Write operation to restart the PCF8563 register at index 2 ('secs' field) */
    BSC0_DLEN = 1;            // one byte
    BSC0_FIFO = 2;            // value 2
    BSC0_S = CLEAR_STATUS;    // Reset status bits (see #define)
    BSC0_C = START_WRITE;     // Start Write (see #define)
 
    wait_i2c_done();
 
    /* Start Read of RTC chip's time */
    BSC0_DLEN = 7;
    BSC0_S = CLEAR_STATUS;  // Reset status bits (see #define)
    BSC0_C = START_READ;    // Start Read after clearing FIFO (see #define)
 
    wait_i2c_done();
 
    /* Store values in struct */
    t.tm_sec = bcd_to_decimal(BSC0_FIFO & 0x7f);
    t.tm_min = bcd_to_decimal(BSC0_FIFO & 0x7f);
    t.tm_hour = bcd_to_decimal(BSC0_FIFO & 0x3f);
    t.tm_mday = bcd_to_decimal(BSC0_FIFO & 0x3f);
    t.tm_wday = bcd_to_decimal(BSC0_FIFO & 0x07);
    t.tm_mon = bcd_to_decimal(BSC0_FIFO & 0x1f) - 1; // 1-12 --> 0-11
    t.tm_year = bcd_to_decimal(BSC0_FIFO) + 100;
 
    printf("%02d:%02d:%02d %02d/%02d/%02d (UTC on PCF8563)\n",
        t.tm_hour,t.tm_min,t.tm_sec,
        t.tm_mday,t.tm_mon + 1,t.tm_year - 100);
 
    /* Unmap the peripheral */
    unmap_peripheral(&gpio);
    unmap_peripheral(&bsc0);decimal_to_bcd
}

Example 2 - IMU (MPU6050)

For reading the MPU6050 IMU I will just provide the source code and hope that you understand what to do with it after the previous examination. If there still is something that is not clear, please contact me, and I will add more explanation here.

MPU6050.h

#ifndef _INC_MPU6050_H
#define _INC_MPU6050_H
 
#include "RPI.h"
#include <stdio.h>
 
#define MPU6050_ADDR 0b01101001 // 7 bit adres 
#define CONFIG 0x1A
#define ACCEL_CONFIG 0x1C
#define GYRO_CONFIG 0x1B
#define PWR_MGMT_1 0x6B
 
void MPU6050_Read(short * accData, short * gyrData);
void MPU6050_Init(void);
void MPU6050_SetRegister(unsigned char regAddr, unsigned char regValue);
 
#endif

MPU6050.c

#include "MPU6050.h"
 
void MPU6050_Init(void)
{
//  MPU6050_SetRegister(PWR_MGMT_1, 0x80);  // Device Reset
    MPU6050_SetRegister(PWR_MGMT_1, 0x00);  // Clear sleep bit
    MPU6050_SetRegister(CONFIG, 0x00);  
    MPU6050_SetRegister(GYRO_CONFIG, 0x08);
    MPU6050_SetRegister(ACCEL_CONFIG, 0x08);
}
 
void MPU6050_SetRegister(unsigned char regAddr, unsigned char regValue)
{
    // See datasheet (PS) page 36: Single Byte Write Sequence
 
    // Master:   S  AD+W       RA       DATA       P
    // Slave :            ACK      ACK        ACK
 
    BSC0_A = MPU6050_ADDR;
 
    BSC0_DLEN = 2;
    BSC0_FIFO = (unsigned char)regAddr;
    BSC0_FIFO = (unsigned char)regValue;
 
    BSC0_S = CLEAR_STATUS;  // Reset status bits (see #define)
    BSC0_C = START_WRITE;     // Start Write (see #define)
 
    wait_i2c_done();
}
 
void MPU6050_Read(short * accData, short * gyrData)
{
    // See datasheet (PS) page 37: Burst Byte Read Sequence
 
    // Master:   S  AD+W       RA       S  AD+R           ACK        NACK  P
    // Slave :            ACK      ACK          ACK DATA       DATA
 
    BSC0_DLEN = 1;        // one byte
    BSC0_FIFO = 0x3B;     // value of first register
    BSC0_S = CLEAR_STATUS;  // Reset status bits (see #define)
    BSC0_C = START_WRITE;     // Start Write (see #define)
 
    wait_i2c_done();
 
    BSC0_DLEN = 14;
 
    BSC0_S = CLEAR_STATUS;  // Reset status bits (see #define)
    BSC0_C = START_READ;      // Start Read after clearing FIFO (see #define)
 
    wait_i2c_done();
 
    short tmp;
 
    int i = 0;  
    for(i; i < 3; i++)    // Accelerometer
    {
  tmp = BSC0_FIFO << 8; 
  tmp += BSC0_FIFO;
  accData[i] = tmp; 
    }
 
    tmp = BSC0_FIFO << 8;   // Temperature
    tmp += BSC0_FIFO; 
 
    i = 0;  
    for(i; i < 3; i++)    // Gyroscope
    {
  tmp = BSC0_FIFO << 8;
  tmp += BSC0_FIFO;
  gyrData[i] = tmp; 
    }
}

GitHub Repository

All this code can also be found in my GitHub repository: https://github.com/Pieter-Jan/PJ_RPI. You will need CMake (sudo apt-get install cmake) and git (sudo apt-get install git) to use it.

To use it, create a folder on the pi where you want to put the files in and extract the git repository in there:

git clone git://github.com/Pieter-Jan/PJ_RPI.git 

Then you have to install the library, so cd into the PJ_RPI directory and:

mkdir Build 
cd Build 
cmake ../ 
make 
sudo make install

Now the library is installed and you can build the examples that are in the "Examples" folder using CMake.

If you want to build the MPU6050 example you will also need ncurses for the visualization (sudo apt-get install ncurses-dev).

Comments

It was a great help and You are the great man. Now I nearly understand Raspberry's GPIO. I thought I won't but this tutorial helped me a lot.

It's my start with electronics and low level programming...
Sorry for my poor English :)

Greetings

Thanks very much for this tutorial. I have been looking to use RPi as a microcontroller and this has set me on the road.
Thanks again
Roger

Thanks a lot for the explanation on GPIO. I am specifically interested in I2C Communication on Raspberry PI.
I want to know if the above will work for a Rev 2 Raspberry PI Board too? Please let me know....

Thanks a lot again for your help,
trivalent.

Thank u for the clear explanation about 'how to access GPIO using C code'.
It is very useful for me to start with GPIO programming.

thanks man, i couldnt understand lot of stuff as i am a new to this, but i just loved it..

great work , it really helps to build better understanding for gpio than just using python or high level abstraction,
thank you.

thanks a lot for this clear explanation, it is very helpful for me,
I am find difficulty to find some low level example to implement a synchronized ADC read, if you have some example likes ISR() enough fast to read an ADC at 50ksps I appreciate it very much.
thanks,
Vitor

Just excellent! Thank you for taking the time to explain the Macros. You should be a professor - thanks again.

Hi Piotr,

Wonderful explanation of the Macros, to set the GPIOs as INPUT, OUTPUT, SET and CLEAR. Can you give any Macro to read the status of any GPIO pin, exposed on the Expansion Header, something like GPIO_READ to check the status of the signal recieved if HIGH/1 or LOW/0.?

Thanks,
Rajiv.

Parabéns pelo execelente tutorial. Muito bom

Amazing post!
It is really clear and explains very well what's going on in the registers!
Thanks!
Sidney

Hey, great stuff.

Ok, so I'm looking for a way to send varying voltage down the differential cables of the USB port to provide an analogue output to devices (to vary speed of motors, without using breakout or other add-ons to the Pi).

I've looked at the manual, but can't see how to start. Do your investigations shed any light on this?

Tim

Hi Piotr,

Great job.Nice explanation.Very usefull.

Thanks,
Youghandhar

Wassp People !! I am KAREN MOODY. I belong to Downey.
This may i will be 28. I might join The Proud Academy of Flabbergasted Education in Reno.
I have a job as Mercer. Iam a fan of Learning An Instrument.

Great article, Pieter-Jan. Very clear explanation of the low-level interfaces from a programmers viewpoint. Thanks for taking the time.

I am actually surprise you code isn't causing you problems with the c compiler making optimizing and range errors

usually accessing a register that changes outside the c code requires a volatile statement
#define BSC0_C *(bsc0.addr + 0x00)
#define BSC0_S *(bsc0.addr + 0x01)

So I would have expected you would have had to put it like this

#define BSC0_C *((volatile unsigned char *)(bsc0.addr + 0x00)
#define BSC0_S *((volatile unsigned char *)(bsc0.addr + 0x01)

The reasons are obvious if you have two read statements close together

a = BSC0_C
b = BSC0_C

A good optimizing compiler will do one read in a and transfer the register result of a into b rather than do two actual reads of the hardware. The volatile tells the compiler it can't assume the last value and it will force the second read

The other problem I see with your macro is you can accidently write words to a byte port etc you have no range check protection. That is why the macros above the type and therefore size of the registers will be maintained so range checking remains in force for C code

Love cut and paste errors put the bracket at end above

#define BSC0_C *((volatile unsigned char *)(bsc0.addr + 0x00))
#define BSC0_S *((volatile unsigned char *)(bsc0.addr + 0x01))

Sorry one last piece of housekeeping you obviously know how to write macros so I don't understand why you didn't write the input that's required before any OUT_GPIO into the macro itself. Your macro statements should have brackets around them ... good coding practice for if they get nested

INP_GPIO just needs brackets around the substitution statement ... like so

#define INP_GPIO(g) (*(gpio.addr + ((g)/10)) &= ~(7<<(((g)%10)*3)))

OUT_GPIO I would have gone for the short hand version

#define OUT_GPIO(g) ( INP_GPIO(g); (*(gpio.addr + ((g)/10)) |= (1<<(((g)%10)*3))))

but some like the long hand version with the input and then output

#define OUT_GPIO(g) ((*(gpio.addr + ((g)/10)) &= ~(7<<(((g)%10)*3)))\
(*(gpio.addr + ((g)/10)) |= (1<<(((g)%10)*3))))

I think either is acceptable and gives you a lot more safety around your code and you aren't relying on your memory to do the input first

My dick got rock hard just reading this! I think your article cured my aids.

Hey, fantastic tutorial! I only have one question, in the table that shows the Register View, the registers seem to be 4 addresses apart, does that mean they are adressed byte-wise?

Your way of telling all in this post is truly nice, every one be capable of simply
understand it, Thanks a lot.

Great tutor but I don't understand onet thing... till now :)
Why BLOCK_SIZE is defined as (4*1024) bytes?
Can I use BLOC_SIZE of 104 bytes (41 registers * 32 bits) if I oly want to use GPIO?

Awesome article
By far the best Raspberry Pi GPIO tutorial I've read to date. Great job

Well done man! Great tutorial, really helpful!

Has anyone got this working an a Ver2? I have been trying to get it to compile and it does not seem to find the lib package...

Very nice tutorial, I mean what in raspberry pi Rev. 2 pin 3(sda) and 5(scl) it is BSC1 addresses, you this picture:
http://alanbarr.github.io/RaspberryPi-GPIO/i2c.html

in raspberry pi Rev. 2 pin 3(sda) and 5(scl) it is BSC1 addresses, you this picture:
http://alanbarr.github.io/RaspberryPi-GPIO/i2c.html

in raspberry pi Rev. 2 pin 3(sda) and 5(scl) it is BSC1 addresses, you this picture:
http://alanbarr.github.io/RaspberryPi-GPIO/i2c.html

Thanks for your code, I tried to compile in my Raspberry, but it told me many errors, can somebody tell me how to compile this?
Thanks

Good answers in return of this matter with genuine arguments and explaining the whole thing on the topic of that.

Pages

Add new comment

DIY Raspberry Pi Breakout Cable

Suprisingly enough, a lot of people have asked me how I made the breakout / expansion cable for my Raspberry Pi. It's so easy! So if you don't want to wait on some online order, and want to get started as soon as possible doing epic stuff, you can follow the simple instructions below. A cable like this can be extremely useful for connecting the Raspberry Pi to a breadboard and doing some prototyping!

The following things are needed:

  • 2x13 flat cable
  • 2 times 1x13 male header with 2.54 mm (1 inch) spacing
  • Electrical tape with a nice color
It's also necessary that you know how to solder and have the right equipment for this.

The first step is cut the flat cable nicely in two pieces. If you have a flat cable from an old computer that is 2x13, you can definitely use this! If you don't have one (my case), there are a lot of stores where you can find it. We will continue working with one half of the cable as shown in the figure below.

The second step is to split the cable in 2 pieces, so that you have 2 parts that each contain 13 wires. Then you need to seperate the individual wires a little bit (not too far!) with a knife. Now you should be able to strip them, and solder them to the 1x13 header. You will have to do this for both parts of the cable. Note that it might be useful to make the wires in the middle a little bit shorter than those at the ends, because the header is a lot wider than the half flat cable. The result should look like shown in the picture below. Make sure that you don't make short circuits! If you are in doubt, double check with a multimeter.

Because the above result does not look good, and having unstripped wires is a bit dangerous, I decided to tape this in with some electrical tape. Be careful not to apply too much pressure to the wires so that the pins of the header don't bend!

You should end up with a nice looking cable ready for action!

Comments

Its such as you read my thoughts! You appear to know a lot
about this, such as you wrote the e book in it or
something. I believe that you simply can do with some % to
power the message house a bit, however instead of that,
this is great blog. A fantastic read. I'll definitely be back.

I enjoy wҺаt you guys are usսally uƿ too.
Ƭhis sort of clever ѡork and exposure! Keeρ up the superb
workѕ guys I've aԁded you guys tto my personal blogroll.

Pretty portion of content. I just stumbled upon yiur
website and inn accrssion capital to say that I acquire actually loved account your blog posts.
Anyhway I'll be subscribing in your augment or even I fulfillment you get right of entry to
constantly fast.

Ԝe're ɑ groսp of volunteers аnd οpening a new scheme in our community.
ϒour web site provided սs with valuable informаtion to woгk on.
You ɦave performed an impressive job aand ߋur wɦole grouƿ
will proƅably be thankfgul tο yοu.

I think the admin of this weeb site is iin fact working hard
foor his web page, for the reason that here
every information is quality based stuff.

Hello just wanted to give you a quick heads up. The words in your article seem to be running off
the screen in Safari. I'm not sure if this is a formatting issue or something
to do with internet browser compatibility but I
thought I'd post to let you know. The design and style look great though!
Hope you get the issue solved soon. Cheers

I seriously love your site.. Very nice colors & theme.
Did you build this web site yourself? Please reply back as I'm wanting
to create my own website and want to learn where you got this
from or exactly what the theme is named. Many thanks!

I visited several blogs exxcept the audio feature for audio songs present at this website is genuinely superb.

What's Taking place i am new to this, I stumbled upon this
I've discovered It positively useful and it has helped me out loads.

I'm hoping to give a contribution & aid different users like
its helped me. Great job.

Thank you for anny other excellent post. The place else may just anyone
get that type of info iin such ann ideal method of writing?
I've a presentation subsequent week, and I'm at the look for
such info.

When someone writes an post he/she retains the plan of a user in his/her
mind that how a user can understand it. Thus that's why this paragraph is amazing.
Thanks!

I go to see day-to-day a ffew blogs and sites to read articles, but
this weblog gives quality based writing.

I've been surfing online more than three hours lately, but I by no means discovered any fascinating article like yours.
It's beautiful worth enough for me. In my
view, if all webmasters and bloggers made good content as you did, the internet
will be a lot more useful than ever before.

I'm not sure wheree you are getting your info,
but good topic. I needs to spend some time leadning much more or understanding more.
Thanks for fantastic information I was looking for this informtion for mmy mission.

Hi there to all, how is all, I think every one is getting more from this
web page, and your views are fastidious in support of new viewers.

When someone writes an piece of writing he/she keeps the idea of a
user in his/her mind that how a user can be aware
of it. Thus that's why this piece of writing is outstdanding.

Thanks!

Pages

Add new comment

Reading a IMU Without Kalman: The Complementary Filter

These days, IMU's (Intertial Measurement Units) are used everywhere. They are e.g. the sensors that are responsible for keeping track of the oriëntation of your mobile phone. This can be very useful for automatic screen tilting etc. The reason I am interested in this sensor is because I want to use it to stabilize my quadrocopter. If you are here for another reason, this is not a problem as this tutorial will apply for everyone.

When looking for the best way to make use of a IMU-sensor, thus combine the accelerometer and gyroscope data, a lot of people get fooled into using the very powerful but complex Kalman filter. However the Kalman filter is great, there are 2 big problems with it that make it hard to use:

  • Very complex to understand.
  • Very hard, if not impossible, to implement on certain hardware (8-bit microcontroller etc.)
In this tutorial I will present a solution for both of these problems with another type of filter: the complementary filter. It's extremely easy to understand, and even easier to implement.

Why do I need a filter?

Most IMU's have 6 DOF (Degrees Of Freedom). This means that there are 3 accelerometers, and 3 gyrosocopes inside the unit. If you remember anything from a robotics class you might have taken, you might be fooled into thinking that the IMU will be able to measure the precise position and orientation of the object it is attached to. This because they have told you that an object in free space has 6DOF. So if we can measure them all, we know everything, right? Well ... not really. The sensor data is not good enough to be used in this way.

We will use both the accelerometer and gyroscope data for the same purpose: obtaining the angular position of the object. The gyroscope can do this by integrating the angular velocity over time, as was explained in a previous article. To obtain the angular position with the accelerometer, we are going to determine the position of the gravity vector (g-force) which is always visible on the accelerometer. This can easily be done by using an atan2 function. In both these cases, there is a big problem, which makes the data very hard to use without filter.

The problem with accelerometers

As an accelerometer measures all forces that are working on the object, it will also see a lot more than just the gravity vector. Every small force working on the object will disturb our measurement completely. If we are working on an actuated system (like the quadrocopter), then the forces that drive the system will be visible on the sensor as well. The accelerometer data is reliable only on the long term, so a "low pass" filter has to be used.

The problem with gyroscopes

In one of the previous articles I explained how to obtain the angular position by use of a gyroscope. We saw that it was very easy to obtain an accurate measurement that was not susceptible to external forces. The less good news was that, because of the integration over time, the measurement has the tendency to drift, not returning to zero when the system went back to its original position. The gyroscope data is reliable only on the short term, as it starts to drift on the long term.

The complementary filter

The complementary filter gives us a "best of both worlds" kind of deal. On the short term, we use the data from the gyroscope, because it is very precise and not susceptible to external forces. On the long term, we use the data from the accelerometer, as it does not drift. In it's most simple form, the filter looks as follows:

The gyroscope data is integrated every timestep with the current angle value. After this it is combined with the low-pass data from the accelerometer (already processed with atan2). The constants (0.98 and 0.02) have to add up to 1 but can of course be changed to tune the filter properly.

I implemented this filter on a Raspberry Pi using a MPU6050 IMU. I will not discuss how to read data from the MPU6050 in this article (contact me if you want the source code). The implementation of the filter is shown in the code snippet below. As you can see it is very easy in comparison to Kalman.

The function "ComplementaryFilter" has to be used in a infinite loop. Every iteration the pitch and roll angle values are updated with the new gyroscope values by means of integration over time. The filter then checks if the magnitude of the force seen by the accelerometer has a reasonable value that could be the real g-force vector. If the value is too small or too big, we know for sure that it is a disturbance we don't need to take into account. Afterwards, it will update the pitch and roll angles with the accelerometer data by taking 98% of the current value, and adding 2% of the angle calculated by the accelerometer. This will ensure that the measurement won't drift, but that it will be very accurate on the short term.

It should be noted that this code snippet is only an example, and should not be copy pasted as it will probably not work like that without using the exact same settings as me.

#define ACCELEROMETER_SENSITIVITY 8192.0
#define GYROSCOPE_SENSITIVITY 65.536
 
#define M_PI 3.14159265359	    
 
#define dt 0.01							// 10 ms sample rate!    
 
void ComplementaryFilter(short accData[3], short gyrData[3], float *pitch, float *roll)
{
    float pitchAcc, rollAcc;               
 
    // Integrate the gyroscope data -> int(angularSpeed) = angle
    *pitch += ((float)gyrData[0] / GYROSCOPE_SENSITIVITY) * dt; // Angle around the X-axis
    *roll -= ((float)gyrData[1] / GYROSCOPE_SENSITIVITY) * dt;    // Angle around the Y-axis
 
    // Compensate for drift with accelerometer data if !bullshit
    // Sensitivity = -2 to 2 G at 16Bit -> 2G = 32768 && 0.5G = 8192
    int forceMagnitudeApprox = abs(accData[0]) + abs(accData[1]) + abs(accData[2]);
    if (forceMagnitudeApprox > 8192 && forceMagnitudeApprox < 32768)
    {
	// Turning around the X axis results in a vector on the Y-axis
        pitchAcc = atan2f((float)accData[1], (float)accData[2]) * 180 / M_PI;
        *pitch = *pitch * 0.98 + pitchAcc * 0.02;
 
	// Turning around the Y axis results in a vector on the X-axis
        rollAcc = atan2f((float)accData[0], (float)accData[2]) * 180 / M_PI;
        *roll = *roll * 0.98 + rollAcc * 0.02;
    }
} 

If we use the sweetness of having a real computer (Raspberry Pi) collecting our data, we can easily create a graph using GNUPlot. It's easily seen that the filter (red) follows the gyroscope (blue) for fast changes, but keeps following the mean value of the accelerometer (green) for slower changes, thus not feeling the noisy accelerometer data and not drifting away eiter.

The filter is very easy and light to implement making it perfect for embedded systems. It should be noted that for stabilization systems (like the quadrocopter), the angle will never be very big. The atan2 function can then be approximated by using a small angle approximation. In this way, this filter could easily fit on an 8 bit system.

Hope this article was of any help.

Comments

I also tried to use the same complimentary filter on ARdrone for attitude estimation with weight being 0.99, 0.01. I find that for small movement, the estimation is fine, but the filter can not track quick movement of the drone in flight. Do you have any suggestions?

The quick movement should not be dependant on the type of filter, as this would be almost purely the data from the gyroscope integration. What is your sampling rate? Do you have the same problem if you don't use a filter and just look at the gyroscope data?

I'm using the same IMU (MPU-6050) with Arduino to calculate the x-axis angle, I'm doing it with simplified kalman filter and its giving a stable data when its rotating without any linear motion, when I start to move it in the x direction the value of the angle gets missed up till I stop moving it, I'm working on project (inverted pendulum on two wheels) so I need the angle stable regardless the motion in the x direction, what can I do to fix it?

Your algorithm is probably too dependant on the data from the accelerometer. Why don't you use the Complementary Filter I proposed in this article? Notice that I only use 2% of the data from the accelerometer? This ensures that linear motion does not interfere with our angle measurement. Also the "if" condition checks if the accelerometer data is not too big, which would be the case if there is a large force working on the object. You don't want to use the data then. I hope it works out for you.

Why do you subtract roll but add pitch?

CJ Luu, if you combine the accelerometer angle and the gyroscope angle, you have to make sure that they have the same sign for the same excitation direction. This is why ...

I am using arduIMU which has IMU6000 (3-axis gyro and accelerometer) and on-board Atmega328p to make a self-stabilizing platform . Is complementary (or kalman) filter preferred for this. What should be the suitable tuning values for gyro and acce. ??

You can use either the Kalman or complementary filter. Just choose what you think is best. I would suggest that you start reading your datasheet very carful. This is the start of any electronics project. The "tuning values" (I guess that you mean sensitivity values) will be in there.

Hi Pieter-Jan,

Thank you for sharing your work. I'm using PIC18F46J50 as MCU along with Sparkfun IMU - 6DOF ITG3200/ADXL345, and I'm trying to combine accelerometer and giroscope data using the complementary filter. I've ported your code (to CCS v4.140):

float RwAcc[3]={0, 0, 0}; //Projection of normalized gravitation force vector on x/y/z axis, as measured by accelerometer
float Gyro_ds[3]={0, 0, 0}; //Gyro readings

float interval = 0;
unsigned int timerCount250us=0; //Timer count. Each unit counts 250 us
unsigned int timerCount63750us=0;

float pitch=0, roll=0;

void ComplementaryFilter()
{
float pitchAcc, rollAcc;

interval = 0.250*(float)timerCount250us + 63.750*(float)timerCount63750us; //Units: ms
interval /= 1000.0; //Conversion to sec.
//Restart the counters
timerCount250us = 0;
timerCount63750us = 0;

// Integrate the gyroscope data -> int(angularSpeed) = angle
// Gyro_ds is already divided by sensitivity (14.375 LSB per °/s )
pitch += Gyro_ds[0] * interval; // Angle around the X-axis
roll -= Gyro_ds[1] * interval; // Angle around the Y-axis

// Compensate for drift with accelerometer data if !bullshit
// Sensitivity= -2g to 2g at 10bit -> 2g = 512 && 0.5g = 128... ok???
// RwAcc is already divided by sensitivity (256 LSB per g) -> 128/256=0.5 && 512/256=2
float forceMagnitudeApprox = abs(RwAcc[0]) + abs(RwAcc[1]) + abs(RwAcc[2]);
if (forceMagnitudeApprox > 0.5 && forceMagnitudeApprox < 2.0)
{
// Turning around the X axis results in a vector on the Y-axis
pitchAcc = atan2(RwAcc[1], RwAcc[2]) * 180 / PI;
pitch = pitch * 0.98 + pitchAcc * 0.02;

// Turning around the Y axis results in a vector on the X-axis
rollAcc = atan2(RwAcc[0], RwAcc[2]) * 180 / PI;
roll = roll * 0.98 + rollAcc * 0.02;
}
}

And I use an internal interruption so as to calculate the interval. Timer overflows every 250us:

#int_TIMER0
void TIMER0_isr(void)
{
timerCount250us+=1;
if(timerCount250us==255) {
timerCount63750us+=1;
}
set_timer0(64036);
}

However, it doesn't seem to be working properly. When I tilt the sensor it works, but then it returns to the original position... I've recorded a video to clarify my problem:

http://www.youtube.com/watch?v=ohNkNRYQg2I

I would greatly appreciate if you could help me.

Thank you.

Mmh I don't really have time to evaluate your code into detail but from what I see everything looks like it is programmed correctly.

From the video it seems like your gyroscope data is correct, as the fast changes are recorded correctly. Are you sure that your accelerometers and gyroscopes are oriented in the same way as in my case (MPU6050)? What if you only read the angle from the accelerometer data? Is it the angle you expect?

If the orientation is correct, it might be that your gyroscope angles drift much harder than mine and "wins" from the compensation of the accelerometer. Do you use the full resolution of your sensors? Try a slower sampling interval? MEMs are not made to be sampled that fast ... Mechanical processes are slow! And most importantly. Tweak the values from your complementary filter so that you use more accelerometer and less gyroscope so that you get rid of that nasty drift!

Pieter-Jan: Thank you very much for your quick response and guide! It was a matter of different orientation of my accelerometer and gyroscope. Now it works well enough... except for one thing. If I tilt, for instance, pitch angle to 90 or -90 deg, roll angle goes crazy. The same occurs to pitch if I tilt roll to +-90. I don’t know if I’ve explained myself clearly, so I’ve made a video:

http://www.youtube.com/watch?v=V1IMvj_iWlo

I don't know why this is happening.
Anyway, here is my code:

void ComplementaryFilter()
{
float pitchAcc, rollAcc;

interval = (0.250*(float)timerCount250us + 63.750*(float)timerCount63750us)/1000.0; //Units: sec
//Restart the counters
timerCount250us = 0;
timerCount63750us = 0;

rollAcc = atan2(RwAcc[1], RwAcc[2]) * 180 / PI;
pitchAcc = atan2(RwAcc[0], RwAcc[2]) * 180 / PI;

// Integrate the gyroscope data -> int(angularSpeed) = angle
// Gyro_ds is already divided by sensitivity (14.375 LSB per °/s )
roll += Gyro[0] * interval; // Angle around the X-axis
pitch -= Gyro[1] * interval; // Angle around the Y-axis

// Compensate for drift with accelerometer data if !bullshit
// Sensitivity= -2g to 2g at 10bit -> 2g = 512 && 0.5g = 128...
// RwAcc is already divided by sensitivity (256 LSB per g) -> 128/256=0.5 && 512/256=2
float forceMagnitudeApprox = abs(RwAcc[0]) + abs(RwAcc[1]) + abs(RwAcc[2]);
if (forceMagnitudeApprox > 0.5 && forceMagnitudeApprox < 2.0)
{
// Turning around the X axis results in a vector on the Y-axis
roll = roll * 0.97 + rollAcc * 0.03;
// Turning around the Y axis results in a vector on the X-axis
pitch = pitch * 0.97 + pitchAcc * 0.03;
}
}

If you have any idea of what the problem can be, please tell me.
Thank you very much.

Good that it's working now!

I see the same behaviour as you when one of the angles goes to 90°. This is because at this value, the gravity vector shifts to a different axis and the atan2 function does not behave properly. I did not fix this in my code as I am only interested in variations of +-60°. You could try this document: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf It describes mathematically how to read the orientation from the accelerometers, and gives a different result as the one I used. Maybe it is better suited. The document should be good as it is referenced to a lot on the internet. Sorry I can not help you with more detail. Let me know if you can fix it!

Ok, I'll let you know if I am able to fix it!
Anyway, I think I won't probably need +/- 90º variations... I just asked in case I was doing something wrong, or it existed a quick solution.
I'd like to reiterate my gratitude to your help and quick responses. Congratulations for your blog, it has helped me a lot when I have gone crazy trying to implement Kalman Filter... unaware of the Complementary Filter goodness.

Hallo Pieter-Jan,
thank you for the great work.
I want to use the complementary filter for stabilize my quadcopter, there is only one problem.
when the motors are working, the vibration lets the data drift, how can i filter out the vibrations?

And what if i dont use the filter on an infinite loop?
should i change dt? or is dt always 0.01?

I need an example of a very simple program with codevision( Complementary Filter )
I am using the adxl203&mlx90609(gyro) module and i need filter data
please help me...
thanks a lot...
Sorry for my bad English because I'm from Iran & my English very poor

Hi Pieter-Jan,
I am using an MPU6050 on my device that is connected to a Ti AM3517 via i2c. The driver I am using is ported from the RaspberryPi and generates angle and accelerometer data and it all looks nice.
I get angle in +/-degrees and acceleration is around 4000 for z when stationary (which I take to be 1g)

I have an initial problem with drift when I have initialised the 6050 to use the DMP, it takes around 12 seconds to settle for the x axis, do you see any issues with the startup of your device ?

I would like to find a way to stop the error as I may need the x,y,z data to b ready as soon as it has been initialised.

What are you using for your setup ? do you use the DMP and also what are the gyro and accel resolutions you are using (I'm using +/-2000 deg)

Hope you can help.

Thanks
Marc

your code wrong?
*pitch = *pitch * 0.98 + pitchAcc * 0.02; <=== where is the angle in first term?

From the given Eq.
angle = 0.98*(angle+gryData*dt) + 0.02(accData)

So It should be?
*Oldpitch = 0.98* (*Oldpitch + Pitch)+ pitchAcc * 0.02;
when
pitch += ((float)gyrData[0] / GYROSCOPE_SENSITIVITY) * dt; // Angle around the X-axis

No, the code I posted is correct.

can you give me the source code to read data from MPU-6050?
my email is: nguyentiensu91@gmail.com
thanks!

Hi!
why did you choose the scale is -2G to +2G (why didn't -4G to +4G?), the sensitive of the accelerometer is 8192 (0.5G) and the sensitive of the gyroscope is 65.536?
thanks!

Hi ,
Can You please send me source code for reading data from MPU600 module.

Hi!
First thank you for sharing your work, it is well explained and very helpful to me.
I juste have one question about the results you had : how much accurate do you think this method could be? Is the error approximatively 5°, 1°, 0.1°?
I think it is very interesting to have good results with a simple method!

how can i use this filter for 9dof (gy-80) ?

Hi Pieter,

I will soon be working on a quadrotor and I will be using the MPU-6000 IMU.
While waiting for my mcu, I started looking into how I should treat the gyro and accel data and I had started looking into the Kalman filter. As you said, it is quite difficult to implement and all so I was quite happy when I found your blog in which you used the complimentary filter.
I was wondering if you could send me your code so I can better understand how you acquire the accelerometer and gyroscope data as well as how it's implemented in the complimentary filter.
my email is: jonathan_rompre@hotmail.com

Thank you very much.

Hi, Peter !!

thanks for ur work .. please, do u mind if send me the code of reading from MU6050 .?!!

i am working on platform STM32F303 ( i know different Gyroscope and ACC model ) i am interesting to ss ur code .. my mail mostafa.e.mansour@gmail.com

Best regards

Hi my friend! I want to say that this post is amazing, great written and come with
approximately all vital infos. I would like to see more posts like this
.

Hi Pieter-Jan,

your code calculates roll and pitch. Is it possible to calculate yaw also?

Kind regards

i need code for integration of gyro signal,

Pages

Add new comment

Replacing the Raspberry Pi's SD Card Socket

Not so long ago I dropped my Raspberry Pi. It fell on the SD card, which resulted in breaking the slot. After a long search on the internet it was clear that a lot of people had the exact same problem, but all of them solved it by gluing the slot back together in some way or another. Because this solution was not satisfactory for me, I replaced my slot with a new one. In this article I will describe how to do that.

Choosing an appropriate replacement slot

First we need to know how the SD Card slot of the Raspberry Pi is built up. This is shown in the figure below. (Sorry it is hand drawn and everything, but I really didn't have the time to make proper figures on the computer)


As you can see it is a 13-pin slot, using 2 pins for both for the "Write Protect" and "Card Detect". This is a problem, as most of the available slots we can find at electronic parts vendors have either 9 pins (no "Card Detect" and "Write Protect") or 11 pins, using just one pin for each function.

I chose to use a 11-pin slot that I found on ebay. Most of the 11 pin slots have a similar layout. It's very cheap and shown in the picture below.

The layout of the slot is given in the picture below:

Because of the bad positioning of the "Write Protect" and "Card Detect" pins, we will not be able to attatch these pins correctly to the Raspberry Pi. We can solder the "Card Detect" pins on the pi together, so they make a short circuit and the Pi will work. I don't think this is a big problem. And correct me if I'm wrong, but I might have read somewhere that the "Wire Protect" is not even used.

Desoldering the remainders of the old slot & soldering the new slot in place

First get rid of the old slot by desoldering all the broken parts!

Then just solder on the new slot and leave the pins "Card Detect" and "Write Protect" from the SD connector floating (or cut them off if you want to be safe). The other pins should nicely align with the solder pads on the Raspberry Pi. Use the figures (poorly drawn sketches) above for reference and proper alignment. Make sure to short circuit the "Card Detect" solder pads on the Raspberry Pi. For mechanical strength, I added a wire as tightly as possible to the slot and the solder pads of the Raspberry Pi. It is shown in the picture below. I strongly recommend doing this, as it will increase the lifetime of the slot A LOT.


Enjoying the enhanced Pi

Now you should be done. When you plug in your SD Card and power on the Pi, everything should work as it did before. Enjoy your frankenberry pi!


It even still fits in a pibow!

Comments

Neat job - and you got your Pi back! You might have a problem if you want to put it in a case, though. I did the same job (http://exartemarte.site40.net/?p=4), using a half-size cardholder which fits within the Pi envelope, and a Dremel to cut through the contacts before unsoldering so as to minimise the risk to the board.

Half-size card holders with the same pinout as yours are also available on eBay: http://www.ebay.co.uk/itm/221122087175?ssPageName=STRK:MEWNX:IT&_trksid=...

Thanks! I didn't think of using a half-sized SD slot. It still fits inside the standard cases though. I added a picture of mine in a PiBow for those who are concerned!

Hey, thanks - just what I was looking for!

Honestly, I think a larger case would be good to better protect the memory card, particularly for dedicated applications (where you don't need to remove the card often...) maybe two inches longer to accommodate the SD card and a WiFi USB. I think I'll make one!

While I was fully able to send a simple service
and choose which specific services they would meet you.
As with Sympathy Flowers do vary, I want a visitation or a four-story
office building, as durability is not felled best chiropractor in orlando it will save a
lot easier for them. These are placed in electric compartments for the environment than cremation. Granted, best chiropractor
in orlando different Michigan funeral homes.

Adsense is actually a really great program for those who maintain blogs, as blogs get updated all
the time and the Adsense possibilities are almost limitless.
The website speed test at Secret Search Engine
Labs will analyze how fast a page on your site is loading
and give you tips on how to improve it. You need to make your potential customers aware of your products and
services to ensure that they recognize them as valid solutions to their everyday
problems.

Pages

Add new comment

Reading Nintendo 64 controller with PIC microcontroller

I have a few old N64 controllers lying around and figured that it would be pretty cool to use them to control other things. In this article I will describe in detail every step I took to achieve this. I've used a PIC microcontroller, but it should not be too hard to port the code to any other architecture.

Connector

The N64 controller has three connections. From right to left with the round side up: Ground, Data and VCC (3.3V). Just plug some cables in the holes and you're ready to go.

Hardware

The circuit I created for reading out the controller is shown in the schematic below. It is based around a PIC18F2550. I chose a pic from the 18F range, because I am interested in the USB capabilities, as I might try to use the N64 controller with a computer some day (future article maybe?). I did however limit my oscillator frequency to 20 MHz, so that everything should work on a 16F pic as well if you don't need USB.

I've connected three LED's to RB5, RB4 and RB3. They will provide some visual feedback so we can see if everything works properly. A 3.3V voltage regulator provides the voltage for the board. The maximum voltage the N64 controller can handle is 3.6V, so don't exceed that. If you use 5V you will risk frying your controller. The N64 controller is connected to the board with a simple screw connector. Here the supply voltage is given, and the data pin is connected to the RB7 pin of the PIC18F2550.

I've also connected a header (JP1) for the PICKit2. This allows me to program the PIC without having to take it out of the circuit (ICSP). At the moment it also provides the supply voltage to the board because I am to lazy to find a good power supply. Make sure you set the PICKit to 3.3V instead of the default 5V for the reason I mentioned earlier.

The image below shows the fully built circuit built on perfboard. The rectangular thing to the right of the microcontroller is a wireless RF transmitter. I will talk about this in a next article, but it is of no importance to us now.

Interface

Now we're going to take a quick look at the communication with the Nintendo 64 controller. The interface looks a little bit like one wire, but is still different. It's a serial interface, which means that all the signals discussed here, will take place on one pin: the DATA (middle) pin of the connector (white wire in the pictures above).

Signal Shapes

The signal shapes used by the N64 controller for one and zero are shown below. As you can see these signals are very fast with a period of 4 uS and a resolution of 1 us! On a PIC microcontroller, one instruction cycle equals 4 oscillator cycles, which means that with a 20 MHz oscillator, 1 uS takes only 5 instruction cycles! To generate these signals, we will need a low-level programming language like assembly, as C just won't cut it. I will give as much information as needed, so if this is your first experience with assembly, you will be able to follow easily. A good reference for the PIC instruction set can be found here: PIC Instructions.

Zero
One
movlw 0x00
movwf PORTB
nop
nop
nop
nop
nop
nop
nop
nop
nop
nop
nop
nop
nop
movlw 0x80
movwf PORTB
nop
nop
nop
5
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
1
2
3
4
movlw 0x00
movwf PORTB
nop
nop
nop
movlw 0x80
movwf PORTB
nop
nop
nop
nop
nop
nop
nop
nop
nop
nop
nop
nop
nop
15
1
2
3
4
5
1
2
3
4
5
6
7
8
9
10
11
12
13
14

Below both signal shapes I have included a very simple program to generate the signal in asm (of course there are other possibilities e.g. with bsf and bcf). There are only three different instructions used:

  • movlw: place the value that you indicate in the W register.
  • movwf: place the value that is currently in the W register, in the register that you indicate.
  • nop: do nothing.
It's easy to see that a combo of movlw with movwf can set a pin by modifying the PORTB register. As our N64 controller is connected to pin RB7, we set this pin high by sending the binary signal 0b10000000 to PORTB, which is 0x80 in hexadecimals. Ofcouse sending 0x00 makes the pin 0.

All three instructions we used take 1 instruction cycle (check the PIC 18F instruction set here). This means that it is very easy to count every step. As I said previously: 1 uS takes 5 instruction cycles. That means that, to generate a zero, we have to set pin RB7 low, wait for 15 instructions, and then set it high for 5 instructions. I have counted and indicated the instructions that the pin is high with a bold font, and the instructions that it is low with a gray font. You have to think about these code blocks as if a lot of them are cascaded after each other, so that the first instruction of every code block, is actually the last instruction of the previous block. This will make the timing exact.

Of course this code will only work with an oscillator frequency of 20 MHz on a PIC microcontroller. If you have another oscillator you can easily calculate how many instruction cycles you need for 1 uS though. If you are using another architecture (AVR, ARM, ...) you have to figure out how many oscillator cycles one instruction takes first (it is usually less than 4, which makes it easier to use C on those architectures).

Controller Data

The N64 controller data consists of a 32 bit (4 bytes), which gives you the status (on/off) of all buttons and the joystick position (2 bytes) and one stop bit. The array is built up like this:

0
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16-23
24-31
32
A
B
Z
Start
Up
Down
Left
Right
/
/
L
R
C-Up
C-Down
C-Left
C-Right
X-Axis
Y-Axis
Stop bit (1)
e.g. if the Z button is pressed, the 3th element of this array will read one etc.

The position of the joystick can be determined from the two bytes given by "X-Axis" and "Y-Axis".

Polling Signal

To receive the data array from the controller, a polling signal has to be sent first. This signal is: 0b000000011 (9 bits). After this signal is sent, the controller will respond with the 33 bit array (4 bytes + stop bit). This is shown in the picture below. The B-button is pushed so the second bit of the response is 1.

The assembly code used to generate the polling signal is shown below. Instead of copy pasting 7 zero blocks and 2 one blocks from above (which would work perfectly). I used the decfsz (decrement file, skip if zero) instruction to create loops. This instruction will decrement the register you indicate, and skip the next instruction if this register becomes zero. It takes two instruction cycles if it skips, else one! The registers d1, d2 and d3 can be defined in the beginning of the program using a cblock. After the decfsz instruction there is a goto instruction, which will move the program counter to the instruction after the word that you have indicated. It should be clear to see how a combo of these two instructions create a loop.

All instructions use one instruction cycle (4 oscillator cycles), except for: goto, which uses 2 instruction cycles. Of course decfsz also takes two instruction cycles when it skips, as mentioned earlier. Its not so hard to distinguish the two code blocks we saw earlier, only this time I tried to make them a little shorter by replacing some of the nop's with loops (zeroloop, oneloop). If you take the time to count the instruction cycles carefully for each loop (which I strongly recommend), you will see that there would be no difference if we would copy paste 7 zero's and 2 one's from the previous code.

bcf TRISB, 7            ; make RB7 output
    
; 0b000000011
movlw 0x07              ; number of zeros that need to be sent
movwf d2

movlw 0x02              ; number of ones that need to be sent
movwf d3

zero
    movlw 0x00
    movwf PORTB
    movlw 0x03
    movwf d1
    zeroloop            ; 3 instruction cycles * 3 = 9
        decfsz d1, f
        goto zeroloop
    nop
    nop
    movlw 0x80
    movwf PORTB
	decfsz d2
    goto zero

one
    movlw 0x00
    movwf PORTB
    nop
    nop
	nop
    movlw 0x80
    movwf PORTB
    movlw 0x02
    movwf d1
    oneloop             ; 4 instruction cycles * 2 = 8
        nop
        decfsz d1, f
        goto oneloop
    decfsz d3
goto one

note: the pin RB7 is set to function as an output pin by setting the TRISB register (duh!).

In the image below you can see a picture of an oscilloscope measuring the polling signal generated with the code above. You can clearly see the signal 0b00000011.

This is a close up of the 7th (0) and 8th (1) bit of the polling signal. The timebase is set to 1 uS. As you can see, the signal is extremely correct.

If you now connect the N64 controller, you should see that it responds immediately after the polling signal was sent. I have shown this in the pictures below. In the second picture the B button is pressed, so you can see that the second bit of the response is a 1. Once you attach the controller, the signal wil deform because of its impedance. This is normal and not really a problem for us.


Reading The Controller Data

Now we succeeded in triggering the Nintendo 64 controller so that it sends its data, we have to find a way to read & store this data properly. The easiest way to do this, is to wait until the middle of the signal (approx 2 uS) and then see if it is one or zero with the btfss (bit file, skip if set) instruction. This instruction will check the bit you indicate (e.g. "PORTB, 7" -> 7th bit of the PORTB register) and then skip the next instruction if this bit is one. It will take one instruction cycle, except if it skips when it will take 2 instruction cycles (it compensates automatically for the skipped instruction - nice!). Of course, we have to switch the RB7 pin to an intput now by setting it to 1 in the TRISB register.

Once we know if the bit was a 0 or a 1, we can store this somewhere in the RAM by using indirect addressing. The INDF0 register contains the value of the register pointed to by FSR0L, so if we increment FSR0L, we go to the next adress etc.

We can now read and store all values, so we will do this 33 times as there are 33 bits in the N64 array (see higher).

movlw 0x22              ; 0x21 = 33 bits
movwf d4            	; contains the number of buttons

movlw 0x20              ; set up the array adress for indirect adressing
movwf FSR0L
    
bsf TRISB, 7            ; RB7 as input 
    
readLoop                ; read all 33 bits and store them on locations 0x20 etc.
    nop                 ; Wait
    nop
    nop
    nop
    nop
    nop
    nop
    movlw 0xFF          ; if bit is one
    btfss PORTB, 7      ; read the bit (9th instruction)
    movlw 0x00          ; if bit is zero
    movwf INDF0         ; Wait & store the value of the bit in the array
    incf FSR0L          ; Go to next adres in the array
    nop
    nop			
    nop
    nop
    nop
    decfsz d4           ; end loop if all 33 bits are read
goto readLoop

note 1: the syntax for indirect addressing is a little bit different on 16F architectures. They use FSR and INDF.

note 2: this is definitely not the best way to read out a serial signal. A better way would be to re-sync the signal after every bit, by searching for a rising edge and then falling edge. This way you know for sure that you are at the beginning of the pulse form. I have tried to do this, but it seemed impossible with the slow pic (4 oscillator cycles / instruction cycle). You might succeed with this approach on an AVR though (1 oscillator cycle / instruction). Please let me know if you find a cleaner way! (This works perfectly though ...)

The Full Assembly (ASM) program

The full assembly program is given below. It blinks the leds RB3, RB4 and RB5 respectively when the buttons A, B & Z are pressed. I made use of the mpasm (v5.46) compiler which comes with the MPLAB X IDE. It's probably pretty easy to figure out what everything does if you look up the instructions on the webpage I referred to earlier, so I won't go into detail here.

#include <P18F2550.INC>

CONFIG WDT = OFF        ; disable watchdog timer
CONFIG FOSC = HS        ; Oscillator crystal of 20MHz!
CONFIG LVP = OFF        ; Low-Voltage programming disabled
CONFIG DEBUG = OFF
CONFIG CP0 = OFF
CONFIG CP1 = OFF
CONFIG MCLRE = OFF

org 0

init
    cblock
        d1
        d2              
        d3           
        d4              
    endc

    movlw 0x00
    movwf LATB
    movwf TRISB
    movwf PORTB

    movlw 0x80              ; initalise the line = HIGH
    movwf PORTB

Main
    ; Part 1 - Read the N64 controller and store the status values

    movlw 0x22              ; 0x21 = 33 bits
    movwf d4                ; contains the number of buttons

    movlw 0x20              ; set up the array adress
    movwf FSR0L

    bcf TRISB, 7            ; make RB7 output
    call poll               ; Send the polling signal (0b000000011)

    bsf TRISB, 7            ; RB7 as input  (is also last instruction of poll)

    readLoop                ; read all 33 bits and store them on locations 0x20 etc.

        nop                 ; Wait
        nop
        nop
        nop
        nop
        nop
        nop

        movlw 0xFF          ; if bit is one
        btfss PORTB, 7      ; read the bit (9th instruction)
        movlw 0x00          ; if bit is zero

        movwf INDF0         ; Wait & store the value of the bit in the array
        incf FSR0L          ; Go to next adres in the array

        nop
        nop
        nop
        nop
        nop

        decfsz d4           ; end loop if all 33 bits are read
    goto readLoop

    ; Blink some leds

    movlw 0x20              ; read A button (0x20) from array
    movwf FSR0L
    movlw 0x20              ; led RB5
    btfsc INDF0, 0
    iorwf LATB, PORTB

    incf FSR0L              ; read B button (0x21) from array
    movlw 0x10              ; led RB4
    btfsc INDF0, 0
    iorwf LATB, PORTB

    incf FSR0L              ; read Z button (0x22) from array
    movlw 0x08              ; led RB3
    btfsc INDF0, 0
    iorwf LATB, PORTB

    call delay
goto Main

poll                        ; 0b000000011
    movlw 0x07              ; number of zeros that need to be sent
    movwf d2

    movlw 0x02              ; number of ones that need to be sent
    movwf d3

    zero
        movlw 0x00
        movwf PORTB
        movlw 0x03
        movwf d1
        zeroloop            ; 9 instruction cycles
            decfsz d1
            goto zeroloop
        nop
        nop
        movlw 0x80
        movwf PORTB
        decfsz d2
        goto zero

    one
        movlw 0x00
        movwf PORTB
        nop
        nop
        nop
        movlw 0x80
        movwf PORTB
        movlw 0x02
        movwf d1
        oneloop             ; 8 instruction cycles
            nop
            decfsz d1
            goto oneloop
        decfsz d3
        goto one
return


delay   movlw   0x5f        ; +- 1500 uS = 1.5 ms
        movwf   d1
outer   movlw   0x60
        movwf   d2
inner   nop
        nop
        decfsz  d2
        goto    inner       ; Inner loop = 5 cycles = 1uS
        decfsz  d1
        goto    outer       ; outer loop = inner + 4 cycles
return

end

Video

Here is a small video of the result. I'm sorry that I didn't do anything cooler with it than blinking some leds, but that's for when I have some more time!

Comments

I have a little question, You have to send the polling signal everytime you want the controller to send the data right? could you do this so that you can use two controllers at the same time alternating between both of them? Im working on a project that uses 4 controllers but I just have access to 2 data lines so I was planning on spliting the times between both of them. Thanks! Your work is awesome

Well I have another question, What about RublePak, ExpansionPak and other extensions that were connected to the controller? do you know how to access them? I will dig a little more under this subject anyway. Thanks (again)

I have just found this: http://svn.navi.cx/misc/trunk/wasabi/devices/cube64/notes/n64-observations and yes there were other codes two so Thanks anyway ´cause it uses the same way of connection and that makes your post realy useful!

Hi,
I was wondering if I can use PWM input to treat the signals, you think this is possible, instead of pure assembly code?
Thanks

This is definitely possible, and can be used for increased certainty about the signal being received. For example, if you sample at 2 or 3us the pull up resistor may be (almost never is with marketed products, but may be) too slow for a 1 to have been read. By oversampling the data you can get a ratio of high to low and determine the received signal that way.

You really make it seem so easy with your presentation but I find this matter to be actually something which I think I would never understand.
It seems too complicated and extremely broad for me.
I'm looking forward for your next post, I will try to get the hang of it!

I think the admin of this site is genuinely working hard for his web page, as
here every data is quality based stuff.

Ferrets are captivating little pets with their cute faces and comical antics.
If your dog is still a puppy then chances are introducing the two would go
pretty smooth. Spa Presents - Take care of her to a day at a beauty spa exactly where she can loosen up and rejuvenate.

Pages

Add new comment

Pages

Subscribe to Front page feed