Debugging malfunctioning cruising state mode

07-17

Line 40 test fail issue with the calculation of waypoints.

Note that the PM cruising algorithm can only be applied to fixed wings. The takeoff and landing algorithm can only be applied to quad.

On CrusingStateManagerTest.cpp line 113 both tests did not pass. Both the results are different from expected. Note that this test checks the desired track straight. So the path that is calculated should be for straight lines.

PM_CruisingStateManager.cpp line 131 the waypoint_type variable was commented out and on line 137 the waypoint_type is hard coded to be PATH_FOLLOW (data->waypoint_type = PATH_FOLLOW; ).

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

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.

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

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.

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:

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

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.

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

The course angle is calculated using:

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!

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

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

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:

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.