Sunday, August 26, 2012

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.

40 comments:

  1. Looking at this, I really don't understand why this isn't standard behavior, especially when the code is already there... Have you tested this controller on a hexrotor or octorotor? Perhaps the code isn't functional in hex or oct mode....

    ReplyDelete
    Replies
    1. I've only tested it on the quad so far. The rotor configuration is handled by the mixer. This code is "before" the mixer and is commented out entirely, not possible to enable with any configuration flag that I know of.

      My only guess is that it's for a planned future firmware release and hadn't been tested enough to be included in v1.2. It could have some major bug that I haven't come across yet.

      Delete
  2. Have you told the rcgroups people about this yet? I'm curious what the response will be. Also, I wonder if the KK developer has comments about it too. Very interesting!

    ReplyDelete
    Replies
    1. Nope, I haven't. Not sure if I want to open that can of worms. Half of that thread seems to be about problems re-flashing firmware with third-party AVR programmers. I did try to ask the board's creator about it but he doesn't accept private messages (understandable).

      If it really is for a future firmware upgrade, people will love it! It flies so nicely. I have been flying it outside and refining the gains a little more and it's just amazing.

      Delete
  3. Thank you for sharing this ! .... any chance of letting us know what your settings ended up to be ( a bit can be heard in the audio ) .... would be interesting..

    ReplyDelete
  4. Please, what your settings, i am a newbie in hobby the quadricopter.
    Thank you.

    ReplyDelete
  5. I can give you what I have from a couple days of outdoor testing. Every setup will require different tuning, though. So I wouldn't rely on these too much. You'll have to adapt it to your frame and flying style. One thing is I would keep the stick scaling low because it sets the maximum angle in self-level mode and it may not work for flying at high angles.

    Test at your own risk!!! At least one person has said this firmware mod caused a failure. Official/stable firmware is available on the RCGroups thread.

    Turnigy Talon v1 (500mm)
    NTM 28-30-750rpm/V, 10x4.7SF
    4S/1.8Ah

    Ptich/Roll (Linked):
    P Gain: 23
    P Limit: 100
    I Gain: 14
    I Limit: 10

    Yaw:
    P Gain: 50
    P Limit: 20
    I Gain: 20
    I Limit: 10

    Stick Scaling:
    Roll: 30
    Pitch: 30
    Yaw: 40
    Throttle: 90

    Misc:
    Min Throttle: 20 (!! Abnormally high - need for my ESCs. 10 is default, I think.)
    Height Dampening: 10
    Height D Limit: 30
    Servo Filter: 50 (N/A)

    Self Level:
    P Gain: 85
    P Limit: 40
    I Gain: 0
    I Limit: 0

    ReplyDelete
  6. Thanks for doing this! I'm very impressed.

    ReplyDelete
  7. I found a issue when you try to take off it will (i think) try to flip because it thinks it is upside down. When i turn off auto level it will work fine but when you take off and it is on it will try to flip. Anyway i think this is the bug that KK found and why it was commented out. however it fly's like a dream with this mod.

    ReplyDelete
  8. Same thing happened to me today.

    ReplyDelete
    Replies
    1. yeah, me too. cost me a propeller for a sudden front flip when trying to take off.
      But when it works, it works.

      Delete
    2. I've had this happen when trying to take off a second time after landing. (Never on the first try.) I think maybe the angle estimate gets corrupted either in flight or during the landing and can't recover (outside the range where the accelerometers can fix it). If I see that it's about to try to flip on takeoff, I have to power reset to get back to normal.

      If it's happening every time on the first takeoff, the only thing I can think of is to try turning down the integral gains. (Self-level I should be zero, and I keep the rate I low as well.) I also always power up with it already on the ground and don't move it until takeoff.

      Delete
  9. Yes, there seem to still be cases that can make it go crazy. Definitely not recommended for anything other than experimental purposes at this point. I have been pretty careful with it. Maybe I'll start pushing it harder to see where it breaks. But I haven't seen any problem with trying to flip on take-off.

    One idea is it takes some time for the angle filter to converge to accelerometer values. I typically stay on the ground for 10 seconds or more before taking off, just to do some checks. Otherwise, maybe sensor calibration?

    Is the self-level I Gain = 0?

    ReplyDelete
  10. Keep,up the good work I thing this board has a lot of potential

    ReplyDelete
  11. Hi there,

    Thank you for the article, I have found it truly interesting. I have been trying to unwrap the maths for the 2 complementary filter implementation. However, I can't see the equivalence to the 1st representation. Could you elaborate a little on this?

    Thanks in advance.

    ReplyDelete
    Replies
    1. Yes, that one is particularly nasty.

      **Spoiler Alert**
      http://web.mit.edu/scolton/www/cf2.pdf

      #1 has the fewest intermediate variables.
      #2 is maybe the simplest structure.
      #3 is maybe the most intuitive.

      It's amazing to me how they can all be functionally equivalent.

      Delete
    2. Thanks ever so much for that document. Yeah, the complementary filter is fantastic, simple and functional.
      Keep the amazing work up, it is awesome.

      Delete
  12. It's great that you understand this stuff, and actually like it. Thanks!

    ReplyDelete
  13. Self level works great for the first time you contact the Lipo, but if you switch on self level after you fly around it flips verry fast. Try to fix it then it will be perfect.
    Martin, Germany

    ReplyDelete
    Replies
    1. There is a scenario where that can happen if while flying (with self-level off) you get past about 26º (not sure the exact number) and have yaw inputs. The angle estimate gets locked into an incorrect large value and stops using the accelerometer to correct itself.

      You can probably make a fix for this by modifying (or removing?) the conditional that determines when the accelerometer correction is bypassed (in imu.asm). I may try this at some point in the future. If it works, I'll post an update.

      Delete
    2. This comment has been removed by the author.

      Delete
  14. I haven't had that problem, I flashed one of my boards and it works so well I flashed the rest of them

    ReplyDelete
  15. I have been waiting for a real auto-level!! Tried this firmware and plugged in your settings, and right away my Tricopter was a whole new machine! Thanks a million for getting this done. Of course, after 1 crash, it appeared the tricopter wanted to flip over on takeoff (with auto-level on). A quick power cycle fixed the issue. Just so long as it doesnt flip over spontaneously in flight I am okay with it :)

    ReplyDelete
    Replies
    1. Check out my later post for a possible explanation of the problem you encountered, and a fix:

      http://scolton.blogspot.com/2012/09/a-bit-more-kk20-modding.html

      Delete
  16. hi yes i have the same Prob with 1.2 acc mod.
    Mind oyu the P and i in the AL was kept stock. 20 20 0 and 0.
    But when i uped the P and I the bank angle would increase vs stock FW it would just increase the sensitivity of my Quad while in AL.
    I like that feature as i could change it for different camera scenarios.

    i have ordered a new board and was planning to leave it stock.
    please watch video. I posted on RCGroups with not much said about the Problems
    www.youtube.com/watch?v=rM0B6WIQTOU

    ReplyDelete
    Replies
    1. Oh, yes I saw this video on RCG. I did update the firmware a bit which MIGHT help with the problem:

      http://scolton.blogspot.com/2012/09/a-bit-more-kk20-modding.html

      The newest version is now in the LazyZero flash tool I believe, although I don't control the flash tool so I'm not sure.

      Another thing to try, if you haven't already, is sensor calibration. Place the frame on a level surface and run the sensor calibration from the menu.

      If neither of those fixes work, or if you're not willing to risk it, definitely stick with the stock firmware. The modified version is still experimental.

      Delete
  17. YOU ARE FREAKING AWESOME!!! THANKS I am going to try this tomorrow. :D

    ReplyDelete
  18. You seem to be very experinced with quads can you help me out?
    check my post at rcg
    http://www.rcgroups.com/forums/showthread.php?t=1741110
    i uploaded video of my quad using your settings with stock firmware
    (im not using self level mode) will you check out the text and video and see if you can help me?
    thanks you !

    ReplyDelete
  19. It works fine, thanks a lot ( explorer- tricopter)

    ReplyDelete
  20. Latest firmware flys great. Thanks for the efforts.

    ReplyDelete
  21. I am new to Multi-Copter flight controller software and Hardware. I am planning to build Tri-Copter using KK2 from Hobbyking. I am highly interested in knowing how to exploit all its features. Is there programming documentation/reference/flowcharts/diagrams/programming examples etc. available? Any help on this will be greatly appreciated. Thank you.

    ReplyDelete
  22. I wonder how many people have crashed their Quadcopters using the suggested settings that come with the board. I did mine using a cheep 4 channel TX. It would be nice to see D/R settings you used on your TX Both high and low. Nothing is more of a bummer than to build a Quad and crash it the first time out. Mine also flips when going from heading hold to level flight. After two frames and a new TX I finally got mine to fly The whole reason I bought the KK2 board was so I didn't have to carry my laptop around to make adjustments.

    ReplyDelete
  23. Dear Mr. Shane Colton.

    Kindly advise about this post
    http://www.rcgroups.com/forums/showthread.php?t=1675613&page=615#post23777471

    ReplyDelete
  24. hello !

    one question: does the new KK firmware v1.5 do better self-level job as previous versions, or is this modified firmware still better in self-leveling ?

    i know this firmware has some problems, but I can live with that. I just realized that in november 2012 new version 1.5 come up.

    thanks!

    ReplyDelete
    Replies
    1. v1.4/1.5 now have fully-capable self-level including the complementary filter. The version that I made is now obsolete.

      Delete
  25. Thank you for sharing this!any chance of letting us know what your settings ended up to be (bit can be heard in the audio)would be interesting..
    Regard's,
    Ben Lines,
    bestuurbare auto

    ReplyDelete
    Replies
    1. Hi Ben,

      The settings I used are in a comment above (Aug 28). They worked pretty well for the Talon, but I doubt they'd port well if anything was changed (frame, motors, props, total weight).

      Delete
  26. nice blog..am gonna use it KK2. planning to write my own code for it. but no idea about the probability of that. cheers

    ReplyDelete
  27. I used your 3rd implementation of complementary filter and yes it is BEST than 1st implementation.Thank You very much !!

    ReplyDelete