Page Comparison

...

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

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.

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:

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:  

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.