Quadrocopter 1 – MPU-6050 Accelerometer and Gyro

One of the great aspects of Flowcode v6 is the ability to use the simulation to help create and debug real world devices. I am in the process of building a Quadrocopter using Flowcode v6 and will release details here as I make progress.

When starting a project like this it can often be easy to become lost in all the technical details being thrown around so lets start by keeping things simple. A good starting point would be to use the very nice MPU-6050 IC which contains a 3-axis accelerometer and 3-axis gyro. This device is perfect for a Quadrocopter project or any other balancing robotics as it provides fundamentals to allow the pitch and roll tilt angles to be reliably calculated. I purchased my MPU-6050 sensor on a breakout board from eBay for around £6 from the UK but there are lots of people now selling these modules so they should be easy to get hold of. The module also features a I2C Master mode to allow it to integrate with other I2C magnetometer sensors to create a 9 axis sensor capable of generating pitch, roll and yaw.

The accelerometer provides data which is very accurate in the long term but in the short term can be very noisy and subject to acceleration as well as the gravitational pitch and roll we are interested in. The gyroscope provides data which is very accurate in the short term but is subject to offset drift in the long term. Combining the two sensors will be covered later, for now we simply want to make some sense of the direct sensor readings.

I put the hardware together into a test rig by using a section of breadboard along with an ECIO40P module. The MPU-6050 was powered by using 2 x 200 Ohm resistors to create a potential divider to divide the 5V output from the ECIO into 2.5V. This 2.5V was then used for the I2C pullup resistors so as not to over voltage the I/O. The address pin was connected directly to ground.

To start the project I first created a simple Flowcode program on an ECIO40 (MPU-6050_ECIOUSB.fcfx) which sampled the data from the module using the MPU-6050 Flowcode component and then passed the data through to the PC a byte at a time using the USB Serial component. I used a ‘:’ character to signify the start of each section of data and a \n\r to signify the end and make things easier to read in a terminal window. When complete I downloaded the program to the ECIO, all the other programs are simulation only at the moment to interpret the values from the ECIO connected to the MPU-6050 sensor. I then used RealTerm to check that I had data coming in which changed as I moved the sensor module around. A Flowcode simulation with an RS232 component with a loop calling the receive function would work just as well by viewing the console window. Other sensors can be used by tweaking the property values to match those of your sensor.

Next I created another simple Flowcode program (MPU-6050_SimAccel.fcfx) to sit at the other end of the serial data to allow me to get the calculations correct for working out pitch and roll from the accelerometer data. Using a basic shape object on the Flowcode system panel I was able to replicate the pitch and roll from the sensor using the following equations.

Roll = atan2 (Y, Z) * (180 / PI)

Pitch = atan2 (X, sqrt((Y*Y) + (Z*Z))) * (180 / PI)

With the ECIO plugged in and the RS232 com port set to the right channel, clicking start on the simulation allows me to vary the pitch and roll of the object on the panel by rotating the accelerometer by hand.

The next step was to modify the Flowcode program (MPU-6050_SimAccelGyro.fcfx) to make sense of the gyroscope sensor so I created a second object on the panel to manipulate and went about working on the mathematics to covert the gyro readings into rotation coordinates.

Roll = Roll + (X * BitSensitivity * TimeOffset)

Pitch = Pitch + (Y * BitSensitivity * TimeOffset)

With the two sensors running together it is very easy to see the inherent noise of the accelerometer and the offset of the gyro after a few seconds of operation.

There are various ways to combine the two sensors together to create a stable reading. One way is to use what is called a Kalman filter which tries to guesstimate the next reading and then uses the error between the prediction and the actual readings to try to improve the prediction. For what we want this is probably overkill so first we will have a look at using a very basic complementary filter to combine the two sensor outputs. Thanks to the Flowcode simulation we can tune the filter to give us an accurate reading without slugging the response too much.

The next program (MPU-6050_SimFilter.fcfx) uses the following algorithm to calculate the pitch and roll rotation for a third panel object labelled filter. The Response Time property is used to control how responsive the filter is. Note that I had to swap the axis around a bit so the two sensors added together correctly which is why the object on the panel in the next picture seems to be tilting the wrong way.

a = ResponseTime / (ResponseTime * GyroTime)

Angle = a * (Angle + GyroAngle * GyroTime) + (1 – a) * AccelAngle

My next task will be to have a crack at using a Kalman filter to see if the response becomes even better. The filtered output seems very stable when the sensor is the correct way up but there is a bit of wierdness going on with my maths when the module is rotated upside down around the y axis so I will have a look into this problem (Edit: This is related to the missing Yaw rotation parameter, adding a magnetometer would allow the Yaw to be calculated and resolve this problem). Collecting the sensor angle for the accelerometer requires access to the atan2 function so I will either create a look up table to drive this on the ECIO or switch to a AVR or dsPIC device further down the road.

Here are all the Flowcode v6 source files from the project so far.

quad1

Download your free 30-day trial of Flowcode v6 from here: http://www.matrixmultimedia.com/flowcode.php