Fun with the Complementary Filter / MultiWii

I first started using a complementary filter, not even knowing what it was called at the time, on the DIY Segway project six years ago. It's a convenient way to combine measurements from an accelerometer and an angular rate sensor (gyro) into a better angle estimate than either could provide on its own. The single-axis case (like on a self-balancing vehicle or robot) is very simple, especially if the balancing platform stays close to horizontal where the small angle approximation can be used to linearize the accelerometer measurement. This is the case I wrote up in the Balance Filter white paper.

Since then, I've used the 1DOF small-angle complementary filter on a bunch of projects, including Segstick, Mini-Segstick (which was really just Segstick in disguise), and some quadrotors.

Wait, but quadrotors are not constrained to a single axis of rotation. They have the ability to pitch, roll, and yaw, as well as translate in 3D. A full representation of orientation in 3D is itself a difficult concept to grasp. I've mostly avoided the complexity by working only with small pitch and roll angles, using a separate rate-only controller for yaw. On 4pcb, the pitch and roll complementary filters are completely independent, and take up only four lines of code in total.

Recently, I found a similar implementation of independent pitch/roll complementary filters in the firmware for the KK2.0 flight controller. It was written in a different form than the one I've used on my projects, which got me thinking about equivalent forms. There are now at least three different implementations I've seen that are functionally identical. Anyway, the complementary filter-based angle estimate was disabled in the KK2.0 firmware as of v1.2, and I wondered why that was since it seemed to fly well in my testing. My question was answered by KK himself:

It treats the Roll and Pitch axis separately. It keeps track of the absolute angle by integrating the output from the gyro on that axis, and corrects the angle with a small part of the angle from the accelerometer on that axis when it is within a certain range (+- 10 degrees or something like that).

This angle is then fed into the control PI loop resulting in roll/pitch angles controlled by the stick. 

Unfortunately it was not that easy. It works very well as long as no yaw is commanded when the craft is not level, as seen in the video earlier in the thread. If yaw is applied when not level, both roll and pitch angles change, but roll/pitch gyros does not sense this rotation, resulting in wrong angles. Try for your self with a airplane model in your hand.

So, the scenario of concern is when then the normal vector of the quadrotor isn't vertical. Then, rotating about that normal axis (yaw) will cause the pitch and roll angles to change in a way that isn't measurable by the X and Y rate gyros. This animation shows yaw with and without the normal axis vertical. The accelerometers will still pick up the changing pitch and roll angles, but they are low-pass filtered in the complementary filter so they will only update slowly. Rapid yaw while not horizontal could result in bad angle estimates, and consequently a loss of control.

Tangent Alert:

Now is about the time I would normally expect someone to suggest using a Kalman filter. I would argue that the choice between a Kalman filter and a complementary filter lies on a completely different axis (of decision making...) than the choice of 3D angle representation. For example, you could have a complementary filter acting on quaternions, which would solve the above problem by having a complete set of 3D kinematic equations. Conversely, you can have a Kalman filter operating independently on each single axis.

Until recently, I hadn't seen a good, well-explained example of a Kalman filter applied to a single axis. Most sites refer to it as more of a buzzword, hand-waiving the explanation and using copy-pasted code from another project. However, this post lives up to its title by completely developing a simple and easy to implement Kalman filter for a single rotational axis. It uses a method I only recently began to appreciate where the rate sensor signal is treated as an input, rather than a measurement. The state equations are thus purely kinematic; no information is required about the plant dynamics. The states are the angle and the rate sensor bias. The output is the accelerometer-measured angle, either from a linearized approximation or an arctan operation.

One interesting thing I learned about the Kalman filters in 2.KalmanFilters is that in the bone-stock Kalman filter with known, constant process and measurement noise variance (Q and R), it's possible to pre-compute the time-dependent covariance matrix (and hence the Kalman gains) before you even take your first measurement. This is definitely true in the above-mentioned post: P[i][j], S, K[0], and K[1] don't depend on states or measurements. If the filter is stable, the gains should converge after some amount of time. If they do, the remaining state observer looks like this:

rate = newRate - bias;
angle += dt * rate;
y = newAngle - angle;
angle += K[0] * y;
bias += K[1] * y;

This simple algorithm is very similar to Implementation #3 of the complementary filter from my previous post, the method that is in the KK2.0 firmware. It's based on feedback of the error between the angle prediction using the rate sensor alone and the angle measurement using the accelerometer. In this case, K[0] sets the time constant of the complementary filter. This implementation has a nice addition, though: a bias estimator that can track an unknown and variable gyro bias over time. The rate at which the bias estimate is updated based on the output error is set by K[1].

End of tangent.

I finally understand how a single-axis Kalman filter might work, but it won't solve the problem at hand, which is cross-axis coupling between yaw rate and pitch/roll angle on the quadrotor. I'm not quite ready for a full 3D angle representation, though. I'm a fan of inexpensive hardware like the KK2.0 or my recently-acquired HobbyKing MultiWii clone, both of which are under $30 and have a full 6DOF inertial sensor set (plus more, in the case of the MultiWii clone). However, they use 8-bit fixed-point processors that would struggle to do the math required for a full 3D angle estimation algorithm.

Actually...that's not really true. As far as I can tell from reading the source code, MultiWii now implements an angle estimation algorithm that covers any possible orientation and would have no trouble handling the yaw-while-not-horizontal scenario. It's based on a complementary filter and uses an interesting representation that (I think) would not have problems with gimbal lock. (Pitch and roll are always uniquely defined based on the orientation of the gravity vector with respect to the frame. Heading is treated separately.) Interestingly, the problem of yaw while not level is addressed in the second page of this development thread.

I guess the real reason why I want to implement a simple complementary filter is because I will always prefer a piece of code that I developed from start to finish, even if it's inferior to something that already exists. I like understanding how all the pieces work, including their limitations. So, I'd rather make a small improvement to my existing angle estimation code that solves the problem of yaw rate coupling. With that in mind, I erased my MultiWii and started from scratch...

// integrate pitch rate into pitch angle
int_pitch = angle_pitch + gyro_pitch * DT;
// integrate component of yaw rate into pitch angle
int_pitch += angle_roll * INV_RAD * gyro_yaw * DT;
// filter with error feedback from pitch accelerometer
error_pitch = accel_pitch - int_pitch;
angle_pitch = int_pitch + AA * error_pitch;

// integrate roll rate into roll angle
int_roll = angle_roll + gyro_roll * DT;
// integrate component of yaw rate into roll angle
int_roll -= angle_pitch * INV_RAD * gyro_yaw * DT;
// filter with error feedback from roll accelerometer
error_roll = accel_roll - int_roll;
angle_roll = int_roll + AA * error_roll;

This Implementation #3 again, but with an extra term to include a component of yaw rate in the integration from the previous angle to the gyro-only new angle estimate. The component of yaw rate integrated into pitch is proportional to the roll angle. The component of yaw rate integrated into roll is proportional to the (negative) pitch angle. This linear approximation should hold for small angles (less than 30º) to within 5%. It should provide reasonable performance at the cost of two extra lines of code with some multiplies (still no trig).

To test it, I made a MultiWii-on-a-stick that I could spin around in a cordless drill...

Since it obviously couldn't be USB-tethered, this required an on-board battery and an XBee radio connected to the TX pin of the ATmega328. (It's broken out for use with an external serial LCD module.) This way, I could power it, tilt to some angle, and yaw like crazy while collecting data wirelessly.

First, I did a test with the yaw terms turned off, making the two complementary filters independent again. This is what I have running on 4pcb, but 4pcb is so twitchy anyway I wouldn't have noticed any quirks with yaw-while-not-horizontal. As expected, at a constant angle of about 30º to horizontal, yawing causes the pitch and roll angles to do strange things:

The complementary filter was set to a time constant of 0.99s, so even a modest yaw rate of 65º/s was enough to cause major offset problems due to the accelerometer lag. At higher rates (570º/s), it's hopeless. The filter barely has time to react before a complete rotation is finished, so it barely registers a change in orientation at all. Here's what it looks like in phase space:

If all were working as it should, the initial -30º roll angle would trace out a circle of 30º radius (yes, the radius is now measured in degrees) as it transforms into pitch angle, then back to roll, etc. Clearly, all is not working as it should. At the higher yaw rate, the "circle" becomes a blob off in one corner.

With the extra integration terms from the yaw rate turned on , the result is much better:

Now it seems to have no trouble with yawing at any rate right up to the gyro limit of 2000º/s. The initial angle traces out a nice circle in phase space as the yaw rate shifts it back and forth between pitch and roll. There's a little bit of skew, but it could very likely be from my hack spinny board mount.

So that fixes the yaw problem. I can already think of other minor tweaks to make that wouldn't cost any trig or matrix math. But I think this was the biggest of them. Now that I've taken care of reading in sensors and estimating angles, I think I should have no trouble finishing up my ultra-simple MultiWii control software. My goal is to have a working flight controller in less than 500 lines of (Arduino) code. (The sensor reading and angle filtering code is only 150 lines, 4pcb's entire flight controller is 495 lines.)  

It will only work for "small angles", but with +/-30º of pitch and roll range (45º in a pinch), it should handle the non-acrobatic flying that I do. Maybe when I get myself an STM32F3Discovery board I will do a full 3D flight controller with real math. In the mean time, I'll also attempt to implement the yaw decoupling terms on the KK2.0, but that will be a bit harder since it's all written in assembly.

FF v1.1 and v1.2s Design Files

I'm finally getting around to collecting and organizing the hardware documentation for my two newest motor controller designs. These are the FF line motor controllers, which I've been testing for a few months now. They're small single-board three-phase motor controllers, designed for sensorless field-oriented control. v1.1 is the larger, higher-power version and v1.2s is the smaller, lower-power version.

FF v1.1 (left) and v1.2s (right).

Both versions share the same logic layout. The microcontroller used is the STM32F103C4, although the design should be pin-compatible with many other 48-pin STM32 microcontrollers, including some with floating-point capability. It can be programmed with a 3.3V FTDI adapter or wirelessly over XBee. The firmware is still very much a work-in-progress, but I've also uploaded two different firmware versions (IAR EWARM Kickstart Projects) here, for reference only:

1. Air Firmware (STM32_air.zip): Framework for closed-loop RPM control based on 1000-2000μs RC-style PWM input. Inner loop is field-oriented current control based on sensorless position estimate.
   
2. Ground Firmware (STM32_ground.zip): Framework for torque-controlled electric vehicles using field-oriented current control. The input is an analog throttle signal. There is an optional ramping start-up but it's not perfect.

Note that the firmwares are my working copies, not release versions. I won't even give them version numbers because they're that non-functional. Repeat: This will not run your motor. Each motor needs parameter tuning and the only way to do that right now is using hard-coded constants in the firmware. I'm only uploading them as examples that others can refer to for development. Also note that this hardware cannot do six-step BLDC control. It's designed with sinusoidal commutation / SVM and synchronous rectification in mind.

Both versions of the controller hardware also share the same gate drive solution, based on the Texas Instruments DRV8301. I call this the magic everything chip, because it's much more than just a nice 2A three-phase gate driver. It also has dual low-side phase current amplifiers, integrated gate and logic power supplies, integrated bootstrap diodes, and a completely independent buck controller with internal switch for creating a logic supply for the rest of the board.

Both versions also have the same I/O broken out, including pins for analog throttle, I2C, UART (shared with the FTDI programming port and with the XBee headers), RC-style PWM input, and three auxiliary inputs that can be either timer capture inputs (encoder), hall sensor inputs, phase voltage sensing, or general-purpose analog inputs. 3.3V and 5V supplies are also broken out to pin headers.

The differences between v1.1 and v1.2s are entirely in the power section of the board. v1.1 has a beefier power stage based on D2Pak-7 MOSFETs, while v1.2s has a smaller Super SO-8 MOSFET layout. The details and design files are below:

FF v1.1 [Design Files]

Size (board only): 3.00in x 1.50 in x 0.50in

Size (w/ external cap): 4.20in x 1.50in x 0.50in

Weight (board only): 28g

Weight (w/ wires and cap): 66g

Suggested MOSFETs: IRFS3004-7PPbf, IPB017N06N3 G, IPB010N06N, IRFS3107-7PPbf

Suggested External Capacitor: 680uF, 63V (x2 for high current)

Approx. Power: 48V / 30A (cont.), 75A (peak)
BOM Cost: $96.90 (1x), $74.38 (10x), $53.39 (100x)

This version of the controller was originally designed with large multirotors in mind, but it's also been tested on a number of ground vehicles at up to 75A acceleration current (the maximum phase current at full throttle). The continuous current capability is significantly lower due to the lack of large heat sink. 30A is my best guess with any of the MOSFETs listed above, but with adequate airflow it could be higher. The maximum voltage depends on the capacitors and MOSFETs used, but is limited to 50V by other components.

FF v1.2s [Design Files]

Size (board only): 2.00in x 1.50 in x 0.28in
Size (w/ external cap): 2.79in x 1.50in x 0.32in
Weight (board only): 11g
Weight (w/ wires and cap): 35g
Suggested MOSFETs: BSC016N04LS G, BSC010N04LS, BSC014N06NS
Suggested External Capacitor: 470uF, 35V (x2 for high current)
Approx. Power: 48V / 15A (cont.), 30A (peak)
BOM Cost: $56.89 (1x), $47.14 (10x), $33.32 (100x)

This version was designed with smaller multirotors in mind, although it should be fine on the not-so-heavily-loaded CineStar 6 as well. It's not really suitable for vehicles, but might work fine for robots / RC cars /  small servo-mechanisms. The maximum voltage and current depend on the MOSFETs and external capacitor chosen, but it should have no trouble at 15A continuous and 30A peak, higher with more air flow or heat sinking.

That's it for now. Later I hope to post a lengthy document outlining the sensorless position estimation algorithm at work in the reference firmware. Even later I hope to make a GUI for configuring the firmware for a particular motor. For now, you're on your own with that.

Excellent Video Presentation on Field Oriented Control

Here's a really great video presentation on Field Oriented Control (FOC) by Dave Wilson of Texas Instruments:

I was lucky enough to see this presentation live at a TI workshop in the spring. It covers all the basics of (sensorless) FOC, which is what most of my motor controllers are using now. By itself, it's probably not enough to write an FOC controller from scratch, but combined with the other information on TI's Motor Blog, most of the important techniques are covered. Of course, for any gaps there is still the ultimate guide to brushless motors, James Mevey's 2009 Masters Thesis.

In case you're wondering how the back EMF observer in this presentation compares to the dirt-simple flux observer I've been using in all of my FOC code, here's roughly one page of math converting between them:

The conclusion is that my dirt-simple flux observer differs from the back EMF observer in the presentation by only a second-order low-pass filter (2DLPF). This is pretty amazing since the structure of the two observers, and even the quantity that they are observing, seem so different. 

The properties of the extra 2DLPF are determined by the PI controller gains in the current error feedback of the back EMF observer. Higher gains give a lower time constant and faster tracking, but also more sensitivity to current sensor noise. My flux observer doesn't have this degree of freedom; it trusts the current sensor all the time and is therefore more susceptible to noise on that signal. (It's the extreme case of the feedback gains being very high.)

Speaking of flux observers...I promise I will write up my Gen. 1 sensorless algorithm soon...

KK2.0 Testing / Talon Cam Returns

After finding the makings of a complementary filter in the KK2.0 flight controller firmware and enabling it on the Talon Quad, I've been flying it for several days to test out the fast angle control. I've heard from a number of other people who have tried it with mixed results. Several people got it working, confirming that the self-leveling is faster and doesn't overshoot like crazy. But since it has, in at least two cases, lead to crashes, I'll just reiterate the warning that the modified firmware should be considered experimental and tested at ones own risk!

One thing that's 100% certain is that the self-level is set up for small angles. Do not expect it to recover from any position at any time. Do not try to fly acrobatically with self-level on. For one, the accelerometer component is mixed in only for angles that are less than a certain deviation from the horizontal. There's some confusion about what the maximum angle for which accelerometers are factored in is - I would guess based on my reading of the code that it's 26º.

Since the raw accelerometer inputs are treated linearly, the small angle approximation would allow less than 5% deviation from the true angle up to 30º and less than 10% deviation up to 45º. I might try increasing the maximum value for accelerometer merging up to the higher limit of 45º, just to give it more workspace for aggressive flying. Past 45º, the linearizations really start to break down and it would be wise to switch to a gyro-only estimate and hope for the best. (This is assuming that trig operations or other methods of handling large angles are out of the question on an 8-bit microcontroller.)

That's not the only problem, though. As KK himself points out in the now-226-page RCGroups thread, the simple controller based on two complementary filters begins to lose its fast attitude estimate when you combine a yaw command with either a pitch or a roll command:

Unfortunately it was not that easy. It works very well as long as no yaw is commanded when the craft is not level, as seen in the video earlier in the thread. If yaw is applied when not level, both roll and pitch angles change, but roll/pitch gyros does not sense this rotation, resulting in wrong angles. Try for your self with a airplane model in your hand.

That, plus a more direct warning:

In short: only yaw when level. 

Mathematically it makes sense. If you do happen to yaw while within the 26º (?) envelope, the accelerometer readings can still help the angles recover over time. But the fast gyro update is lost. If you yaw while outside the 26º envelope, all bets are off. Since my flying is decidedly conservative, it's possible that the problem had just never come up. So, more aggressive flying is called for.

I felt comfortable enough to put the GoPro back on. It's basically indestructible anyway, and it's a good way of registering the angle of the quad. You can see that I attempted, mostly successfully, some long moderately banked turns. This seems like the most likely scenario where a yaw + roll input would occur and where the sensors have enough time to stray from correct values. While in the turns, I had no trouble maintaining attitude. Coming out of the turns and returning to level flight was a bit trickier (I had one unintentional landing resulting in no damage), but not at all impossible.

Granted, I never really got past 30º in the banked turns. It could start to fail more catastrophically at steeper angles. But the ironic thing is that, until now, I've never felt comfortable enough with the performance of the self-leveling to even attempt these maneuvers. I can also fly nose-in and at larger height with much more confidence thanks to the faster and more stable angle control.

I can think of ways to handle the yaw + roll and yaw + pitch inputs at small angles (45º or less), and they only require math that the ATmega324PA should be able to handle. Maybe I will dig into the assembly more and try to implement something. But, 4pcb has lived this long with nothing but independent pitch/roll complementary filters. (Not that it's the most smooth-flying thing in the world...) Some day I'd like to make my own flight controller from scratch, maybe with a bit more fancy math on a 32-bit processor. But right now this is working very well for me.

More FFv1.2s Testing / The Short-Lived FFv1.3ss

After successfully testing one FFv1.2s motor controller on the Talon quad, I got enough parts to complete a full set of six.

Unlike the first prototype, I built these in the "minimum thickness" configuration, meaning no XBee or XBee headers. They can only be programmed with an FTDI breakout board. They also use lower-profile pushbuttons and inductors. Here's a comparison to the "minimum thickness" configuration of FFv1.1:

The overall dimensions are 2.00in x 1.50in x 7mm and 11.3g for the FFv1.2s board alone. (FFv1.1 board-only dimensions are 3.00in x 1.50in x 12mm and 28.4g.) With 16AWG wiring, an external DC bus capacitor, and connectors, the FFv1.2s controller weighs in at 35.1g. This makes it a much better fit for the Talon quad than the 66.0g FFv1.1 with giant D2Pak-7 FETs at 14AWG wiring.

With a cameo by Twitch.

I also switched back to the KK2.0 flight controller to make the Talon quad as light as possible. (You will see why in a bit...) I did a bit of firmware hacking on the KK2.0 to enable the fast angle estimate, which I describe in the previous post. The result is quite amazing: it now has very precise and fast angle control. After the first indoor test from the previous post, I have been further tuning it and flying outside. It's wonderfully stable and easy to fly now, even in some wind.

The controllers are all running closed-loop speed control, so the signal received is directly commanding an RPM. So far this has been working well. Here's a data capture from some outdoor testing with the KK2.0 + FFv1.2s ESC combination:

It shows several seconds of rapid back-and-forth roll inputs, followed by a few seconds of hovering, then a few seconds of climb. The RPM tracks well, although there are some small and rapid oscillations similar to what can be sort-of seen in the indoor test video. The oscillations are especially apparent in the current data. This is, I suspect, because the inner rate loop gains were slightly too high. Testing in the wind with lower rate P gain yielded much smoother flight (still with wonderful fast self-leveling).

The reason for reverting to the lightest possible configuration for the Talon wasn't to go easy on the new controllers, though. It was to try an ambitious payload. This quad normally carries a GoPro camera, which weighs about 200g with its impact-resistant case. The heaviest thing it's carried is my handheld video camera, at 300g. But with the new motors, I suspect it could carry a lot more. Enter TOBL2.

Photo credit: ycraf.blogspot.com.

TOBL2 is the second generation of Max's multifaceted iPhone-controlled robot, probably best known for it's wall-flip ability. It will be making an appearance at Marker Faire NY in September and we thought it would be cool to try to skycrane it with a quadrotor. At about 1lb (450g), though, the first step was to see if the Talon quad could even lift it...

Well that was relatively successful. Other than the swaying, which can probably be minimized with a better rope configuration, the payload seems to be fine. Now we just have to see if TOBL can lower itself down somehow...

The Short-Lived FFv1.3ss

I've been watching Texas Instruments' line of motor drive chips for a while now. In addition to the DRV8301, which has performed flawlessly as a gate driver / current sense amplifier / buck converter controller on FFv1.1 and FFv1.2s, I've been wanting to try out the DRV8332. The DRV8332 is a fully integrated three-phase driver, meaning logic-in / FET half bridge-out. It goes up to 50V and the heatsinkable version (8332) has a phase current rating of 8A continuous, 13A peak. 

And it's tiny. Perfect for FFv1.3ss (super-small?), I thought, so I started a layout for a 1.00in x 1.00in controller based on the DRV8332. To show just how small it is, here's a comparison to the IXYS mini-brick modules I use on the 3ph line of motor controllers:

The 1.00in x 1.00in board is roughly half the board area as the IXYS module alone. The blue chip on the back of the board is the DRV8332 itself. Unlike the DRV8301, it doesn't include the nice buck converter controller or current shunt amplifier, so there was a good deal of work to do adding tiny versions of those in:

The MSOP-8 on the left side of the board is a dual op-amp, configured for low-side current sense differential inputs using two resistor networks. I really liked this layout. The cluster of components on the right side of the board are two switching power supplies. The first, a wide-input buck converter based on the  LT3991-5, would allow me to take full advantage of the 50V input range of the DRV8332 while still supplying an efficient 5V/500mA BEC for logic and receiver power. The second, a tiny boost converter based on the LT3460, would supply 12V/100mA for the gate drive.

Sadly, that's as far as I got before shelving this design. It has a fundamental flaw that I hadn't considered until this point: the FETs in the DRV8332 are not that great. Even though its current rating says it would be a viable controller for something the size of the Talon quad, the FET losses make it not worth it. Even at a relatively modest phase current amplitude of 5A, the 80mΩ per-phase resistance would add 3W of total dissipation to the controller (with sinusoidal commutation). Compare this to the minuscule 0.075W that FFv1.2s would dissipate with with its Super SO-8 FETs. 

At 150W/kg, the rough power-to-weight ratio for the Talon quad, the extra 2.9W of dissipation would be equivalent to 19.3g of weight. So, to justify the tradeoff, the FFv1.3ss board would have to weigh 19.3g (or more) less than the FFv1.2s. (Assuming the wiring and capacitors weights are fixed, as they would be for a given current.) Since the FFv1.2s boards are only 11.3g, excluding wiring and caps, there is no way the extra dissipation of the FFv1.3ss is justified. 

Factoring in the weight of the necessary heat sink for the DRV8332, I'm not sure there is a likely scenario where it would be justified based on a power-to-weight analysis. If volume is weighted more highly than weight, as it might be for some other applications like a mini robot, the DRV8332 might win out. It might also be justified on size alone at very low current where the efficiency is less of an issue. But in that case, there are things like the Toshiba TB6588FG that I used on 4pcb which could probably do the job.

It's too bad, because I was really starting to like this layout. But it seems like it wasn't meant to be, as far as flying things are concerned.

KK2.0 Firmware Mod: Unleash the Complementary Filter

The KK2.0 is HobbyKing's ultra-cheap ($30) multirotor flight controller, which I bought a few of for testing some time ago. Unlike its predecessor, the KK2.0 has a full 6DOF IMU with an InvenSense 3-axis gyro and an Analog Devices three-axis accelerometer.

That means it should be fully-capable of doing angle control (a.k.a. attitude control, self-leveling), where the joystick axes correspond to a commanded pitch and roll angle, and center-stick returns the multirotor to level. This is in contrast to rate control, where the joystick axes correspond to a commanded pitch and roll rate, and center-stick holds the current angle (but does not return the quadrotor to level).

Rate control is good for doing tricks and flips, but angle control is better in most other cases when you want to stay within say +/-30º of horizontal. If you get disoriented, you can go to center-stick and know that the quadrotor will return to level and not go rocketing off into the distance. It's also more well-suited to camera platforms, since it helps maintain a level horizon.

The KK2.0 has a "self-leveling" feature that I was very eager to try, but I was a little disappointed in the performance. It was sluggish, and increasing the gains to make it return to level faster would lead to pendulum-like behavior, especially in the wind. It would eventually return to level, but not before overshooting back and forth a couple times.

Skimming through the 200+ page RCGroups thread for the KK2.0, which is frequently updated by the designer of the KK board, it seems that in the initial firmwares (v1.0, v1.1, and v1.2), self-leveling is implemented using only the accelerometer angle estimate. From the tuning guide,

Q: Why is Autolevel so slow?

A: The autolevel is based only on the ACC. It takes some time before the ACC registers tilt due to the horizontal acceleration of the craft when tilting. It needs some speed to get air resistance and a force on the ACC. Therefore the autolevel works best on draggy crafts. Later I will try to implement a AHRS algorithm, which uses both ACC and gyros to get a immediate tilt angle.

This all sounds very familiar. I have explored the specific problem of creating an angle estimate using both accelerometers and gyros at length on many projects, using a technique known as a complementary filter. I used it on all the self-balancing things I've built or helped build, as well as two quadrotors: 4pcb and the original Edgerton Center copter.

Tangent: Two complementary filter implementations in simple code.

I won't go into the theoretical details here, since they are well-explained in several of the links above, but I will put forward the typical code snippet I use to implement the complementary filter on a single axis of the accelerometer/gyro data. It takes as its input the gyro rate (gyro_rate) and the accelerometer angle (acc_angle), pre-converted into compatible units (deg and deg/s, or rad and rad/s, for example). The accelerometer angle can be calculated with an arctan function or by linearization for small angles.

// Complementary Filter Implementation #1:
filtered_angle = (A)*(filtered_angle+gyro_rate*dt); 
filtered_angle += (1-A)*acc_angle;

The previous angle estimate is incremented by the small angle predicted by multiplying the rate by the loop time (dt). This gyro-only angle estimate is high-pass filtered. The accelerometer-only estimate is low-pass filtered and the two estimates are added to created the new filtered angle estimate (filtered_angle). The time constant of the high-pass and low-pass filters (they are the same) is:

tau = A / (1-A) * dt;

and it depends on the loop time. Larger time constant trusts the gyro for longer and filters the accelerometer more. Making this time constant long enough to overcome the time required for buildup of aerodynamic drag mentioned in tuning FAQ is the key. However, if the gyro rate has a lot of bias, a time constant that is too large can lead to offset in the angle estimate.

Interestingly, the complementary filter can also be formulated as follows:

// Complementary Filter Implementation #2:
filtered_gyro = A*filtered_gyro + (1-A)*gyro_rate;
filtered_acc = A*filtered_acc + (1-A)*acc_angle;
filtered_angle = tau*filtered_gyro + filtered_acc;

Unwrapping the math to show that this is equivalent to Implementation #1 above is a little tricky, but it definitely works out. The nice thing about Implementation #2 is that it's just two low-pass filters, a multiply, and an add. It could be done entirely in analog hardware if one so desired.

Ok but what does this have to do with the KK2.0?

Well, while browsing through the open-source KK2.0 firmware v1.2 (the latest at the time of writing) I stumbled onto a curious section of code. Now, it's entirely written in assembly, and it's been a long time since I programmed in assembly, so it took me some time to interpret it. It's in imu.asm in a section labeled "Calculate Tilt Angle" and another section labeled "Correct tilt angle". If I'm reading it correctly, it behaves roughly like this:

// Complementary Filter Implementation #3:
filtered_angle = filtered_angle + gyro_rate*dt;
error_angle = acc_angle - filtered_angle;
filtered_angle = filtered_angle + (1-A)*error_angle;

And I label it as Implementation #3 because in fact, if you rearrange the math, you get exactly the same thing as the other two. This version is formulated more as an error feedback approach, which is intuitive.

The strange thing about this complementary filter isn't that it's written in a different way...it's that it's entirely commented out! The complementary-filtered angle estimate (I call it filtered_angle, the firmware uses GyroAnglePitch and GyroAngleRoll) is never used. Instead, the self-leveling angle estimate is straight from the accelerometer (what I call acc_angle, and the firmware calls AccAnglePitch and AccAngleRoll).

Maybe it's a planned future upgrade and needs more testing. In any case I decided to turn it on and see what happens. I uncommented the "Calculate Tilt Angle" and "Correct tilt angle" sections in imu.asm. Two other lines also needed to be changed:

b16sub Error, AccAngleRoll, RxRoll ;calculate error

becomes

b16sub Error, GyroAngleRoll, RxRoll ;calculate error

and

b16sub Error, AccAnglePitch, RxPitch ;calculate error

becomes

b16sub Error, GyroAnglePitch, RxPitch ;calculate error

And that's it. Here is the modified firmware I used. No other changes were made to the project. If you want to stare at the code or rebuild the project yourself, you'll need AVR Studio. Otherwise, KK2_1V2_modS.hex is the file to be flashed. Note: Use at your own risk! Most likely, this is a future upgrade that may not be fully ready yet!

I wasn't really expecting this to work so easily, but on the fist test flight after loading the new firmware, I was able to increase the self-level gain  a bit and get very nice angle control:

The stick response is very fast. Going back to center gives very fast return to level, with no overshoot and no pendulum effect. It now flies as well or better than other (way more expensive) flight controllers I've tried. I'm sure that by tuning the complementary filter time constant, the self-level P gain and the inner rate loop PI gains, I could get it to be even better.

So now my $30 flight controller looks like an incredibly good deal.

FFv1.2s: First Test

I got the boards for FFv1.2s, a slightly smaller version of my latest line of motor controllers:

The boards are from OSH Park (formerly DorkbotPDX PCB Order, you might be able to tell by the color scheme). It's a terrific deal for small boards: $5/in^2 for three boards. For example, this board was 3in^2, so I got six for $30. The total time from order to delivery was a bit under three weeks.

These boards have exactly the same logic section as v1.1, but an entirely redesigned, smaller power section. Instead of massive D2Pak-7 FETs, it uses Super SO-8s. These are extremely popular in RC ESCs because of their low cost, relatively high power density, and thin package (more compact and easier to heat sink). I'll be trying two different FETs: the Infineon BSC016N04LS G (40V, 1.6mΩ) and BSC028N06LS3 G (60V, 2.8mΩ).

One difficulty with using Super SO-8 FETs is that, although it's possible to solder them from the sides, good thermal performance will probably only be possible if the drain pad under the chip is reflowed. The DRV8301 magic chip needs its ground pad reflowed anyway, so this doesn't add any extra steps for me.

It's just one trip to the bagel toaster.

Even though v1.2s is only reduced to 2/3 of the area footprint of v1.1, there's also a significant reduction in height thanks to the low-profile FETs:

The left-most board is a v1.1 without any wires or external capacitors. The middle board is v1.2s with XBee headers fitted. The right-most board is v1.2s with no XBee headers. In the smallest configuration, it's 7mm in thickness. The tallest components are now the three pushbuttons, and even those are lower profile versions of the ones in v1.1.

Here's what it looks like wired up:

The total weight with 16AWG wire and connectors is 35g, compared to 66g for FFv1.1 with 14AWG wire. Most of the weight is in the wire and connectors. The FETs are nowhere near as beefy as the D2Pak-7s, but I think the 40V/1.6mΩ version might still be able to handle 20A continuous and 40A peak current. (Compared to ~40A continuous and 75A peak for v1.1.)

The first assembled board survived power-up without exploding, so therefore it was flightworthy.

Poor Talon quad - it really has become a test bed for just about every piece of hardware. It has so much stuff attached to it now in such a disorganized and heavy way. I put a single FFv1.2s on, loaded with identical firmware as the other three v1.1's. Since they're running closed-loop RPM control now, the slight mismatch shouldn't matter much. Here it is ready for its first test flight with the Chair of Safety still in place:

After a few successful indoor test flights, I also took it outside a few times with the data collection running. The data is from the v1.2s, which was fitted with the XBee. The other three v1.1's are running blind. Here are a couple data segments showing climbs and fast maneuvers:

RPM tracking looks good and the average current is around 5A, peak 10A. The FETs are room temperature and I suspect I won't be able to even get them warm until I try it out on the CineStar...

One more motor controller done. Time to start a new one. Here's the teaser: