I´ve been working some weeks in a code for improvement of LOITER since I found not good enough for me (IMHO) the one of AC2.1.1.r8 (and previous up to 2.0.49).
I´ve started with the AP-NG that I´ve remembered quite good. I´ve translated to AC2 with some modifications.
Now I have a version that works quite well at least in HIL (need to real flight test).
I´ve implemented in AC2.1.1.r8 in a very simple way: by adding two new functions in "Navigation.pde".
static void calc_loiter_AFD(int x, int y)
static void calc_loiter_pitch_roll_AFD()
The rest of the code is quite the same (except the call of this funtions in Arducopter.pde and variables naming). PID´s has been tuned in HILL with AeroSim-RC and works right for light to hard wind.
This is under Dev-Team consultance for the moment, but you are free to test
I'm getting a permission denied message when I try to download those attachments...
The simulator is very different from real life. The GPS values are nearly perfect in the simulator but that certainly not the case in real life.
Same here... Angel check the attachment... :P
I´ve attached it again (the system didn´t like rar files)
I Know Randy, but I´m testing both HIL and real from long time ago, and I´m able to predict (more or less) if the thing will go right or not. HIL it´s a true help and H/W saver.
But, it´s true, the real test will tell the truth.
In fact, present offitial code flies perfect in the sim, but not in real life.
Work fine Angel! ++++
I have some excesses wobbling in loiter, but it looks nice planted, even with the wind! Let's say that this is not the problem has to do when looking for flights with zero wind, without touching the pid.
Which quad with AeroSim? And you left unchanged pids in the simulator? A strong wind does not take me with the position, missing to keep a good inclination for mantain the position, anyway drift or circle here.
I´m using the standar MK quad but with this internal PIDs in AeroSim:
Accel (X/Y) PID = 0.01, 0.003, 0.05
Gyro PID = 0.0, 0.0, 0.0
I put the default PIDs on MP (except rate_yaw_P=0.015) . With this I get a similar behavior than real flight. Then, I´ve saved that model for HIL testing. I think that this is a closer approach to reality.
For the moment, the code implementation is rudimentary up to see if it´s worth to continue this line. I´ve asked Jason to consider this. We´ll see...
I´ve improved the code to reduce the wobbling (in HILL test for now), but need to see in real flight. I remember the original Ardupirates that had also this small "pulses" when in loiter.
There are two main damping parameters:
- the long displacement damping: related with the D limitation
gps_roll_D = constrain(gps_roll_D,-500,500);
- the small wobbling: related with K_D. I´ve used 0.03, but you can play (up to 0.1 is OK)
Update: this LOITER solution implemented to 2.1.1.r9
Some improvements are implemented also.
A log about the changes implemented are given in doc file attached
I´ll real flight tomorrow.
Wait, is your loiter solution in the "official" r9 in the download area? Or did you just put it into your own r9 and attach that here?
So can you explain what exactly your solution does differently? Just curious.
I did a bunch of analysis on the method you posted. First, I ran it as a benchmark and then refactored it to match the current version. I've actually been able to surpass the accuracy of the zipped file, just a bit and I've gotten rid of some of the jerky movements.
First, it's not a standard PID loop, even though it tries to be one. The I-term is incorrectly implemented and fails to function as an Iterm at all. I've seen this all over the NG code and I think Jose was just doing it wrong. If you plot out the code it simply ramps to full value within a second. Then the iterm is cleared as the error approaches zero. This completely negates the entire effect of an iterm as a correction for steady-state error. If you plot out the effect of the iterm you'll see it's acting only as a P term, although slightly delayed.
What this does is make the Pterm double in strength since these two values are summed. (degrees are * 100)
A normal I term would build up a 3-4° counter to the wind and make the P corrections much smaller.
Next is the D term. The output shows up as a series of 5° spikes every time we change lat or lon values. Since we are are the maximum accuracy of the GPS, changes look like we're suddenly traveling 1m/s when we're really traveling .1m/s
This leads to the jerky movements as we hover over the target. Not great for shooting videos!
So what we need is better smoothing of the output, which can only be had with attitude estimation and inertial feedback. Hopefully we can get there.
Here is the tuned output:
Here is the old code, which suffers from a too low P term which allow's I to build up sending in around.
Here is the code:
static void calc_loiter(int x_error, int y_error)
int16_t lon_PI = g.pi_loiter_lon.get_pi(x_error, dTnav);
int16_t lon_D = 3 * x_actual_speed ; // this controls the small bumps
int16_t lat_PI = g.pi_loiter_lat.get_pi(y_error, dTnav);
int16_t lat_D = 3 * y_actual_speed ;
//limit of terms
lon_D = constrain(lon_D,-500,500);
lat_D = constrain(lat_D,-500,500);
nav_lon = lon_PI - lon_D;
nav_lat = lat_PI - lat_D;
"3" will need to be replaced by a user settable D term.
Loiter P is set to 2.4
Loiter I is set to .1
¡Thanks a lot, Jason!. I´m really happy to help a bit in this exciting project.
I saw some of the code problems you drescibed, but I didn´t know how to fix them.
Can´t wait to test it! ...damn flu!
I´d like to give some explanations of my code (before Jason´s):
I term. I saw in the simulator that I-term "load" when out of the spot, but when the error goes to cero and cross to the other side in the oscillation, it continue sometime unloading until it gets loaded in the other sens. So, during some time is doing an "amplification" instead of his real work. This is the reason I´ve implemented an I_term "reset" when error is close to zero; (sorry it´s difficult to explain for me)
I´ve tested this idea in previous r9 code and ¡it improves a lot! (on the HIL...)
D term. I saw the spikes, and I remember them in AP-NG code in real flight (there were not so dramatic impact, but anyway is better to avoid them).
The peak is controlled by the "constrain" limit, and the rate by the K_D factor, but appart of that spikes, D is really effitient to damp the loiter, this is clear.
For info, the space satellites ADCS (Actitude Determination and Control System) use impulses suplied by the small thrusters that control the "loiter" perfectly. Obviously the effect is highly damped by the big mass and inertia of the body, which is not our case (just the oposite: low mass and powerfull motors)