Monthly Archives: August 2014

…And the code is up

You can find the WIP silkopter code here:


It’s quite a lot of code as it includes a 3d engine used in the ground station.

It depends on boost, freetype2 and bullet physics for the simulator.

I use QTCreator on Ubuntu 14.04 for development but the code should work just fine on windows as 6 months ago I used to develop in Visual Studio 2013.


The code is published under GPL.



Motor Mixing #2

I think I have a better way to mix motors that preserves both throttle and torque at the expense of dynamic range.

What it does is first compute the maximum dynamic range allowed from the current throttle and the min/max setting. If the throttle is close to min, the available dynamic range is small as to avoid going below min. Same when close to max.

The best dynamic range is when the throttle is in the middle.

Code is below:

void Motor_Mixer::update_mixing()
    math::vec3f speeds2d(-m_roll, m_pitch, 0.f);

    //first calculate the possible dynamic range from the throttle and min/max
    float dynamic_range = 0.f;
        float min_v = math::max(m_throttle - m_min_output, 0.f);
        float max_v = math::max(m_max_output - m_throttle, 0.f);
        dynamic_range = math::min(max_v, min_v);

    for (auto& motor: m_motors)
        float output = math::dot(math::vec3f(, 0.f), speeds2d);
        if (
            output += m_yaw;
            output -= m_yaw;
        output = math::clamp(output, -dynamic_range, dynamic_range) + math::max(m_throttle, m_min_output);
        motor.output = output;
        QASSERT(output >= m_min_output);
        QASSERT(output <= m_max_output);

The only problem I can see with this is with full throttle. In this case the motors are at max and they usually have very different max thrust – so the UAV will tend to rotate one way or another. Since at max throttle I also have zero dynamic range I won’t be able to compensate the UAV rotation. I need to handle this case in a different way.

Maybe at max thrust (or over half thrust) I should favor dynamic range over preserving throttle… This way I get maximum maneuverability over raw climbing rate which is desirable…

For now I’ll just limit the max to 70-80% throttle to leave some dyn range for stability.


TODO list

Here’s my todo list:

* means completed
& is almost done, a bit more work needed

*- Video_Server: multi stream encoding
* – high res 720-900p for file saving

* – medim res 360/480p for streaming
* – low res 140p for emergency streaming
&- Video_Server: camera controls – shutter speed, saturation, contrast, brightness, iso
*- Input: secure TCP communication channel
*- Sensor_Board: comm channel with the crius
*- crius FW
* – idle mode
* – armed mode
* – calibration mode
*- GS: ground station calibration update
* rate pid
* UAV inputs – ouya controller
* Camera inputs
* Sim: simulator update
*- drotek 10dof imu support

– fix motor mixer
– raspi PWM output tests using servoblaster
– stability pid
– statibility tests
– latency tests
– GPS code
– odroid-w support
– ADC for current/voltage sensors
– open source the code

– basic flight modes
– idle
– armed
– flying
– auto take off
– auto land

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.

Mixing motors

I flew the quad in the simulator this weekend trying to tune the rate PIDs.

First problem is the D factor which is very sensitive to sensor noise. For this I added a butterworth filter with a 5Hz cutoff. I’ll probably need to tune this later.

Next problem is that my motor mixing doesn’t preserve total thrust, but instead tries to preserve angular velocity. Rookie mistake I presume.

First a stolen picture of a quad in ‘plus’ configuration:


Motor 1 is towards the front of the quad, 2 to the right, 3 is in the back and 4 to the left. As you can tell from the picture, diagonally opposite motors spin in the same direction – so 1 & 3 spin clockwise while 2 & 4 spin counter-clockwise.

If all motors were spinning in the same direction, the whole quad would start to spin in the opposite direction to counter the rotation of the motors. For this reason the motors spin the way they do – to counter this effect.

Now if the quad wants to go forward, it runs motor 3 faster and motor 1 slower causing it to tilt forward. This will get it to accelerate forward as some of the downward thrust goes backwards now.

Same for going left, right and backwards.

If the quad wants to rotate clockwise, it spins motors 2 & 4 faster than motors 1 & 3. This causes the quad frame to spin in the opposite direction than motors 2&4 – so clockwise. 

Back to the mixing:

The inputs to the mixer are 4 quantities:

1. collective throttle

2. yaw rotation factor – rotation along the Z axis obtained with differential thrust of diagonal motors

3. pitch factor – rotation along the X axis

4. roll factor – along the Y axis

The mixer then proceeds to calculate the throttle of each motor based on their position in the X-Y plane. The hard constraints are 1 – never go below a min throttle to prevent the motor stopping and 2 – never go above full throttle (because physics..)

I had code in place to satisfy these hard constraints in a clever way but it turns out I wasn’t so clever. Take this for example:

//check if under minimum
    if (min < m_min_output)
        for (auto& motor: m_motors)
            motor.output += m_min_output - min;
            max = math::max(max, motor.output);
        min = m_min_output;
    //check if over maximum
    if (max > m_max_output)
        for (auto& motor: m_motors)
            motor.output -= max - m_max_output;
            min = math::min(min, motor.output);
        max = m_max_output;

If a motor goes above max (100%), distribute the spill to all other motors. Same when going below min (5%).

Turns out this doesn’t preserve total throttle, but it does preserve angular velocity.

As a consequence sometimes my quad would fly away even at min throttle.

Time to fix this.

Control Scheme

Since I’ll be using a PS3/Ouya controller for the UAV I have to come up with an alternative way to control the throttle since the left stick is centered.

This is what I came up with:

 enum class Throttle_Mode : uint8_t
 RATE, //throttle rate of change, per second. 
 STABLE, //-1 .. 1 are offsets from current throttle. 
 ASSISTED, //climb speed, meters per second

In RATE mode, the user controls the throttle rate of change. When centered, the UAV will keep the current throttle. Stick up/down will increase/decrease the throttle with a rate proportional to how much the stick is moved.

In STABLE mode the user can offset the current throttle with a value that depends on the stick. Centering the stick will revert to the initial throttle.

ASSISTED mode is basically altitude hold, and the stick controls the rate of ascend/descend in meters per second based on a configured range of speeds.

The advantage of these modes is that switching from one to the other is always consistent without any risk, since a centered stick – in all modes – means no change to the throttle.

Pitch/Roll are controlled by this:

enum class Pitch_Roll_Mode : uint8_t
 RATE, //angle rate of change - radians per second
 STABLE, //angle from horizontal. zero means horizontal
 ASSISTED, //speed, meters per second

RATE is the classic rate mode where the sticks control the rotation rate of the UAV.

In STABLE mode the sticks control the angle with the horizontal and once released, the UAV returns to horizontal.

ASSISTED mode is position hold. The user controls the forward/backwards and lateral speed in m/s in this mode.

Again, transitions are well defined and safe.

For now yaw is always in rate mode.

This simplified the input code quite a bit by treating ALT_HOLD and POSITION_HOLD as input modes instead as flight modes.

Let’s see where this leads. Tonight I’ll have the UAV fly in the simulator.

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.