/* This function calculates any maneuvers that are necessary for the
current plane to avoid looping. Returns a waypoint based on calculations.
If no maneuvers are necessary, then the function returns the current
destination. */
sim::waypoint sim::takeDubinsPath(PlaneObject &plane1) {
/* Initialize variables */
sim::coordinate circleCenter;
sim::waypoint wp = plane1.getDestination();
double minTurningRadius = 0.75*MINIMUM_TURNING_RADIUS;
bool destOnRight;
/* Calculate cartesian angle from plane to waypoint */
double wpBearing = findAngle(plane1.getCurrentLoc().latitude,
plane1.getCurrentLoc().longitude, wp.latitude, wp.longitude);
/* Calculate cartesian current bearing of plane (currentBearing is stored as Cardinal) */
double currentBearingCardinal = plane1.getCurrentBearing();
double currentBearingCartesian = toCartesian(currentBearingCardinal);
if (fabs(currentBearingCardinal) < 90.0)
/* Figure out which side of the plane the waypoint is on */
destOnRight = ((wpBearing < currentBearingCartesian) &&
(wpBearing > manipulateAngle(currentBearingCartesian - 180.0)));
else
destOnRight = !((wpBearing > currentBearingCartesian) &&
(wpBearing < manipulateAngle(currentBearingCartesian - 180.0)));
/* Calculate the center of the circle of minimum turning radius on the side that the waypoint is on */
circleCenter = calculateLoopingCircleCenter(plane1, minTurningRadius, destOnRight);
/* If destination is inside circle, must fly opposite direction before we can reach destination */
if (findDistance(circleCenter.latitude, circleCenter.longitude, wp.latitude, wp.longitude) <
minTurningRadius)
return calculateWaypoint(plane1, minTurningRadius, !destOnRight);
else
return wp;
}
/*
Given a waypoint (latitude, longitude, and altitude) as well as the bearing and angular distance to travel,
calculateCoordinate will return the new location in the form of a waypoint.
*/
AU_UAV_ROS::waypoint calculateCoordinate(AU_UAV_ROS::waypoint currentPosition, double bearing, double distance){
// Calculate final latitude and longitude; see movable-type.co.uk/scripts/latlong.html for more detail
bearing *= DEGREES_TO_RADIANS; // convert angle of force to radians
double lat1 = currentPosition.latitude*DEGREES_TO_RADIANS; // lat1 = current latitude in radians
double dLat = distance*cos(bearing); // calculate change in latitude
double lat2 = lat1 + dLat; // calculate final latitude
double dPhi = log(tan(lat2/2+PI/4)/tan(lat1/2+PI/4));
double q = (!(dPhi < 0.0001)) ? dLat/dPhi : cos(lat1); // East-West line gives dPhi=0
double dLon = distance*sin(bearing)/q; // calculate change in longitude
// check for some daft bugger going past the pole, normalise latitude if so
if (abs(lat2) > PI/2)
lat2 = lat2>0 ? PI-lat2 : -(PI-lat2);
double lon2 = (currentPosition.longitude*DEGREES_TO_RADIANS+dLon) * RADIANS_TO_DEGREES; // calculate final latitude and convert to degrees
//wrap around if necessary to ensure final longitude is on the interval [-180, 180]
lon2 = manipulateAngle(lon2);
lat2 *= RADIANS_TO_DEGREES; // convert final latitude to degrees
AU_UAV_ROS::waypoint coordinate;
coordinate.latitude = lat2;
coordinate.longitude = lon2;
coordinate.altitude = currentPosition.altitude;
return coordinate;
}
//This will take two waypoints and measure the heading between them (based on position)
//This is an estimation of plane heading based on the position heading from a point a time t and time t-1
//waypoints must be in meters
//A zero degree heading points directly North (and East is 90 degrees and West is -90 degrees to keep in [-180,180] range)
double getNewHeading(AU_UAV_ROS::position first, AU_UAV_ROS::position second)
{
double deltaX = second.x_coordinate - first.x_coordinate;
double deltaY = second.y_coordinate - first.y_coordinate;
double heading = atan2(deltaX,deltaY);
heading = (heading*RADIANS_TO_DEGREES);
heading = manipulateAngle(heading);
return heading;
}
/* Convert angle in the Cartesian plane to a Cardinal direction */
double toCardinal(double angle){
angle = manipulateAngle(angle); /* get angle on the interval [-180, 180] */
if (angle <= 90 && angle >= -90) /* angle is in the first or fourth quadrant */
return 90 - angle;
else if (angle >= 90 && angle <= 180) /* angle is in the second quadrant */
return -1*angle + 90;
else if (angle <= -90 && angle >= -180) /* angle is in third quadrant */
return -180 + -1*(90 + angle);
else
return -999; /* should never happen in current setup */
}
/* Convert Cardinal direction to an angle in the Cartesian plane */
double toCartesian(double UAVBearing){
UAVBearing = manipulateAngle(UAVBearing); /* get angle on the interval [-180, 180] */
if (UAVBearing < 180 && UAVBearing >= 0) /* UAV bearing is in the first or fourth quadrant */
return 90 - UAVBearing;
else if (UAVBearing < 0 && UAVBearing >= -90) /* UAV bearing is in the second quadrant */
return -1*UAVBearing + 90;
else if (UAVBearing < -90 && UAVBearing > -180) /* UAV bearing is in the third quadrant */
return -1*(UAVBearing + 180) - 90;
else if (UAVBearing == 180 || UAVBearing == -180)
return -90;
else
return -999; /* should never happen in current setup */
}
