Category Archives: ahrs

Stability pids

While waiting for the new components to arrive I’ve worked a bit on the stability pids.

The stability pids are responsible of controlling the orientation of the quad. So if I tell the quad to lean forward 30 degrees, the stab pids have to figure out how to control the motors lean forward with exactly 30 degrees. Same for leaning left/right.

Silkopter uses quaternions to represent rotations so it’s not as simple as just subtracting the pitch and roll from my target pitch and roll and use the delta. First of all I want to avoid as much as possible using euler angles because of the dreaded gimbal lock. Second of all there’s the wrapping problem of euler angles – going from 0 to 360 or from -180 to 180 which I always get wrong.

So quaternion it is.

My approach is this:

  • First compute the target quaternion from the input. I treat the input as euler angles and build a quaternion fro them. This presents no issue as the input is limited by design to +- 90 degrees. Let’s call this target_q
  • Get the current quad quaternion. This represents the orientation of the quad relative to the world frame of reference (the Earth). Let’s call this uav_q
  • Now compute their difference – or how much apart is target_q from uav_q
    • diff_q = inverse(uav_q) * target_q 
      • where inverse(q) = quat(-q.x, -q.y, -q.z, q.w)
    • This step is the key – by doing the math with quaternions instead of euler angles I avoid the gimbal lock. Also, the quat difference give me the shortest path between the 2 rotations
  • I convert this q_diff to euler and this is how much I have to rotate in each axis to get to my desired orientation. These represent the stability pid inputs – how much the uav deviates from the desired pose – and the stab pids targets is 0.
  • I process the pids and the error is send as the target of the rate pids.
  • The rate pid outputs is sent to the motor mixer which in turn decides – based on the geometry of the uav – what motors need more or less thrust.
  • Repeat this 200 times per second.

Here’s a video of the quad simulation trying to match a pose. I control the pose (the 3 axis system to the right) with a ps3 controller and the quad is matching the pose using the algorithm above. It’s rock solid even if I move the joysticks randomly and it always follows the minimal path to the target rotation. Thank you, quaternions!

HW is ready

3 weeks ago I did some vibration tests with all the motors at half throttle and I realized I really need to properly isolate the IMU. I was getting +- 8G in all the axes most of the time. This was driving crazy my AHRS code so something had to be done.

It took one week of designing and printing a new case but I managed to incorporate some dampers.

Here are some pics of the case:

The case with the sonar and Dtotek 10dof IMU mounted. The imu sits on some spongy double-sided tape.case_sonar_imu

Now the the raspberry pi on top:

This is the Quattro ESC + Alfa wifi board that will be mounted on top of the raspi.wifi_esc_voltage

The whole case is mounted on the frame with 8 vibration dampers that I recovered from a some camera gimbals.

First test showed a big improvement. Accelerometer vibration dropped to +-1G but now I get some low frequency oscillation in the gyro, probably due to the resonance frequencies of the dampers. I will try soon to put some ear plugs in the center of the rubber dampers to see what happens.

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.


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:


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:





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.


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.