Hi,
I changed the attitude control loop a little bit to achieve more stability. Rate control with a PID(T1) instead of a PI controller. Here I present the necessary changes in the code to do this.
Pro:
- Much more attitude stability
- Much faster response on disturbances or new setpoints
Con:
- You need to tune the control loop again after this change
- This change is experimental, it works for me but you do it on your own risk ;)
I'm starting from Software AC 2.1. First I modified the library "PID" to save calculating time. I saved the following floating point operations per call to "get_pid": 2 x division, 4 x multiplication. You can download the modified library here. Unzip it and put the folder "PID_fast" in the "libraries" folder.
In file "ArduCopter.pde":
Add this line to the includes:
#include <PID_fast.h> // PID library
For the following 4 blocks, I post it like the diff you get in GIT. The "-" means that you should look for this line and remove it. Replace it with the following lines starting with the "+". Of cause, the lines in the code have no "-" or "+". It's just to show what to remove and what to add instead.
- g.pi_rate_roll.reset_I();
- g.pi_rate_pitch.reset_I();
+ g.pid_rate_roll.reset_I();
+ g.pid_rate_pitch.reset_I();
- g.pi_rate_roll.kP(tuning_value);
- g.pi_rate_pitch.kP(tuning_value);
+ g.pid_rate_roll.kP(tuning_value);
+ g.pid_rate_pitch.kP(tuning_value);
- g.pi_rate_roll.kI(tuning_value);
- g.pi_rate_pitch.kI(tuning_value);
+ g.pid_rate_roll.kI(tuning_value);
+ g.pid_rate_pitch.kI(tuning_value);
- g.pi_rate_yaw.kP(tuning_value);
+ g.pid_rate_yaw.kP(tuning_value);
In file "Attitude.pde":
In function "get_stabilize_roll(int32_t target_angle)":
- rate = g.pi_rate_roll.get_pi(error, G_Dt);
+ rate = g.pid_rate_roll.get_pid(error, G_Dt);
In function "get_stabilize_pitch(int32_t target_angle)":
- rate = g.pi_rate_pitch.get_pi(error, G_Dt);
+ rate = g.pid_rate_pitch.get_pid(error, G_Dt);
In function "get_stabilize_yaw(int32_t target_angle)":
- rate = g.pi_rate_yaw.get_pi(error, G_Dt);
+ rate = g.pid_rate_yaw.get_pid(error, G_Dt);
- rate = g.pi_rate_yaw.get_pi(error, G_Dt);
+ rate = g.pid_rate_yaw.get_pid(error, G_Dt);
In function "get_rate_pitch(int32_t target_rate)":
- target_rate = g.pi_rate_yaw.get_pi(error, G_Dt);
+ target_rate = g.pid_rate_yaw.get_pid(error, G_Dt);
In file "Parameters.h":
Increase the number after "static const uint16_t k_format_version" by one. Warning: This will make the APM erase your EEPROM and Log after flashing the new compiled firmware. So save your settings first! After this, you have to reload your settings and level the copter again.
Later on in the file:
- k_param_pi_rate_roll = 235,
- k_param_pi_rate_pitch,
- k_param_pi_rate_yaw,
+ k_param_pid_rate_roll = 235,
+ k_param_pid_rate_pitch,
+ k_param_pid_rate_yaw,
- APM_PI pi_rate_roll;
- APM_PI pi_rate_pitch;
- APM_PI pi_rate_yaw;
+ PID_fast pid_rate_roll;
+ PID_fast pid_rate_pitch;
+ PID_fast pid_rate_yaw;
- pi_rate_roll (k_param_pi_rate_roll, PSTR("RATE_RLL_"), RATE_ROLL_P, RATE_ROLL_I, RATE_ROLL_IMAX * 100),
- pi_rate_pitch (k_param_pi_rate_pitch, PSTR("RATE_PIT_"), RATE_PITCH_P, RATE_PITCH_I, RATE_PITCH_IMAX * 100),
- pi_rate_yaw (k_param_pi_rate_yaw, PSTR("RATE_YAW_"), RATE_YAW_P, RATE_YAW_I, RATE_YAW_IMAX * 100),
-
+ pid_rate_roll (k_param_pid_rate_roll, PSTR("RATE_RLL_"), RATE_ROLL_P, RATE_ROLL_I, RATE_ROLL_D, RATE_ROLL_IMAX * 100),
+ pid_rate_pitch (k_param_pid_rate_pitch, PSTR("RATE_PIT_"), RATE_PITCH_P, RATE_PITCH_I, RATE_PITCH_D, RATE_PITCH_IMAX * 100),
+ pid_rate_yaw (k_param_pid_rate_yaw, PSTR("RATE_YAW_"), RATE_YAW_P, RATE_YAW_I, RATE_YAW_D, RATE_YAW_IMAX * 100),
In file "config.h":
You have to add a few lines. Lines to add are indicated by "+". Other lines are for reference to find the right place.
#ifndef RATE_ROLL_I
# define RATE_ROLL_I 0.0
#endif
+#ifndef RATE_ROLL_D
+# define RATE_ROLL_D 0.0
+#endif
#ifndef RATE_PITCH_I
# define RATE_PITCH_I 0 //0.18
#endif
+#ifndef RATE_PITCH_D
+# define RATE_PITCH_D 0.0
+#endif
#ifndef RATE_YAW_I
# define RATE_YAW_I 0.0
#endif
+#ifndef RATE_YAW_D
+# define RATE_YAW_D 0.0
+#endif
In file "system.pde":
- Log_Write_Data(12, g.pi_rate_roll.kP());
- Log_Write_Data(13, g.pi_rate_pitch.kP());
+ Log_Write_Data(12, g.pid_rate_roll.kP());
+ Log_Write_Data(13, g.pid_rate_pitch.kP());
Now, all changes are done. Now save your current settings of the APM, compile and load the code... You will find 3 new parameters (RATE_RLL_D, RATE_PIT_D and RATE_YAW_D) you can edit like any other mavlink parameter. These are the D-Terms for all rate control loops.
Testing and tuning:
Be really carefull, propellers are dangerous! I do the tuning of the control loops while I hold the copter in my hand and try if it compensates the disturbances without overshot or oscillation. You should decide for yourself if cou can hold your copter in one hand while you throttle up and test the stability. Even small propellers can harm your fingers! If you want to proceed, I do it like this:
- Reload your last setup and level the copter.
- Set all parameters for stabilize STB_***_P and STB_***_I to zero.
- Set all parameters for rate control RATE_***_P, RATE_***_I and RATE_***_D to zero.
- Hold the copter with one hand, throttle up until it becomes weightless, then turn it. Nothing should happen, because all control loop parameters are zero. You have just manual throttle control.
- Increase the D-Terms and test again while holding the copter in your hand. Remember, it can't fly with most parameters set to zero. Increase the D-Term and test again, until you got some oscillation. Let's call the value you found 100 percent. Now reduce it to 50..70 percent. Do this for ROLL and PITCH.
- Proceed with the P-Terms. Increase and test until it gets unstable. Then reduce it to 50..70 percent of the original value.
- Set the I-Terms to 30..50 percent of the P-Terms. Test again. If it is unstable, reduce P and I and maybe D a little bit.
- Now go to the STABILIZE control parameters. Increase the P-Term. Now the copter should return to level position automatically. Increasing of the P-Term will lead to a faster return to level position after disturbance or also to a fast respond to the desired angle by pilot input. Find a value where the copter returns fast from a disturbance without overshot or oscillation.
- Always leave the STABILIZE I-Term STB_***_I zero in this controller configuration!
- For YAW you can leave the D-Term zero and use your previous settings for STABILIZE and RATE control. Or you tune it, but I didn't try it at YAW yet.
- If everything works well and the copter returns to level position after disturbance or pilot input without overshot or oscillation, then you can try to hover and fly carefully. Not before!
Here are some controller parameters from my Tricopter (80 cm from motor to motor, 1440 g in flight weight), the D-Terms are quite small:
STB_PIT_I,0
STB_PIT_IMAX,1500
STB_PIT_P,9
STB_RLL_I,0
STB_RLL_IMAX,1500
STB_RLL_P,9
STB_YAW_I,0
STB_YAW_IMAX,1500
STB_YAW_P,4
RATE_PIT_D,0.015
RATE_PIT_I,0.02
RATE_PIT_IMAX,1000
RATE_PIT_P,0.09
RATE_RLL_D,0.012
RATE_RLL_I,0.02
RATE_RLL_IMAX,1000
RATE_RLL_P,0.09
RATE_YAW_D,0.004
RATE_YAW_I,0.002
RATE_YAW_IMAX,500
RATE_YAW_P,0.5
It would be really nice, if other people try this modification. For me, it was a significant increase in stability.
Regards, Igor
Comments
Yes Igor, I meant that the D term reacts on angular acceleration. But I think this has not been entirely clear for everybody in this thread. Thus the occasional confusion present in some posts on this topic...
The inner PIDT1 loop in my implementation controls the angular rate/velocity, not the angular acceleration. (Or do you mean the D term reacts on angular acceleration? Thats right.) The outer loop controls the angle with a simple P control.
And in my opinion outer loop with P and inner loop with PIDT1 is the best thing that you can do (with traditional controllers). You always have to choose the right controller structure. Putting PIDT1 everywhere is not a solution, because two I Terms in series generates oscillation.
Can someone with a strong background in control theory join in? I had control theory one year at university and have some hobby experiences with controllers. And I tried combinations of outer and inner loop for attitude stabilisation, and P->PIDT1 works the best.
Continuing…
And I do share Igor´s conviction that the purpose of the outer ”extra” I-term, implemented to counteract outer disturbance like offset CG, could (and should) be handled by the inner (rate) loop I-factor, commanded by the attitude (Stable) P-term. Think of it. If we hang a battery pack on one of the copter arms:
1. The arm will initially dip, and simultaneously there will be an output from the attitude (Stable) P-term.
2. The attitude P-term output is immediately fed into the inner loop, the rate loop, which is a PIDT1. As long as there is an input (attitude error) it WILL be integrated by the rate loop I-term, which at some point (preferably close to instantly) will build up enough to force the arm up to level. And that is when the attitude P-term returns to zero, while the rate integrator remembers the value needed to correct for that stupid battery dangling on one arm.
3. If somebody disagrees, please specify exactly what was wrong in 1 or 2. Thanks.
So I firmly believe that ACRO (or the inner rate loop) has a clear purpose:
I just came in from two brief test / tuning flights of my stock 3DR APM2 quad, AC 2.2.v4. And the ACRO mode is not capable of the purpose above. It is flyable, but attitude wanders without command. Copter obeys rate inputs a bit sloppy with slight overshoots. If somebody tells me that the ACRO control loop is already perfect, well then then the gyro references is not working properly. Something is just half baked. If somebody disagrees, PLEASE provide detailed tuning instructions on the two tunable terms: ACRO Pitch and Roll Proportional and Integrating.
Stable mode works decently, but has artifacts. Like for example: I command tilt forward, the copter gains speed and flies forward. Then I ease off on the stick to neutral, thereby ordering horizontal level. Instead, the copter starts leaning slightly backwards (uncommanded) to me, starting backwards flight towards me. After a moment it has once again returned to level. It looks like a slow attitude oscillation, probably a result of the double I-terms existing in the control diagram. I do not understand how to get rid of that artifact with the tools at hand.
When writing this, I understand the PIDT1 is already a bit on the road to be included in next release.
I really look forward to that, with great expectations!
Guys, I am so glad this conversation exists. It brings me hope.
While not having profound experience of all the FW versions, every time I have been flying, tweaking and testing STAB and ACRO I have been left with "ok, this might work but really it MUST be possible to make it better". A gut feeling that something is missing here. One could argue that "hey, you just have to be more rigorous with you PI-tuning, and it will be fine", but too many fellow reports indicate that there IS really a missing link between the AC firmware and that perfect locked-in-flight experience. And if we cannot get the ACRO mode perfect (well, the inner control loop), then we cannot expect to get the higher level functions perfect either, since they always depend on the basic inner loop. The higher functions will have to tolerate and compensate for the not fully performing inner loop, instead of just doing their job.
Even if it was many years ago, I have studied control theory, and after having penetrated Igor´s arguments I have to say that I concur. Implementing PIDT1 and cleaning up the loop arcitechture seems like a necessity to move on. Flight tests performed by Marco indicates likewise.
Seems to be some confusion on the "D-term" concept in this conversation. From my observers seat it looks like this confusion is due to jason referring to the attitude, while Igor refers to the basic, inner rate loop (rate). (Correct me if I am wrong. ) So when Igor talk D-term he refers to angular acceleration, while Jason refers to angular speed (rate). If true, this explanins why Jason, a bit puzzled, insists that there IS already a D-term in action. Yes, for the attitude, but not for the angular rate.
To be continued...
jason,
you are not using a d-term - even if the input is the gyro rate.
wiki d-term
have a good look on the formula!
what i like is the filtering on the gyro rate. have not yet tried that.
will do so when i have refurbished my flying vehicle.
@jasonshort:
Why do you want to use the D term only with the sensor signal to get dampening? You mentioned that "the copter was so dampened it wasn't any fun to fly". So your D term dampening work AGAINST the pilot input?
I know that in my implementation the pilot input goes to the D term as well, thats what I want! I get very fast response to pilot input and also very fast response to disturbance with this. In stabilize mode on disturbance the P controller will generate the input instead of the pilot. I dont want to have just dampening, I want to have control! And to avoid to high controller output values you should limit the output.
If the reaction on pilot input is to fast, you should lowpass filter the pilot input a little. The D term is already filtered in the library.
In my suggestion the outer loop for stabilize has just a P term, the inner rate loop has a PIDT1.
One thing thing you really need to do is sample the gyro output at full rate and pull them into excel. I've done most of the work tuning the controller empirically doing just this.
A couple of notes: The gyro rates - omega.x and omega.y are the derivatives. They are the radians per second of rotation. There is no need to calculate them twice by taking the change in angle error and divide by elapsed time. You will just end up with exactly or a slightly less accurate version of the omega.x and y values directly from the Gyros.
The MultiWii implementation was grabbed because I needed a quick implementation of the D term. If you capture the actual noisy gyro data at full rate and put it into excel and run the MW algorithm you'll see it's a clever little filter that works to limit the gyro noise. It does work but I saw lots of odd behavior with it. I decided to do a simple filter in Beta 4 like this:
// D term
current_rate = (current_rate *.7) + (omega.x * DEGX100) * .3;
int16_t d_temp = current_rate * g.stablize_d;
rate -= d_temp;
I chose this based on the sampled data and running the filter in excel. It was a good compromise between noise and latency. It also leverages that fact that the rates are floats. It saves a lot of ram dedicated to those previous 4byte rates
I've flown this with a value of g.stablize_d from 0 to .08 and it had a predictable dampening response. I'm sure you could go a little higher, but the copter was so dampened it wasn't any fun to fly. If you go higher still you will get some nasty oscillations which is typical with a high D.
The PID_Fast controller has some issues which you may be aware of. Namely the derivative is calculated manually, rather than just using the raw and clean signal from the gyros. And the user input is pumped into the D calculation. If I move the stick to 45° pitch, the controller gets a sudden error change and the D portion of the controller will spike. What needs to happen is the sensor value needs to be used and NOT the error.
If you look at the term above from Beta4, you'll see I have a simple filter, just like PID_Fast. I think Doug made that PID filter and it's a little too fancy for me. I agree, however that you must filter the D term or you will get nasty artifacts in the signal.
The idea of a second order derivative, seems a little far fetched. I'd be surprised if the signal was anything but noise. Maybe the Stanford folks did it, but not with our sensors. And honestly I don't think we need it. The real bottleneck is the ESC and motors. If you upgrade the ESC firmware you'll see the very small wobbles go away because this is a latency issue. The motors are not getting the commands to speed up fast enough due to filtered signals in the ESC firmware.
Hope this helps.
Jason
My impressions and video about "PIDT1" here (thanks Igor, it's fantastic):
http://www.diydrones.com/forum/topics/arducopter-2-2-beta?id=705844...
hi roy,
the base issue is the inherently unstable base.
maybe a bit to harsch - for a sunshine flyer ok but...
dcm has an integrator - theoretically it collects the gyrodrift - but it collects all things we don't wanna have.
instead of an integrator we need a proportional gain!
having two integrators in serie is a no go to me.
robert
Although both the current and the proposed architectures can be made to work, I think they both needlessly complicate the tuning process - which is complicated enough on its own. Also, the gains and some of the signals do not mean what you may think they do, if compared to more traditional PID architectures.
First, you should realize that angular rate is the derivative of the attitude, or put another way, that the attitude is the integral of rate (from a linear perspective). Second, you really shouldn't have more than one true integral term in a loop, and you should only integrate an actual error - that is, a desired (or setpoint) value minus an actual (or sensed) value. The intent of an integral path is to drive an error to zero.
If you look at Igor's forum post (http://www.diydrones.com/forum/topics/attitude-control-structure-ch...), you can see the current and his proposed code. The point labelled "Desired Rate" in the current structure is no such thing. To see why, imagine that the "target_angle" is moving at say 10 deg/sec. In an ideal world, the "angle in" would be perfectly following the target, which results in the "Desired Rate" being zero. But, the actual "Rate" will be 10 deg/s. That is, the rate feedback always opposes all motion (or in other words provides damping) - which can be OK. But, the "Rate Error" isn't really an error in the sense I mentioned above. Even in the ideal case, it won't be zero, and hence should not be integrated.
Now, let's swich to Igor's implementation. Assuming we're considering attitude feedback, we can "unwrap" the paths to show how the gains actually map to a more traditional PID loop structure.
The true proportional gain is P (standalone) times the P (from the PID) + I (from PID, due to the rate term - integral of rate is attitude)
The true integral gain is P (standalone) times I (from the PID)
The true derivative gain is P (standalone) times D (from the PID) + P (from PID, due to the rate term - rate is the derivative of attitude)
The true 2nd derviative gain is D (from PID, due to the rate term - you're taking the derivative of rate, which is itself a derivative of attitude). Note, other quads such as Stanford's STARMAC use "double-d" feedback. Bill Premerlani has also expressed interest.
The fact that we don't have a "true target_rate" complicates the above picture slightly (as does the fact that rate isn't simply the derivative of attitude in the full, non-linear sense), but I hope you see my point. The current control structure has similar issues.
If you are going to re-write the PID libraries anyway, I suggest you start over with a clean implementation such that each gain is easily identifiable and independent of the other gains.
- Roy
-
2
-
3
-
4
-
5
-
6
of 6 Next