diydrones/ardupilot — pull request #101 by brnjones (2013-02-24 22:00:15+00:00)
Title: Plane: L1 Control for Straight and Curved Path Following

This is an implementation of a guidance algorithm described in [A New Nonlinear Guidance Logic for Trajectory Tracking (AIAA 2004-4900)](http://acl.mit.edu/papers/gnc_park_deyst_how.pdf). This provides controlled path following around curves and straight lines, simplified user setup, and architecture extensible to future higher level path generation algorithms.

The control law consists of 2 parts, 1) Calculation of a reference waypoint (L1_ref) along the trajectory to be followed, and 2) Calculation of a lateral acceleration command that intercepts the calculated L1 reference point. For aircraft, this lateral acceleration command is equivalent to a bank angle command.

Major advantages to the L1 control law are:

* Reduction of number of configurable gains: Nav_Roll PID and xtrack error gain are replaced with an insensitive L1 reference distance. This largely eliminates tuning for path following.
* Improved disturbance rejection and circular path tracking in wind
* Extensible to future higher level trajectory generation algorithms, follow-me, 4DT, vector fields, obstacle avoidance, formation flight, etc. Behavior can be updated by introducing another L1 reference point calculation function.

### Implementation

First, a reference waypoint called “L1_ref”, is calculated within the navigation control loop. L1_ref is placed differently depending on straight line or circular following. Furthermore, L1_ref is placed differently depending on which region the aircraft is located. Once the L1_ref point is calculated, the angle between the velocity vector and desired path is calculated. This is performed at 10Hz in navigation.pde. In attitude.pde, a bank angle command is calculated to put the aircraft on a circular path to intercept the L1_ref point.

In essence, the aircraft follows a reference point that constantly slides along the desired path. The method for calculating L1_ref for straight line and circular path following are shown below:

![Waypoint_diagrams1](https://f.cloud.github.com/assets/2054578/190040/436b30ae-7ec4-11e2-90a7-01fce8717392.png)

![Waypoint_diagrams2](https://f.cloud.github.com/assets/2054578/190059/fd559b3e-7ec5-11e2-8584-d21e2d81f952.png)

### How to Use
To enable L1_Control for loiter and straight line following add the following to APM_Config.h:

#define L1_CONTROL ENABLED
#define L1_REFERENCE_LENGTH [length (meters)]

With the APM1 or 2 on a Skyfun at ~20 meters/sec, a L1 reference length of 40 meters works well.