-
Notifications
You must be signed in to change notification settings - Fork 2
Tuning
With the default PID settings, APM will fly typical RC airframes (not full size, full speed airframes) okay right out of the box. But to fly really well, with tight navigation and reliable performance in wind, you'll want to tune your autopilot.
The easiest way to do this is via the PID settings screen in the Mission Planner:
http://ardupilot-mega.googlecode.com/svn/ArduPilotMegaImages/missionplannerPIDsAPM.PNG
You can also load and save configuration files (to your PC) from this screen. Configuration files are versioned -- an older parameter file found by ArduPilot is ignored if the parameter version number is not current.
The column on the left are the full parameter set. The screen on the right are some (but not all) of the more commonly modified parameters. You can use either side to make the same parameter change. Parameter updates are sent to the connected board as soon as you press "write params", many affect the current flight (if any) immediately, and are written to EEPROM so they survive power cycling.
You will note that several of these parameters are PID (Proportional, Integral, Differential) settings.
If you are not quite familiar with PID tuning, it might be helpful to first know that the simplest PID controller has just a P term (I and D is zero and INT_MAX is not used when I is zero).
The correction applied is then (P)roportional to the current error, according to the "P" number. A number 1 is sort of a 1:1 reaction, less than 1 is a subdued reaction, more than 1 is an over-driven reaction, and 0 is no reaction at all.
We've provided some configuration files for common aircraft here, but if the airframe you're using is not listed there, here's a brief guide to the recommended tuning process:
Here's our recommended process when starting from scratch with a new plane:
- Fly in manual mode just to make sure everything is hooked up right
- On the ground check Stabilization mode. If you bank the plane you should see the ailerons (or rudder for 3 channel) respond to return the wings level. If you pitch the plane you should see the elvator respond to correct the pitch. If you are using 4 channel control the rudder should respond in the same direction as the ailerons. Change REVERSE_ROLL, REVERSE_PITCH and REVERSE_RUDDER to get these correct..
- Fly in Stabilization mode and verify that the autopilot will indeed return the aircraft to level flight from a banked or pitched condition. If the aircraft oscillates in pitch or roll severely, reduce the P gains (see below).
- Fly in Fly-By-Wire (FBW) mode (A or B), checking both pitch and roll. The plane should hold the attitude you tell it. The transmitter stick position directly sets the bank and pitch angles. Start by flying straight and level, then push the stick all the way to one side. The plane should bank to 45 degrees or so and maintain that bank. Decrease SERVO_ROLL_P if it's unstable and swinging back and forth; increase it if it's too slow. Repeat with pitch, up and down.
- If things still aren't quite right in FBW, look at your settings for LIM_ROLL_CD, LIM_PITCH_MAX and LIM_PITCH_MIN (the units of all of them are in hundredths of a degree). If you move the stick to the extreme right the plane should go into a bank of angle LIM_ROLL_CD (and stay there). Does it do this? If it gets there slowly increase the roll servo P gain. If it oscillates in roll decrease the servo gain. Work on pitch in a similar manner. The goal is for the plane to respond in pitch and roll in a crisp manner to your stick inputs. Work on this without worrying about the other gains until when you flick the stick and hold it to the side the plane will respond crisply and as quickly as in manual flight. You will likely only need the P gain, but some airframes will need I gain to hold the desired bank angle well without steady state error.
- Now, staying in Fly-By-Wire, again throw the stick all the way over and watch to see if it maintains altitude. If it descends increase PITCH_COMP by .10. You should be able to hold a sustained HEAD_MAX bank turn so you neither gain or loose altitude.
- Now try RTL mode. The aircraft should come back to you. If you set the SERVO_ROLL_P correctly in the previous step, the aircraft should navigate pretty well. But if it's flying away, increase NAV_ROLL_P by 25%. If it's still not responding enough, increase NAV_ROLL_I. If, on the other hand, the aircraft comes back to you but weaves and snakes, decrease NAV_ROLL_P. If that's not enough, increase XTRACK_GAIN.
- Finally, it's worth noting that if you change your cruising speed significantly you may need to re-tune your gains. So if you've been testings at 1/3 throttle but want to fly at full throttle, you may find that issues return. It's best to test at about the throttle setting you want to normally fly at.
Let's say you've got your plane stabilizing well, but it seems to "snake" a lot between waypoints or back to launch, or even worse it's easily blown downwind and can't make it to waypoints at all. What to do?
The above guidelines should help you, but here are some specific tips:
- The best way to test is with a four-waypoint box pattern in a large enough area so that the aircraft has enough room to show it's pattern, but not so big that you can't switch it back into manual and bring it back if things aren't going right.
- Start with the NAV_ROLL I and D terms and the Crosstrack Gain all at 0; This will allow us to dial in one term at at time.
- The first thing to check is your SERVO_ROLL_P. Did you do that 45-degree fly-by-wire (FBW) test? Odds are that if your plane isn't navigating well you don't have enough servo authority and you'll need to turn that value up. But the only way to be sure is to do the FBW test.
- If you've got SERVO_ROLL_P dialed in well, check NAV_ROLL_P on that four-waypoint course. If the plane lacks navigation authority, increase it; if it's snaking, decrease it.
- That should get you pretty decent waypoint following. The last thing to do to really get it to follow your paths well is to play with the Crosstrack gain. What it does is bring the aircraft back to the line between waypoints, rather than just flying directly towards the next waypoint "as the crow flies". This diagram shows what crosstrack error correction does:
http://ardupilot-mega.googlecode.com/svn/ArduPilotMegaImages/crosstrack2.png
With the gain at 0, you should get the red line. As you turn the gain up higher (100 is the default), you should get something closer to the blue line. If the aircraft weaves when following a track, decrease XTRACK_GAIN from the default 100. The faster the aircraft flies, the less this number should be. In order to ignore tracks completely, just navigate line of sight from way point to way point, decrease to zero, then any remaining snaking will be influenced entirely by !Nav_Roll and Roll.
An interactive guide to Crosstrack tuning is here.
PID can be thought of as control according to "Past, Present, Future" data.
The Proportional gain (P) is the simplest form of control, it is the "present" error. Autopilot wants 10 degrees of pitch, has 5 degrees, that is an error of 5: apply some amount of elevator. (the amount applied for the amount of error is determined - scaled - by the P number). Past data ("I") is taking into consideration recent error. If Proportional gain cannot drive the control surface to zero the error, then "I" gain will attempt to do so. Future ("D") is (without a crystal ball) extrapolation of the current error into the future for some period. We choose a control input to attempt to head off the predicted (future) change.
The final control applied to the plane surface, or throttle, is a sum of these three calculations.
Tuning P, PI or PID values can improve how quickly an observed error between desired attitude (pitch, speed, bearing, whatever) and actual attitude can be cancelled out, without undue oscillation.
Luckily the default PID values do work well enough to fly straight, turn, operate the throttle, and navigate, so if in the first mode beyond Manual (Stabilize) you see crazy reactions it is probably NOT something that would be solved by fine tuning P (or I or D) values.
The best way of seeing PID in action is in the simulator setup. Fly straight and level in stabilize mode then move the nose of the plane up then release the sticks. The P term of the elevator PID servo setup box will determine the quality and strength of the elevator reaction. It will either be aggressive, but over-shoot, or too slow and perhaps "never get there". The "I" value can step in to drive the pitch to level more quickly. And finally, the "D" term can react to delays inherent in the control, and further improve the reaction. However if "D" is too large, oscillations in pitch can start to occur, and snowball. This is why tuning "D" values in the air by picking random numbers is NOT recommended.
(from Doug Weibel)
APM is based on a cascaded PID controller. The navigation control loop determines a desired bank angle based on where we are headed and where we want to go. The navigation gains affect this desired bank angle. For example if we are 10 degrees off course then one roll navigation P gain might produce a desired bank angle of 5 degrees, while doubling that gain would produce a desired bank angle of 10 degrees. The second control loop controls the servo to reduce the error between the desired bank angle and the actual bank angle. When I recommend tuning the servo gains in FLY BY WIRE mode it is because in that mode you are basically inputting the desired bank angle with the stick position and isolating this lower level control loop.
Due to the cascaded nature of the control loops there is a mathematical relationship between the gains and the ultimate effect. However the whole point of using the cascaded controller is to make it intuitive and understandable for the user. Just get the servo gains right first so that the system gives you the desired roll and pitch angles, and then work on the navigation gains for optimal navigation performance. It is much simpler and effective to do it in that order.