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:
...
Implementation
Managing Path Data
All of the waypoints along the flightpath will be stored in the following structure:
| Code Block | ||
|---|---|---|
| ||
typedef struct {
struct _WaypointData * next; // Next waypoint
struct _WaypointData * previous; // Previous waypoint
long double latitude; // Latitude of waypoint
long double longitude; // Longitude of waypoint
int altitude; // Altitude of waypoint
float turnRadius; // If turn is commanded, this is the radius of the turn
char waypointId; // Id of the waypoint
char arrayIndex; // Position of the waypoint within the waypointBuffer (array)
} _WaypointData; |
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 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)
| Code Block | ||
|---|---|---|
| ||
// Private functions
_WaypointData* initialize_waypoint(); // Creates a blank waypoint
_WaypointData* initialize_waypoint_and_next(); // Creates a blank waypoint with the next waypoint defined
void append_waypoint(_WaypointData* newWaypoint); // 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); // Deletes the waypoint with the specified ID
void update_waypoint(_WaypointData* updatedWaypoint, int waypointId); // Updates the waypoint with the specified ID
void clear_path_nodes(); // Empties 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.
| Code Block |
|---|
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
...