Overview

This module determines the desired track 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 track 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.

For details on implementation, refer below.

How does Waypoint Management Work?

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

Introduction

Waypoint 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

track it should follow. If your plane is on the desired path, the

track 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

track perpendicular to the line, in order to regain distance. This is the premise of the straight line path following algorithm. As a result, a

track vector field would look as such:

Image Modified

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

track 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

track. 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:

Image Modified

The cross track error is then useful to determine the desired

track of the aircraft. Once again, using the arctan function is suitable to do so:

desired_

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

Note, as the atan term increases, the

track 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

track. Therefore, to get the true

track, 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.

Image Modified

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

track to converge onto the orbit. The direction of travel (λ) which can be either 1 or -1, reverses the effect of the

track 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

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

Image Modified

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.

The coordinates at which this occurs can be calculated using:

Image Modified

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:

Image Modified

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

This section covers how the Waypoint Manager is implemented. It is long so I suggest looking at the bolded headings to find the information you are looking for.

Managing Path Data

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

_PathData {
    int waypointId;                   // Id 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
    _WaypointOutputType waypointType; // The type of output
};

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 contain 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, a problem arises as the state machine 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 :)

Initializing the Waypoint Manager

The waypoint manager was created using a class structure. As a result, a constructor must be called before anything is done! To simplify logistics with telemetry, a default set of reference coordinates are available and used when the default constructor, WaypointManager(), is called.

The reference coordinates are 43.467998128, -80.537331184 (lat, long), which are the gps coordinates of the University of Waterloo Parking Lot C in degree format.

WaypointManager();

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 to call one of these three overloaded methods. The first one returns a generic waypoint, 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

If the waypoint is a regular node within the plane’s flight path (the plane flies towards it then moves onto the next), waypointType should be PATH_FOLLOW. If at the waypoint the plane should circle until further instruction, set waypointType to HOLD_WAYPOINT.

The home base should be created as a HOLD_WAYPOINT.

Note that these methods create a _PathData object on the heap. In the instance that this allocation is not successful, the method will return NULL.

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 the method below to initialize the flight path and homeBase:

**
* Initializes 

home base

The currentLocation parameter is a pointer that will initialize the homeBase parameter of the WaypointManager class. It is set to a default value, so if you don’t want to modify it, don’t pass it in.

Note that the waypointBuffer array must be empty (no elements) when you call these methods. So, be sure to call the method void clear_path_nodes() if the waypointBuffer has waypoints in it. If this is not done, the method will return UNDEFINED_FAILURE (equal to 4). Additionally, if the numberOfWaypoints parameter exceeds the maximum amount of elements in the waypointBuffer array (stated by PATH_BUFFER_SIZE), then the method will only copy the first PATH_BUFFER_SIZE amount of elements from the initialWaypoints array. The method will also return TOO_MANY_WAYPOINTS (equal to 5) to let the state machine know that it did this.

Getting Desired Track and Altitude

One of the two tasks performed by this module is determining the track 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 desiredTrack;              // Desired track 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
    int desiredAirspeed;
    uint32_t timeOfData;                // The time that the data in this structure was collected
    _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 track and altitude. It contains current gps coordinates (formatting is done by the sensor driver), altitude, and track.

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

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 track
* @param[out] _WaypointManager_Data_Out &Data -> Memory address for a structure that holds the data for the state machine
*
* @return status variable stating if any errors occured (0 means success)
*/
_WaypointStatus get_next_directions(_WaypointManager_Data_In currentStatus, _WaypointManager_Data_Out *Data);

// Private methods
void follow_waypoints(_PathData * currentWaypoint, float* position, float track);                               // Determines which of the methods below 

 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 call 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 track, 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 track and altitude.

Circle in Holding Pattern

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

3. The state machine calls the start_circling() method and passes in false for the cancelTurning parameter. In this case, the user must define the desired altitude, turning direction, and radius. The method then calculates the coordinates of the centre of the turn. The vector connecting this point to the plane has a magnitude equal to the turn radius and a direction 90º to the track of the plane.

/**
* 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 -> 0 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
*/
_WaypointStatus 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 WaypointManager class that is set to true when the plane is holding :)) ).

Ensure that the radius input is greater than 0 and that the turn direction is either 0 (CW) or 1 (CCW). If the parameters are incorrect, then the get_next_directions() method will return INVALID_PARAMETERS (equal to 4) and will result in the output data not being updated.

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

Head Home

In the case where the plane needs to head to the home base, the state machine should call the head_home() method.

// Used to specify the status of the head_home() method
enum _HeadHomeStatus {HOME_TRUE = 0, HOME_FALSE, HOME_UNDEFINED_PARAMETER};

/**
* Called if user wants the plane to just head home
*
*  @param[in] bool starttrackHome -> true if you want plane to head home, false if you want plane to start following the flight path in waypointBuffer.
*/
_HeadHomeStatus head_home(bool starttrackHome);

Before calling this method, ensure that the homeBase parameter is defined. If it is not defined, then the head_home() method will return HOME_UNDEFINED_PARAMETER (equal to 2).

Assuming all goes well: as for return values, when the starttrackHome parameter is set to true, the method returns HOME_TRUE (equal to 0). If the parameter is set to false, then the method returns HOME_FALSE (equal to 1).

When this method is called and starttrackHome is true, the waypointBuffer is cleared by calling clear_path_nodes() and the goingHome parameter is set to true. Afterwards, the state machine should call get_next_directions() as normal.

If you would like the plane to start following the waypoints in the waypointBuffer array, just call the head_home() method again and set starttrackHome false. The method will set the goingHome parameter to false.

Tracking the Current Waypoint

I will not lie, this part is perhaps one of the weaker links in the chain that is this module.

The WaypointManager class has a parameter called currentIndex which is the index of the waypoint that the plane is currently at. Unlike what you may assume, this is not the waypoint the plane is track towards, but the waypoint it just passed. Another way to think about it is this: the next parameter of the current waypoint is the waypoint that the plane is targeting.

Now, when the plane passes a waypoint, it must increment the currentIndex parameter. For an explanation of the reasoning behind this, refer to the code in waypointManager.cpp (inside the follow_waypoints() method).

Changing the currentIndex parameter manually

The state machine can manually change the currentIndex parameter of the waypoint manager. It needs to call the following method:

/**
*  @param[in] int id -> id of the waypoint that we want to set as the current waypoint (the plane will head towards the waypoint that is stored in its "next" parameter)
* 
*  @return -> returns error code in case the transfer was not successful
*/ 

_WaypointStatus change_current_index(int id);

I would recommend calling this method any time that you are exiting a holding pattern or if you are doing a massive change to the waypointBuffer array.

Modifying the Flight Path

This section discusses the waypoint manager’s second responsibility: managing the flight path.

There are five operations that can be done to modify the flight path:

1. Append a waypoint

2. Insert a waypoint

3. Update a waypoint

4. Delete a waypoint

5. Empty the waypointBuffer array and reinitialize it

With the exception of operation 5, the state machine can do these operations by calling the update_path_nodes() method.

// Used to specify the modification type when updating the waypointBuffer array
enum _WaypointBufferUpdateType {APPEND_WAYPOINT = 0, UPDATE_WAYPOINT, INSERT_WAYPOINT, DELETE_WAYPOINT};

/**
* 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 (Set to 0 when appending)
* @param[in] int 

of the waypoint that will come before the inserted waypoint. Set to 0 by default, so does not need to be passed (Set to 0 when NOT inserting)
* @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 (Set to 0 when NOT inserting)
* 
* @return status variable stating if any errors occured (0 means success)
*/
_WaypointStatus update_path_nodes(_PathData *waypoint, _WaypointBufferUpdateType updateType, int waypointId, int previousId, int nextId);

void clear_path_nodes();                                                          // Empties waypointBuffer array

// Private methods
int get_waypoint_index_from_id(int waypointId);                                   // If provided a waypoint id, this method finds the element index in the waypointBuffer array
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             

Please pay attention to the comments below as all operations do not require the same parameters. Thus some can be set to default values depending on the operation.

Note that you only need to pass in the waypoint Ids because there is a method called get_waypoint_index_from_id() that determines the waypoint’s index in the waypointBuffer array from its id.

Also note that if you pass in a waypoint and the modification fails, the waypoint you pass will be removed from the heap so as to prevent any memory leaks from occurring.

Appending a Waypoint

This operation adds ONE waypoint to the end of the flight path (the first empty element in the array)

Parameters we care about:

• waypoint

• updateType = APPEND_WAYPOINT

Conditions:

• Array cannot be full when appending. Method will return INVALID_PARAMETERS (equal to 4).

Inserting a Waypoint

This operation inserts a waypoint between two waypoints in the flight path.

Parameters we care about:

• waypoint

• updateType = INSERT_WAYPOINT

• previousId

• nextId

Conditions:

• Array cannot be full. Method will return INVALID_PARAMETERS (equal to 4).

• Must insert waypoint between two already existing waypoints in the waypointBuffer array.

  • The waypoints must exist and be in adjacent elements

  • If condition is not met, method will return INVALID_PARAMETERS (equal to 4).

• You cannot insert a waypoint before the currentIndex

Updating a Waypoint

This operation updates the _Pathdata parameters of an existing waypoint in the waypointBuffer array

Parameters we care about:

• waypoint

• updateType = UPDATE_WAYPOINT

• waypointId

Conditions:

• Waypoint must exist in the waypointBuffer array. If condition is not met, method will return INVALID_PARAMETERS (equal to 4).

Deleting a Waypoint

This operation deletes a waypoint and removes the memory from the heap.

Parameters we care about

• updateType = DELETE_WAYPOINT

• waypointId

Conditions:

• Waypoint must exist in the waypointBuffer array. If condition is not met, method will return INVALID_PARAMETERS (equal to 4).

Re-initializing the waypointBuffer (Or, more scientifically known as the Yeet and go operation)

Removes all waypoints from the array and the heap. To do this, state machine should call clear_path_nodes() (public method).

Afterwards, the state machine should create a new flight path and initialize it using an initialize_flight_path() method (Refer above).

State Machine and Waypoint Manager Interaction

This section gives a “summary” of how the state machine will interact with the waypoint manager.

Public Functions

All public functions and their basic tasks are included below:

1. WaypointManager() → constructor

2. _WaypointStatus initialize_flight_path() → initialize flight path

3. _PathData * initialize_waypoint() → initialize waypoint (overloaded)

4. _WaypointStatus get_next_directions() → updates _WaypointManager_Data_Out structure with desired track, altitude, etc.

5. _WaypointStatus start_circling() → puts plane in hold pattern

   Can be called to exit hold pattern

6. _HeadHomeStatus head_home() → Empties waypointBuffer array and tells waypoint manager to take plane to homeBase. Can also tell plane to start following waypoints in waypointBuffer array.

7. _WaypointStatus change_current_index() → changes index of current waypoint

8. _WaypointStatus update_path_nodes() → called if you want to update the flight path

9. void clear_path_nodes() → empties waypointBuffer array (DANGER)

10. _PathData ** get_waypoint_buffer() → returns the waypointBuffer array

11. _WaypointBufferStatus get_status_of_index() → returns the status of an index in the waypointBuffer array (FREE [equals 0] means empty, FULL [equals 1] means not empty)

12. int get_current_index() → gets currentIndex parameter of WaypointManager class

13. int get_id_of_current_index() → returns the id of the waypoint that is in waypointBuffer[currentIndex]

14. _PathData * get_home_base() → returns the homeBase parameter of the WaypointManager class

15. ~WaypointManager() → Destructor for class. This destructor handles removing all heap-allocated waypoints. To be honest, I only made this for the unit tests so you should never call it.

Initializing WaypointManager Object

Method(s) you care about: 1

What to pass in: nothing

Initializing Waypoints

Method(s) you care about: 3

What to pass in: refer above

Initializing Flight Path

Method(s) you care about: 2

What to pass in: refer above

Getting Next Directions

Method(s) you care about: 4

What to pass in: refer above

Modifying Flight Path

Method(s) you care about: 8, 9

What to pass in: refer above

Changing Current Waypoint

Method(s) you care about: 7

What to pass in: refer above

Status Methods

Method(s) you care about: 10, 11, 12, 13, 14

What to pass in: (This is method-dependent)

• _WaypointBufferStatus get_status_of_index() → pass in index of wayponitBuffer array you care about

Error Codes and What they Mean

Here are all of the enums for this module:

// Error codes from the waypoint manager
enum _WaypointStatus {WAYPOINT_SUCCESS = 0, UNDEFINED_FAILURE, CURRENT_INDEX_INVALID, UNDEFINED_PARAMETER, INVALID_PARAMETERS, TOO_MANY_WAYPOINTS};

// Used in the waypointBufferStatus array to signal which elements of the waypointBuffer are free
enum _WaypointBufferStatus {FREE = 0, FULL};

// Used to specify the modification type when updating the waypointBuffer array (used when calling the update_path_nodes() method)
enum _WaypointBufferUpdateType {APPEND_WAYPOINT = 0, UPDATE_WAYPOINT, INSERT_WAYPOINT, DELETE_WAYPOINT};

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

// Used to specify the status of the head_home() method
enum _HeadHomeStatus {HOME_TRUE = 0, HOME_FALSE, HOME_UNDEFINED_PARAMETER};

_WaypointStatus

_WaypointOutputType

_HeadHomeStatus

Useful resources

• http://www.movable-type.co.uk/scripts/latlong.html#destPoint

  • This link came in clutch to understand the calculations that go behind some of the stuff.

• PICpilot waypoint manager

  • https://github.com/UWARG/PICpilot/blob/staging/Autopilot/Path%20Manager/PathManager.h

  • https://github.com/UWARG/PICpilot/blob/staging/Autopilot/Path%20Manager/PathManager.c