When in LOITER mode, suppress crosstrack correction to make good circles

[GoogleCodeExporter]

What steps will reproduce the problem?
1. Command LOITER or wait until RTL has brought plane back to home and is 
LOITERing
2. Observe the loiter is not very circular and there are discontinuities in 
nav_roll instead of smooth changes
3. That's it

What is the expected output? What do you see instead?
Smooth symmetrical loitering in low wind conditions

What version of the product are you using? On what operating system?
ArduPilotMega 2.23

Please provide any additional information below.

The problem is that crosstrack_bearing was calculated when a waypoint was 
created to loiter around a fixed point (home, or a waypoint). As the plane 
loiters, if it is in such a position that crosstrack_bearing becomes "in 
range", then update_crosstrack() suddenly adds the calculated factor to 
nav_roll. Then when cross track no longer becomes a factor (45 degrees or more 
away) it is suddenly removed.

:
if (abs(wrap_180(target_bearing - crosstrack_bearing)) < 4500) {
rosstrack_error = sin(radians((target_bearing - crosstrack_bearing) / 
(float)100)) * (float)wp_distance;        // Meters we are off track line
                nav_bearing += constrain(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get());
                nav_bearing = wrap_360(nav_bearing);
}

Obviously one does not care about cross track correction when point loitering 
is running, or RTL has arrived home, so this function needs to be aware that 
the plane is loitering, or has arrived near the current target way point 
(probably the easiest fix), and then cease to influence nav_roll.

When I disable cross track gain completely in the parameters, the loitering 
becomes very smooth and symmetrical with a steady and only slowly changing bank 
angle (+- 4 degrees). Re-enabled, and it becomes messy with large bank angle 
jumps.

Original issue reported on code.google.com by justinbe...@gmail.com on 5 Sep 2011 at 2:03

[GoogleCodeExporter]

Just to be clear(er) ..

a) nav_roll is calculated from nav_bearing and GPS ground track or magnetic 
bearing.
So when nav_bearing jumps, bearing error jumps, when bearing_error jumps, 
nav_roll jumps according to its PID. When nav_roll jumps, the servo PID loop 
has to suddenly respond to its error discontinuity (sudden error between plane 
roll and nav_roll), and that is what makes the controls (and actual roll) 
un-smooth.

b) even with no stored waypoints, and therefore no "track", crosstrack_bearing 
is still created when a synthetic waypoint is created for a navigation target.. 
so RTL creates a waypoint, and a crosstrack_bearing (a line between the point 
you issued the command and Home). And that line is followed with cross track 
gain.. until the arrival loitering.

(I think changes to nav_roll (and nav_bearing and nav_pitch) should have 
maximum slew rates attached, so that discontinuous error terms do not cascade 
and cause oscillations in the control PID loops .. This would smooth autopilot 
reactions during mode switches, and waypoint changes).

Original comment by justinbe...@gmail.com on 5 Sep 2011 at 2:28

[GoogleCodeExporter]

Justin, 

I have investigated your issue and believe your result is simply a result of 
bad tuning on your setup.  

For loitering the update_crosstrack function is only called when you are 
outside of g.loiter_radius + LOITER_RANGE distance from the waypoint (loiter 
center).  Most people's tuning (including my own) rely on crosstrack correction 
and use no navigation I gain as this produces much more satisfactory results 
for general navigation.  However without navigation I gain the actual loiter 
radius will always be larger than g.loiter_radius.  If your navigation P gain 
is sloppy it would be easy for the loiter radius to exceed g.loiter_radius + 
LOITER_RANGE, which would produce the effect you are complaining about.  If you 
tune so that your loiter radius is less than g.loiter_radius + LOITER_RANGE 
then you should see no issue with crosstrack correction being applied.

I have never noticed this effect myself.  Please try tuning your navigation 
gains and see if this resolves your issue.

Also, on your suggestion of a slew limit for roll, we used to have that feature 
and removed it.  With proper tuning of the PID gains it is really not necessary.

Original comment by dewei...@gmail.com on 7 Sep 2011 at 3:20

• Changed state: CannotReproduce

[GoogleCodeExporter]

Ah OK I got it -  but sloppy tuning was not actually why I stumbled on this 
issue .. both waypoint_radius and loiter_radius are 8 bit signed ints, so the 
max input is 127 meters. In addition, loiter_range is hard coded to 60, so I'm 
not sure how that will work out if loiter_radius is increased to reflect a 
faster aircraft.

The tuning was good, servo pids and navigation pids are all tuned however since 
I was testing with X-Plane I guess the assumption in the elimination of cross 
track correction, and the default loiter, are just for slow planes, not full 
size or fast ones that take 500m+ of space to do circles.

BTW the APM does not seem to alert on overflow of entered parameter values 
where there are limits like Int8 or Int16 (there are some others too). They 
appear to write, but then read back wrongly.

Original comment by justinbe...@gmail.com on 7 Sep 2011 at 4:09