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
Angel
Replies
Hi Angel,
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.
http://www.iaeng.org/publication/WCECS2010/WCECS2010_pp949-954.pdf.
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.
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:
C:\ProgramData\AeroSim_RC
Then start AeroSim, go to "Rec/Play - Replay" and select "Afernan Loiter V2.2b2", hit play for view.
REC-120116144704.bin
Regarding Loiter mode.
I've tested it on a quad in strong wind today (7-12m/s) on ground level.
When in loiter mode, If I roll it.. it's taken by the wind instantly.
How is loiter expected to respond to pitch/roll ? I'd expect it to change it's desired position by stick movement, and just move over to "left/right/ahead" just as well as it's holding the position.
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:
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
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.
Angel
ArduCopter 2.1.1r9c.zip
AFD LOITER implementation log.doc
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.