/* Find the new collision avoidance waypoint for the plane to go to */
AU_UAV_ROS::waypoint AU_UAV_ROS::calculateWaypoint(PlaneObject &plane1, double turningRadius, bool turnRight){
AU_UAV_ROS::waypoint wp;
double V = MPS_SPEED;
double delta_T = TIME_STEP;
double cartBearing = toCartesian(plane1.getCurrentBearing())* PI/180;
double delta_psi = V / turningRadius * delta_T;
if (turnRight) delta_psi *= -1.0;
ROS_WARN("Delta psi: %f", delta_psi);
double psi = (cartBearing + delta_psi);
wp.longitude = plane1.getCurrentLoc().longitude + V*cos(psi)/DELTA_LON_TO_METERS;
wp.latitude = plane1.getCurrentLoc().latitude + V*sin(psi)/DELTA_LAT_TO_METERS;
wp.altitude = plane1.getCurrentLoc().altitude;
ROS_WARN("Plane%d new cbearing: %f", plane1.getID(), toCardinal( findAngle(plane1.getCurrentLoc().latitude, plane1.getCurrentLoc().longitude, wp.latitude, wp.longitude) ) );
//ROS_WARN("Plane%d calc lat: %f lon: %f w/ act lat: %f lon: %f", plane1.getID(), wp.latitude, wp.longitude, plane1.getCurrentLoc().latitude, plane1.getCurrentLoc().longitude);
return wp;
}
/* Find the new collision avoidance waypoint for the plane to go to */
au_uav_ros::waypoint au_uav_ros::calculateWaypoint(PlaneObject &plane1, double turningRadius, bool turnRight){
au_uav_ros::waypoint wp;
double V = MPS_SPEED * MPS_WAYPOINT_MULTIPLIER;
double delta_T = TIME_STEP;
double cartBearing = plane1.getCurrentBearing()* PI/180;
double delta_psi = V / turningRadius * delta_T;
if (turnRight) delta_psi *= -1.0;
ROS_WARN("Delta psi: %f", delta_psi);
double psi = (cartBearing + delta_psi);
V = V * MPS_WAYPOINT_MULTIPLIER;
wp.longitude = plane1.getCurrentLoc().longitude + V*cos(psi)/DELTA_LON_TO_METERS;
wp.latitude = plane1.getCurrentLoc().latitude + V*sin(psi)/DELTA_LAT_TO_METERS;
ROS_INFO("long+%f, lat+%f, distanceBetween UAV and AvoidWP%f", V*cos(psi)/DELTA_LON_TO_METERS, V*sin(psi)/DELTA_LON_TO_METERS,
distanceBetween(plane1.getCurrentLoc(), wp));
wp.altitude = plane1.getCurrentLoc().altitude;
ROS_WARN("Plane%d new cbearing: %f", plane1.getID(), toCardinal( findAngle(plane1.getCurrentLoc().latitude, plane1.getCurrentLoc().longitude, wp.latitude, wp.longitude) ) );
//ROS_WARN("Plane%d calc lat: %f lon: %f w/ act lat: %f lon: %f", plane1.getID(), wp.latitude, wp.longitude, plane1.getCurrentLoc().latitude, plane1.getCurrentLoc().longitude);
return wp;
}
/* This function is 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*/
au_uav_ros::waypoint au_uav_ros::takeDubinsPath(PlaneObject &plane1) {
/* Initialize variables*/
au_uav_ros::coordinate circleCenter;
au_uav_ros::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 = toCardinal(plane1.getCurrentBearing());
double currentBearingCartesian = plane1.getCurrentBearing();
if (fabs(currentBearingCardinal) < 90.0)
/* Figure out which side of the plane the waypoint is on*/
if ((wpBearing < currentBearingCartesian) &&
(wpBearing > manipulateAngle(currentBearingCartesian - 180.0)))
destOnRight = true;
else destOnRight = false;
else
if ((wpBearing > currentBearingCartesian) &&
(wpBearing < manipulateAngle(currentBearingCartesian - 180.0)))
destOnRight = false;
else destOnRight = true;
/* 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 {
//we can simply pass the current waypoint because it's accessible
//ROS_WARN("FINE: %f", findDistance(circleCenter.latitude, circleCenter.longitude, wp.latitude, wp.longitude));
return wp;
}
}
void AU_UAV_ROS::PlaneObject::update(const AU_UAV_ROS::TelemetryUpdate &msg) {
//Update previous and current position
this->setPreviousLoc(this->currentLoc.latitude, this->currentLoc.longitude, this->currentLoc.altitude);
this->setCurrentLoc(msg.currentLatitude, msg.currentLongitude, msg.currentAltitude);
//Calculate actual Cardinal Bearing
double numerator = (this->currentLoc.latitude - this->previousLoc.latitude);
double denominator = (this->currentLoc.longitude - this->previousLoc.longitude);
double angle = atan2(numerator*DELTA_LAT_TO_METERS,denominator*DELTA_LON_TO_METERS)*180/PI;
if (this->currentLoc.latitude != this->previousLoc.latitude && this->currentLoc.longitude != this->previousLoc.longitude)
{
this->setCurrentBearing(toCardinal(angle));
}
else this->setCurrentBearing(0.0);
// Update everything else
this->setTargetBearing(msg.targetBearing);
this->setSpeed(msg.groundSpeed);
this->updateTime();
}