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

...

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.

...

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 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!

/**
    * 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.

...

// 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
*
* @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 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

...

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.

...

...

1. 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 heading 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 -> -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() (hintHint: there is a inHold boolean parameter in the WaypoinManager WaypointManager class that is set to true when the plane is holding :)) ).In all cases, to exit the holding pattern, call the

Ensure that the radius input is greater than 0 and that the turn direction is either -1 or 1. 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. (NB: Work will be done to handle this case so the plane does not fly aimlessly)

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)In the case where the plane needs to head to the home base, the state machine should call the head_home() method.

// 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 arraybool head_home();

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

As for return values, when the goingHome parameter is set to true, the method returns true. If it is set to false, then it returns false. (NB: Work will be done to make this an enum so there is no ambiguity between errors and setting the parameter to false)

When this method is called, the waypointBuffer is cleared 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 it will set the goingHome parameter to false (assuming it was true before hand).

Modifying 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 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 (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 functions
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.

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. Function 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

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

  • the waypoints must exist and be in adjacent elements

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

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

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

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)

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.

...

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().

...