Tuning crazyflie for smooth, stable and responsive flight
Posted: Wed Apr 06, 2016 5:43 am
Hey Crazyfliers
I've been on a mission to tune the stabilization of the crazyflie 2.0 the past several weeks, and I thought it might be helpful to track my progress on the forum as well as get input from any of you who might have gone through similar exercises. I'm pretty experienced in embedded systems, sensor interaction, and controls but am new in applying all this stuff to quads.
Backstory: In general I found the crazyflie to be really stable and responsive. It's solid indoors. I never thought anything of it, until a couple friends of mine picked up the Hovership 3dfly running cleanflight:
http://shop.hovership.com/3dfly-micro-quad-kit/
This little guy is in the same class of crazyflie, but a bit more powerful (8.5mm motor cans vs 7mm on the crazyflie) and heavier. Out of the box, it flies way nicer than the crazyflie (details below). I didn't see any reason why crazyflie can't achieve the same results given it's basically the same Invensense IMU and equipped with quite powerful motors, so off on the mission I went, using the 3dfly as a frame of reference.
First, my setup: I'm using a crazyflie 2.0 with a modded Devo7e transmitter running deviation. In all testing, I've disabled the recently added tilt-thrust-compensation feature, but the rest of the firmware is stock. In some videos I post, you'll see my battery mounted on the bottom. This is because I've been flying with a cheap FPV camera lately and I need a nice level platform to mount the camera on. I've got another thread (viewtopic.php?f=6&t=1924) discussing the effects of lowering the center of gravity in this way. It sounds like this might be working against me, but in practice I don't really notice a big difference. Either way, for all tuning experiments I try to make sure I test things out in the default config (battery on top) so that this is still applicable to most crazyfliers.
Here's some examples of the instability I'm aiming to improve on:
-Flight in level mode (the default) is undertuned and overcorrects. The effects of this are kind of a braking effect when the sticks are released -- fly forward at some pitch (20-30 degrees or something) and let go of the control stick. The flie overshoots level and slows itself down. Now, I could see how this behavior is actually somewhat beneficial to newer fliers, but I find it detrimental for more advanced flight. In particular, flying outside at higher speeds, the flie is prone to inverting itself if the sticks are released too quickly, especially when turning. When you repeat the same experiment with the 3dfly, it returns to level smoothly without overshoot (though it reacts noticeably more slowly than the crazyflie's default settings) and continues on smoothly via inertia - reminds me of skating on ice. Here's an example video of a string test demonstrating the overshoot (stabilization enabled on pitch only). It's subtle here, but is exacerbated by wind resistance when actually flying. https://onedrive.live.com/redir?resid=4 ... ideo%2cmp4
-Flight in rate mode (aka acro) is impossible. The crazyflie isn't really intended for acro flight, so the PIDs aren't tuned for this. However, the benefits of a well tuned rate PID should bubble up to the level PID, since the level PID simply feeds in to the rate PID. Plus, I want to fly in acro mode with my FPV camera . Here's the default rate mode (again, stabilization enabled only for pitch): https://onedrive.live.com/redir?resid=4 ... ideo%2cmp4
-The crazyflie is highly susceptible to wind. Flying outside in anything stronger than a moderate breeze causes tumbles, spirals, and hard crashes. I certainly expect something as small and light as the crazyflie to get knocked around a fair bit, but it should be able to hold level while being blown around laterally, instead of getting inverted and crashing out of the sky (and I've seen it get into a spin ~50-100 feet up and not manage to recover -- I'm not talking about a couple feet off the ground).
First of all: I can't stress enough the importance of balanced props and non-damaged motors. I was having terrible performance for a while, and it was due partially to a slightly bent motor shaft, and partially to unbalanced props. I admit I originally scoffed at the notion of putting tape on my props, but it really, really matters. Try this: hold the flie in your hand, hold 3 of the props still, and give it a very short burst of throttle (don't lay on the throttle or you'll burn out your motors). You can REALLY feel the vibration from an unbalanced prop. You can barely feel anything from a balanced prop. Imagine what your IMU thinks about that
So, physical aspects aside, I decided to start playing with PID tuning since I figured that'd probably get me the greatest return. The crazyflie PID design is two stage: the angle PID runs on the fused IMU data to generate a desired rate of rotation. This rate of rotation feeds in to the rate PID which produces motor setpoints. In rate/acro mode, the output of the angle PID is simply ignored, and the values coming from the transmitter are used as the rates of rotation. All the conventional wisdom I can find in the quadcopter community is that the rate PID should always be tuned first, and the level PID (which sits on top logically) later. The default coefficients in the firmware are Kp = 70, Ki = 0, Kd = 0. With no Ki, it's no surprise the performance is poor -- it's not correcting for incremental errors at all. (As an aside, this still works reasonably well for level mode because the sensor fusion is effectively accounting for incremental errors). Choosing coefficients is mostly guesswork. Here are a few videos of things I tried along the way:
Kp = 110 Ki = 45 Kd = 0: A little better, but not really
https://onedrive.live.com/redir?resid=4 ... ideo%2cmp4
Kp = 200 Ki = 200 Kd = 0: Much better, but oscillations can be seen from the Ki term. Note, the slow drift seen here turned out to be due to my transmitter. I had it configured for a linear stick response, and it wasn't quite at zero when I let go of the stick. It was sending something in the neighborhood of 5-10 deg/s, which is what you see.
https://onedrive.live.com/redir?resid=4 ... ideo%2cmp4
Kp = 250 Ki = 500 Kd = 2.5: This is what I've settled on for now for both pitch and roll. (I've also bumped up the Ki windup from 33 to 360, which might be a little risky) I'll probably keep experimenting, but this is very responsive, stable, and feels great to fly. Here's a video demonstrating how the Ki term can handle errors from external forces.
https://onedrive.live.com/redir?resid=4 ... ideo%2cmp4
With this last setting, the crazyflie is VERY flyable in rate/acro mode. Even better, this has solved my first bullet above! The crazyflie doesn't overshoot anymore in level mode. However, I highly suspect I've just masked the root problem by slowing the rate of response via Kd. I intend to dig in more, but it's a great first step.
What's next? I want to keep digging in to the overshoot in level mode, as well as instability under windy conditions. Here's some ideas I have, roughly in the order in which I suspect them to matter:
-The sensor fusion loop. This has its own PI control loop which appears to have a pretty high latency/settle time. I suspect this is the real cause of any overshoot in level mode (and possibly instability in wind), and my increased Kd term on the rate PID simply slowed rotation enough for the attitude output to settle. I'll poke at tuning this, as well as checking out the alternate implementation (madgwick) that's ifdef'ed out.
-Level PID frequency -- the level PID is running at half the rate (250hz) of the angle PID and I don't entirely understand why. Would love some insight from the devs behind this choice. Also it's been a while since I looked at this part of code, but I believe I recall seeing the "dt" term being set to 500hz instead of 250hz, which might mess up the integral/derivative calculations.
-Sensor fusion/IMU sampling might get out of sync with the PIDs -- both are operating at 500hz, but they're not coupled with a data ready line from the IMU or anything. If they become out of phase with one another, that's adding a bit of unnecessary latency
-IMU biasing - gyro bias calculation can be improved by upscaling to 10ths of degrees, and the accel bias isn't being sampled by default. I don’t suspect either of this will have dramatic impact on the problems described above, because the bias looks quite insignificant as is
Hopefully this wasn't too long winded and boring. I'm obsessing over this a little bit, going through a big learning process, and am really hoping this'll end up helping someone else out, or someone else will chime in and help ME out. More updates to come as I learn things.
Sean
I've been on a mission to tune the stabilization of the crazyflie 2.0 the past several weeks, and I thought it might be helpful to track my progress on the forum as well as get input from any of you who might have gone through similar exercises. I'm pretty experienced in embedded systems, sensor interaction, and controls but am new in applying all this stuff to quads.
Backstory: In general I found the crazyflie to be really stable and responsive. It's solid indoors. I never thought anything of it, until a couple friends of mine picked up the Hovership 3dfly running cleanflight:
http://shop.hovership.com/3dfly-micro-quad-kit/
This little guy is in the same class of crazyflie, but a bit more powerful (8.5mm motor cans vs 7mm on the crazyflie) and heavier. Out of the box, it flies way nicer than the crazyflie (details below). I didn't see any reason why crazyflie can't achieve the same results given it's basically the same Invensense IMU and equipped with quite powerful motors, so off on the mission I went, using the 3dfly as a frame of reference.
First, my setup: I'm using a crazyflie 2.0 with a modded Devo7e transmitter running deviation. In all testing, I've disabled the recently added tilt-thrust-compensation feature, but the rest of the firmware is stock. In some videos I post, you'll see my battery mounted on the bottom. This is because I've been flying with a cheap FPV camera lately and I need a nice level platform to mount the camera on. I've got another thread (viewtopic.php?f=6&t=1924) discussing the effects of lowering the center of gravity in this way. It sounds like this might be working against me, but in practice I don't really notice a big difference. Either way, for all tuning experiments I try to make sure I test things out in the default config (battery on top) so that this is still applicable to most crazyfliers.
Here's some examples of the instability I'm aiming to improve on:
-Flight in level mode (the default) is undertuned and overcorrects. The effects of this are kind of a braking effect when the sticks are released -- fly forward at some pitch (20-30 degrees or something) and let go of the control stick. The flie overshoots level and slows itself down. Now, I could see how this behavior is actually somewhat beneficial to newer fliers, but I find it detrimental for more advanced flight. In particular, flying outside at higher speeds, the flie is prone to inverting itself if the sticks are released too quickly, especially when turning. When you repeat the same experiment with the 3dfly, it returns to level smoothly without overshoot (though it reacts noticeably more slowly than the crazyflie's default settings) and continues on smoothly via inertia - reminds me of skating on ice. Here's an example video of a string test demonstrating the overshoot (stabilization enabled on pitch only). It's subtle here, but is exacerbated by wind resistance when actually flying. https://onedrive.live.com/redir?resid=4 ... ideo%2cmp4
-Flight in rate mode (aka acro) is impossible. The crazyflie isn't really intended for acro flight, so the PIDs aren't tuned for this. However, the benefits of a well tuned rate PID should bubble up to the level PID, since the level PID simply feeds in to the rate PID. Plus, I want to fly in acro mode with my FPV camera . Here's the default rate mode (again, stabilization enabled only for pitch): https://onedrive.live.com/redir?resid=4 ... ideo%2cmp4
-The crazyflie is highly susceptible to wind. Flying outside in anything stronger than a moderate breeze causes tumbles, spirals, and hard crashes. I certainly expect something as small and light as the crazyflie to get knocked around a fair bit, but it should be able to hold level while being blown around laterally, instead of getting inverted and crashing out of the sky (and I've seen it get into a spin ~50-100 feet up and not manage to recover -- I'm not talking about a couple feet off the ground).
First of all: I can't stress enough the importance of balanced props and non-damaged motors. I was having terrible performance for a while, and it was due partially to a slightly bent motor shaft, and partially to unbalanced props. I admit I originally scoffed at the notion of putting tape on my props, but it really, really matters. Try this: hold the flie in your hand, hold 3 of the props still, and give it a very short burst of throttle (don't lay on the throttle or you'll burn out your motors). You can REALLY feel the vibration from an unbalanced prop. You can barely feel anything from a balanced prop. Imagine what your IMU thinks about that
So, physical aspects aside, I decided to start playing with PID tuning since I figured that'd probably get me the greatest return. The crazyflie PID design is two stage: the angle PID runs on the fused IMU data to generate a desired rate of rotation. This rate of rotation feeds in to the rate PID which produces motor setpoints. In rate/acro mode, the output of the angle PID is simply ignored, and the values coming from the transmitter are used as the rates of rotation. All the conventional wisdom I can find in the quadcopter community is that the rate PID should always be tuned first, and the level PID (which sits on top logically) later. The default coefficients in the firmware are Kp = 70, Ki = 0, Kd = 0. With no Ki, it's no surprise the performance is poor -- it's not correcting for incremental errors at all. (As an aside, this still works reasonably well for level mode because the sensor fusion is effectively accounting for incremental errors). Choosing coefficients is mostly guesswork. Here are a few videos of things I tried along the way:
Kp = 110 Ki = 45 Kd = 0: A little better, but not really
https://onedrive.live.com/redir?resid=4 ... ideo%2cmp4
Kp = 200 Ki = 200 Kd = 0: Much better, but oscillations can be seen from the Ki term. Note, the slow drift seen here turned out to be due to my transmitter. I had it configured for a linear stick response, and it wasn't quite at zero when I let go of the stick. It was sending something in the neighborhood of 5-10 deg/s, which is what you see.
https://onedrive.live.com/redir?resid=4 ... ideo%2cmp4
Kp = 250 Ki = 500 Kd = 2.5: This is what I've settled on for now for both pitch and roll. (I've also bumped up the Ki windup from 33 to 360, which might be a little risky) I'll probably keep experimenting, but this is very responsive, stable, and feels great to fly. Here's a video demonstrating how the Ki term can handle errors from external forces.
https://onedrive.live.com/redir?resid=4 ... ideo%2cmp4
With this last setting, the crazyflie is VERY flyable in rate/acro mode. Even better, this has solved my first bullet above! The crazyflie doesn't overshoot anymore in level mode. However, I highly suspect I've just masked the root problem by slowing the rate of response via Kd. I intend to dig in more, but it's a great first step.
What's next? I want to keep digging in to the overshoot in level mode, as well as instability under windy conditions. Here's some ideas I have, roughly in the order in which I suspect them to matter:
-The sensor fusion loop. This has its own PI control loop which appears to have a pretty high latency/settle time. I suspect this is the real cause of any overshoot in level mode (and possibly instability in wind), and my increased Kd term on the rate PID simply slowed rotation enough for the attitude output to settle. I'll poke at tuning this, as well as checking out the alternate implementation (madgwick) that's ifdef'ed out.
-Level PID frequency -- the level PID is running at half the rate (250hz) of the angle PID and I don't entirely understand why. Would love some insight from the devs behind this choice. Also it's been a while since I looked at this part of code, but I believe I recall seeing the "dt" term being set to 500hz instead of 250hz, which might mess up the integral/derivative calculations.
-Sensor fusion/IMU sampling might get out of sync with the PIDs -- both are operating at 500hz, but they're not coupled with a data ready line from the IMU or anything. If they become out of phase with one another, that's adding a bit of unnecessary latency
-IMU biasing - gyro bias calculation can be improved by upscaling to 10ths of degrees, and the accel bias isn't being sampled by default. I don’t suspect either of this will have dramatic impact on the problems described above, because the bias looks quite insignificant as is
Hopefully this wasn't too long winded and boring. I'm obsessing over this a little bit, going through a big learning process, and am really hoping this'll end up helping someone else out, or someone else will chime in and help ME out. More updates to come as I learn things.
Sean