diff --git a/src/AIModel/AIAircraft.cxx b/src/AIModel/AIAircraft.cxx index 93f6f96be..c46cd0320 100644 --- a/src/AIModel/AIAircraft.cxx +++ b/src/AIModel/AIAircraft.cxx @@ -86,6 +86,8 @@ FGAIAircraft::FGAIAircraft(FGAISchedule *ref) : alt_lock = false; roll = 0; headingChangeRate = 0.0; + + holdPos = false; } @@ -226,7 +228,7 @@ void FGAIAircraft::Run(double dt) { pos.getLongitudeDeg(), hdg, speed, - altitude_ft); + altitude_ft, dt); //if (controller->hasInstruction(getID())) // { processATC(controller->getInstruction(getID())); @@ -979,20 +981,20 @@ void FGAIAircraft::announcePositionToController() //snprintf (buffer, 10, "%d", node); switch (leg) { case 3: - cerr << trafficRef->getRegistration() - << " taxiing to runway at segment " - << fp->getCurrentWaypoint()->routeIndex - << endl; + //cerr << trafficRef->getRegistration() + // << " taxiing to runway at segment " + // << fp->getCurrentWaypoint()->routeIndex + // << endl; //cerr << "Match check between taxisegment and taxiroute : " << node << " " // << fp->getCurrentWaypoint()->name << endl; if (trafficRef->getDepartureAirport()->getDynamics()->getGroundNetwork()->exists()) controller = trafficRef->getDepartureAirport()->getDynamics()->getGroundNetwork(); break; case 9: - cerr << trafficRef->getRegistration() - << " taxiing to parking at segment " - << fp->getCurrentWaypoint()->routeIndex - << endl; + //cerr << trafficRef->getRegistration() + // << " taxiing to parking at segment " + // << fp->getCurrentWaypoint()->routeIndex + // << endl; if (trafficRef->getArrivalAirport()->getDynamics()->getGroundNetwork()->exists()) controller = trafficRef->getArrivalAirport()->getDynamics()->getGroundNetwork(); break; @@ -1006,8 +1008,8 @@ void FGAIAircraft::announcePositionToController() } if ((controller != prevController) && (prevController != 0)) { prevController->signOff(getID()); - cerr << trafficRef->getRegistration() - << " signing off " << endl; + //cerr << trafficRef->getRegistration() + // << " signing off " << endl; } prevController = controller; if (controller) { @@ -1024,12 +1026,28 @@ void FGAIAircraft::processATC(FGATCInstruction instruction) //cerr << "Processing ATC instruction (not Implimented yet)" << endl; if (instruction.getHoldPattern ()) { } + + // Hold Position if (instruction.getHoldPosition ()) { - } - if (instruction.getChangeSpeed ()) { - AccelTo(instruction.getSpeed()); - }else { - if (fp) AccelTo(fp->getPreviousWaypoint()->speed); + if (!holdPos) { + if (trafficRef) + cerr << trafficRef->getCallSign() << "Holding Position " << endl; + holdPos = true; + } + AccelTo(0.25); + } else { + if (holdPos) { + if (trafficRef) + cerr << trafficRef->getCallSign() << " Resuming Taxi " << endl; + holdPos = false; + } + // Change speed Instruction. This can only be excecuted when there is no + // Hold position instruction. + if (instruction.getChangeSpeed ()) { + AccelTo(instruction.getSpeed()); + }else { + if (fp) AccelTo(fp->getPreviousWaypoint()->speed); + } } if (instruction.getChangeHeading ()) { hdg_lock = false; diff --git a/src/AIModel/AIAircraft.hxx b/src/AIModel/AIAircraft.hxx index 095b21b37..a848346cf 100644 --- a/src/AIModel/AIAircraft.hxx +++ b/src/AIModel/AIAircraft.hxx @@ -124,6 +124,8 @@ private: double prevSpeed; double prev_dist_to_go; + bool holdPos; + bool _getGearDown() const; bool reachedWaypoint; string callsign; // The callsign of this tanker. diff --git a/src/Airports/groundnetwork.cxx b/src/Airports/groundnetwork.cxx index a6d997eda..7936a3e19 100644 --- a/src/Airports/groundnetwork.cxx +++ b/src/Airports/groundnetwork.cxx @@ -61,6 +61,7 @@ FGTaxiNode::FGTaxiNode() **************************************************************************/ FGTaxiSegment::FGTaxiSegment() { + oppositeDirection = 0; } void FGTaxiSegment::setStart(FGTaxiNodeVector *nodes) @@ -262,6 +263,102 @@ void FGTrafficRecord::setPositionAndHeading(double lat, double lon, double hdg, altitude = alt; } +int FGTrafficRecord::crosses(FGGroundNetwork *net, FGTrafficRecord &other) +{ + if (checkPositionAndIntentions(other) || (other.checkPositionAndIntentions(*this))) + return -1; + intVecIterator i, j; + int currentTargetNode = 0, otherTargetNode = 0; + if (currentPos > 0) + currentTargetNode = net->findSegment(currentPos )->getEnd()->getIndex(); // OKAY,... + if (other.currentPos > 0) + otherTargetNode = net->findSegment(other.currentPos)->getEnd()->getIndex(); // OKAY,... + if ((currentTargetNode == otherTargetNode) && currentTargetNode > 0) + return currentTargetNode; + if (intentions.size()) + { + for (i = intentions.begin(); i != intentions.end(); i++) + { + if (currentTargetNode == net->findSegment(*i)->getEnd()->getIndex()) + { + cerr << "Current crosses at " << currentTargetNode <findSegment(*i)->getEnd()->getIndex()) + { + cerr << "Other crosses at " << currentTargetNode <findSegment(*i)->getEnd()->getIndex(); + otherTargetNode = net->findSegment(*j)->getEnd()->getIndex(); + if (currentTargetNode == otherTargetNode) + { + //cerr << "Routes will cross at " << currentTargetNode << endl; + return currentTargetNode; + } + } + } + } + return -1; +} + +bool FGTrafficRecord::isOpposing (FGGroundNetwork *net, FGTrafficRecord &other, int node) +{ + // Check if current segment is the reverse segment for the other aircraft + FGTaxiSegment *opp; + //cerr << "Current segment " << currentPos << endl; + if ((currentPos > 0) && (other.currentPos > 0)) + { + opp = net->findSegment(currentPos)->opposite(); + if (opp) { + if (opp->getIndex() == other.currentPos) + return true; + } + + for (intVecIterator i = intentions.begin(); i != intentions.end(); i++) + { + for (intVecIterator j = intentions.begin(); j != intentions.end(); j++) + { + // cerr << "Current segment 1 " << (*i) << endl; + if (opp = net->findSegment(*i)->opposite()) + { + if (opp->getIndex() == + net->findSegment(*j)->getIndex()) + { + cerr << "Nodes " << net->findSegment(*i)->getIndex() + << " and " << net->findSegment(*j)->getIndex() + << " are opposites " << endl; + if (net->findSegment(*i)->getStart()->getIndex() == node) { + { + cerr << "Found the node" << endl; + return true; + } + } + } + } + } + } + } + return false; +} + + /*************************************************************************** * FGGroundNetwork() **************************************************************************/ @@ -273,6 +370,7 @@ FGGroundNetwork::FGGroundNetwork() totalDistance = 0; maxDistance = 0; currTraffic = activeTraffic.begin(); + } void FGGroundNetwork::addSegment(const FGTaxiSegment &seg) @@ -306,16 +404,39 @@ void FGGroundNetwork::init() { hasNetwork = true; int index = 1; + sort(nodes.begin(), nodes.end()); + sort(segments.begin(), segments.end()); FGTaxiSegmentVectorIterator i = segments.begin(); while(i != segments.end()) { //cerr << "initializing node " << i->getIndex() << endl; i->setStart(&nodes); i->setEnd (&nodes); i->setTrackDistance(); - i->setIndex(index++); + i->setIndex(index); //cerr << "Track distance = " << i->getLength() << endl; //cerr << "Track ends at" << i->getEnd()->getIndex() << endl; i++; + index++; + } + i = segments.begin(); + while(i != segments.end()) { + FGTaxiSegmentPointerVectorIterator j = i->getEnd()->getBeginRoute(); + while (j != i->getEnd()->getEndRoute()) + { + if ((*j)->getEnd()->getIndex() == i->getStart()->getIndex()) + { + int start1 = i->getStart()->getIndex(); + int end1 = i->getEnd() ->getIndex(); + int start2 = (*j)->getStart()->getIndex(); + int end2 = (*j)->getEnd()->getIndex(); + int oppIndex = (*j)->getIndex(); + cerr << "Oppossite of " << i->getIndex() << " (" << start1 << "," << end1 << ") " + << "happens to be " << oppIndex << " (" << start2 << "," << end2 << ") " << endl; + break; + } + j++; + } + i++; } //exit(1); } @@ -349,27 +470,38 @@ int FGGroundNetwork::findNearestNode(double lat, double lon) } FGTaxiNode *FGGroundNetwork::findNode(int idx) -{ - for (FGTaxiNodeVectorIterator - itr = nodes.begin(); - itr != nodes.end(); itr++) +{ /* + for (FGTaxiNodeVectorIterator + itr = nodes.begin(); + itr != nodes.end(); itr++) { - if (itr->getIndex() == idx) - return itr->getAddress(); - } - return 0; + if (itr->getIndex() == idx) + return itr->getAddress(); + }*/ + + if ((idx >= 0) && (idx < nodes.size())) + return nodes[idx].getAddress(); + else + return 0; } FGTaxiSegment *FGGroundNetwork::findSegment(int idx) -{ +{/* for (FGTaxiSegmentVectorIterator itr = segments.begin(); itr != segments.end(); itr++) { if (itr->getIndex() == idx) return itr->getAddress(); + } + */ + if ((idx > 0) && (idx <= segments.size())) + return segments[idx-1].getAddress(); + else + { + cerr << "Alert: trying to find invalid segment " << idx << endl; + return 0; } - return 0; } FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end) @@ -583,7 +715,8 @@ void FGGroundNetwork::signOff(int id) { } } -void FGGroundNetwork::update(int id, double lat, double lon, double heading, double speed, double alt) { +void FGGroundNetwork::update(int id, double lat, double lon, double heading, double speed, double alt, + double dt) { TrafficVectorIterator i = activeTraffic.begin(); // Search search if the current id has an entry // This might be faster using a map instead of a vector, but let's start by taking a safe route @@ -600,16 +733,48 @@ void FGGroundNetwork::update(int id, double lat, double lon, double heading, dou i->setPositionAndHeading(lat, lon, heading, speed, alt); current = i; } + + setDt(getDt() + dt); + + // Update every three secs, but add some randomness + // to prevent all IA objects doing this in synchrony + //if (getDt() < (3.0) + (rand() % 10)) + // return; + //else + // setDt(0); + checkSpeedAdjustment(id, lat, lon, heading, speed, alt); + checkHoldPosition (id, lat, lon, heading, speed, alt); +} +void FGGroundNetwork::checkSpeedAdjustment(int id, double lat, + double lon, double heading, + double speed, double alt) +{ + // Scan for a speed adjustment change. Find the nearest aircraft that is in front // and adjust speed when we get too close. Only do this when current position and/or // intentions of the current aircraft match current taxiroute position of the proximate - // aircraft. + // aircraft. For traffic that is on other routes we need to issue a "HOLD Position" + // instruction. See below for the hold position instruction. + TrafficVectorIterator current, closest; + TrafficVectorIterator i = activeTraffic.begin(); + if (activeTraffic.size()) + { + while ((i->getId() != id) && (i != activeTraffic.end())) + { + i++; + } + } + else + { + return; + } + current = i; double mindist = HUGE; if (activeTraffic.size()) { double course, dist, bearing, minbearing; - + //TrafficVector iterator closest; for (TrafficVectorIterator i = activeTraffic.begin(); i != activeTraffic.end(); i++) @@ -622,19 +787,19 @@ void FGGroundNetwork::update(int id, double lat, double lon, double heading, dou i->getLatitude (), i->getAltitude ()); other.CourseAndDistance(curr, &course, &dist); - bearing = fabs(heading-course); - if (bearing > 180) - bearing = 360-bearing; - if ((dist < mindist) && (bearing < 60.0)) - { - mindist = dist; - closest = i; - minbearing = bearing; - } + bearing = fabs(heading-course); + if (bearing > 180) + bearing = 360-bearing; + if ((dist < mindist) && (bearing < 60.0)) + { + mindist = dist; + closest = i; + minbearing = bearing; + } } } - //cerr << "Distance : " << dist << " bearing : " << bearing << " heading : " << heading - // << " course : " << course << endl; + //cerr << "Distance : " << dist << " bearing : " << bearing << " heading : " << heading + // << " course : " << course << endl; current->clearSpeedAdjustment(); // Only clear the heading adjustment at positive speeds, otherwise the waypoint following // code wreaks havoc @@ -655,7 +820,7 @@ void FGGroundNetwork::update(int id, double lat, double lon, double heading, dou // Getting close: Slow down to a bit less than the other aircraft double maxAllowableDistance = (1.1*current->getRadius()) + (1.1*closest->getRadius()); - if (mindist > maxAllowableDistance) + if (mindist > maxAllowableDistance) { if (current->checkPositionAndIntentions(*closest)) { @@ -673,8 +838,8 @@ void FGGroundNetwork::update(int id, double lat, double lon, double heading, dou } else { - double newSpeed = (maxAllowableDistance-mindist); - current->setSpeedAdjustment(newSpeed); + double newSpeed = (maxAllowableDistance-mindist); + current->setSpeedAdjustment(newSpeed); } } } @@ -685,36 +850,11 @@ void FGGroundNetwork::update(int id, double lat, double lon, double heading, dou double newSpeed; if (mindist > 10) { newSpeed = 0.01; - current->setSpeedAdjustment(newSpeed); + current->setSpeedAdjustment(newSpeed); } else { newSpeed = -1 * (maxAllowableDistance-mindist); current->setSpeedAdjustment(newSpeed); current->setHeadingAdjustment(heading); - // if (mindist < 5) { - // double bank_sense = 0; - // current->setSpeedAdjustment(-0.1); - // // Do a heading adjustment - // double diff = fabs(heading - bearing); - // if (diff > 180) - // diff = fabs(diff - 360); - - // double sum = heading + diff; - // if (sum > 360.0) - // sum -= 360.0; - // if (fabs(sum - bearing) < 1.0) { - // bank_sense = -1.0; // turn left for evasive action - // } else { - // bank_sense = 1.0; // turn right for evasive action - // } - // double newHeading = heading + bank_sense; - // if (newHeading < 0) newHeading += 360; - // if (newHeading > 360) newHeading -= 360; - // current->setHeadingAdjustment(newHeading); - // //cerr << "Yikes: TOOOO close. backing up and turning to heading " << newHeading - // // << endl; - // cerr << "Troubleshooting: " << current->getId() << " Closest : " << closest->getId() - // << endl; - // } } } } @@ -722,13 +862,116 @@ void FGGroundNetwork::update(int id, double lat, double lon, double heading, dou } } +void FGGroundNetwork::checkHoldPosition(int id, double lat, + double lon, double heading, + double speed, double alt) +{ + // Check for "Hold position instruction". + // The hold position should be issued under the following conditions: + // 1) For aircraft entering or crossing a runway with active traffic on it, or landing aircraft near it + // 2) For taxiing aircraft that use one taxiway in opposite directions + // 3) For crossing or merging taxiroutes. + + TrafficVectorIterator current, closest; + TrafficVectorIterator i = activeTraffic.begin(); + if (activeTraffic.size()) + { + while ((i->getId() != id) && i != activeTraffic.end()) + { + i++; + } + } + else + { + return ; + } + current = i; + current->setHoldPosition(false); + double course, dist, bearing, minbearing; + for (i = activeTraffic.begin(); + i != activeTraffic.end(); i++) + { + if (i != current) + { + int node = current->crosses(this, *i); + if (node != -1) + { + // Determine whether it's save to continue or not. + // If we have a crossing route, there are two possibilities: + // 1) This is an interestion + // 2) This is oncoming two-way traffic, using the same taxiway. + //cerr << "Hold check 1 : " << id << " has common node " << node << endl; + SGWayPoint nodePos(findNode(node)->getLongitude (), + findNode(node)->getLatitude (), + alt); + SGWayPoint curr (lon, + lat, + alt); + + SGWayPoint other (i->getLongitude (), + i->getLatitude (), + i->getAltitude ()); + //other.CourseAndDistance(curr, &course, &dist); + bool needsToWait; + if (current->isOpposing(this, *i, node)) + { + needsToWait = true; + //cerr << "Hold check 2 : " << id << " has opposing segment " << endl; + // issue a "Hold Position" as soon as we're close to the offending node + // For now, I'm doing this as long as the other aircraft doesn't + // have a hold instruction as soon as we're within a reasonable + // distance from the offending node. + // This may be a bit of a conservative estimate though, as it may + // be well possible that both aircraft can both continue to taxi + // without crashing into each other. + } + else + { + other.CourseAndDistance(nodePos, &course, &dist); + if (dist > 2.0*i->getRadius()) + { + needsToWait = false; + //cerr << "Hold check 3 : " << id <<" Other aircraft approaching node is still far away. (" << dist << " nm). Can safely continue " + // << endl; + } + else + { + needsToWait = true; + //cerr << "Hold check 4: " << id << " Would need to wait for other aircraft : distance = " << dist << " nm" << endl; + } + } + curr.CourseAndDistance(nodePos, &course, &dist); + if (!(i->hasHoldPosition())) + { + + if ((dist < 2.5*current->getRadius()) && + (needsToWait) && + (!(current->getId() == i->getWaitsForId())) && + (!(current->getSpeedAdjustment()))) + + { + current->setHoldPosition(true); + //cerr << "Hold check 5: " << id <<" Setting Hold Position: distance to node : " << dist << " nm"<< endl; + } + else + { + //cerr << "Hold check 6: " << id << " No need to hold yet: Distance to node : " << dist << " nm"<< endl; + } + } + } + } + } +} + + bool FGGroundNetwork::hasInstruction(int id) { TrafficVectorIterator i = activeTraffic.begin(); // Search search if the current id has an entry // This might be faster using a map instead of a vector, but let's start by taking a safe route - if (activeTraffic.size()) { - while ((i->getId() != id) && i != activeTraffic.end()) { + if (activeTraffic.size()) + { + while ((i->getId() != id) && i != activeTraffic.end()) { i++; } } diff --git a/src/Airports/groundnetwork.hxx b/src/Airports/groundnetwork.hxx index f4a8dd41f..fb6089dfa 100644 --- a/src/Airports/groundnetwork.hxx +++ b/src/Airports/groundnetwork.hxx @@ -76,6 +76,7 @@ public: FGTaxiNode *getAddress() { return this;}; FGTaxiSegmentPointerVectorIterator getBeginRoute() { return next.begin(); }; FGTaxiSegmentPointerVectorIterator getEndRoute() { return next.end(); }; + bool operator<(const FGTaxiNode &other) const { return index < other.index; }; }; typedef vector FGTaxiNodeVector; @@ -93,25 +94,32 @@ private: FGTaxiNode *start; FGTaxiNode *end; int index; + FGTaxiSegment *oppositeDirection; public: FGTaxiSegment(); - FGTaxiSegment(FGTaxiNode *, FGTaxiNode *, int); + //FGTaxiSegment(FGTaxiNode *, FGTaxiNode *, int); void setIndex (int val) { index = val; }; void setStartNodeRef (int val) { startNode = val; }; void setEndNodeRef (int val) { endNode = val; }; + void setOpposite(FGTaxiSegment *opp) { oppositeDirection = opp; }; + void setStart(FGTaxiNodeVector *nodes); void setEnd (FGTaxiNodeVector *nodes); void setTrackDistance(); FGTaxiNode * getEnd() { return end;}; + FGTaxiNode * getStart() { return start; }; double getLength() { return length; }; int getIndex() { return index; }; FGTaxiSegment *getAddress() { return this;}; + bool operator<(const FGTaxiSegment &other) const { return index < other.index; }; + FGTaxiSegment *opposite() { return oppositeDirection; }; + }; @@ -194,6 +202,7 @@ public: void setAlt (double val) { alt = val; }; }; +class FGGroundNetwork; /************************************************************************************** * class FGTrafficRecord @@ -219,6 +228,10 @@ public: bool hasInstruction() { return instruction.hasInstruction(); }; void setPositionAndHeading(double lat, double lon, double hdg, double spd, double alt); bool checkPositionAndIntentions(FGTrafficRecord &other); + int crosses (FGGroundNetwork *, FGTrafficRecord &other); + bool isOpposing (FGGroundNetwork *, FGTrafficRecord &other, int node); + + bool getSpeedAdjustment() { return instruction.getChangeSpeed(); }; double getLatitude () { return latitude ; }; double getLongitude() { return longitude; }; @@ -237,6 +250,8 @@ public: void clearHeadingAdjustment() { instruction.setChangeHeading(false); }; bool hasHeadingAdjustment() { return instruction.getChangeHeading(); }; + bool hasHoldPosition() { return instruction.getHoldPosition(); }; + void setHoldPosition (bool inst) { instruction.setHoldPosition(inst); }; void setWaitsForId(int id) { waitsForId = id; }; @@ -265,7 +280,7 @@ public: double hdg, double spd, double alt, double radius) = 0; virtual void signOff(int id) = 0; virtual void update(int id, double lat, double lon, - double heading, double speed, double alt) = 0; + double heading, double speed, double alt, double dt) = 0; virtual bool hasInstruction(int id) = 0; virtual FGATCInstruction getInstruction(int id) = 0; @@ -295,6 +310,11 @@ private: double totalDistance, maxDistance; void printRoutingError(string); + + void checkSpeedAdjustment(int id, double lat, double lon, + double heading, double speed, double alt); + void checkHoldPosition(int id, double lat, double lon, + double heading, double speed, double alt); public: FGGroundNetwork(); @@ -314,7 +334,7 @@ public: virtual void announcePosition(int id, FGAIFlightPlan *intendedRoute, int currentRoute, double lat, double lon, double hdg, double spd, double alt, double radius); virtual void signOff(int id); - virtual void update(int id, double lat, double lon, double heading, double speed, double alt); + virtual void update(int id, double lat, double lon, double heading, double speed, double alt, double dt); virtual bool hasInstruction(int id); virtual FGATCInstruction getInstruction(int id); };