Page Comparison

...

PM_CruisingStateManager follow_last_line_segment(line 389), follow_line_segment(line 355), next_waypoints(line 265) is where the math takes place. TODO figure out what they do and see why the math is not correct.

07-24

Added the below print statement to see what values were calculated (then realized that they are already printed😕 ) :

Code Block

language	cpp

std::cout << "Actual Track: " << out1.desiredTrack << "\n" << "Actual Altitude: " << out1.desiredAltitude << "\n" << "Actual distance to next waypoint: " << out1.distanceToNextWaypoint << "\n";
std::cout << "Desired Track: " << ans1.desiredTrack << "\n" << "Desired Altitude: " << ans1.desiredAltitude << "\n" << "Distance to next waypoint: " << ans1.distanceToNextWaypoint << "\n";

Results:

...

Since the DesiredTrack and DesiredAltitude are both 0, I think that the issue is just that they are never calculated but I can’t find the line that is not calculating. 😢 However I am going through the function calls and it seems that they are being called and performing calculations.

...

TODO: Do the math manually to see how the calculation is done (bruh i hate math)

Quick Overview About Waypoint Manager Math

Waypoint manager math breaks down into three big chunks: Straight path following, orbit following, and blending of both.

Straight path following

Given two points, get a line in-between.

If given XY direction, calculate the line’s direction using trigonometry.
If given GPS coordinates, convert them into XY coordinates using Haversine formula.

By subtracting the two XY coordinates, you’ll get direction vector. We have XY coordinates, and a direction vector. Applying atan (Recall SOH CAH TOA) gets you the angle of the track. 2 pi corrections can be done at this point.

Planes can get slightly off the track due to environmental factors or mechanical factors- that’s when cross-track error kicks in.

Cross-track error is the distance between the plane and the line that connects two waypoints.

Code Block
cross_track_error = cos(courseAngle) * (positionY - targetWaypointY) - sin(courseAngle) * (positionX - targetWaypointX)

Image Added

To resolve the error, we would have to apply a correction factor, desired track. If the plane is close to the line, redirecting its heading to perpendicular will cause the plane to directly cross the line, causing the same issue.

Thus, we will have to adjust the angle of its heading depending on how far the plane is from the line. The farther the plane is from the line, the angle of redirection gets closer to 90 degrees.

Code Block

language	applescript

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

Orbit Following

Follows the curvy path of certain radius, either in clockwise or counterclockwise direction.

To maintain radius, we need to calculate Euclidean radius:

Code Block
float orbitDistance = sqrt(pow(position[0] - center[0],2) + pow(position[1] - center[1],2));

where:

position[0] → x coordinate of plane’s current position
position[1] → y coordinate of plane’s current position
center[0] → x coordinate of orbit center
center[1] → y coordinate of orbit center

To put it simply, we’re just computing

Image Added

This.

This orbit distance is then used to compute the cross-track error but for curve. Atan is used once again for the similar reason as the straight path follow.

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

The arctan function ensures the track converges onto the orbit. The direction of travel lambda, either 1 or -1 (They represent counter or clockwise direction), counteracts track perturbations and is then added to the course angle as a perturbation. Note that a gain value must be tuned for the convergence rate.

The course angle can be determined by the vehicle's position on the orbit.

If the plane is in the first quadrant of a counterclockwise circle, the track ranges from 270° to 0° (On the right positive x-axis).

Image Added

The course angle is calculated using:

Code Block
float courseAngle = atan2(position[1] - center[1], position[0] - center[0]);

Blending Following

Blending mixes two methods together and use them when needed. Path will be straight, so we use straight path follow. However, to travel the corner, we’d need orbit path follow because, unlike quadcopters, planes can’t make a straight 90 degrees turn to travel a corner!

Image Added

To find the tangent (two lines tangent to the circle), we use trigonometry.

Image Added

And now we’ll find the turning angle using dot product of two vectors. The formula is:

Code Block

language	c

float turningAngle = acos(-deg2rad(waypointDirection[0] * nextWaypointDirection[0] + waypointDirection[1] * nextWaypointDirection[1] + waypointDirection[2] * nextWaypointDirection[2]));

where:

Index 0 is the x-coordinate
Index 1 is the y-coordinate
Index 2 is the z-coordinate

We consider ‘boundary’ as a checkpoint to switch the turn from straight path following to orbit path following, and vice-versa. To tell if a plane passed the boundary, we use dot product formula:

Image Added

Note: if the value is positive, that means they passed the boundary. The direction vector here are normalized. Path index incremented when they pass checkpoints.

TLDR: The dot product of two vectors are a⋅b=∣a∣∣b∣cos(θ). By checking the sign of the dot product before and after movement, you can determine if the vehicle has crossed the plane. D=(x−x0)⋅(current position – halfplane) will hold positive value.

Code Breakdown

To determine the desired track and altitude, we get GPS coordinates (latitude, longitude), altitude, then a track. Formatting is done by the sensor driver.

So when you are given an input like this:

Code Block

language	cpp

WaypointManager_Data_In input1 = {43.467998128, -80.537331184, 11, 100};

It simply means :

Code Block
WaypointManager_Data_In input = {latitude, longitude, altitude, track};

We are assuming that the flight path and home base is already initialized in this test.

Let’s look into more detail:

Code Block
auto status1 = cruisingState.pathFollow(input1, &out1);

When you go to the pathFollow function, it calls a function called ‘get_next_directions’.

This simply allows you to get the next direction using waypoint manager logic. We introduce a new array called position. We then call a function to get coordinates.

Code Block

    WaypointStatus CruisingStateManager::get_next_directions(WaypointManager_Data_In currentStatus, WaypointManager_Data_Out *Data)
    {
        // follow waypoints
        float position[3]; 
        // Gets current track
        float currentTrack = (float) currentStatus.track;
        PM::Waypoint::get_coordinates(currentStatus.longitude, currentStatus.latitude, position);
        position[2] = (float) currentStatus.altitude;

        follow_waypoints(waypointBuffer[currentIndex], position, currentTrack);

        // update return data
        update_return_data(Data);
        
        return WAYPOINT_SUCCESS;
    }

Get coordinates calls get distance. They calculate longitude and latitude relative to defined origin, and output it into position[0] and position[1]. In this case, xyCoordinates[0] and xyCoordinates[1].

Code Block

void get_coordinates(long double longitude, long double latitude, float* xyCoordinates) { // Parameters expected to be in degrees
        xyCoordinates[0] = get_distance(REFERENCE_LATITUDE, REFERENCE_LONGITUDE, REFERENCE_LATITUDE, longitude); //Calculates longitude (x coordinate) relative to defined origin (RELATIVE_LONGITUDE, RELATIVE_LATITUDE)
        xyCoordinates[1] = get_distance(REFERENCE_LATITUDE, REFERENCE_LONGITUDE, latitude, REFERENCE_LONGITUDE); //Calculates latitude (y coordinate) relative to defined origin (RELATIVE_LONGITUDE, RELATIVE_LATITUDE)
    }

As I mentioned, to convert XY coordinates, we need to calculate Haversine Formula. Why do we need Haversine formula? Because Earth is not flat and square, it is spherical. The distance between two waypoints lies along the spherical surface - This is called the great-circle distance.

Earlier I said that we receive inputs from Telemetry Manager and store it in Waypoint buffer. In our test, we have pre-defined custom data we feed to the system for the purpose of testing without using RTOS and actual board and setup. We modify the path using editFlightPath function. This will assign necessary information to the system.

Code Block

float latitudes[numPaths] = {43.47075830402289, 43.469649460242174, 43.46764349709017, 43.46430420301871, 43.461854997441996, 43.46144872072057};
    float longitudes[numPaths] = {-80.5479053969044, -80.55044911526599, -80.54172626568685, -80.54806720987989, -80.5406705046026, -80.53505945389745};
    float altitudes[numPaths] = {10, 20, 30, 33, 32, 50};

    WaypointManager_Data_In inputData;
    WaypointsCommand telemetryData;
    telemetryData.num_waypoints = numPaths;
    for (uint8_t i=0; i<numPaths; ++i) {
      telemetryData.waypoints[i] = {
        i, // waypoint_id
        i, // seq_num
        longitudes[i], // longitude
        latitudes[i], // latitude
        altitudes[i] // altitude     
      };    
    }

Now we proceed with the next line in get_next_direction function, follow_waypoint.

Code Block

void CruisingStateManager::follow_waypoints(WaypointData* currentWaypoint, float* position, float track)
    {
        if (currentWaypoint->next == nullptr) 
        { // If target waypoint is not defined
            follow_last_line_segment(currentWaypoint, position, track);
        }
        else if (currentWaypoint->next->next == nullptr) 
        { // If waypoint after target waypoint is not defined
            follow_line_segment(currentWaypoint, position, track);
        }
        else
        { // If there are two waypoints after target waypoint
            next_waypoints(currentWaypoint, position, track);
            
        }
    }

Because we have our test waypoints fed to the system, you’ll jump to next_waypoints.

Code Block

  void CruisingStateManager::next_waypoints(WaypointData* currentWaypoint, float* position, float track)
    {
        float waypointPosition[3]; 
        PM::Waypoint::get_coordinates(currentWaypoint->longitude, currentWaypoint->latitude, waypointPosition);
        waypointPosition[2] = currentWaypoint->altitude;

        // Defines target waypoint
        WaypointData * targetWaypoint = currentWaypoint->next;
        float targetCoordinates[3];
        PM::Waypoint::get_coordinates(targetWaypoint->longitude, targetWaypoint->latitude, targetCoordinates);
        targetCoordinates[2] = targetWaypoint->altitude;
       
        // Defines waypoint after target waypoint
        WaypointData* waypointAfterTarget = targetWaypoint->next;
        float waypointAfterTargetCoordinates[3];
        PM::Waypoint::get_coordinates(waypointAfterTarget->longitude, waypointAfterTarget->latitude, waypointAfterTargetCoordinates);
        waypointAfterTargetCoordinates[2] = waypointAfterTarget->altitude;

This part is pretty self explanatory, it defines current waypoint, target waypoint, and the waypoint after the target waypoint. The altitude of each waypoint, in case of input1, will be 10, 20, and 30.

We would then calculate direction to waypoint. It calculate the direction by computing the norm first, and dividing the difference between two waypoints by the norm.

Code Block

  float waypointDirection[3];
        PM::Waypoint::calculate_direction_to_waypoint(targetCoordinates, waypointPosition, waypointDirection);
        std::cout << "waypointDirection " << waypointDirection[2] <<std::endl;


        float nextWaypointDirection[3];
        PM::Waypoint::calculate_direction_to_waypoint(waypointAfterTargetCoordinates, targetCoordinates, nextWaypointDirection);
        //Before it was PM::Waypoint::calculate_direction_to_waypoint(targetCoordinates, waypointPosition, waypointDirection); Is this intended to be overwritten? I think not...

Code Block

 void calculate_direction_to_waypoint(float* nextWaypointCoordinates, float* prevWaypointCoordnates, float* waypointDirection)
    {
        float norm = sqrt(pow(nextWaypointCoordinates[0] - prevWaypointCoordnates[0],2) + pow(nextWaypointCoordinates[1] - prevWaypointCoordnates[1],2) + pow(nextWaypointCoordinates[2] - prevWaypointCoordnates[2],2));
        waypointDirection[0] = (nextWaypointCoordinates[0] - prevWaypointCoordnates[0])/norm;
        waypointDirection[1] = (nextWaypointCoordinates[1] - prevWaypointCoordnates[1])/norm;
        waypointDirection[2] = (nextWaypointCoordinates[2] - prevWaypointCoordnates[2])/norm;
    }

We now compute turning angle and tangent factor to compute half plane. Refer to orbit following for details.

Code Block

        // Required turning angle
        float turningAngle = acos(-DEG_TO_RAD(waypointDirection[0] * nextWaypointDirection[0] + waypointDirection[1] * nextWaypointDirection[1] + waypointDirection[2] * nextWaypointDirection[2]));
        // Calculates tangent factor that helps determine centre of turn 
        float tangentFactor = turnRadius/tan(turningAngle/2);

        float halfPlane[3];
        halfPlane[0] = targetCoordinates[0] - tangentFactor * waypointDirection[0];
        halfPlane[1] = targetCoordinates[1] - tangentFactor * waypointDirection[1];
        halfPlane[2] = targetCoordinates[2] - tangentFactor * waypointDirection[2];

We then compute distance to next waypoint. This function simply computes the distance between the current waypoint and next waypoint.

Code Block
distanceToNextWaypoint = PM::Waypoint::calculate_distance_to_waypoint(waypointPosition, position);

We would then receive orbit status from TM. This will decide whether we will be computing follow_straight_path or follow_orbit_path. For math, refer to the straight path and orbit path follow section. In our test case, we only follow the straight path.

The error with desired track and desired altitude was so simple yet so weird.

You just have to assign the output variables to the values.

...

Python Haversine formula simulator

import math

Code Block

#Coordinates in decimal degrees (e.g. 2.89078, 12.79797)

lon1 = -80.5373
lat1 = 43.468
lon2 = -80.5479
lat2 = 43.468

R = 6371000  # radius of Earth in meters
phi_1 = math.radians(lat1)
phi_2 = math.radians(lat2)

delta_phi = math.radians(lat2 - lat1)
delta_lambda = math.radians(lon2 - lon1)

a = math.sin(delta_phi / 2.0) ** 2 + math.cos(phi_1) * math.cos(phi_2) * math.sin(delta_lambda / 2.0) ** 2

c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))

meters = R * c  # output distance in meters
km = meters / 1000.0  # output distance in kilometers

meters = round(meters, 3)
km = round(km, 3)

print(f"Distance: {meters} m")
print(f"Distance: {km} km")

Versions Compared

Old Version 2

New Version Current

Key

Quick Overview About Waypoint Manager Math

Orbit Following

Blending Following

Code Breakdown