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:

...

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.

...

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

Tracking the Flight Path

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.

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

//**
Used* to specify the modification type when updating@param[in] int id -> id of the waypointBufferwaypoint arraythat enumwe _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)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] int_PathData* previousIdwaypoint -> storesIn the instance IDthat ofwe theneed waypointto thatupdate, willinsert, comeor beforeappend thea insertednew waypoint., Setthis towill 0be byused
default, so does not need to be passed (Set to 0 when NOT inserting* @param[in] _WaypointBufferUpdateType updateType -> the type of modification to the waypointBuffer array (look above)
* @param[in] intnumWaypoints nextId -> stores thenumber ID of thewaypoints waypoint that willare come afterin the insertedwaypoint waypoint.array Set(will tobe 01 byfor defaultinsertion, soupdating, doesand not need todeleting). May be passedgreater (Setthan to1 0for whenappending
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);                            * @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();       // If provided a waypoint id, this method finds the element index in the waypointBuffer array void append_waypoint(_PathData* newWaypoint);                                     // AddsEmpties awaypointBuffer waypointarray
to
the// firstPrivate freefunctions
element in the waypointBuffer (array)
void insert_new_waypoint(_PathData* newWaypoint, int previousId, int nextIdint get_waypoint_index_from_id(int waypointId);     // Inserts new waypoint in between the specified waypoints (identified using the waypoint IDs) void delete_waypoint(int waypointId);              // If provided a waypoint id, this method finds the element index in the waypointBuffer array
void append_waypoint(_PathData* newWaypoint);             // Deletes the waypoint with the specified ID void update_waypoint(_PathData* updatedWaypoint, int waypointId);            // Adds a waypoint to //the Updatesfirst thefree waypointelement within the specifiedwaypointBuffer ID(array)
void insert_new_waypoint(_PathData* newWaypoint, int previousId, int nextId);      

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

...

// 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 = INSERTAPPEND_WAYPOINTpreviousId

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

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

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

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

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)

...

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

State Machine and Waypoint Manager Interaction

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