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:

...

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 _WaypointDataPathData * next;          // Next waypoint
    struct _WaypointDataPathData * previous;      // Previous waypoint
    long double latitude;             // Latitude of waypoint
    long double longitude;            // Longitude of waypoint
    int altitude;                     // Altitude of waypoint
    float turnRadius;                 // Ifif turnhold is commanded,  (type = 2), then this is the radius of the hold turncycle
    char waypointIdwaypointType;                  // Id0 of= theregular waypoint, 2 = hold waypoint (plane will circle)
    char arrayIndexwaypointId;                  // PositionId of the waypoint within the waypointBuffer (array)
} _WaypointDataPathData;

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 _WaypointBuffer 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
 _WaypointData*int initializeget_waypoint_index_from_id(int waypointId);                    // Creates a blank waypoint _WaypointData* initialize_waypoint_and_next();           // If Createsprovided a blank waypoint withid, thethis nextmethod waypointfinds definedthe void append_waypoint(_WaypointData* newWaypoint);     element index in the waypointBuffer array
_PathData* initialize_waypoint();    //   Adds a waypoint to the first free element in the waypointBuffer (array) void insert_new_waypoint(_WaypointData* newWaypoint, int previousId, int nextId); // Inserts new waypoint in between the specified waypoints (identified using the waypoint IDs) void delete_waypoint(int waypointId);          // Creates a blank waypoint
_PathData* initialize_waypoint_and_next();                                           // Creates Deletesa theblank waypoint with the next specifiedwaypoint IDdefined
void updateappend_waypoint(_WaypointDataPathData* updatedWaypoint, int waypointIdnewWaypoint);             // Updates the waypoint with the specified ID void clear_path_nodes();               // 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);                                             // EmptiesDeletes the waypoint 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.

enum _WaypointBufferUpdateType {ADD_WAYPOINT = 0, UPDATE_WAYPOINT, INSERT_WAYPOINT, DELETE_WAYPOINT};

/*
* Public function!
* @param _WaypointData* waypoint -> In the instance that we need to update, insert, or append a new waypoint, this will be used 
* @param int waypointId -> the ID of the waypoint that will be updated or deleted
* @param _WaypointBufferUpdateType updateType -> the type of modification to the waypointBuffer array (look above)
* @param 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
* @param 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
*/
void update_path_nodes(_WaypointData* waypoint, int waypointId, _WaypointBufferUpdateType updateType, int previousId = 0, int nextId = 0);

Inputs and Outputs

Inputs

• Current GPS Coordinates (longitude and latitude)

• Current Altitude

• Current Heading

Outputs

• Desired Heading

• Desired Altitude

• Distance to next waypoint

This is the structure that will be outputted:

enum _WaypointStatus {WAYPOINT_SUCCESS = 0, WAYPOINT_NOT_FOUND, WAYPOINT_PARAMETERS_NOT_DEFIND, UNDEFINED_FALIURE};
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.

enum _WaypointBufferUpdateType {ADD_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] 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 previousId = 0, int nextId = 0, int waypointId = 0);The ID System

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

Inputs and Outputs

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

• Desired Heading

• Desired Altitude

• Distance to next waypoint

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

...

Calculating Desired Heading and Altitude:

 /*
*
Public Function!! * @paramUpdates the _GpsWaypointManager_Data_Out structure currentPositionwith ->new containsvalues.
the current coordinates,* altitude,
and heading */ void get_next_directions(@param[in] _Gps_Data currentPosition); //Updates the _WaypointManager_Data structure with new values

/*
* Public Function!! -> 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
  */
 _WaypointManager void get_next_directions(_Gps_Data currentPosition, GetResult(_WaypointManager_Data_Out &*Data); 

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

2. Algorithm stuff [ADD LATER]

3. State machine calls GetResult() and passes in a reference address for a _WaypointManager_Data structure.

4. The function sets the members of the structure using the newly calculated values.

Modifying waypointBuffer Array:

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

2. update_path_nodes() calls the appropriate private function and that function updates the waypointBuffer array.