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:
Con:
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:
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
Comment by Tomas Soedergren on January 25, 2012 at 5:49am 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...
Comment by Tomas Soedergren on January 25, 2012 at 9:30am 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!
Comment by Igor van Airde on January 25, 2012 at 9:36am 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.
Comment by Tomas Soedergren on January 25, 2012 at 10:12am 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...
Comment by robert bouwens on January 25, 2012 at 10:13am nice!
nothing more to say.
Comment by Igor van Airde on January 25, 2012 at 10:18am Hi Tomas, my comment above is to your first comment.
Now to your second: As I understand, there are different controllers used for acro mode than for the rate loop in the stabilize mode. In acro mode you use just a PI controller for rate. Its another controller than the rate controller in stabilize mode. In my opinion a double implementation that leads to confusion. But my PIDT1 mod has nothing changed in the acro mode.
About your assumption about acro mode "When stick input is zero, copter keeps present attitude. Quickly recovering from any outside disturbance": No, it will quickly recover to the desired rate (angular velocity, e.g. zero on stick middle position) but not to an attitude. Best compraison of the acro flight behaviour is a traditional helicopter. You could implement the behaviour you described, but its not like this at the moment.
You can change the acro mode also from PI to PIDT1. Should bring more stability of cause. But the clean way would be to use the same control loop for acro and the inner rate loop in stab mode. It would also improve the ability to tune, because you could tune your PIDT1 acro loop until it is perfect. Then tuning the P term of stab mode is easy. Now you basically tuning 2 loops (or 4 parameters) at the same time.
About your in flight problem I dont know. The I term of your outer loop should be zero.
Comment by Tomas Soedergren on January 25, 2012 at 10:28am It has been suggested, since the ESC response is a part of the overall system time constant, that control performance would benefit from ESC´s not having digital low-pass filters on their inputs (as they currently do). By this reason there are bold do-it-yourselfers flashing that filter away from the ESC firmware (there are forum threads about it).
Do you think that the standard ESC low-pass input filtering affects overall control performance significantly?
I ask because Castle Creations is doing alpha tests of a firmware that accepts high frame rate and enables instant output response on input signal, without the low-pass filter.
I was thinking, maybe some of our bold test pilots here on DIY would like to be push the limit by tweaking the PIDT1 in combination with unrestricted ESC response, to find out how far it is relevant to go in overall response speed. CC might even sponsor test pilots with some ESC:s... Not a promise, but a possibility :)
Anybody volunteer for that? ;-)
Comment by Tomas Soedergren on January 25, 2012 at 10:56am Speaking of control structure,
I recall Robert Lefebvre some time ago proposed the concept of Gain scheduling control loops.
Is this concept in the ArduCopter developers tool box yet? If not, maybe it should be considered?
Situations I can think of where scheduling might be relevant are:
Comment by Igor van Airde on January 25, 2012 at 10:59am I dont have the standard ESCs. But if there is a low pass filter, it will affect the over all performance significantly!
As I tried the original P->PI controller structure, even a heavy propeller instead of a lighter one could do the difference between stable and unstable. I started the PIDT1 change, because I need bigger propellers to get the thrust from the slow running motors. And with the original controller I didnt get it stable.
So you have to retune the control loops if you change battery, ESCs, motors, propellers, total weight, moment of inertia, ...
And about ESCs: They should be fast but they need a overcurrent protection. And you have to check if the ESC will not stop the motor due to fast change on input values BEVORE flying!
And about fast changing values: The control loop runs at 200 Hz. The output to the ESCs runs at 400 Hz. As I understand the code, they are not synchronized. Increasing the ESC update rate is not necessary, because the microcontroller cant calculate much faster than 200Hz. There is much more basic work to do until we have a sufficiant copter control.
Comment by Igor van Airde on January 25, 2012 at 11:08am About Gain scheduling : I think in ArduPlane is something that the controls depend on the airspeed. On a copter my concept was always to hand tune a easy but sufficiant control structure. You can make things better with gain scheduling or adaptive controls, but you can also make things worse. The more complex, the more possibilities to make mistakes. And my opinion is, that we really have to clean up the controller structure on ArduCopter first, bevore we can think of more. And keep in mind, that the calculating power is limited with the current µC.
Season Two of the Trust Time Trial (T3) Contest has now begun. The fourth round is an accuracy round for multicopters, which requires contestants to fly a cube. The deadline is April 14th.24 members
51 members
185 members
133 members
1298 members
© 2013 Created by Chris Anderson.
Powered by

You need to be a member of DIY Drones to add comments!
Join DIY Drones