Monthly Archives: July 2014


Just found this:

It will launch in August and it’s a smaller alternative to the raspberry pi.

It also comes with several ADC and a real-time clock, a step-up converter (for powering it from a 3.7v lipo), more GPIO and best of all – raspicam compatibility.

It runs stock raspbian.

I think I’ll order one. It will allow me to make a smaller quadcopter case and possibly get rid of the crius board entirely and replace it with a way smaller IMU.


I’ve managed to get some of the simulator UI and rendering done.

For now the sensors will be simulated by taking the uav rigid body (cylinder) state and applying some random white noise to it. I’ll probably have to add some very low frequency noise to simulate temperature drift.

This data is then sent to the brain board through a tcp channel with the same interface as the serial channel the brain uses to talk to the io_board (the avr). The simulator basically replaces the io_board with a bullet physics simulation.



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:





I managed to get the mpu6000 fifo done and I now have pretty good gyro readings. It seems like I was getting 20-30 duplicate samples per second.

Here’s how the Ground Station sensor page looks like:


The bottom 3 sensors also have an average graphed (the blue line).


With the gyro fixed, I tested the dead reckoning and god pretty bad results.


Top graph is linear acceleration, middle one is velocity and bottom one position. They are reset every 10 seconds. So the bottom graph shows 2-3m of drift when the quad is perfectly still.

This indicated a mismatch between the quad rotation I’m calculating and the real rotation. So the gravity cancellation doesn’t work correctly.


This needs more work.


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 #2

The result of going 1000Hz is… total disaster. Rotating the quad 90 degrees results in a 105 degree rotation in the AHRS so I either integrate too much rate or I’m doing it wrong.

Digging through the internets I found these:


Long story short, I was doing it wrong. My current code is as follows:


//integrate gyros
if (gyroscope.sample_idx != m_last_gyroscope_sample_idx)
m_last_gyroscope_sample_idx = gyroscope.sample_idx;
auto av = gyroscope.value.angular_velocity*dt*0.5f;
auto hd = av; //the rate quat
auto const& a = m_local_to_world_quat;
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;

I’m basically treating the angular velocity as the axis of a quaternion and adding it to the current rotation. The correct way – apparently – is to build the hd quaternion like this:

float l = math::length(av);
float s = sin(l) / l;
hd.x = av.x * s;
hd.y = av.y * s;
hd.z = av.z * s;
hd.w = cos(l);

I kind of get why – the rates can be expressed as an angle axis rotation – with the angle == length(av) and axis == av. But not sure why the sin is divided by the length.

Well, I’ll see tomorrow.



By removing the rate pid from the crius, sending quantized data instead of floats through the serial port and compiling with -DNDEBUG to remove asserts I managed to send data at 1000Hz. I can go up to 1800Hz but since the MPU6050 can sample at max 1000 there’s no need for that.

Good to know that I still have extra CPU for when I’ll add the GPS code.


So 1000Hz is possible through a serial port at 5000000 baud, with cpu to spare.


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.


UAV simulator

How does one write UAV controller software without crashing a single UAV? By using a simulator, of course.

It turns out that simulating a UAV is pretty simple actually. A UAV can be abstracted like this:

– a simple rigid body with 4 forces – the propellers – pushing upward based on their throttle

– gyro value is the angular momentum of the rigid body + any drift noise you want to simulate

– accelerometer value is the body acceleration

– compass is some random world vector projected in the bodys space.

– air friction is 4 forces pushing in the direction opposite to the movement on 4 arms, with a strength depending on orientation of the quad and some random coefficient


This is enough to test the rate/stab pids and ahrs. Add a simulated GPS and you can test RTL. And so on.


The tricky part is to have the same code base running in the simulator and on the actual UAV. The more the better. Since raspberry pi is debian – just like ubuntu – this is actually pretty simple and it boils down, in my case, to have a io_board simulator. That is – the code that runs on the AVR crius board.


The next step is to add a UI for the simulator where I can configure sensor noise and fail rates and wind.