Join GitHub today
GitHub is home to over 40 million developers working together to host and review code, manage projects, and build software together.Sign up
Plane: L1 Control for Straight and Curved Path Following #101
This is an implementation of a guidance algorithm described in A New Nonlinear Guidance Logic for Trajectory Tracking (AIAA 2004-4900). 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:
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:
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.