Page Comparison

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.

...

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:

...

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

...

This section covers how the Waypoint Manager is implemented.

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 {
    structint _PathDatawaypointId; * next;          // Next waypoint     struct// _PathDataId * of the waypoint
    _PathData * next;                 // Next waypoint
    _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); } _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].

An additional waypoint will be stored in _PathData homeBase; which will store the position of the landing strip.

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 :)

Creating the Waypoint Manager

The waypoint manager was created using a class structure. As a result, a constructor must be called before anything is done!

/**
    * Constructor for this class
    *
    * @param[in] float relLat -> This is the relative latitude of the point that will be used as (0,0) when converting lat-long coordinates to cartesian coordiantes. 
    * @param[in] float relLong -> This is the relative longitude of the point that will be used as (0,0) when converting lat-long coordinates to cartesian coordiantes.
    */
    WaypointManager(float relLat, float relLong); // Call this to get an instance of the class

Setting up the Initial Flight Path and Home Base

The waypoint manager is great in the sense that it handles almost all the work with regards to creating, updating and deleting waypoints. To create waypoints, the state machine needs ton call one of these three overloaded methods. The first one returns a generic waypoint (should almost never be called), the second returns a regular waypoint, and the third returns a hold waypoint.

enum _WaypointOutputType {PATH_FOLLOW = 0, ORBIT_FOLLOW, HOLD_WAYPOINT};

_PathData* initialize_waypoint();                                                                                                              // Creates a blank waypoint
_PathData* initialize_waypoint(long double longitude, long double latitude, int altitude, _WaypointOutputType waypointType);                   // Initialize a regular waypoint
_PathData* initialize_waypoint(long double longitude, long double latitude, int altitude, _WaypointOutputType waypointType, float turnRadius); // Initialize a "hold" waypoint

For the waypointType parameter, pass in PATH_FOLLOW for the second method (Line 4) and HOLD_WAYPOINT for the third method (Line 5).

The home base should be created as a hold waypoint.

Okay, now you know how to make the waypoints, so let’s create a flight path! You will want to create an array of pointers of the type _PathData and initialize them using the methods above. It’s as easy as that!

As for the home base, create a pointer to a _PathData object and initialize it using the third method above.

Now you have created your flight path and home base, you can call one of two methods to initialize the parameters in the waypoint manager class:

**
* Initializes the flight path
*
* @param[in] _PathData * initialWaypoints -> These waypoints will be used to initialize the waypointBuffer array
* @param[in] int numberOfWaypoints -> Number of waypoints being initialized in the waypointBuffer array
* @param[in] _PathData* currentLocation -> Home base.
*/
_WaypointStatus initialize_flight_path(_PathData ** initialWaypoints, int numberOfWaypoints, _PathData *currentLocation); // Sets flight path and home base
_WaypointStatus initialize_flight_path(_PathData ** initialWaypoints, int numberOfWaypoints);                             // Sets flight path

The first method initializes both the flight path and home base. The second only initializes the flight path (this is useful if you want to scrap the flight path but keep the home base). Note that whenever you call these methods that the waypointBuffer array must be empty (no elements). So be sure to call the method void clear_path_nodes() if the waypointBuffer had waypoints in it before.

Getting Desired Heading and Altitude

One of the two tasks performed by this module is determining the heading and altitude required for the plane to stay on course. To do this, there are multiple functions that are called depending on the situation at hand. After calculating the desired values, the module will update the parameters in the following structure which is passed by reference by the state machine:

// Stores error codes for the waypoint manager
enum _WaypointStatus {WAYPOINT_SUCCESS = 0, UNDEFINED_FAILURE, CURRENT_INDEX_INVALID, UNDEFINED_PARAMETER, INVALID_PARAMETERS};

// Used to specify the type of output
enum _WaypointOutputType {PATH_FOLLOW = 0, ORBIT_FOLLOW, HOLD_WAYPOINT};

struct _WaypointManager_Data_Out{
    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)
    float radius;                       // Radius of turn if required
    int turnDirection;                  // Direction of turn -> -1 = CW (Right bank), 1 = CCW (Left bank). (Looking down from sky)
    _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 (THIS VALUE IS NOT UPDATED CURRENTLY)
    _WaypointOutputType out_type;       // Output type (determines which parameters are defined)
};

Input Data

The following structure contains the data that is required when determining the desired heading and altitude. It contains current gps coordinates (formatting is done by the sensor driver), altitude, and heading.

struct _WaypointManager_Data_In {
    long double latitude;
    long double longitude;
    int altitude;
    uint16_t heading;
};

Assumptions in this section: a flight path and home base have already been initialized.

Regular Path Following

For regular path following, these are the methods that are at play:

/**
* Updates the _WaypointManager_Data_Out structure with new values.
*
* @param[in] _Waypoint_Data_In 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
*/
_WaypointStatus get_next_directions(_WaypointManager_Data_In currentStatus, _WaypointManager_Data_Out *Data);

// Private methods
void follow_waypoints(_PathData * currentWaypoint, float* position, float heading);                               // Determines which of the methods below to call :))
void follow_line_segment(_PathData * currentWaypoint, float* position, float heading);                            // In the instance where the waypoint after the next is not defined, we continue on the path we are currently on
void follow_last_line_segment(_PathData * currentWaypoint, float* position, float heading);                       // In the instance where the next waypoint is not defined, follow previously defined path
void follow_straight_path(float* waypointDirection, float* targetWaypoint, float* position, float heading);       // Makes a plane follow a straight path (straight line following)
void follow_orbit(float* position, float heading);                                                                // Makes the plane follow an orbit with defined radius and direction

To start the process, the state machine calls the get_next_directions() method and passes in the input data and a pointer to the output structure (this will be updated once the desired values are calculated). In regular path following, the get_next_directions() method will do some stuff before calling the follow_waypoints() method. This method does some fancy stuff.

Remember how all the waypoints are linked? Well before doing anything, the follow_waypoints() method checks the currentWaypoint to see if its next parameter is defined. Additionally, it checks if the next parameter of the currentWaypoint’s next parameter is initialized as well! If any of these checks fail, special cases are triggered (discussed later).

In the case that all is good, the waypointManager does fancy math and then, based on whether the plane needs to orbit or move in a straight line, calls either follow_orbit() or follow_straight_path(). These methods calculate the desired heading, altitude, etc. that will be put into the _WaypointManager_Data_Out structure.

Now for the special cases in follow_waypoints():

1. Next parameter of currentWaypoint not defined: follow_waypoints() calls follow_last_line_segment() which makes the plane follow the path it was on previously. In extreme circumstances, it will make the plane enter a holding pattern (to exit this pattern, call the start_circling() method and pass in true as the cancelTurning parameter).

2. Next parameter of the currentWaypoint’s next parameter not defined: follow_waypoints() calls follow_line_segment() which makes the plane follow its current path.

Both extreme circumstances call follow_straight_path() to get desired heading and altitude.

Circle in Holding Pattern

For the plane to

There are three ways that the plane can enter a holding pattern:

1. It encountered a hold waypoint in its flight path

2. The waypointBuffer ran out of waypoints (to exit this pattern, call the start_circling() method and pass in true as the cancelTurning.

3. The state machine calls the start_circling() method and passes in false for the cancelTurning parameter.

/**
* Called if user wants the plane to start circling
*
* Even while circling, state machine should call get_next_direction().
* When user wants to exit this cycle, user can call this method again and pass in true for cancelTurning. This will set inHold to false.
*
* @param[in] _WaypointManager_Data_in currentStatus -> stores current gps info 
* @param[in] float radius -> radius of the turn
* @param[in] int direction -> -1 means clockwise (bank right); 1 means counter-clock wise (bank left)
* @param[in] int altitude -> altitude of hold pattern
* @param[in] bool cancelTurning -> false means we want plane to orbit. True means we want plane to stop orbiting and follow waypointBuffer array
*/
void start_circling(_WaypointManager_Data_In currentStatus, float radius, int direction, int altitude, bool cancelTurning);

Even when the plane is holding, the state machine should call the get_next_directions() method as it has a condition where it will go straight to calling follow_orbit() (hint: there is a inHold boolean parameter in the WaypoinManager class that is set to true when the plane is holding :)) ).

In all cases, to exit the holding pattern, call the start_circling() method and pass in true as the cancelTurning parameter.

Head Home

Modifying the Flight Path

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 Deleteselement theindex waypointin with the specifiedwaypointBuffer IDarray
void_PathData* updateinitialize_waypoint(_PathData* updatedWaypoint, int waypointId););                                 // Updates the waypoint with the specified ID         // Creates a blank waypoint
void clear_path_nodes_PathData* initialize_waypoint_and_next();                                        // Creates a blank waypoint with the next waypoint defined
void append_waypoint(_PathData* newWaypoint);          //   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.

...

                       // 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);

State Machine and Waypoints

...

1. State machine calls the get_next_directions() and passes in appropriate parameters (GPS data and pointer to the output data structure).

2. Algorithm stuff [ADD LATER]

3. Waypoint manager updates the parameters in the _WaypointManager_Data_Out structure that was passed into get_next_directions().

...