World Maker Faire: NY 2012

 

All packed up and ready to go to World Maker Faire 2012 in New York! Come find me and the rest of the MITERS group in Zone B, or just follow the gradient of ultralight go-karts, scooters, quadrotors, and Tesla coils. But unfortunately...

A bit more KK2.0 modding.

I haven't gotten around to implementing yaw decoupling on my modified KK2.0 firmware yet, but I wanted to make another quick fix that I think is actually more important for preventing failures. There's a scenario where the angle estimate used in self-level mode with the modified firmware could get latched into a range where no accelerometer correction is used:

One of the steps in modifying the firmware was to change the angle variable used to calculate the self-level error from the accelerometer-only angle to the filtered gyro+accelerometer angle. In code, I replaced:

b16sub Error, AccAngleRoll, RxRoll  ;calculate error

with

b16sub Error, GyroAngleRoll, RxRoll ;calculate error

and did the same for pitch. The variables GyroAngleRoll and GyroAnglePitch are the complementary-filtered angle estimates that use a combination of both sensors. However, when the angle is larger than a certain value (26º, I believe), the complementary filter is skipped and the accelerometer is ignored. The conditional that does this is:

b16ldi Temp, 600  ;skip correction at large tilt angles
b16cmp GyroAnglePitch, Temp
longbrge im40
b16cmp GyroAngleRoll, Temp
longbrge im40

b16neg Temp
b16cmp GyroAnglePitch, Temp
longbrlt im40
b16cmp GyroAngleRoll, Temp
longbrlt im40

The value 600 roughly corresponds to 26º (I think), and if the filtered angle estimate falls above that value (or below its negative) on either the pitch or the roll axis, the accelerometer correction is skipped. Only the integrated gyro signals are used to update the angle estimates in this condition.  If each axis was truly independent, this might work okay. The gyros can keep track of the angle for a short period of time until the quadrotor returns to level (or at least within 26º of level) and accelerometer correction comes back into play.

However, when the axes become coupled by yaw, the gyro-only angles are corrupted. Imagine pitching up 45º, then yawing left 90º. The true state of the quadrotor is now 0º pitch and 45º left roll. But, neither the roll gyro nor the pitch gyro has updated its angle, since the second input was yaw only. The controller will still think it's pitched up by 45º. And, because it's outside of the 26º bounds, the accelerometers aren't used to correct the problem. Now the controller is "latched" into the corrupted angles and it would be difficult to get it to self-level again.

This can also happen while the quadrotor is not armed. The angle estimator is running all the time, from as soon as the board gets power. So, if you plug in your multirotor, then pick it up and move it, and in the process of doing so cause the angles to become latched into incorrect states, as described above, then it will be nearly impossible to take off. Even worse, if you are flying in rate mode and the angles become latched in the state described above, it will dart off in one direction as soon as you switch into self-level mode.

The fix I tried was two-part: First, extend the range of the angle for which accelerometer correction is used to +/-45º from horizontal. The small-angle approximation for calculating the accelerometer-based angle without doing trig start to lose accuracy from 30-45º, but sin(π/4) / (π/4) ≈ 0.9, so the error is only still about 10%. Second, and more importantly, the angle that is used to evaluate the conditional for when the accelerometer correction is used is the accelerometer-only angle. This makes more sense to me anyway: whenever the accelerometer thinks the quad is near enough to level, it should be allowed to run the complementary filter. The further-modified code:

b16ldi Temp, 1042  ;skip correction at large tilt angles
b16cmp AccAnglePitch, Temp
longbrge im40
b16cmp AccAngleRoll, Temp
longbrge im40

b16neg Temp
b16cmp AccAnglePitch, Temp
longbrlt im40
b16cmp AccAngleRoll, Temp
longbrlt im40

The value 1042 corresponds to 45º (I think). AccAnglePitch and AccAngleRoll (the accelerometer-only angle estimates) are compared to this bound and if both are within +/-45º from horizontal, the complementary filter is allowed to correct the gyro angle estimates using the accelerometer. The major benefit of this modification is that the angles can't become permanently "latched" in a state where the accelerometer is ignored even though the quad is level for a while. As long as the accelerometer angles are within +/-45º, the complementary filter is used.

If the above made little-to-no sense, I also tried to illustrate the problem and the solution in a video:

You can see how the yaw coupling could cause the angle estimate to become "latched" in an invalid state (outside of 26º) before the controller is even armed, making it impossible to take off. The worse scenario is that the angle estimate becomes latched outside of 26º due to aggressive rate-mode flying. Then, when the controller is switched over to self-level mode in the air, it will have corrupted angle estimates and go darting off in one direction.

With the fix, the angle estimates are corrected before take-off even though the frame is yawed while at a steep angle before arming. As long as the quad goes back to level (or a state where the accelerometers think it's within 45º of being level), the complementary filter can run and correct the angles over time. It can't get stuck.

If you're interested in trying out the new mod, I have posted it here:

http://web.mit.edu/scolton/www/kk2_1V2_modS2.zip

You can get the whole AVR Studio 4 project if you're interested in seeing the code itself. The only file I've modified from the default firmware is imu.asm. The rest is stock. If you're only interested in the .hex, it's also in there: KK2_1V2_modS2.hex. As usual, try it at your own risk! It's not officially-supported firmware and it still has the yaw coupling problem.

That said, I've been enjoying the modified firmware w/ complementary filter enabled a lot. I wouldn't specifically recommend it to people because making recommendations only leads to trouble, but it definitely flies better and feel like a much more capable controller. Here's a bit more on-board video from the past day or two of testing the new mod:

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...