Page Comparison

...

Now the path error (or cross-track error) is calculated. This is calculated as:

cos(courseAngle) * (positionY - targetWaypointY) – sin(courseAngle)*(positionX – targetWaypointX)

On a map, the _cross-track error _looks like this:

...

The cross track error is then useful to determine the heading of the aircraft. Once again, using the arctan function is suitable to do so:

...

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

...

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

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.

...

90 - rad2deg(courseAngle + direction \* (PI/2 + atan(k\_gain[ORBIT] \* (orbitDistance - radius)/radius)))

...

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

...

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

...

Path data is stored in a structure, which contains all necessary information for a single path (line) segment. The construct looks as follows:

typedef struct _PathData{

    struct _PathData* next;

    struct _PathData* previous;

    long double longitude;  //TODO: Longitude and Latitude is bulky. Use cartesian 2D approximations

    long double latitude;

    float altitude;

    float radius; //Radius of turn

    char id;    //Array ID

    char index;

} PathData;

...

The included functions are:

PathData* initializePathNode(void);

unsigned int destroyPathNode(PathData* node);

PathData* initializePathNodeAndNext(void);

unsigned int appendPathNode(PathData* node);

unsigned int removePathNode(unsigned int ID);

void clearPathNodes(void);

unsigned int insertPathNode(PathData* node, unsigned int previousID, unsigned int nextID);

These functions are all executed in the _checkAMData() _function. This function polls input over the DMA/SPI bus. When a new input is detected (via the WaypointWrapper structure), a corresponding function is executed.