Waypoint/Path Manager

Overview

This module determines the desired heading and altitude of the aircraft and manages the waypoints (also called “path nodes”) of the plane’s flight path. The module takes in GPS coordinates and altitude measurements and calculates the heading and altitude the plane needs to stay on course. Additionally, the module takes instructions from the state machine to modify the waypoints in the plane’s flight path.

How does Waypoint/Path Management Work?

Note that a lot of this information comes from the PicPilot docs, which was last updated on January 6, 2016.

Introduction

Path management is the process of dynamically determining the optimal course to navigate a set of pre-defined waypoints. It can be broken down into three sections: straight path following, orbit following, and the blending of straight path and orbit following.

Straight Path Following

Straight path following is basically following a line (shocking).

The principle is simple. Given two points, and the location of your vehicle, determine what heading it should follow. If your plane is on the desired path, the heading of the plane will be the same direction as the line connecting the two points. However, things get interesting when the plane is slightly off the path. In this case, the plane should face slightly towards the path to reduce its cross-track error (the distance between the plane and the line connecting the two waypoints). We don’t make the plane follow a path perpendicular to the line since it may result in the plane crossing the line, resulting in the same issue all over again. Now, if the plane is an infinite distance from the line, then the plane should head at a heading perpendicular to the line, in order to regain distance. This is the premise of the straight line path following algorithm. As a result, a heading vector field would look as such:

As the plane moves further away from the line, the angle between the plane's desired path and the line approaches 90º.

The equations that govern this behaviour are not complicated. There are only a few things required to make this calculation. Firstly, you must know the heading of the line. If you have the XY coordinates of the line endpoints, you can easily determine that through simple trigonometry. If you only have the GPS coordinates, you should use the Haversine Formula in order to get the XY coordinates.

Once you have the coordinates, subtracting them will give you the direction of travel (in an XY plane). Furthermore, by applying the arctan function on the direction of travel, one will determine the line’s heading. In addition, the value should be between –pi and +pi. Thus, any 2pi corrections that need to be made can be done at this point.

The cross-track error is calculated as follows (can be derived with trigonometry):

cross_track_error = cos(courseAngle) * (positionY - targetWaypointY) - sin(courseAngle) * (positionX - targetWaypointX)

The following diagram provides a good visualization of the variables in this equation:

The cross track error is then useful to determine the desired heading of the aircraft. Once again, using the arctan function is suitable to do so:

desired_heading = 90 - rad2deg(courseAngle - MAX_PATH_APPROACH_ANGLE * 2/PI * atan(k_gain[PATH] * pathError))

Note, as the atan term increases, the heading approaches the (courseAngle -__MAX_PATH_APPROACH_ANGLE).

Note, that the courseAngle is calculated with respect to the x-axis, on the x-y plane. In other words, a path going from West to East would have a courseAngle of 0 degrees. This would be 90 degrees in terms of a true heading. Therefore, to get the true heading, you must subtract the courseAngle from 90 degrees.

Orbit Following

Orbit following is less straight-forward as straight path following, but it basically involves following a curve of a certain radius. An orbit is depicted by a radius, a center location, and the direction of travel (clockwise or anti-clockwise).

In order to maintain a certain radius, the Euclidean distance needs to be calculated between the center of the orbit and the plane itself. The goal of this function is to maintain this Euclidean distance constant. The Euclidean distance can be calculated as such:

float orbitDistance = sqrt(pow(position[0] - center[0],2) + pow(position[1] - center[1],2));

position[0] → x coordinate of plane’s current position
position[1] → y coordinate of plane’s current position
center[0] → x coordinate of orbit center
center[1] → y coordinate of orbit center

This value is then used to determine the equivalent of cross_track _error, but for an orbit. This is done very easily. The term d (Euclidean distance) subtracted by the ρ (desired radius) provides the relative error, which must be minimized.

orbit_cross_track_error = 90 - rad2deg(courseAngle + direction \* (PI/2 + atan(k\_gain[ORBIT] \* (orbitDistance - radius)/radius)))

This equation is actually very similar to the equation governing straight line path following. The arctan function forces the heading to converge onto the orbit. The direction of travel (λ) which can be either 1 or -1, reverses the effect of the heading perturbations. This is then added onto the course angle as a perturbation. Once again, a gain value needs to be tuned to determine the rate of convergence.

The course angle can be determined easily based on the location of the curve. For instance, if the vehicle is in the first quadrant of the circle/orbit, the heading will range between 270° and 0°, assuming a counter-clockwise rotation. This course angle can be calculated using this equation:

float courseAngle = atan2(position[1] - center[1], position[0] - center[0]);

Blending Straight Path and Orbit Following

The orbit following and straight path following algorithms are used in combination in order to assemble a path. Both algorithms alternate in usage. Every corner uses the orbit following algorithm. Every straight line uses the straight path following algorithm.

In order to put the two together, you must draw in orbits between each set of points. The additional restriction is that they must be tangent to both lines, as depicted in this diagram:

Between waypoints we use straight line following. When switching between straight line paths, we use orbit path following for a smooth transition. Notice that the two straight line paths are tangential to the circle defining the orbit.

Figuring out where the tangent will touch requires some basic trigonometry. [WILL TOUCH ON LATER]. The coordinates at which this occurs can be calculated using:

NextX/Y/Z refer to the coordinates of the point W_(I+1) and targetX/Y/Z refer to the coordinates of the point W_i.

he turning angle can be calculated via the following equation:

float turningAngle = acos(-deg2rad(waypointDirection[0] * nextWaypointDirection[0] + waypointDirection[1] * nextWaypointDirection[1] + waypointDirection[2] * nextWaypointDirection[2]));

Index 0 is the x-coordinate
Index 1 is the y-coordinate
Index 2 is the z-coordinate

This is simply the dot product of the (W_i - W_(i-1)) vector and the (W_(i+1) - W_i) vector. Given the dot product formula, you can use the arccos function to determine the angle between the two lines.

At the points where the straight line paths touch the orbit path, there is a checkpoint. Imagine a giant plane perpendicular to the path. As soon as the plane crosses this boundary, the next step is executed. For instance, if the vehicle is travelling straight along a path, then passes the plane, it will initiate a turn (orbiting algorithm). Once it passes the next plane, it will initiate the straight line path following algorithm once again.

In order to detect if a vehicle passes the boundary, the dot product of two vectors must be taken. If the value is positive, it is an indicator that the vehicle has crossed the boundary.

The dot product is:

Both vectors have X, Y, and Z components. Likewise, equations that depict the half plane are stated above.

Note that in the code, all "direction vectors" such as W_i – W_(i-1) are normalized.

For every pair of "checkpoints" the path index is incremented once they are passed. The index is used to identify the data in a linked list through the wireless communications.

Implementation (Section needs to be updated [Will be done by Nov. 30])

Managing Path Data

All of the waypoints along the flightpath will be stored in the following structure:

/**
* Structure stores information about the waypoints along our path to the destination and back. 
*/
typedef struct {
    struct _PathData * next;          // Next waypoint
    struct _PathData * previous;      // Previous waypoint
    long double latitude;             // Latitude of waypoint
    long double longitude;            // Longitude of waypoint
    int altitude;                     // Altitude of waypoint
    float turnRadius;                 // if hold is commanded (type = 2), then this is the radius of the hold cycle
    char waypointType;                // 0 = regular waypoint, 2 = hold waypoint (plane will circle)
    char waypointId;                  // Id of the waypoint
} _PathData;

The structure links to the previous and next waypoint, making it easier to traverse from one waypoint to another.

All waypoints will be stored in an array called _PathData waypointBuffer[100].

Should the flight path of the plane be modified, there are the following methods to add, insert, delete, update, and clear all waypoints: (These changes modify the waypointBuffer array)

// Private functions
 int get_waypoint_index_from_id(int waypointId);                                   // If provided a waypoint id, this method finds the element index in the waypointBuffer array
_PathData* initialize_waypoint();                                                 // Creates a blank waypoint
_PathData* initialize_waypoint_and_next();                                        // Creates a blank waypoint with the next waypoint defined
void append_waypoint(_PathData* newWaypoint);                                     // Adds a waypoint to the first free element in the waypointBuffer (array)
void insert_new_waypoint(_PathData* newWaypoint, int previousId, int nextId);     // Inserts new waypoint in between the specified waypoints (identified using the waypoint IDs)
void delete_waypoint(int waypointId);                                             // Deletes the waypoint with the specified ID
void update_waypoint(_PathData* updatedWaypoint, int waypointId);                 // Updates the waypoint with the specified ID             
void clear_path_nodes();                                                          // Empties waypointBuffer array

These functions are executed by calling the following function. Note that not all parameters will be used for each modification to the waypointBuffer array. The goal of this function is to call the appropriate function listed above and pass in the required data.

/**
  * Adds, inserts, updates, or deletes a single waypoint in the waypointBuffer array
  * 
  * @param[in] _PathData* waypoint -> In the instance that we need to update, insert, or append a new waypoint, this will be used 
  * @param[in] _WaypointBufferUpdateType updateType -> the type of modification to the waypointBuffer array (look above)
  * @param[in] numWaypoints -> number of waypoints that are in the waypoint array (will be 1 for insertion, updating, and deleting). May be greater than 1 for appending
  * @param[in] int waypointId -> the ID of the waypoint that will be updated or deleted. Set to 0 by default, so does not need to be passed (not needed for appending or insertion)
  * @param[in] int previousId -> stores the ID of the waypoint that will come before the inserted waypoint. Set to 0 by default, so does not need to be passed (only needed for insertion)
  * @param[in] int nextId -> stores the ID of the waypoint that will come after the inserted waypoint. Set to 0 by default, so does not need to be passed (only needed for insertion)
  */
  void update_path_nodes(_PathData* waypoint, _WaypointBufferUpdateType updateType, int numWaypoints, int previousId = 0, int nextId = 0, int waypointId = 0);

The Waypoint ID System

The waypoint management module is not responsible for creating new waypoints. Instead, the state machine will send waypoints to the waypoint management system and the module will initialize/update the waypointBuffer array accordingly.

When the state machine creates a new waypoint, it will need to assign it a unique ID. This can be as simple as assigning the first waypoint an ID of 1, and the nth an ID of n. The ID of the waypoint, which is the waypointID parameter in the _PathData structure, is an essential feature for finding waypoints within the waypiointBuffer array.

Here's an explanation I gave on a PR earlier:

When a waypoint is created, it is stored in the waypointBuffer array. However, as waypoints are inserted, deleted, etc. the index of the waypoints will change during the duration of the flight.

For example, say when you initialized the waypointBuffer array, you put w1 at waypointBuffer[2] and w2 at waypointBuffer[3]. However because of deletions and insertions, w1 and w2 are now found at waypointBuffer[5] and waypointBuffer[6]. Now, say you want to insert a new waypoint, w3, between w1 and w2, the state machine cannot do this since it does not know the indices of w1 and w2.

This is where the ID system comes into play. We give each waypoint an ID and make the state machine store an array of integers, where each element is the ID of a waypoint. The order of the elements in the array will match the order with which the waypoint manager executes the waypoints (this will make it easier insert waypoints). If the state machine wants to update or insert a waypoint, it will pass in the IDs of the affected waypoints to the waypoint manager. Using the IDs, the waypoint manager will call a method called get_waypoint_index_from_id(int), which will use the ID of the waypoint to get its index in the waypointBuffer array. As a result, we have a reliable way to find the waypoints within the waypointBuffer array :)

State Machine and Waypoints

Since the state machine will be creating waypoints, a question that comes up is: what exactly does the state machine need to initialize within the _PathData structure before passing it to the waypoint manager:

Previous and next _PathData objects → Doesn’t need to be initialized. Waypoint manager will take care of it.
- Constructor of waypoint manager object:
  - Ensure the array of _PathData objects stores the waypoints in order of execution, the waypoint manager can link them together, so these parameters do not need to be initialized.
Latitude and longitude → Initialize this
Altitude → Initialize this
turnRadius → Only initialize this if the waypoint is a “hold” waypoint.
- At a “hold” waypoint, the plane will circle in the sky and wait until further instruction.
waypointType → Initialize this. This has a few allowable values
- 0 → regular waypoint. Plane will use it to navigate to destination
- 2 → “Hold” waypoint. Plane will circle and await further instruction
waypointId → Initialize this. This parameter must be unique to each object, otherwise array navigation will be messed up

Inputs and Outputs

Inputs

There are two different operations that this module is responsible for. Each one has different required inputs:

Getting desired directions
- Current GPS Coordinates (longitude and latitude)
- Current Altitude
- Current Heading
Updating waypointBuffer array
- Appending waypoint → structure of new waypoint
- Insert waypoint → structure of new waypoint, ID of previous and next waypoint
- Updating waypoint → structure of updated waypoint, ID of waypoint that needs to be updated
- Deleting waypoint → ID of waypoint that is to be deleted.

Outputs

There are two different operations that this module is responsible for. Each one has different outputs:

Getting desired directions
- Desired Heading
- Desired Altitude
- Distance to next waypoint
Updating waypointBuffer array
- No output

This is the structure that will be outputted:

enum _WaypointStatus {WAYPOINT_SUCCESS = 0, WAYPOINT_NOT_FOUND, WAYPOINT_PARAMETERS_NOT_DEFIND, UNDEFINED_FALIURE};

/**
  * Structure contains the data that will be returned to the Path Manager state manager. 
  * This data will be used by the PID and coordinated turn engine to determine the commands to be sent to the Attitude Manager.
  */
typedef struct {
    uint16_t desiredHeading;            // Desired heading to stay on path
    int desiredAltitude;                // Desired altitude at next waypoint
    long double distanceToNextWaypoint; // Distance to the next waypoint (helps with airspeed PID)
    _WaypointStatus errorCode;          // Contains error codes
    bool isDataNew;                     // Notifies PID modules if the data in this structure is new
    uint32_t timeOfData;                // The time that the data in this structure was collected
} _WaypointManager_Data_Out;

Steps Executed when Module is Called

There are two different processes that this module executes: calculating desired heading and altitudes, and modifying the waypointBuffer array.

Calculating Desired Heading and Altitude:

 /**
  * Updates the _WaypointManager_Data_Out structure with new values.
  * 
  * @param[in] _Gps_Data currentPosition -> contains the current coordinates, altitude, and heading
  * @param[out] _WaypointManager_Data_Out &Data -> Memory address for a structure that holds the data for the state machine
  */
  void get_next_directions(_Gps_Data currentPosition, _WaypointManager_Data_Out *Data);

State machine calls the get_next_directions() and passes in appropriate parameters (GPS data and pointer to the output data structure).
Algorithm stuff [ADD LATER]
Waypoint manager updates the parameters in the _WaypointManager_Data_Out structure that was passed into get_next_directions().

Modifying waypointBuffer Array:

State machine calls update_path_nodes() and passes in parameters along with specifying the modification type via the _WaypointBufferUpdateType enum.
1. Some parameters have default values, so for certain operations, they do not need to be passed. (Look at comments in code)
update_path_nodes() calls the appropriate private function and that function updates the waypointBuffer array.