Arducopter 2.1.1. alpha Loiter proposal

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



You need to be a member of diydrones to add comments!

Join diydrones

Email me when people reply –


  • 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.

    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.
  • Developer

    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.


  • 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.

  • Developer

    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

  • 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.



    AFD LOITER implementation log.doc
  • Developer

    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.

This reply was deleted.


DIY Robocars via Twitter
RT @TinkerGen_: "The Tinkergen MARK ($199) is my new favorite starter robocar. It’s got everything — computer vision, deep learning, sensor…
DIY Robocars via Twitter
DIY Robocars via Twitter
RT @roboton_io: Join our FREE Sumo Competition 🤖🏆 👉 #sumo #robot #edtech #competition #games4ed…
Nov 16
DIY Drones via Twitter
First impressions of Tinkergen MARK robocar
Nov 16
DIY Robocars via Twitter
Our review of the @TinkerGen_ MARK robocar, which is the best on the market right now
Nov 15
DIY Robocars via Twitter
RT @Ingmar_Stapel: I have now explained the OpenBot project in great detail on my blog with 12 articles step by step. I hope you enjoy read…
Nov 15
DIY Robocars via Twitter
RT @DAVGtech: This is a must attend. Click the link, follow link to read the story, sign up. #chaos2020 #digitalconnection #digitalworld ht…
Nov 15
DIY Robocars via Twitter
RT @a1k0n: Got a new chassis for outdoor races (hobbyking Quantum Vandal) but I totally didn't expect that it might cause problems for my g…
Nov 11
DIY Drones via Twitter
First impressions of the Intel OpenBot
Nov 10
DIY Robocars via Twitter
Nov 9
DIY Robocars via Twitter
Excellent use of cardboard instead of 3D printing!
Nov 7
DIY Robocars via Twitter
RT @chr1sa: We've got a record 50 teams competing in this month's @DIYRobocars @donkey_car virtual AI car race. Starting today at 10:00am…
Nov 7
DIY Robocars via Twitter
Nov 6
DIY Robocars via Twitter
RT @a1k0n: Car's view, using a fisheye camera. The ceiling light tracking algorithm gave me some ideas to improve ConeSLAM, and having grou…
Nov 5
DIY Robocars via Twitter
RT @a1k0n: To get ground truth I measured the rug, found the pixel coordinates of its corners, calibrated my phone camera with my standard…
Nov 5
DIY Robocars via Twitter
RT @a1k0n: @DIYRobocars is back in December, but outside. Time to reinvestigate ConeSLAM! I rigged up a quick and dirty ground-truth captur…
Nov 5