Sunday, September 23, 2012

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:


28 comments:

  1. Very nice I will flash one of my boards tomorrow

    ReplyDelete
  2. Awesome work Shane! This is much more usable as a backup for Rate-Mode now.

    ReplyDelete
  3. A very quick and dirty way to do trig functions in well under a microsecond is do just use a lookup table. A 360 byte table will give trig function on one-degree increments to under 1% accuracy. For sub-degree resolution you can interpolate, but that is seldom necessary. You can also use two-byte or 4-byte table entries.

    ReplyDelete
    Replies
    1. Yep, I use sine tables in my motor controllers extensively. The arctan function (needed for computing angle from two accelerometer axes) is a bit harder, since the range of the argument is not bounded. But there are fast arctan approximations, for sure. The stock MultiWii code uses one, I think.

      Delete
  4. What is the refresh rate of your controllers

    ReplyDelete
    Replies
    1. Anything up to 1,000Hz. Presently they use about 490Hz, which is the output rate of the KK2.0 and the maximum output rate possible with 1-2ms PWMs.

      Delete
  5. This version of firmware is in Lazysero KKTool?
    Or do I need to download the firmware from here?
    Thanks!!!

    ReplyDelete
  6. Hi Shane, could I say a BIG thanks for your modified firmware, I am not he best pilot in the world and this mod has made the board great to fly. I have put about 15 batteries through it now with no problems at all. You have a great blog by the way.

    ReplyDelete
  7. HI Shane - this could be a noobie question but will this also work with the KK2.0 in tricopter mode?

    Cheers

    ReplyDelete
    Replies
    1. It should also work in tricopter mode, although I haven't tried it myself.

      Delete
    2. works pretty good in my tricopter - so far anyway...

      Delete
  8. Hi Shane !

    Great great great job ... Finaly a Self-level who do his job !! (Sorry for my poor English ...i'm Little Frenchie ^^)
    No more weird reaction since the last "patch" , level soooo fast , with 60 of p-gain in AL , 40 of Stick scaling (via D/R switch on my radio , 90 in rate mode) . Some video on my Youtube channel : http://www.youtube.com/user/opossome64/

    ++

    ReplyDelete
    Replies
    1. Looks great! I really like the idea of using a dual-rate switch to change the stick scaling when switching from rate mode to self-level mode. Otherwise, you have to turn the stick scaling down to keep the self-level from going to extreme angles but it makes the rate response too slow. I'll have to try out your solution on my fancier transmitter at some point.

      Delete
  9. Hi Mister Colton

    In france we are someone who have test your firmware
    and we are all very satisfy with your work for the "self level"
    congratulation
    Can't you take a look on the "height dampening" and make it work find please
    many thanks in advance
    christophe

    ReplyDelete
    Replies
    1. Thanks, I'm glad to hear that it works well for you. I do fly with height dampening on with a very low gain (5-10) and it help me a lot with altitude control, although it makes the throttle response slower. I'm not sure how I would improve it without an absolute altitude measurement from a barometer.

      Delete
  10. Thats an awesome improvement, thanks a lot!

    Only question I have: If I use autolevel and quickly move a stick to the right, when going back to level it oscillates a bit (overshoots hard, corrects) - can you tell me what parameters affect this? I've tried autolevel gains but then it just goes back to level slower, but still oscillates

    Are the normal gain values affecting this?
    Thanks a lot for all your work

    ReplyDelete
    Replies
    1. I think the amount of overshoot depends on both the self-level gains and the rate gains (normal PI settings). It's hard to justify modifying the rate gains if you're used to flying mostly in rate mode and using self-level only as a back-up. But if you fly self-level most of the time, it could be useful to try turning down the rate P and I a little to kill off the oscillation.

      Delete
  11. Hi Shane, I've dropped you a PM on RCG about this code train.

    ReplyDelete
  12. Hi, I just flashed KK2.0 board with this firmware and it's awesome! what about yaw coupling problem ? I am not familiar with technical details written here http://scolton.blogspot.sk/2012/09/fun-with-complementary-filter-multiwii.html , can you please explain, how this yaw coupling problem appears and when ? or point me to some page if it's written already

    thanks !

    ReplyDelete
    Replies
    1. Imagine if you pitch up 45º, then yaw to the left 90º. During the yaw operation, only the z-axis gyro measures rotation, so the pitch and roll angles do not immediately change. However, the true condition is now 0º pitch, 45º roll, as measured by the accelerometers. This new condition will be updated slowly by the complementary filter. However, if the yaw is too fast for too long, it may cause control problems before the complementary filter can catch up.

      In my MultiWii code, I have dealt with this issue. But I haven't implemented the solution on the KK2.0 board yet, because I have more difficulty writing code in assembly language.

      So, the best advice for now is to avoid fast yaw while steeply tilted. Shallow banked turns seem okay to me.

      Delete
  13. hi everyone greetings from belgium
    I have the following problem
    I have kk12 remap 1.2
    if I let the copter hovering
    continues for 5 seconds and then drops from height
    I must take the trotlle up before he hits the ground
    what may be a problem

    ReplyDelete
  14. hi i`m from holand
    tommorow i will flash the kk2
    its for the tricopter.

    ReplyDelete
  15. Hi Shane, I've been flying my KK2 around for a while on FPV and I want to add telemetry and OSD to it. I need a firmware change for this but I can't figure out how to do it. Basically I need the KK2 to feed me info about its roll/pitch/yaw angles, battery level, etc etc. via its serial link, which I plan to then use on an Arduino Mini, a 433MHz modem and an OSD injector from Sparkfun to make an OSD and give it telemetry support. I also need to be able to write config settings on the KK2 by sending it serial commands. Would you be interested in being a part of this project? :)

    ReplyDelete
  16. This comment has been removed by the author.

    ReplyDelete
    Replies
    1. Hi Sean,

      I flew with both versions and they worked well. The only time the first version gave me trouble was in heavy wind, where the pitch/roll angles would get pushed outside the limits and get "locked" like I showed in the video above. The second version fixed that problem. I tend to keep the I value pretty low, so I didn't observe the effect you're describing.

      In any case, both versions were made obsolete be KK's v1.5 and now v1.6 update. They seem pretty solid, so I have been recommending people switch to that when they have time to do so. (It does require some re-tuning.)

      Delete
  17. Hi Shane. Thanks. I can't seem to use the other versions like 1.3 to 1.6 as the auto level settings don't hold even with no vibration on the craft. Was using a 2s battery on a 1100 kv motor with settings 40 and 100 (I) on the auto level settings. Super stable. I changed to 4s and had to changed the values to 47 and 37 (I) after much experimentation. The higher "I" values was using too much power to correct and the self level(P) was too weak for the hold. Now it is just as stable as your first version. A lot of people are still having problems with V1.6 et al, but yours still works a treat! You might be surprised as to how many people are still using V1.2 SC!

    ReplyDelete
    Replies
    1. That's really interesting. I've read through the code for v1.5 (I admit I haven't gotten around to looking at v1.6 yet) and it uses almost the same angle estimation filter as I used in v1.2modS, but with extra treatment for full 3D. (Mine was limited to about +/-45º on pitch and roll.) I've been flying v1.6 on my quad, though. Seems very snappy and autolevels nicely. I tend to run pretty low I values compared to other people, though.

      One thing I will say is that the gains in v1.2modS might be different from KK's new versions. The numbers are just software values that only relate to real life if the software treats them the same way. There could be scaling factors in there that make the new gains seems lower even though in fact they are the same. Not sure on that, though.

      Delete