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
Yes, and that makes it unstable, and causes it to be grabbed by the wind, and waste energy getting back.
I tested it 2 days ago or so in 7-11 m/s wind.
The best way to deal with input, would be to modify the desired position based on input - NOT disable AP temporarily.
Accurate, in fact to change the Loiter position fairly accurately I go back to Stabilize, move the quad to the new location and then I put the Loiter again.
But then I think the auto reset loiter not right to exist, like autolanding because it does not work as expected :-)
switching back to stabilize for movement is no good.
Please try to that in windy conditions: it feels like: (tested in 7-11m/s ~2 days ago)
-AP is in loiter mode, copter is standing steady in headwind.
-you switch to stabilize wishing to move 5 meters to the "right"
-now the copter is 10meters back, while you fight the wind with pitch, and at the same time try to get to the right.
-you finally arrive. , enable loiter
-the copter is taken far back with the wind - and fights again towards the position
--- energy & time wasted, my suggestion of telling AP to adjust target position with stick is better than this, I added it as a feature request, hoping somebody else sees the advantage. If stick were moved to right, causing desired position be moved to right, AP would still keep the pitch attitude and power, and just go right as smooth, as it did getting to the loiter point the first time.
Ok I agree with you, but I think this is quite normal, the new marked position when re-engade the Loiter in that case has a gap due to an error of even GPS.
I'm talking about another problem here.
These tests should be done by moving the drone of over 10 meters, with the Mediatek GPS the miracle are not possible.. :-)
Yes Angel and I know that is not an error derived from your mods, but the loiter reset working only if you have define this:
#define AUTO_RESET_LOITER 1
I think the default is "0" (disable) but anyway I forced this option in my config for this test, and in this condition the bug is present (i think) because if you move the quad (try with like some 100 meters) and then release the ale/ele stick you can see that, the quad going at 10/15 meters from the original point, NOT in the point recorded when you are released the stick! :-)
Just for security i tried again with the resetter enabled, and in fact in this way works as it should., even if wrong over 10 meters of the "stick released" point, very bad!
RTL is the only way to actually get back to a position well-recorded, absolutely not possible with Loiter, in both case there's no precision in the management of stored position (return to the original loiter point or return to the new loiter point).
Test yourself and tell me if I'm wrong.
Sounds good Afernan, this is your Loiter implementation recorded with AeroSim in cross wind, Loiter P = 1,0 and Loiter I = 0,2.
For see just put this file in the "data" directory of AeroSim, in Windows 7 the path is:
Then start AeroSim, go to "Rec/Play - Replay" and select "Afernan Loiter V2.2b2", hit play for view.
Thanks for testing, Marco.
I had a code error in the version I´ve put before (I´ve erased the post).
I´m working in the same line, but I´ll prefer to close the circle and real test before posting any possible solution. I have now a very promissing version (in HIL) that I´l real test tomorrow. Stay tuned.
I read your paper above on the use of the D squared term to damp oscillation. Two things occurred to me.
1. With the D squared term, the damping is similar to vehicle suspension systems, where the dampers (shock absorbers) use the resistance of fluid through an orifice to perform the damping function.These have been developed to be very good at controlling the ride of a vehicle.
Automotive do a couple of things extra that we don't:
a. The damping is dependent on the direction of motion. The rebound rate is typically higher the compression rate. I.e. when moving away from the set point the damping is lower than when moving back.
b. Damping rates can change as they open and close orifices depending on velocity.
One thing we do that suspension systems generally don't: Use an I term. In the world of suspension this would be self-leveling. The self-leveling suspensions I've seen are very slow to react, typically only after they've decided that the load is stable. The analogy to our application would be to correct for steady state wind, but not for gusts.
This could mean that there could be some value in changing the damping rates depending on whether we're moving towards or away from the loiter target.
I found a paper on damper modeling. They went up to 4th order to accurately model the behaviour.
I also noted that the damping force increased rapidly near zero velocity, but less so at velocity extremes.
I need to think more about what this really means for us. As I said, I'm thinking about this only because car suspensions do their job pretty well. Suspensions do other things like rising rate (non-linear P-term) as well. This may be something to explore.
2. (This may be a minor point) If non linear (e.g. square) functions are to be used then lat and long components can't be treated separately. Ideally, you would compute the magnitude of the velocity vector (sq root of (x*x + y*y)) to compute the damping. Otherwise the control law will be different if the movement is at 45° to the coordinates, compared to movement along the coordinates. For square laws, the maximum difference would be a factor of 1.4.
Another minor point: I haven't actually looked at the code for a little while, but I did see that displacement (in cm) was computed directly from lat and long errors without compensating the longitude error by cos(latitude). At my latitude (~37°) this is a factor of 0.79. If ~20% changes in PID parameters are making a difference then this may be relevant. So far, I haven't found the P or I terms to be that critical, so again this probably a minor point any has to balanced with the cost of doing the extra computation, but could be the cause of oval shaped loiter paths.
Interesting analogy. I was just about to point out about variable rate springs, but you got it.
Basically, what you're talking about here is "gain scheduling". A topic that we discussed about a month ago. It's an interesting concept, something I'd like to see attempted.
first of all thanks for taking your time for this idea.
Second, I apologize for the code error I had using "^" as expo (a FORTRAN residual).
Now, I´ve corrected the error , and continue working on the bottom line idea. I´ll post results after HIL+real test in my side. Stay tuned.
Some comments about the code:
The long compensation is done in the line 96:
long_error = (float)(next_loc->lng - current_loc.lng) * scaleLongDown;
To use the quadratic "D" factor is necesary to keep the D sign, for example using this code:
//int16_t lon_D = 10 * (x_actual_speed)*abs(x_actual_speed) ;
In fact, now I´m not using quadratic D. Im testing high D factors (=10) that In HIL works great.
Best and thanks for the paper; I´ll read it.
Some more reflexions...
Basically we are dealing with three problems:
1.- transition to the set point. We want it quick and highly damped
2.- the precision on keeping the set point. Obvious.
3.- avoiding of oscillations, circles or divergences
All are really linked, but "2" is a bit more complex due to GPS precission. In this case inertia corrections using accelerometers should be used (not yet implemented).
If we concentrate in 1 and 3 we experience the following:
Imagine we are already steady oscillating around set point. This is caused by overshoot the correction. If we have a look to the P and D components during the oscillation, we can see that in the very moment of crossing the set point (P-term = Kp * error = 0) D-term is governing and is in the right direction of stoping the movement, but being not enough strong, oscillation continues. In that case, we need more "D-term".
D-term is the "error speed". We add it with minus sign in order to damp the oscillation. In the oscillation, the speed is zero at the two ends, and maximum in the center. So is perfect to increase D effect using a quadratic (or more) D term but keeping the sign. This reduce the "overshoot" component: far from set point the quadratic D-term is smaller than linear; and close to set point is the oposite.
We don´t want "asimetrical" D-term (flying away lower, flying to set point higher) since we want both directions to dump the movement as much as possible. So we keep it simetrical.
For the moment I keep a high D factor (10) but still linear with wide limits. I´ll flight today this approach to see the real reaction. I will report it.