X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FAirports%2Fgroundnetwork.cxx;h=62449b0cb13fe04cc6c499e0131630ec21609e27;hb=fd99e9fdfb47fd1a2ed85c9fa9757ed5fc030514;hp=12faa8515a783aeefcdc1eb591269362b002fca8;hpb=ed30b0c9a3818d5b8a306827184ac3525a1d8bdf;p=flightgear.git diff --git a/src/Airports/groundnetwork.cxx b/src/Airports/groundnetwork.cxx index 12faa8515..62449b0cb 100644 --- a/src/Airports/groundnetwork.cxx +++ b/src/Airports/groundnetwork.cxx @@ -26,6 +26,7 @@ #include #include +#include #include @@ -34,15 +35,16 @@ #include #include -#include #include #include #include +#include #include #include #include +#include #include #include @@ -51,6 +53,8 @@ #include "groundnetwork.hxx" +using std::string; + /*************************************************************************** * FGTaxiSegment **************************************************************************/ @@ -113,6 +117,44 @@ void FGTaxiSegment::setDimensions(double elevation) // headingDiff = fabs(headingDiff - 360); //} +void FGTaxiSegment::block(int id, time_t blockTime, time_t now) +{ + BlockListIterator i = blockTimes.begin(); + while (i != blockTimes.end()) { + if (i->getId() == id) + break; + i++; + } + if (i == blockTimes.end()) { + blockTimes.push_back(Block(id, blockTime, now)); + sort(blockTimes.begin(), blockTimes.end()); + } else { + i->updateTimeStamps(blockTime, now); + } +} + +// The segment has a block if any of the block times listed in the block list is +// smaller than the current time. +bool FGTaxiSegment::hasBlock(time_t now) +{ + for (BlockListIterator i = blockTimes.begin(); i != blockTimes.end(); i++) { + if (i->getBlockTime() < now) + return true; + } + return false; +} + +void FGTaxiSegment::unblock(time_t now) +{ + if (blockTimes.size()) { + BlockListIterator i = blockTimes.begin(); + if (i->getTimeStamp() < (now - 30)) { + blockTimes.erase(i); + } + } +} + + /*************************************************************************** * FGTaxiRoute @@ -210,7 +252,8 @@ bool compare_trafficrecords(FGTrafficRecord a, FGTrafficRecord b) return (a.getIntentions().size() < b.getIntentions().size()); } -FGGroundNetwork::FGGroundNetwork() +FGGroundNetwork::FGGroundNetwork() : + parent(NULL) { hasNetwork = false; foundRoute = false; @@ -220,17 +263,25 @@ FGGroundNetwork::FGGroundNetwork() count = 0; currTraffic = activeTraffic.begin(); group = 0; + version = 0; networkInitialized = false; } FGGroundNetwork::~FGGroundNetwork() { +// JMT 2012-09-8 - disabling the groundnet-caching as part of enabling the +// navcache. The problem isn't the NavCache - it's that for the past few years, +// we have not being running destructors on FGPositioned classes, and hence, +// not running this code. +// When I fix FGPositioned lifetimes (unloading-at-runtime support), this +// will need to be re-visited so it can run safely during shutdown. +#if 0 //cerr << "Running Groundnetwork Destructor " << endl; bool saveData = false; ofstream cachefile; if (fgGetBool("/sim/ai/groundnet-cache")) { - SGPath cacheData(fgGetString("/sim/fg-home")); + SGPath cacheData(globals->get_fg_home()); cacheData.append("ai"); string airport = parent->getId(); @@ -267,6 +318,7 @@ FGGroundNetwork::~FGGroundNetwork() if (saveData) { cachefile.close(); } +#endif } void FGGroundNetwork::saveElevationCache() { @@ -274,7 +326,7 @@ void FGGroundNetwork::saveElevationCache() { bool saveData = false; ofstream cachefile; if (fgGetBool("/sim/ai/groundnet-cache")) { - SGPath cacheData(fgGetString("/sim/fg-home")); + SGPath cacheData(globals->get_fg_home()); cacheData.append("ai"); string airport = parent->getId(); @@ -390,7 +442,7 @@ void FGGroundNetwork::init() //cerr << "Done initializing ground network" << endl; //exit(1); if (fgGetBool("/sim/ai/groundnet-cache")) { - SGPath cacheData(fgGetString("/sim/fg-home")); + SGPath cacheData(globals->get_fg_home()); cacheData.append("ai"); string airport = parent->getId(); @@ -452,6 +504,28 @@ int FGGroundNetwork::findNearestNode(const SGGeod & aGeod) return index; } +int FGGroundNetwork::findNearestNodeOnRunway(const SGGeod & aGeod) +{ + double minDist = HUGE_VAL; + int index = -1; + + for (FGTaxiNodeVectorIterator itr = nodes.begin(); itr != nodes.end(); + itr++) { + if (!((*itr)->getIsOnRunway())) { + continue; + } + double d = SGGeodesy::distanceM(aGeod, (*itr)->getGeod()); + if (d < minDist) { + minDist = d; + index = (*itr)->getIndex(); + //cerr << "Minimum distance of " << minDist << " for index " << index << endl; + } + } + + return index; +} + + int FGGroundNetwork::findNearestNode(double lat, double lon) { return findNearestNode(SGGeod::fromDeg(lon, lat)); @@ -467,7 +541,7 @@ FGTaxiNode *FGGroundNetwork::findNode(unsigned idx) return itr->getAddress(); } */ - if ((idx >= 0) && (idx < nodes.size())) + if (idx < nodes.size()) return nodes[idx]->getAddress(); else return 0; @@ -516,9 +590,23 @@ FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end, } FGTaxiNode *firstNode = findNode(start); + if (!firstNode) + { + SG_LOG(SG_GENERAL, SG_ALERT, + "Error in ground network. Failed to find first waypoint: " << start + << " at " << ((parent) ? parent->getId() : "")); + return FGTaxiRoute(); + } firstNode->setPathScore(0); FGTaxiNode *lastNode = findNode(end); + if (!lastNode) + { + SG_LOG(SG_GENERAL, SG_ALERT, + "Error in ground network. Failed to find last waypoint: " << end + << " at " << ((parent) ? parent->getId() : "")); + return FGTaxiRoute(); + } FGTaxiNodeVector unvisited(*currNodesSet); // working copy @@ -542,6 +630,13 @@ FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end, seg != best->getEndRoute(); seg++) { if (fullSearch || (*seg)->isPushBack()) { FGTaxiNode *tgt = (*seg)->getEnd(); + if (!tgt) + { + SG_LOG(SG_GENERAL, SG_ALERT, + "Error in ground network. Found empty segment " + << " at " << ((parent) ? parent->getId() : "")); + return FGTaxiRoute(); + } double alt = best->getPathScore() + (*seg)->getLength() + (*seg)->getPenalty(nParkings); @@ -628,7 +723,11 @@ void FGGroundNetwork::announcePosition(int id, rec.setPositionAndHeading(lat, lon, heading, speed, alt); rec.setRadius(radius); // only need to do this when creating the record. rec.setAircraft(aircraft); - activeTraffic.push_front(rec); + if (leg == 2) { + activeTraffic.push_front(rec); + } else { + activeTraffic.push_back(rec); + } } else { i->setPositionAndIntentions(currentPosition, intendedRoute); @@ -684,15 +783,15 @@ bool FGGroundNetwork::checkTransmissionState(int minState, int maxState, Traffic trans_num->setIntValue(-1); // PopupCallback(n); //cerr << "Selected transmission message " << n << endl; - FGATCManager *atc = (FGATCManager*) globals->get_subsystem("atc"); + //FGATCManager *atc = (FGATCManager*) globals->get_subsystem("atc"); FGATCDialogNew::instance()->removeEntry(1); } else { //cerr << "creating message for " << i->getAircraft()->getCallSign() << endl; - transmit(&(*i), msgId, msgDir, false); + transmit(&(*i), &(*parent->getDynamics()), msgId, msgDir, false); return false; } } - transmit(&(*i), msgId, msgDir, true); + transmit(&(*i), &(*parent->getDynamics()), msgId, msgDir, true); i->updateState(); lastTransmission = now; available = false; @@ -804,7 +903,7 @@ void FGGroundNetwork::checkSpeedAdjustment(int id, double lat, TrafficVectorIterator current, closest, closestOnNetwork; TrafficVectorIterator i = activeTraffic.begin(); bool otherReasonToSlowDown = false; - bool previousInstruction; +// bool previousInstruction; if (activeTraffic.size()) { //while ((i->getId() != id) && (i != activeTraffic.end())) while (i != activeTraffic.end()) { @@ -823,13 +922,14 @@ void FGGroundNetwork::checkSpeedAdjustment(int id, double lat, current = i; //closest = current; - previousInstruction = current->getSpeedAdjustment(); +// previousInstruction = current->getSpeedAdjustment(); double mindist = HUGE_VAL; if (activeTraffic.size()) { - double course, dist, bearing, minbearing, az2; + double course, dist, bearing, az2; // minbearing, SGGeod curr(SGGeod::fromDegM(lon, lat, alt)); //TrafficVector iterator closest; closest = current; + closestOnNetwork = current; for (TrafficVectorIterator i = activeTraffic.begin(); i != activeTraffic.end(); i++) { if (i == current) { @@ -847,7 +947,7 @@ void FGGroundNetwork::checkSpeedAdjustment(int id, double lat, mindist = dist; closest = i; closestOnNetwork = i; - minbearing = bearing; +// minbearing = bearing; } } @@ -871,7 +971,7 @@ void FGGroundNetwork::checkSpeedAdjustment(int id, double lat, // << endl; mindist = dist; closest = i; - minbearing = bearing; +// minbearing = bearing; otherReasonToSlowDown = true; } } @@ -930,7 +1030,7 @@ void FGGroundNetwork::checkSpeedAdjustment(int id, double lat, } } } - if ((closest == closestOnNetwork) && (current->getPriority() < closest->getPriority()) && needBraking) { + if ((closest->getId() == closestOnNetwork->getId()) && (current->getPriority() < closest->getPriority()) && needBraking) { swap(current, closest); } } @@ -961,6 +1061,7 @@ void FGGroundNetwork::checkHoldPosition(int id, double lat, } else { return; } + time_t now = time(NULL) + fgGetLong("/sim/time/warp"); if (i == activeTraffic.end() || (activeTraffic.size() == 0)) { SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkHoldPosition at " << SG_ORIGIN); @@ -972,7 +1073,9 @@ void FGGroundNetwork::checkHoldPosition(int id, double lat, return; } if (current->getAircraft()->getTakeOffStatus() == 2) { + //cerr << current->getAircraft()->getCallSign() << ". Taxi in position and hold" << endl; current->setHoldPosition(false); + current->clearSpeedAdjustment(); return; } bool origStatus = current->hasHoldPosition(); @@ -993,89 +1096,32 @@ void FGGroundNetwork::checkHoldPosition(int id, double lat, } else { nx = tx; } - if (tx->hasBlock() || nx->hasBlock() ) { - current->setHoldPosition(true); - } - } - - - /* for (i = activeTraffic.begin(); i != activeTraffic.end(); i++) { - if (i->getId() != current->getId()) { - int node = current->crosses(this, *i); - if (node != -1) { - FGTaxiNode *taxiNode = findNode(node); - - // 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; - - SGGeod other(SGGeod:: - fromDegM(i->getLongitude(), i->getLatitude(), - i->getAltitude())); - bool needsToWait; - bool opposing; - if (current->isOpposing(this, *i, node)) { - needsToWait = true; - opposing = true; - //cerr << "Hold check 2 : " << node << " 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 { - opposing = false; - if (SGGeodesy::distanceM(other, taxiNode->getGeod()) > 200) // 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 << " meters" << endl; - } - } + //if (tx->hasBlock(now) || nx->hasBlock(now) ) { + // current->setHoldPosition(true); + //} + SGGeod start(SGGeod::fromDeg((i->getLongitude()), (i->getLatitude()))); + SGGeod end (SGGeod::fromDeg(nx->getStart()->getLongitude(), nx->getStart()->getLatitude())); - double dist = - SGGeodesy::distanceM(curr, taxiNode->getGeod()); - if (!(i->hasHoldPosition())) { - - if ((dist < 200) && //2.5*current->getRadius()) && - (needsToWait) && (i->onRoute(this, *current)) && - //((i->onRoute(this, *current)) || ((!(i->getSpeedAdjustment())))) && - (!(current->getId() == i->getWaitsForId()))) - //(!(i->getSpeedAdjustment()))) // && - //(!(current->getSpeedAdjustment()))) - - { - if (!(isUserAircraft(i->getAircraft()))) { // test code. Don't wait for the user, let the user wait for you. - current->setHoldPosition(true); - current->setWaitsForId(i->getId()); - } - //cerr << "Hold check 5: " << current->getCallSign() <<" Setting Hold Position: distance to node (" << node << ") " - // << dist << " meters. Waiting for " << i->getCallSign(); - //if (opposing) - //cerr <<" [opposing] " << endl; - //else - // cerr << "[non-opposing] " << endl; - //if (i->hasSpeefAdjustment()) - // { - // cerr << " (which in turn waits for ) " << i-> - } else { - //cerr << "Hold check 6: " << id << " No need to hold yet: Distance to node : " << dist << " nm"<< endl; - } + double distance = SGGeodesy::distanceM(start, end); + if (nx->hasBlock(now) && (distance < i->getRadius() * 4)) { + current->setHoldPosition(true); + } else { + intVecIterator ivi = i->getIntentions().begin(); + while (ivi != i->getIntentions().end()) { + if ((*ivi) > 0) { + distance += segments[(*ivi)-1]->getLength(); + if ((segments[(*ivi)-1]->hasBlock(now)) && (distance < i->getRadius() * 4)) { + current->setHoldPosition(true); + break; } } + ivi++; } - } */ + } + } bool currStatus = current->hasHoldPosition(); current->setHoldPosition(origStatus); // Either a Hold Position or a resume taxi transmission has been issued - time_t now = time(NULL) + fgGetLong("/sim/time/warp"); if ((now - lastTransmission) > 2) { available = true; } @@ -1083,11 +1129,11 @@ void FGGroundNetwork::checkHoldPosition(int id, double lat, if ((origStatus != currStatus) && available) { //cerr << "Issueing hold short instrudtion " << currStatus << " " << available << endl; if (currStatus == true) { // No has a hold short instruction - transmit(&(*current), MSG_HOLD_POSITION, ATC_GROUND_TO_AIR, true); + transmit(&(*current), &(*parent->getDynamics()), MSG_HOLD_POSITION, ATC_GROUND_TO_AIR, true); //cerr << "Transmittin hold short instrudtion " << currStatus << " " << available << endl; current->setState(1); } else { - transmit(&(*current), MSG_RESUME_TAXI, ATC_GROUND_TO_AIR, true); + transmit(&(*current), &(*parent->getDynamics()), MSG_RESUME_TAXI, ATC_GROUND_TO_AIR, true); //cerr << "Transmittig resume instrudtion " << currStatus << " " << available << endl; current->setState(2); } @@ -1291,7 +1337,7 @@ static void WorldCoordinate(osg::Matrix& obj_pos, double lat, double lon, double elev, double hdg, double slope) { SGGeod geod = SGGeod::fromDegM(lon, lat, elev); - obj_pos = geod.makeZUpFrame(); + obj_pos = makeZUpFrame(geod); // hdg is not a compass heading, but a counter-clockwise rotation // around the Z axis obj_pos.preMult(osg::Matrix::rotate(hdg * SGD_DEGREES_TO_RADIANS, @@ -1322,10 +1368,11 @@ void FGGroundNetwork::render(bool visible) if (visible) { group = new osg::Group; FGScenery * local_scenery = globals->get_scenery(); - double elevation_meters = 0.0; - double elevation_feet = 0.0; + // double elevation_meters = 0.0; +// double elevation_feet = 0.0; + time_t now = time(NULL) + fgGetLong("/sim/time/warp"); //for ( FGTaxiSegmentVectorIterator i = segments.begin(); i != segments.end(); i++) { - double dx = 0; + //double dx = 0; for (TrafficVectorIterator i = activeTraffic.begin(); i != activeTraffic.end(); i++) { // Handle start point int pos = i->getCurrentPosition() - 1; @@ -1362,7 +1409,7 @@ void FGGroundNetwork::render(bool visible) SGGeod center2 = end; center2.setElevationM(SG_MAX_ELEVATION_M); if (local_scenery->get_elevation_m( center2, elevationEnd, NULL )) { - elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5; +// elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5; //elevation_meters += 0.5; } else { @@ -1393,7 +1440,7 @@ void FGGroundNetwork::render(bool visible) geode->addDrawable(geometry); //osg::Node *custom_obj; SGMaterial *mat; - if (segments[pos]->hasBlock()) { + if (segments[pos]->hasBlock(now)) { mat = matlib->find("UnidirectionalTaperRed"); } else { mat = matlib->find("UnidirectionalTaperGreen"); @@ -1422,7 +1469,7 @@ void FGGroundNetwork::render(bool visible) SGGeod center2 = segments[k]->getStart()->getGeod(); center2.setElevationM(SG_MAX_ELEVATION_M); if (local_scenery->get_elevation_m( center2, elevationStart, NULL )) { - elevation_feet = elevationStart * SG_METER_TO_FEET + 0.5; +// elevation_feet = elevationStart * SG_METER_TO_FEET + 0.5; //elevation_meters += 0.5; } else { @@ -1434,7 +1481,7 @@ void FGGroundNetwork::render(bool visible) SGGeod center2 = segments[k]->getEnd()->getGeod(); center2.setElevationM(SG_MAX_ELEVATION_M); if (local_scenery->get_elevation_m( center2, elevationEnd, NULL )) { - elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5; +// elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5; //elevation_meters += 0.5; } else { @@ -1467,7 +1514,7 @@ void FGGroundNetwork::render(bool visible) geode->addDrawable(geometry); //osg::Node *custom_obj; SGMaterial *mat; - if (segments[k]->hasBlock()) { + if (segments[k]->hasBlock(now)) { mat = matlib->find("UnidirectionalTaperRed"); } else { mat = matlib->find("UnidirectionalTaperGreen"); @@ -1492,8 +1539,9 @@ string FGGroundNetwork::getName() { void FGGroundNetwork::update(double dt) { + time_t now = time(NULL) + fgGetLong("/sim/time/warp"); for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) { - (*tsi)->unblock(); + (*tsi)->unblock(now); } int priority = 1; //sort(activeTraffic.begin(), activeTraffic.end(), compare_trafficrecords); @@ -1503,7 +1551,9 @@ void FGGroundNetwork::update(double dt) i != parent->getDynamics()->getStartupController()->getActiveTraffic().end(); i++) { i->allowPushBack(); i->setPriority(priority++); - if (i->isActive(60)) { + // in meters per second; + double vTaxi = (i->getAircraft()->getPerformance()->vTaxi() * SG_NM_TO_METER) / 3600; + if (i->isActive(0)) { // Check for all active aircraft whether it's current pos segment is // an opposite of one of the departing aircraft's intentions @@ -1516,7 +1566,7 @@ void FGGroundNetwork::update(double dt) for (intVecIterator k = i->getIntentions().begin(); k != i->getIntentions().end(); k++) { if ((*k) == posReverse) { i->denyPushBack(); - segments[posReverse-1]->block(); + segments[posReverse-1]->block(i->getId(), now, now); } } } @@ -1524,13 +1574,15 @@ void FGGroundNetwork::update(double dt) } // if the current aircraft is still allowed to pushback, we can start reserving a route for if by blocking all the entry taxiways. if (i->pushBackAllowed()) { + double length = 0; int pos = i->getCurrentPosition(); if (pos > 0) { FGTaxiSegment *seg = segments[pos-1]; FGTaxiNode *node = seg->getEnd(); + length = seg->getLength(); for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) { if (((*tsi)->getEnd() == node) && ((*tsi) != seg)) { - (*tsi)->block(); + (*tsi)->block(i->getId(), now, now); } } } @@ -1539,9 +1591,11 @@ void FGGroundNetwork::update(double dt) if (pos > 0) { FGTaxiSegment *seg = segments[pos-1]; FGTaxiNode *node = seg->getEnd(); + length += seg->getLength(); + time_t blockTime = now + (length / vTaxi); for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) { if (((*tsi)->getEnd() == node) && ((*tsi) != seg)) { - (*tsi)->block(); + (*tsi)->block(i->getId(), blockTime-30, now); } } } @@ -1550,10 +1604,13 @@ void FGGroundNetwork::update(double dt) } } for (TrafficVectorIterator i = activeTraffic.begin(); i != activeTraffic.end(); i++) { + double length = 0; + double vTaxi = (i->getAircraft()->getPerformance()->vTaxi() * SG_NM_TO_METER) / 3600; i->setPriority(priority++); int pos = i->getCurrentPosition(); if (pos > 0) { - if (segments[pos-1]->hasBlock()) { + length = segments[pos-1]->getLength(); + if (segments[pos-1]->hasBlock(now)) { //SG_LOG(SG_GENERAL, SG_ALERT, "Taxiway incursion for AI aircraft" << i->getAircraft()->getCallSign()); } @@ -1562,7 +1619,7 @@ void FGGroundNetwork::update(double dt) for (ivi = i->getIntentions().begin(); ivi != i->getIntentions().end(); ivi++) { int segIndex = (*ivi); if (segIndex > 0) { - if (segments[segIndex-1]->hasBlock()) + if (segments[segIndex-1]->hasBlock(now)) break; } } @@ -1572,9 +1629,11 @@ void FGGroundNetwork::update(double dt) if (pos > 0) { FGTaxiSegment *seg = segments[pos-1]; FGTaxiNode *node = seg->getEnd(); + length += seg->getLength(); for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) { if (((*tsi)->getEnd() == node) && ((*tsi) != seg)) { - (*tsi)->block(); + time_t blockTime = now + (length / vTaxi); + (*tsi)->block(i->getId(), blockTime - 30, now); } } }