...
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.
Next step is to define if we’re following a straight path or orbiting path. Before the decision, we can cook some calculation for orbit following with given data. We compute Euclidean distance:
| Info |
|---|
|
...
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(float* nextWaypointCoordinatestargetCoordinates, float* prevWaypointCoordnates, float*waypointPosition, 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])/normstd::cout << "waypointDirection " << waypointDirection[2] <<std::endl; float nextWaypointDirection[3]; PM::Waypoint::calculate_direction_to_waypoint(waypointAfterTargetCoordinates, targetCoordinates, nextWaypointDirection); waypointDirection[1] = (nextWaypointCoordinates[1] - prevWaypointCoordnates[1])/norm; waypointDirection[2] = (nextWaypointCoordinates[2] - prevWaypointCoordnates[2])/norm; } |
Possible reason of failing test as of current:
...
Wrong calculation. Should we calculate dot product first, find CW or CWW, and apply it to half plane formula first?
...
Whether nextWaypointDirection[3] was dangling or not, the distance to next waypoint and altitude & desired track are still 0.
...
//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") |