Category Archives: gyro

10DOF IMU #2

I2C it is then. After 3 days of trying to get SPI to work with the drotek imu I gave up and switched to i2c.

As expected it worked from the first try. Well, 10th try actually. It seems that if you disable the FIFO and then try to read it repeatedly weird things happen… This is why it seemed like i2c couldn’t keep up with my data rate.


So now I have the accelerometer&gyro sampled at 1000Hz, the compass at 100Hz and the baro/thermometer at 50Hz – all using a 400Khz i2c frequency. It all fits into 200Khz as well so I still have room to increase the sample rate if I need to.

I did a quick test with 800Khz and 1.6Mhz i2c frequencies and to my surprise it worked perfectly. I’ll give this another try when I resume the dead reckoning tests. It should allow me 8Khz gyro/accel sampling frequency.

So right now I’m back to where I was one week ago. Time to resume flight tests in the simulator.

Hopefully next weekend I’ll fly.

SPI, I2C and reading schematics

So I got this imu: 

It has 4 jumpers on it – one controls i2c/spi, 2 control the i2c address for the mpu9250 and MS5611 baro and another one is for pull-up resistors when using i2c.

The ‘documentation’ shows which jumpers to use when using i2c and which ones when using spi:

After reading all of this I decided to use SPI, so I switch the i2c/spi jumper and disconnect the pull-up resistors.

One day later I still cannot get it to say anything else than zero. So I switch to I2C thinking the sensor might be bad and voila – it talks. And it talks a lot! Turns out I2C can’t keep up with the data rate I’m interested ( >= 1Khz ) so I try one more time to get SPI to work without any success. After checking everything – wiring, the raspi, software – I take another look at the schematics: and notice that the i2c address selection on the MPU9250 is connected to the SDO pin!!! So I disconnected it and finally get some 0xFF along with lots of zeros… Now I’m trying to get it to say something more meaningful than garbage but hey:



My new 10 DOF (degrees of freedom) IMU (inertial measurement unit) is here!

Why I like it:

– MPU9250 chip with the 6500 acceletometer/gyro unit. This one is better than the common 6000/6050 unit I already had. The gyro in particular has less drift.

– The board is small so I can place it closer to the center of gravity. This will avoids polluting the accelerometer readings when the UAV rotates

– SPI interface. I’ll be able to read this from the raspberry pi at 4Khz ideally. It has a fifo so I shouldn’t be bothered by thread jitter. Reading the gyro and accelerometer at 4Khz should result in 384Kbits/s which is below the 1Mbit/s SPI clock rate of the MPU9250.

I have to write a new IO_Board implementation for the brain but it’s not a big deal. Most of the code is the same as the current crius board code I’m using.

Accelerometer calibration testing

Just fixed some bugs in the sensor simulation and first thing I did was to test the accelerometer calibration code.

I configured some noise in the sensor – 0.1g, a big bias (2.5, 1, -0.3) and some scale (0.9, 1.1, 1.04) and ran the 6 step Gauss-Newton calibration.



To my surprise the algorithm converged to almost the exact sim input I configured:


The scale found is actually 1/sim scale which is ok.

The error is ~0.01 for the bias and ~0.005 for the scale.


I must say I’m impressed. One less thing to worry about.

Next on the list is calibrating at 2 temperatures to be able to compensate temp drift.




Found the gyro issue.

I had this code:

if (gyro_data.sample_idx > m_last_gyro_sample_idx)
m_last_gyro_sample_idx = gyro_data.sample_idx;

//integrate gyro

Somehow a 90 degree rotation ended up as 110 degree. I checked everything – sampling the imu, sending data through serial to the raspberry pi, everything. Except the type of m_last_gyro_sample_idx – which turned out to be uint8_t for some reason. So sample_idx was always > than the last idx after the first 250ms of runtime. Since the raspberry pi is fast, the main UAV loop executes at around 1200Hz while I get data at 1000Hz.

Changed it to uint32_t and voila! perfect integration of angular velocity and the dead reckoning is back to 1cm per second while idle. Still not happy with this but it’s waay better than yesterday.

Things make sense again.


Now it’s time to test some other AHRS:




MPU6000 FIFO #2

Just realized a new way to use the fifo to avoid duplicated data.

Instead of routing all the data to the fifo, I will route just gyro.x – so 2 bytes.

Then, when polling the mpu:

1. request the fifo size

2. if == to the old fifo size, return. No new data available

3. read all data from the fifo and discard it.

4. read data from the registers instead of the fifo


This has the advantage of not relying on a certain packet structure in the fifo and is immune to fifo overflow issues.

I will test it on sunday and see how it works.


The ideal way to read data from the mpu is with an interrupt. I cannot do this as the interrupt pin is not connected on the crius board. So I’m polling the imu at 1Khz hoping the data is ready but as it turns out, the avr and the imu clocks don’t exactly match. For this reason I sometimes get the same sample twice in a row or skip a sample. This results in errors in the gyro integration – a 90 degree rotation in one axis is interpreted as a 89 or 91 degrees for example.

FIFO to the rescue. The mpu6000/6050 has a built in fifo that you can interrogate regularly. By polling the fifo counter I know if enough data has arrived and read it – or if not enough just wait for it.

The problem is that the FIFO – being a First In First Out buffer – can be read only from the back – the oldest data. And if the buffer overflows (it has 1024 bytes) the oldest data gets discarded.

For this reason I have to make sure that the data packet size is a divisor of 1024 so that when a new packet arrives and overflows the 1024 bytes buffer, it discards one entire packet and not a fragment only.

My packet has 14 bytes right now – 6 for the accelerometer, 6 for the gyro and 2 for temperature – so whenever there is an overflow I get corrupted data from the fifo as I’m not reading whole packets anymore – but some data from one and the rest from the next one..

My temporary solution is to throw away data from the fifo whenever there’s too much in it – say more than packet size * 10.

Something like:

if (fifo_size > packet_size * 10)
i2c::discard(mpu_addr, fifo_rw_register, fifo_size - packet_size); //leave one packet intact
fifo_size = packet_size;
if (fifo_size >= packet_size)
i2c::read(mpu_addr, fifo_rw_register, packet);

This solved most of the too-much-rate problem but not entirely. I still need to debug it a bit more..


Integrating gyros and complimentary filters

I’m integrating the gyro angular rate into a quaternion at 500Hz with this code:

auto av = gyroscope.value.angular_velocity*dt;
auto const& a = m_local_to_world_quat;
auto hd = av*0.5f;
float w = /*(hd.w * a.w)*/ - (hd.x * a.x) - (hd.y * a.y) - (hd.z * a.z);
float x = (hd.x * a.w) /*+ (hd.w * a.x)*/ + (hd.z * a.y) - (hd.y * a.z);
float y = (hd.y * a.w) /*+ (hd.w * a.y)*/ + (hd.x * a.z) - (hd.z * a.x);
float z = (hd.z * a.w) /*+ (hd.w * a.z)*/ + (hd.y * a.x) - (hd.x * a.y);
m_local_to_world_quat.x += x;
m_local_to_world_quat.y += y;
m_local_to_world_quat.z += z;
m_local_to_world_quat.w += w;


It gives me a very accurate rotation with very little drift – if the sensor is properly calibrated.

To eliminate the drift I use a complimentary filter between the gyro quaternion and another (noisy) quaternion built from the compass and accelerometer. The gyro quat captures high frequency rotation and the accelerometer/compass quat captures low frequency rotations but is very noisy on short term. To combine them I do this:

m_local_to_world_quat = math::lerp(m_local_to_world_quat, noisy_quat, mu);

Where mu is the interpolation factor. Initially I used 0.01, big enough to compensate for gyro drift and gyro cross-axis winding and saturation. But it’s too big and the m_local_to_world quat catches a bit of accelerometer noise.
I switched to a variable mu and so far it seems much better:

float mu = 0.001f + math::length(av);

The 0.001 is big enough to compensate for idle drift – but not too big. The av is the gyro rate sample – so the bigger the rotation rate, the bigger the mu.