...
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 | ||
|---|---|---|
| ||
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.
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 |
|---|
|
| 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;
} |
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.
Different inputs; We are now calculating with three first inputs from the list. Maybe Liang calculated it with different inputs and that’s why the test output differ?