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.


Gremsy liked Gremsy's profile
Mar 12
DIY Robocars via Twitter
RT @chr1sa: Donkeycar 4.4 released with tons of new features, including path learning (useful with GPS outdoors), better Web and Lidar supp…
Nov 27, 2022
DIY Robocars via Twitter
RT @NXP: We are already biting our nails in anticipation of the #NXPCupEMEA challenge! 😉 Did you know there are great cash prizes to be won…
Nov 24, 2022
DIY Robocars via Twitter
RT @gclue_akira: レースまであと3日。今回のコースは激ムズかも。あと一歩 #jetracer
Nov 24, 2022
DIY Robocars via Twitter
UC Berkeley's DIY robocar program
Nov 24, 2022
DIY Robocars via Twitter
RT @chr1sa: The next @DIYRobocars autonomous car race at @circuitlaunch will be on Sat, Dec 10. Thrills, spills and a Brazilian BBQ. Fun…
Nov 24, 2022
DIY Robocars via Twitter
RT @arthiak_tc: Donkey car platform ... Still training uses behavioral cloning #TCXpo #diyrobocar @OttawaAVGroup
Nov 20, 2022
DIY Robocars via Twitter
RT @emurmur77: Points for style. @donkeycar racing in @diyrobocars at @UCSDJacobs thanks @chr1sa for taking the video.…
Nov 20, 2022
DIY Robocars via Twitter
RT @SmallpixelCar: Going to @diyrobocars race at @UCSDJacobs
Nov 8, 2022
DIY Robocars via Twitter
RT @SmallpixelCar: Race @diyrobocars at @UCSDJacobs thanks @chr1sa for taking the video.
Nov 8, 2022
DIY Robocars via Twitter
RT @PiWarsRobotics: Presenting: the Hacky Racers Robotic Racing Series in collaboration with #PiWars. Find out more and register your inter…
Oct 23, 2022
DIY Robocars via Twitter
RT @Hacky_Racers: There will be three classes at this event: A4, A2, and Hacky Racer! A4 and A2 are based around UK paper sizing and existi…
Oct 23, 2022
DIY Robocars via Twitter
Oct 23, 2022
DIY Robocars via Twitter
Oct 19, 2022
DIY Robocars via Twitter
Oct 18, 2022
DIY Robocars via Twitter
RT @NeaveEng: Calling all UK based folks interested in @diyrobocars, @f1tenth, @donkey_car, and similar robot racing competitions! @hacky_r…
Oct 13, 2022