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: