From 82863d3f3e121f3166dbaaaa4a437cd085f2aeec Mon Sep 17 00:00:00 2001 From: portree_kid Date: Sun, 6 Feb 2022 21:11:58 +0100 Subject: [PATCH] AI Fixes * Fix turns (left-turn-bug) * Better parking apporach * CSV Logging via property * ft vs m bug in setLeadDistance * Split Runway Entry/Exit calculation * Prototype wait pattern --- src/AIModel/AIAircraft.cxx | 461 +- src/AIModel/AIAircraft.hxx | 44 +- src/AIModel/AIFlightPlan.cxx | 24 +- src/AIModel/AIFlightPlan.hxx | 4 + src/AIModel/AIFlightPlanCreate.cxx | 151 +- src/AIModel/AIFlightPlanCreatePushBack.cxx | 4 +- src/Airports/dynamics.cxx | 3 +- src/Airports/groundnetwork.cxx | 75 +- src/Airports/groundnetwork.hxx | 4 +- test_suite/test_data/YBBN.groundnet.xml | 4958 +++++++++---------- test_suite/unit_tests/AI/test_groundnet.cxx | 2 +- test_suite/unit_tests/AI/test_traffic.cxx | 119 +- test_suite/unit_tests/AI/test_traffic.hxx | 2 + 13 files changed, 3092 insertions(+), 2759 deletions(-) diff --git a/src/AIModel/AIAircraft.cxx b/src/AIModel/AIAircraft.cxx index 63c9a7eeb..40269d383 100644 --- a/src/AIModel/AIAircraft.cxx +++ b/src/AIModel/AIAircraft.cxx @@ -28,6 +28,7 @@ #include #include +#include #include #include @@ -57,12 +58,16 @@ FGAIAircraft::FGAIAircraft(FGAISchedule* ref) : /* HOT must be disabled for AI A _performance(nullptr) { trafficRef = ref; + csvFile = std::make_unique(); if (trafficRef) { groundOffset = trafficRef->getGroundOffset(); setCallSign(trafficRef->getCallSign()); + tracked = getCallSign() == fgGetString("/ai/track-callsign") + || fgGetString("/ai/track-callsign") == "ALL"; } - else + else { groundOffset = 0; + } fp = 0; controller = 0; @@ -102,7 +107,7 @@ FGAIAircraft::FGAIAircraft(FGAISchedule* ref) : /* HOT must be disabled for AI A trackCache.remainingLength = 0; trackCache.startWptName = "-"; - + tcasThreatNode = props->getNode("tcas/threat-level", true); tcasRANode = props->getNode("tcas/ra-sense", true); _searchOrder = ModelSearchOrder::PREFER_AI; @@ -141,6 +146,8 @@ void FGAIAircraft::readFromScenario(SGPropertyNode* scFileNode) { setFlightPlan(scFileNode->getStringValue("flightplan"), scFileNode->getBoolValue("repeat", false)); setCallSign(scFileNode->getStringValue("callsign")); + tracked = getCallSign() == fgGetString("/ai/track-callsign") + || fgGetString("/ai/track-callsign") == "ALL"; } @@ -156,6 +163,17 @@ void FGAIAircraft::update(double dt) { FGAIBase::update(dt); Run(dt); Transform(); + if (tracked && !csvFile->is_open()) { + char fname [160]; + time_t t = time(0); // get time now + snprintf (fname, sizeof(fname), "%s_%ld.csv", getCallSign().c_str(), t); + SGPath p = globals->get_download_dir() / fname; + csvFile->open(p); + dumpCSVHeader(csvFile); + } + if (tracked && csvFile->is_open()) { + dumpCSV(csvFile, csvIndex++); + } } void FGAIAircraft::unbind() @@ -250,7 +268,7 @@ void FGAIAircraft::ClimbTo(double alt_ft ) { void FGAIAircraft::TurnTo(double heading) { if( fabs(heading) < 0.1 ) { - SG_LOG(SG_AI, SG_WARN, "Heading reset to zero"); + SG_LOG(SG_AI, SG_WARN, getCallSign() << "|Heading reset to zero"); } tgt_heading = heading; // SG_LOG(SG_AI, SG_BULK, "Turn tgt_heading to " << tgt_heading); @@ -272,7 +290,7 @@ void FGAIAircraft::setFlightPlan(const std::string& flightplan, bool repeat) // this is the case for Nasal-scripted aircraft return; } - + std::unique_ptr plan(new FGAIFlightPlan(flightplan)); if (plan->isValidPlan()) { plan->setRepeat(repeat); @@ -319,12 +337,12 @@ void FGAIAircraft::ProcessFlightPlan( double dt, time_t now ) { return; } prev = fp->getPreviousWaypoint(); - SG_LOG(SG_AI, SG_BULK, "Previous WP \t" << prev->getName()); + SG_LOG(SG_AI, SG_BULK, getCallSign() << "|Previous WP \t" << prev->getName()); curr = fp->getCurrentWaypoint(); - SG_LOG(SG_AI, SG_BULK, "Current WP \t" << curr->getName()); + SG_LOG(SG_AI, SG_BULK, getCallSign() << "|Current WP \t" << curr->getName()); next = fp->getNextWaypoint(); if( next ) { - SG_LOG(SG_AI, SG_BULK, "Next WP \t" << next->getName()); + SG_LOG(SG_AI, SG_BULK, getCallSign() << "|Next WP \t" << next->getName()); } } if (!curr) { @@ -333,11 +351,11 @@ void FGAIAircraft::ProcessFlightPlan( double dt, time_t now ) { } if (!leadPointReached(curr, next, nextTurnAngle)) { - controlHeading(curr); + controlHeading(curr, nullptr); controlSpeed(curr, next); } else { if (curr->isFinished()) { //end of the flight plan - SG_LOG(SG_AI, SG_BULK, "Flightplan ended"); + SG_LOG(SG_AI, SG_BULK, getCallSign() << "|Flightplan ended"); if (fp->getRepeat()) { fp->restart(); } @@ -364,14 +382,14 @@ void FGAIAircraft::ProcessFlightPlan( double dt, time_t now ) { } prev = fp->getPreviousWaypoint(); - SG_LOG(SG_AI, SG_BULK, "Previous WP \t" << prev->getName() << "\t" << prev->getPos()); + SG_LOG(SG_AI, SG_BULK, getCallSign() << "|Previous WP \t" << prev->getName() << "\t" << prev->getPos()); curr = fp->getCurrentWaypoint(); if (curr) { - SG_LOG(SG_AI, SG_BULK, "Current WP \t" << curr->getName() << "\t" << curr->getPos()); + SG_LOG(SG_AI, SG_BULK, getCallSign() << "|Current WP \t" << curr->getName() << "\t" << curr->getPos()); } next = fp->getNextWaypoint(); if(next) { - SG_LOG(SG_AI, SG_BULK, "Next WP \t" << next->getName() << "\t" << next->getPos()); + SG_LOG(SG_AI, SG_BULK, getCallSign() << "|Next WP \t" << next->getName() << "\t" << next->getPos()); } // Now that we have incremented the waypoints, excute some traffic manager specific code @@ -389,14 +407,15 @@ void FGAIAircraft::ProcessFlightPlan( double dt, time_t now ) { announcePositionToController(); if (fp && props) { - props->getChild("arrival-time-sec", 0, true)->setIntValue(fp->getArrivalTime()); + props->getChild("arrival-time-sec", 0, true)->setIntValue(fp->getArrivalTime()); } } if (next && !curr->contains("END") && !curr->contains("PushBackPointlegend")) { + SG_LOG(SG_AI, SG_BULK, getCallSign() << "|Setting Leaddistance"); fp->setLeadDistance(tgt_speed, tgt_heading, curr, next); } else { - // If we are ending in a parking + // If we are ending in a parking fp->setLeadDistance(1); } @@ -407,10 +426,10 @@ void FGAIAircraft::ProcessFlightPlan( double dt, time_t now ) { if (curr->getCrossat() > -1000.0) { use_perf_vs = false; double dist_m = fp->getDistanceToGo(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr); - double vert_dist_ft = curr->getCrossat() - altitude_ft; + double vert_dist_ft = curr->getCrossat() - altitude_ft; double err_dist = prev->getCrossat() - altitude_ft; tgt_vs = calcVerticalSpeed(vert_dist_ft, dist_m, speed, err_dist); - tgt_altitude_ft = curr->getCrossat(); + tgt_altitude_ft = curr->getCrossat(); checkTcas(); } else { use_perf_vs = true; @@ -422,7 +441,7 @@ void FGAIAircraft::ProcessFlightPlan( double dt, time_t now ) { // Altitude restriction use_perf_vs = false; double dist_m = fp->getDistanceToGo(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr); - double vert_dist_ft = curr->getCrossat() - altitude_ft; + double vert_dist_ft = curr->getCrossat() - altitude_ft; double err_dist = - altitude_ft; tgt_vs = calcVerticalSpeed(vert_dist_ft, dist_m, speed, err_dist); tgt_altitude_ft = curr->getCrossat(); @@ -474,22 +493,33 @@ void FGAIAircraft::clearATCController() towerController = 0; } +bool FGAIAircraft::isBlockedBy(FGAIAircraft* other) { + if(!other) { + return false; + } + int azimuth = SGGeodesy::courseDeg(getGeodPos(), other->getGeodPos()); + const double headingDiff = SGMiscd::normalizePeriodic(-180, 180, hdg-azimuth); + + SG_LOG(SG_AI, SG_BULK, getCallSign() << " blocked by " << other->getCallSign() << azimuth << " Heading " << hdg << " Diff" << headingDiff); + return false; +} + #if 0 void FGAIAircraft::assertSpeed(double speed) { if ((speed < -50) || (speed > 1000)) { - cerr << getCallSign() << " " + SG_LOG(SG_AI, SG_DEBUG, getCallSign() << " " << "Previous waypoint " << fp->getPreviousWaypoint()->getName() << " " << "Departure airport " << trafficRef->getDepartureAirport() << " " << "Leg " << fp->getLeg() << " " << "target_speed << " << tgt_speed << " " << "speedFraction << " << speedFraction << " " - << "Currecnt speed << " << speed << " " - << endl; + << "Currecnt speed << " << speed << " "); } } #endif + void FGAIAircraft::checkTcas(void) { if (tcasThreatNode && tcasThreatNode->getIntValue()==3) @@ -525,14 +555,15 @@ const char * FGAIAircraft::_getTransponderCode() const { } // NOTE: Check whether the new (delayed leg increment code has any effect on this code. -// Probably not, because it should only be executed after we have already passed the leg incrementing waypoint. +// Probably not, because it should only be executed after we have already passed the leg incrementing waypoint. bool FGAIAircraft::loadNextLeg(double distance) { - int leg; - if ((leg = fp->getLeg()) == 9) { + if ((leg = fp->getLeg()) == AILeg::PARKING) { FGAirport *oldArr = trafficRef->getArrivalAirport(); if (!trafficRef->next()) { + SG_LOG(SG_AI, SG_BULK, getCallSign() << "|No following flight killing"); + //FIXME I'm on leg 9 and don't even reach parking. return false; } @@ -544,6 +575,8 @@ bool FGAIAircraft::loadNextLeg(double distance) { repositioned = false; } setCallSign(trafficRef->getCallSign()); + tracked = getCallSign() == fgGetString("/ai/track-callsign") + || fgGetString("/ai/track-callsign") == "ALL"; leg = 0; fp->setLeg(leg); } @@ -556,6 +589,7 @@ bool FGAIAircraft::loadNextLeg(double distance) { } else { double cruiseAlt = trafficRef->getCruiseAlt() * 100; + SG_LOG(SG_AI, SG_BULK, getCallSign() << "|Loading Leg " << leg+1); bool ok = fp->create (this, dep, arr, @@ -572,7 +606,7 @@ bool FGAIAircraft::loadNextLeg(double distance) { distance); if (!ok) { - SG_LOG(SG_AI, SG_WARN, "Failed to create waypoints for leg:" << leg+1); + SG_LOG(SG_AI, SG_WARN, getCallSign() << "|Failed to create waypoints for leg:" << leg+1); } } return true; @@ -598,7 +632,7 @@ void FGAIAircraft::getGroundElev(double dt) { // Only do the proper hitlist stuff if we are within visible range of the viewer. if (!invisible) { - double visibility_meters = fgGetDouble("/environment/visibility-m"); + double visibility_meters = fgGetDouble("/environment/visibility-m"); if (SGGeodesy::distanceM(globals->get_view_position(), pos) > visibility_meters) { return; } @@ -620,7 +654,6 @@ void FGAIAircraft::getGroundElev(double dt) { } } - void FGAIAircraft::doGroundAltitude() { if ((fp->getLeg() == 7) && ((altitude_ft - tgt_altitude_ft) > 5)) { tgt_vs = -500; @@ -639,47 +672,47 @@ void FGAIAircraft::announcePositionToController() { if (!trafficRef) { return; } - + int leg = fp->getLeg(); if (!fp->getCurrentWaypoint()) { // http://code.google.com/p/flightgear-bugs/issues/detail?id=1153 // throw an exception so this aircraft gets killed by the AIManager. - throw sg_exception("bad AI flight plan"); + throw sg_exception("bad AI flight plan. No current WP"); } - + // Note that leg has been incremented after creating the current leg, so we should use // leg numbers here that are one higher than the number that is used to create the leg - // NOTE: As of July, 30, 2011, the post-creation leg updating is no longer happening. + // NOTE: As of July, 30, 2011, the post-creation leg updating is no longer happening. // Leg numbers are updated only once the aircraft passes the last waypoint created for that legm so I should probably just use // the original leg numbers here! - switch (leg) { - case 1: // Startup and Push back + switch (leg) { + case AILeg::STARTUP_PUSHBACK: // Startup and Push back if (trafficRef->getDepartureAirport()->getDynamics()) controller = trafficRef->getDepartureAirport()->getDynamics()->getStartupController(); break; - case 2: // Taxiing to runway + case AILeg::TAXI: // Taxiing to runway if (trafficRef->getDepartureAirport()->getDynamics()->getGroundController()->exists()) controller = trafficRef->getDepartureAirport()->getDynamics()->getGroundController(); break; - case 3: //Take off tower controller + case AILeg::TAKEOFF: //Take off tower controller if (trafficRef->getDepartureAirport()->getDynamics()) { controller = trafficRef->getDepartureAirport()->getDynamics()->getTowerController(); towerController = 0; } else { - cerr << "Error: Could not find Dynamics at airport : " << trafficRef->getDepartureAirport()->getId() << endl; + SG_LOG(SG_AI, SG_BULK, "Error: Could not find Dynamics at airport : " << trafficRef->getDepartureAirport()->getId()); } break; - case 6: + case AILeg::APPROACH: if (trafficRef->getArrivalAirport()->getDynamics()) { controller = trafficRef->getArrivalAirport()->getDynamics()->getApproachController(); } break; - case 8: // Taxiing for parking + case AILeg::PARKING_TAXI: // Taxiing for parking if (trafficRef->getArrivalAirport()->getDynamics()->getGroundController()->exists()) controller = trafficRef->getArrivalAirport()->getDynamics()->getGroundController(); break; default: - controller = 0; + controller = nullptr; break; } @@ -701,7 +734,7 @@ void FGAIAircraft::scheduleForATCTowerDepartureControl(int state) { if (trafficRef->getDepartureAirport()->getDynamics()) { towerController = trafficRef->getDepartureAirport()->getDynamics()->getTowerController(); } else { - cerr << "Error: Could not find Dynamics at airport : " << trafficRef->getDepartureAirport()->getId() << endl; + SG_LOG(SG_AI, SG_WARN, "Could not find Dynamics at airport : " << trafficRef->getDepartureAirport()->getId()); } if (towerController) { towerController->announcePosition(getID(), fp.get(), fp->getCurrentWaypoint()->getRouteIndex(), @@ -717,10 +750,10 @@ void FGAIAircraft::scheduleForATCTowerDepartureControl(int state) { void FGAIAircraft::processATC(const FGATCInstruction& instruction) { if (instruction.getCheckForCircularWait()) { - // This is not exactly an elegant solution, + // This is not exactly an elegant solution, // but at least it gives me a chance to check // if circular waits are resolved. - // For now, just take the offending aircraft + // For now, just take the offending aircraft // out of the scene setDie(true); // a more proper way should be - of course - to @@ -783,12 +816,12 @@ void FGAIAircraft::handleFirstWaypoint() { } prev = fp->getPreviousWaypoint(); //first waypoint - SG_LOG(SG_AI, SG_BULK, "Previous WP \t" << prev->getName()); + SG_LOG(SG_AI, SG_BULK, getCallSign() << "|Previous WP \t" << prev->getName()); curr = fp->getCurrentWaypoint(); //second waypoint - SG_LOG(SG_AI, SG_BULK, "Current WP \t" << curr->getName()); + SG_LOG(SG_AI, SG_BULK, getCallSign() << "|Current WP \t" << curr->getName()); next = fp->getNextWaypoint(); //third waypoint (might not exist!) if( next ) { - SG_LOG(SG_AI, SG_BULK, "Next WP \t" << next->getName()); + SG_LOG(SG_AI, SG_BULK, getCallSign() << "|Next WP \t" << next->getName()); } setLatitude(prev->getLatitude()); @@ -810,7 +843,7 @@ void FGAIAircraft::handleFirstWaypoint() { // If next doesn't exist, as in incrementally created flightplans for // AI/Trafficmanager created plans, // Make sure lead distance is initialized otherwise - // If we are ending in a parking + // If we are ending in a parking if (next && !curr->contains("END") && !curr->contains("PushBackPointlegend")) { fp->setLeadDistance(speed, hdg, curr, next); } else { @@ -861,7 +894,7 @@ bool FGAIAircraft::fpExecutable(time_t now) { * Check to see if we've reached the lead point for our next turn * * @param curr the WP we are currently targeting at. - * @param next the WP that will follow. Used to detect passed WPs (heading diff curr/next > 120°) + * @param next the WP that will follow. Used to detect passed WPs (heading diff curr/next > 120°) * @param nextTurnAngle to detect sharp corners * @return */ @@ -871,20 +904,20 @@ bool FGAIAircraft::leadPointReached(FGAIWaypoint* curr, FGAIWaypoint* next, int double lead_distance_m = fp->getLeadDistance() * SG_FEET_TO_METER; const double arrivalDist = fabs(10.0*fp->getCurrentWaypoint()->getSpeed()); // arrive at pushback end - if ((dist_to_go_m < arrivalDist) && (speed < 0) && (tgt_speed < 0) && fp->getCurrentWaypoint()->contains("PushBackPoint")) { + if ((dist_to_go_m < arrivalDist) && (speed < 0) && (tgt_speed < 0) && fp->getCurrentWaypoint()->contains("PushBackPoint")) { // tgt_speed = -(dist_to_go_m / 10.0); tgt_speed = -std::sqrt((pow(arrivalDist,2)-pow(arrivalDist-dist_to_go_m,2))); - SG_LOG(SG_AI, SG_BULK, "tgt_speed " << tgt_speed); + SG_LOG(SG_AI, SG_BULK, getCallSign() << "|tgt_speed " << tgt_speed); if (tgt_speed > -1) { // Speed is int and cannot go below 1 knot tgt_speed = -1; } if (fp->getPreviousWaypoint()->getSpeed() < tgt_speed) { - SG_LOG(SG_AI, SG_BULK, "Set speed of WP from " << fp->getPreviousWaypoint()->getSpeed() << " to " << tgt_speed); + SG_LOG(SG_AI, SG_BULK, getCallSign() << "|Set speed of WP from " << fp->getPreviousWaypoint()->getSpeed() << " to " << tgt_speed); fp->getPreviousWaypoint()->setSpeed(tgt_speed); } - } + } // arrive at parking if ((dist_to_go_m < arrivalDist) && (speed > 0) && (tgt_speed > 0) && fp->getCurrentWaypoint()->contains("END")) { tgt_speed = (dist_to_go_m / 10.0); @@ -894,18 +927,18 @@ bool FGAIAircraft::leadPointReached(FGAIWaypoint* curr, FGAIWaypoint* next, int } if (fp->getPreviousWaypoint()->getSpeed() < tgt_speed) { - SG_LOG(SG_AI, SG_BULK, "Set speed of WP from " << fp->getPreviousWaypoint()->getSpeed() << " to " << tgt_speed); + SG_LOG(SG_AI, SG_BULK, getCallSign() << "|Set speed of WP from " << fp->getPreviousWaypoint()->getSpeed() << " to " << tgt_speed); fp->getPreviousWaypoint()->setSpeed(tgt_speed); } - } - + } + if (lead_distance_m < fabs(2*speed) * SG_FEET_TO_METER) { //don't skip over the waypoint - SG_LOG(SG_AI, SG_BULK, "Set lead_distance_m due to speed " << lead_distance_m << " to " << fabs(2*speed) * SG_FEET_TO_METER); + SG_LOG(SG_AI, SG_BULK, getCallSign() << "|Set lead_distance_m due to speed " << lead_distance_m << " to " << fabs(2*speed) * SG_FEET_TO_METER); lead_distance_m = fabs(2*speed) * SG_FEET_TO_METER; fp->setLeadDistance(lead_distance_m * SG_METER_TO_FEET); } - + double bearing = 0; // don't do bearing calculations for ground traffic bearing = getBearing(fp->getBearing(pos, curr)); @@ -916,7 +949,7 @@ bool FGAIAircraft::leadPointReached(FGAIWaypoint* curr, FGAIWaypoint* next, int if (onGround() && fabs(nextTurnAngle) > 50 ) { //don't skip over the waypoint const int multiplicator = 4; - SG_LOG(SG_AI, SG_BULK, "Set lead_distance_m due to next turn angle " << lead_distance_m << " to " << fabs(multiplicator*speed) * SG_FEET_TO_METER << " dist_to_go_m " << dist_to_go_m << " Next turn angle : " << fabs(nextTurnAngle) ); + SG_LOG(SG_AI, SG_BULK, getCallSign() << "|Set lead_distance_m due to next turn angle " << lead_distance_m << " to " << fabs(multiplicator*speed) * SG_FEET_TO_METER << " dist_to_go_m " << dist_to_go_m << " Next turn angle : " << fabs(nextTurnAngle) ); lead_distance_m = fabs(multiplicator*speed) * SG_FEET_TO_METER; fp->setLeadDistance(lead_distance_m * SG_METER_TO_FEET); } @@ -934,18 +967,20 @@ bool FGAIAircraft::leadPointReached(FGAIWaypoint* curr, FGAIWaypoint* next, int if ((dist_to_go_m < lead_distance_m) || ((dist_to_go_m > prev_dist_to_go) && (bearing > (minBearing * 1.1))) ) { - SG_LOG(SG_AI, SG_BULK, "Leadpoint reached Bearing : " << bearing << "\tNext Bearing : " << nextBearing << " Next Turn Angle : " << fabs(nextTurnAngle)); + SG_LOG(SG_AI, SG_BULK, getCallSign() << "|Leadpoint reached Bearing : " << bearing << "\tNext Bearing : " << nextBearing << " Next Turn Angle : " << fabs(nextTurnAngle)); minBearing = 360; speedFraction = 1.0; prev_dist_to_go = HUGE_VAL; return true; } else { - if (prev_dist_to_go == dist_to_go_m && fabs(groundTargetSpeed)>0) { + if (prev_dist_to_go == dist_to_go_m + && fabs(groundTargetSpeed)>0 + && this->atGate().empty() ) { //FIXME must be suppressed when parked - SG_LOG(SG_AI, SG_BULK, "Aircraft " << _callsign << " stuck. Speed " << speed); + SG_LOG(SG_AI, SG_BULK, getCallSign() << "|Aircraft stuck. Speed " << speed); stuckCounter++; if (stuckCounter>AI_STUCK_LIMIT) { - SG_LOG(SG_AI, SG_WARN, "Stuck flight " << _callsign << " killed" ); + SG_LOG(SG_AI, SG_WARN, getCallSign() << "|Stuck flight killed on leg " << fp->getLeg() ); setDie(true); } } else { @@ -1004,7 +1039,7 @@ bool FGAIAircraft::handleAirportEndPoints(FGAIWaypoint* prev, time_t now) { // This is the last taxi waypoint, and marks the the end of the flight plan // so, the schedule should update and wait for the next departure time. if (prev->contains("END")) { - SG_LOG(SG_AI, SG_BULK, "Reached " << prev->getName() ); + SG_LOG(SG_AI, SG_BULK, getCallSign() << "|Reached " << prev->getName() ); //FIXME Heading Error should be reset time_t nextDeparture = trafficRef->getDepartureTime(); @@ -1024,21 +1059,39 @@ bool FGAIAircraft::handleAirportEndPoints(FGAIWaypoint* prev, time_t now) { * * @param curr */ -void FGAIAircraft::controlHeading(FGAIWaypoint* curr) { +void FGAIAircraft::controlHeading(FGAIWaypoint* curr,FGAIWaypoint* next) { const double calc_bearing = speed < 0?SGMiscd::normalizePeriodic(0, 360, fp->getBearing(pos, curr) + 180.0):fp->getBearing(pos, curr); - if (fgIsFinite(calc_bearing)) { - double hdg_error = calc_bearing - tgt_heading; - if (fabs(hdg_error) > 0.01) { - TurnTo( calc_bearing ); +// when moving forward we want to aim for an average heading + if(next&& speed>0) { + const double calcNextBearing = fp->getBearing(pos, next); + if (fgIsFinite(calc_bearing) && fgIsFinite(calcNextBearing)) { + double averageHeading = calc_bearing + (calcNextBearing-calc_bearing)/2; + averageHeading = SGMiscd::normalizePeriodic(0, 360, averageHeading); + double hdg_error = averageHeading - tgt_heading; + if (fabs(hdg_error) > 0.01) { + TurnTo( averageHeading ); + } + } else { + SG_LOG(SG_AI, SG_WARN, "calc_bearing is not a finite number : " + << "Speed " << speed + << "pos : " << pos.getLatitudeDeg() << ", " << pos.getLongitudeDeg() + << ", waypoint: " << curr->getLatitude() << ", " << curr->getLongitude() ); + SG_LOG(SG_AI, SG_WARN, "waypoint name: '" << curr->getName() << "'" );; } - } else { - cerr << "calc_bearing is not a finite number : " - << "Speed " << speed - << "pos : " << pos.getLatitudeDeg() << ", " << pos.getLongitudeDeg() - << ", waypoint: " << curr->getLatitude() << ", " << curr->getLongitude() << endl; - cerr << "waypoint name: '" << curr->getName() << "'" << endl; + if (fgIsFinite(calc_bearing)) { + double hdg_error = calc_bearing - tgt_heading; + if (fabs(hdg_error) > 0.01) { + TurnTo( calc_bearing ); + } + } else { + SG_LOG(SG_AI, SG_WARN, "calc_bearing is not a finite number : " + << "Speed " << speed + << "pos : " << pos.getLatitudeDeg() << ", " << pos.getLongitudeDeg() + << ", waypoint: " << curr->getLatitude() << ", " << curr->getLongitude() ); + SG_LOG(SG_AI, SG_WARN, "waypoint name: '" << curr->getName() << "'" );; + } } } @@ -1059,7 +1112,7 @@ void FGAIAircraft::controlSpeed(FGAIWaypoint* curr, FGAIWaypoint* next) { if (!curr->contains("END") && !curr->contains("PushBackPointlegend")) { fp->setLeadDistance(speed, tgt_heading, curr, next); } else { - // If we are ending in a parking the heading will be a + // If we are ending in a parking the heading will be a fp->setLeadDistance(1); } } @@ -1153,7 +1206,7 @@ void FGAIAircraft::updateHeading(double dt) { SG_LOG(SG_AI, SG_BULK, "Oh dear " << _callsign << " might get stuck aka next point is behind us. Speed is " << speed ); stuckCounter++; if (stuckCounter>AI_STUCK_LIMIT) { - SG_LOG(SG_AI, SG_WARN, "Stuck flight " << _callsign << " killed" ); + SG_LOG(SG_AI, SG_WARN, "Stuck flight " << _callsign << " killed on leg " << fp->getLeg() << " because point behind"); setDie(true); } } @@ -1189,7 +1242,7 @@ void FGAIAircraft::updateHeading(double dt) { headingChangeRate -= 3 * dt * sign(headingDiff); } /* - if (headingChangeRate > headingDiff || + if (headingChangeRate > headingDiff || headingChangeRate < headingDiff) { headingChangeRate = headingDiff*sign(roll); } @@ -1291,10 +1344,10 @@ void FGAIAircraft::updateVerticalSpeedTarget(double dt) { void FGAIAircraft::updatePitchAngleTarget() { // if on ground and above vRotate -> initial rotation if (onGround() && (speed > _performance->vRotate())) - tgt_pitch = 8.0; // some rough B737 value + tgt_pitch = 8.0; // some rough B737 value //TODO pitch angle on approach and landing - + // match pitch angle to vertical speed else if (tgt_vs > 0) { tgt_pitch = tgt_vs * 0.005; @@ -1310,7 +1363,7 @@ const string& FGAIAircraft::atGate() return fp->getParkingGate()->ident(); } } - + static const string empty; return empty; } @@ -1381,7 +1434,7 @@ bool FGAIAircraft::reachedEndOfCruise(double &distance) { // return true (=done) here, so we don't just get stuck on this forever return true; } - + if (curr->getName() == string("BOD")) { // Sentry: FLIGHTGEAR-893 if (!trafficRef->getArrivalAirport()) { @@ -1390,30 +1443,26 @@ bool FGAIAircraft::reachedEndOfCruise(double &distance) { // return 'done' here, we are broken return true; } - + double dist = fp->getDistanceToGo(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr); double descentSpeed = (getPerformance()->vDescent() * SG_NM_TO_METER) / 3600.0; // convert from kts to meter/s double descentRate = (getPerformance()->descentRate() * SG_FEET_TO_METER) / 60.0; // convert from feet/min to meter/s double verticalDistance = ((altitude_ft - 2000.0) - trafficRef->getArrivalAirport()->getElevation()) *SG_FEET_TO_METER; double descentTimeNeeded = verticalDistance / descentRate; - double distanceCovered = descentSpeed * descentTimeNeeded; + double distanceCovered = descentSpeed * descentTimeNeeded; - if (trafficRef->getCallSign() != "" && - trafficRef->getCallSign() == fgGetString("/ai/track-callsign")) { - cerr << "Checking for end of cruise stage for :" << trafficRef->getCallSign() << endl; - cerr << "Descent rate : " << descentRate << endl; - cerr << "Descent speed : " << descentSpeed << endl; - cerr << "VerticalDistance : " << verticalDistance << ". Altitude : " << altitude_ft << ". Elevation " << trafficRef->getArrivalAirport()->getElevation() << endl; - cerr << "DecentTimeNeeded : " << descentTimeNeeded << endl; - cerr << "DistanceCovered : " << distanceCovered << endl; + if (tracked) { + SG_LOG(SG_AI, SG_DEBUG, "Checking for end of cruise stage for :" << trafficRef->getCallSign()); + SG_LOG(SG_AI, SG_DEBUG, "Descent rate : " << descentRate); + SG_LOG(SG_AI, SG_DEBUG, "Descent speed : " << descentSpeed); + SG_LOG(SG_AI, SG_DEBUG, "VerticalDistance : " << verticalDistance << ". Altitude : " << altitude_ft << ". Elevation " << trafficRef->getArrivalAirport()->getElevation()); + SG_LOG(SG_AI, SG_DEBUG, "DecentTimeNeeded : " << descentTimeNeeded); + SG_LOG(SG_AI, SG_DEBUG, "DistanceCovered : " << distanceCovered); } distance = distanceCovered; if (dist < distanceCovered) { - if (trafficRef->getCallSign() == fgGetString("/ai/track-callsign")) { - //exit(1); - } SG_LOG(SG_AI, SG_BULK, "End Of Cruise"); return true; } else { @@ -1448,7 +1497,7 @@ void FGAIAircraft::resetPositionFromFlightPlan() * Returns a normalised bearing */ -double FGAIAircraft::getBearing(double crse) +double FGAIAircraft::getBearing(double crse) { double hdgDiff = fabs(hdg-crse); if (hdgDiff > 180) @@ -1477,27 +1526,35 @@ time_t FGAIAircraft::checkForArrivalTime(const string& wptName) { } time_t now = globals->get_time_params()->get_cur_time(); time_t arrivalTime = fp->getArrivalTime(); - - time_t ete = tracklength / ((speed * SG_NM_TO_METER) / 3600.0); + + time_t ete = tracklength / ((speed * SG_NM_TO_METER) / 3600.0); time_t secondsToGo = arrivalTime - now; - if (trafficRef->getCallSign() == fgGetString("/ai/track-callsign")) { - cerr << "Checking arrival time: ete " << ete << ". Time to go : " << secondsToGo << ". Track length = " << tracklength << endl; + if (tracked) { + SG_LOG(SG_AI, SG_BULK, "Checking arrival time: ete " << ete << ". Time to go : " << secondsToGo << ". Track length = " << tracklength); } return (ete - secondsToGo); // Positive when we're too slow... } +time_t FGAIAircraft::calcDeparture() { + time_t departure = this->checkForArrivalTime("DepartureHold"); + if(!departure) { + departure = globals->get_time_params()->get_cur_time(); + } + return departure; +} + double limitRateOfChange(double cur, double target, double maxDeltaSec, double dt) { double delta = target - cur; double maxDelta = maxDeltaSec * dt; - + // if delta is > maxDelta, use maxDelta, but with the sign of delta. return (fabs(delta) < maxDelta) ? delta : copysign(maxDelta, delta); } // Drive various properties in a semi-realistic fashion. -// Note that we assume that the properties are set at -// a waypoint rather than in the leg before. So we need +// Note that we assume that the properties are set at +// a waypoint rather than in the leg before. So we need // to use the previous waypoint (i.e. the one just passed) // rather than the current one (i.e. the next one on the route) void FGAIAircraft::updateModelProperties(double dt) @@ -1505,26 +1562,26 @@ void FGAIAircraft::updateModelProperties(double dt) if ((!fp) || (!fp->getPreviousWaypoint())) { return; } - + double targetGearPos = fp->getPreviousWaypoint()->getGear_down() ? 1.0 : 0.0; double gearPos = GearPos(); - + if (gearPos != targetGearPos) { gearPos = gearPos + limitRateOfChange(gearPos, targetGearPos, 0.1, dt); if (gearPos < 0.001) { gearPos = 0.0; } else if (gearPos > 0.999) { - gearPos = 1.0; + gearPos = 1.0; } GearPos(gearPos); - } + } double targetFlapsPos = fp->getPreviousWaypoint()->getFlaps(); double flapsPos = FlapsPos(); if (flapsPos != targetFlapsPos) { flapsPos = flapsPos + limitRateOfChange(flapsPos, targetFlapsPos, 0.05, dt); - if (flapsPos < 0.001) { + if (flapsPos < 0.001) { flapsPos = 0.0; } else if (flapsPos > 0.999) { flapsPos = 1.0; @@ -1542,103 +1599,119 @@ void FGAIAircraft::updateModelProperties(double dt) TaxiLight(fp->getPreviousWaypoint()->getTaxiLight()); } -void FGAIAircraft::dumpCSVHeader(std::ofstream& o) { - o << "Index\t"; - o << "Lat\t"; - o << "Lon\t"; - o << "Callsign\t"; - o << "headingDiff\t"; - o << "headingChangeRate\t"; - o << "headingError\t"; - o << "hdg\t"; - o << "tgt_heading\t"; - o << "tgt_speed\t"; - o << "minBearing\t"; - o << "speedFraction\t"; - o << "groundOffset\t"; - o << "speed\t"; - o << "groundTargetSpeed\t"; - o << "getVerticalSpeedFPM\t"; - o << "getTrueHeadingDeg\t"; - o << "bearingToWP\t"; +void FGAIAircraft::dumpCSVHeader(std::unique_ptr &o) { + (*o) << "Index\t"; + (*o) << "Lat\t"; + (*o) << "Lon\t"; + (*o) << "Callsign\t"; + (*o) << "headingDiff\t"; + (*o) << "headingChangeRate\t"; + (*o) << "headingError\t"; + (*o) << "hdg\t"; + (*o) << "tgt_heading\t"; + (*o) << "tgt_speed\t"; + (*o) << "minBearing\t"; + (*o) << "speedFraction\t"; + (*o) << "groundOffset\t"; + (*o) << "speed\t"; + (*o) << "groundTargetSpeed\t"; + (*o) << "getVerticalSpeedFPM\t"; + (*o) << "getTrueHeadingDeg\t"; + (*o) << "bearingToWP\t"; - o << "Name\t"; - o << "WP Lat\t"; - o << "WP Lon\t"; - o << "Dist\t"; - o << "Next Lat\t"; - o << "Next Lon\t"; - o << "Departuretime\t"; - o << "Time\t"; - o << "Startup diff\t"; - o << "dist_to_go_m\t"; - o << "Leaddistance\t"; - o << "Leg\t"; - o << "Num WP\t"; - o << "no_roll\t"; - o << "roll\t"; - o << "stuckCounter"; - o << endl; + (*o) << "Name\t"; + (*o) << "WP Lat\t"; + (*o) << "WP Lon\t"; + (*o) << "Dist\t"; + (*o) << "Next Lat\t"; + (*o) << "Next Lon\t"; + (*o) << "Departuretime\t"; + (*o) << "Time\t"; + (*o) << "Startup diff\t"; + (*o) << "Departure\t"; + (*o) << "Arrival\t"; + (*o) << "dist_to_go_m\t"; + (*o) << "leadInAngle\t"; + (*o) << "Leaddistance\t"; + (*o) << "Leg\t"; + (*o) << "Num WP\t"; + (*o) << "no_roll\t"; + (*o) << "roll\t"; + (*o) << "repositioned\t"; + (*o) << "stuckCounter"; + (*o) << endl; } -void FGAIAircraft::dumpCSV(std::ofstream& o, int lineIndex) { +void FGAIAircraft::dumpCSV(std::unique_ptr &o, int lineIndex) { const double headingDiff = SGMiscd::normalizePeriodic(-180, 180, hdg-tgt_heading); - o << lineIndex << "\t"; - o << setprecision(12); - o << this->getGeodPos().getLatitudeDeg() << "\t"; - o << this->getGeodPos().getLongitudeDeg() << "\t"; - o << this->getCallSign() << "\t"; - o << headingDiff << "\t"; - o << headingChangeRate << "\t"; - o << headingError << "\t"; - o << hdg << "\t"; - o << tgt_heading << "\t"; - o << tgt_speed << "\t"; - o << minBearing << "\t"; - o << speedFraction << "\t"; - o << groundOffset << "\t"; + (*o) << lineIndex << "\t"; + (*o) << setprecision(12); + (*o) << this->getGeodPos().getLatitudeDeg() << "\t"; + (*o) << this->getGeodPos().getLongitudeDeg() << "\t"; + (*o) << this->getCallSign() << "\t"; + (*o) << headingDiff << "\t"; + (*o) << headingChangeRate << "\t"; + (*o) << headingError << "\t"; + (*o) << hdg << "\t"; + (*o) << tgt_heading << "\t"; + (*o) << tgt_speed << "\t"; + (*o) << minBearing << "\t"; + (*o) << speedFraction << "\t"; + (*o) << groundOffset << "\t"; - o << round(this->getSpeed()) << "\t"; - o << groundTargetSpeed << "\t"; - o << round(this->getVerticalSpeedFPM()) << "\t"; - o << this->getTrueHeadingDeg() << "\t"; + (*o) << round(this->getSpeed()) << "\t"; + (*o) << groundTargetSpeed << "\t"; + (*o) << round(this->getVerticalSpeedFPM()) << "\t"; + (*o) << this->getTrueHeadingDeg() << "\t"; FGAIFlightPlan* fp = this->GetFlightPlan(); - FGAIWaypoint* currentWP = this->GetFlightPlan()->getCurrentWaypoint(); - FGAIWaypoint* nextWP = this->GetFlightPlan()->getNextWaypoint(); - if (currentWP) { - o << this->GetFlightPlan()->getBearing(this->getGeodPos(), this->GetFlightPlan()->getCurrentWaypoint()) << "\t"; - o << currentWP->getName() << "\t"; - o << currentWP->getPos().getLatitudeDeg() << "\t"; - o << currentWP->getPos().getLongitudeDeg() << "\t"; + if(fp) { + FGAIWaypoint* currentWP = fp->getCurrentWaypoint(); + FGAIWaypoint* nextWP = fp->getNextWaypoint(); + if (currentWP) { + (*o) << fp->getBearing(this->getGeodPos(), currentWP) << "\t"; + (*o) << currentWP->getName() << "\t"; + (*o) << currentWP->getPos().getLatitudeDeg() << "\t"; + (*o) << currentWP->getPos().getLongitudeDeg() << "\t"; + } + if (nextWP) { + (*o) << nextWP->getPos().getLatitudeDeg() << "\t"; + (*o) << nextWP->getPos().getLongitudeDeg() << "\t"; + } else { + (*o) << "\t\t"; + } + if (currentWP) { + (*o) << SGGeodesy::distanceM(this->getGeodPos(), currentWP->getPos()) << "\t"; + (*o) << fp->getStartTime() << "\t"; + (*o) << globals->get_time_params()->get_cur_time() << "\t"; + (*o) << fp->getStartTime() - globals->get_time_params()->get_cur_time() << "\t"; + (*o) << (fp->departureAirport().get()?fp->departureAirport().get()->getId():"") << "\t"; + (*o) << (fp->arrivalAirport().get()?fp->arrivalAirport().get()->getId():"") << "\t"; + if(nextWP) { + double dist_to_go_m = fp->getDistanceToGo(pos.getLatitudeDeg(), pos.getLongitudeDeg(), currentWP); + (*o) << dist_to_go_m << "\t"; + double inbound = bearing; + double outbound = fp->getBearing(currentWP, nextWP); + double leadInAngle = fabs(inbound - outbound); + if (leadInAngle > 180.0) leadInAngle = 360.0 - leadInAngle; + (*o) << leadInAngle << "\t"; + } + } else { + (*o) << "No WP\t\t\t\t\t\t\t\t"; + } + if (fp->isValidPlan()) { + (*o) << fp->getLeadDistance() * SG_FEET_TO_METER << "\t"; + (*o) << fp->getLeg() << "\t"; + (*o) << fp->getNrOfWayPoints() << "\t"; + } else { + (*o) << "FP NotValid\t\t"; + } } - if (nextWP) { - o << nextWP->getPos().getLatitudeDeg() << "\t"; - o << nextWP->getPos().getLongitudeDeg() << "\t"; - } else { - o << "\t\t"; - } - if (currentWP) { - o << SGGeodesy::distanceM(this->getGeodPos(), currentWP->getPos()) << "\t"; - o << this->GetFlightPlan()->getStartTime() << "\t"; - o << globals->get_time_params()->get_cur_time() << "\t"; - o << this->GetFlightPlan()->getStartTime() - globals->get_time_params()->get_cur_time() << "\t"; - double dist_to_go_m = fp->getDistanceToGo(pos.getLatitudeDeg(), pos.getLongitudeDeg(), currentWP); - o << dist_to_go_m << "\t"; - } else { - o << "No WP\t\t\t\t\t\t\t\t"; - } - if (fp->isValidPlan()) { - o << fp->getLeadDistance() * SG_FEET_TO_METER << "\t"; - o << fp->getLeg() << "\t"; - o << fp->getNrOfWayPoints() << "\t"; - } else { - o << "FP NotValid\t\t"; - } - o << this->onGround() << "\t"; - o << roll << "\t"; - o << stuckCounter; - o << endl; + (*o) << this->onGround() << "\t"; + (*o) << roll << "\t"; + (*o) << repositioned << "\t"; + (*o) << stuckCounter; + (*o) << endl; } #if 0 diff --git a/src/AIModel/AIAircraft.hxx b/src/AIModel/AIAircraft.hxx index 7103c4c0a..0e6fe5d9e 100644 --- a/src/AIModel/AIAircraft.hxx +++ b/src/AIModel/AIAircraft.hxx @@ -32,6 +32,23 @@ class FGAIFlightPlan; class FGATCController; class FGATCInstruction; class FGAIWaypoint; +class sg_ofstream; + +namespace AILeg +{ + enum Type + { + STARTUP_PUSHBACK = 1, + TAXI = 2, + TAKEOFF = 3, + CLIMB = 4, + CRUISE = 5, + APPROACH = 6, + LANDING = 7, + PARKING_TAXI = 8, + PARKING = 9 + }; +} class FGAIAircraft : public FGAIBaseAircraft { @@ -57,7 +74,8 @@ public: FGAIFlightPlan* GetFlightPlan() const { return fp.get(); }; void ProcessFlightPlan( double dt, time_t now ); time_t checkForArrivalTime(const std::string& wptName); - + time_t calcDeparture(); + void AccelTo(double speed); void PitchTo(double angle); void RollTo(double angle); @@ -90,6 +108,9 @@ public: void setTrafficRef(FGAISchedule *ref) { trafficRef = ref; }; void resetTakeOffStatus() { takeOffStatus = 0;}; void setTakeOffStatus(int status) { takeOffStatus = status; }; + int getTakeOffStatus() { return takeOffStatus; }; + void setTakeOffSlot(time_t timeSlot) { takeOffTimeSlot = timeSlot;}; + time_t getTakeOffSlot(){return takeOffTimeSlot;}; void scheduleForATCTowerDepartureControl(int state); const std::string& GetTransponderCode() { return transponderCode; }; @@ -107,8 +128,6 @@ public: inline double airspeed() const { return props->getFloatValue("velocities/airspeed-kt");}; const std::string& atGate(); std::string acwakecategory; - - int getTakeOffStatus() { return takeOffStatus; }; void checkTcas(); double calcVerticalSpeed(double vert_ft, double dist_m, double speed, double error); @@ -116,9 +135,9 @@ public: FGATCController * getATCController() { return controller; }; void clearATCController(); - void dumpCSVHeader(std::ofstream& o); - void dumpCSV(std::ofstream& o, int lineIndex); - + bool isBlockedBy(FGAIAircraft* other); + void dumpCSVHeader(std::unique_ptr &o); + void dumpCSV(std::unique_ptr &o, int lineIndex); protected: void Run(double dt); @@ -155,7 +174,8 @@ private: bool handleAirportEndPoints(FGAIWaypoint* prev, time_t now); bool reachedEndOfCruise(double&); bool aiTrafficVisible(void); - void controlHeading(FGAIWaypoint* curr); + void controlHeading(FGAIWaypoint* curr, + FGAIWaypoint* next); void controlSpeed(FGAIWaypoint* curr, FGAIWaypoint* next); void updatePrimaryTargetValues(double dt, bool& flightplanActive, bool& aiOutOfSight); @@ -196,7 +216,7 @@ private: /**Kills a flight when it's stuck */ const int AI_STUCK_LIMIT = 100; int stuckCounter = 0; - + bool tracked = false; /** * Signals a reset to leg 1 at a different airport. * The leg loading happens at a different place than the parking loading. @@ -211,7 +231,8 @@ private: bool needsTaxiClearance = false; bool _needsGroundElevation = true; - int takeOffStatus; // 1 = joined departure queue; 2 = Passed DepartureHold waypoint; handover control to tower; 0 = any other state. + int takeOffStatus; // 1 = joined departure queue; 2 = Passed DepartureHold waypoint; handover control to tower; 0 = any other state. + time_t takeOffTimeSlot; time_t timeElapsed; PerformanceData* _performance; // the performance data for this aircraft @@ -235,4 +256,7 @@ private: _controlsTargetAltitude, _controlsTargetPitch, _controlsTargetSpeed; -}; + + std::unique_ptr csvFile; + long csvIndex; +}; \ No newline at end of file diff --git a/src/AIModel/AIFlightPlan.cxx b/src/AIModel/AIFlightPlan.cxx index 205b816f5..0604a4db4 100644 --- a/src/AIModel/AIFlightPlan.cxx +++ b/src/AIModel/AIFlightPlan.cxx @@ -432,23 +432,30 @@ double FGAIFlightPlan::getDistanceToGo(double lat, double lon, FGAIWaypoint* wp) } // sets distance in feet from a lead point to the current waypoint +// basically a catch radius, that triggers the advancement of WPs void FGAIFlightPlan::setLeadDistance(double speed, double bearing, FGAIWaypoint* current, FGAIWaypoint* next){ - double turn_radius; + double turn_radius_m; // Handle Ground steering // At a turn rate of 30 degrees per second, it takes 12 seconds to do a full 360 degree turn // So, to get an estimate of the turn radius, calculate the cicumference of the circle // we travel on. Get the turn radius by dividing by PI (*2). // FIXME Why when going backwards? No fabs if (speed < 0.5) { - lead_distance_ft = 0.5; - return; + lead_distance_ft = 0.5; + return; + } + if (speed > 0 && speed < 0.5) { + lead_distance_ft = 5 * SG_FEET_TO_METER; + SG_LOG(SG_AI, SG_BULK, "Setting Leaddistance fixed " << (lead_distance_ft*SG_FEET_TO_METER)); + return; } + double speed_mps = speed * SG_KT_TO_MPS; if (speed < 25) { - turn_radius = ((360/30)*fabs(speed)) / (2*M_PI); + turn_radius_m = ((360/30)*fabs(speed_mps)) / (2*M_PI); } else { - turn_radius = 0.1911 * speed * speed; // an estimate for 25 degrees bank + turn_radius_m = 0.1911 * speed * speed; // an estimate for 25 degrees bank } double inbound = bearing; @@ -459,9 +466,12 @@ void FGAIFlightPlan::setLeadDistance(double speed, double bearing, // leadInAngle = 30.0; //lead_distance_ft = turn_radius * sin(leadInAngle * SG_DEGREES_TO_RADIANS); - lead_distance_ft = turn_radius * tan((leadInAngle * SG_DEGREES_TO_RADIANS)/2); + + double lead_distance_m = turn_radius_m * tan((leadInAngle * SG_DEGREES_TO_RADIANS)/2); + lead_distance_ft = lead_distance_m * SG_METER_TO_FEET; + SG_LOG(SG_AI, SG_BULK, "Setting Leaddistance " << (lead_distance_ft*SG_FEET_TO_METER) << " Turnradius " << turn_radius_m << " Speed " << speed_mps << " Half turn Angle " << (leadInAngle)/2); if (lead_distance_ft > 1000) { - SG_LOG(SG_AI, SG_BULK, "Excessive leaddistance possible direction change " << lead_distance_ft << " leadInAngle " << leadInAngle << " inbound " << inbound << " outbound " << outbound); + SG_LOG(SG_AI, SG_BULK, "Excessive leaddistance possible direction change " << lead_distance_ft << " leadInAngle " << leadInAngle << " inbound " << inbound << " outbound " << outbound); } /* if ((lead_distance_ft > (3*turn_radius)) && (current->on_ground == false)) { diff --git a/src/AIModel/AIFlightPlan.hxx b/src/AIModel/AIFlightPlan.hxx index 270f0c830..78befb9a1 100644 --- a/src/AIModel/AIFlightPlan.hxx +++ b/src/AIModel/AIFlightPlan.hxx @@ -174,7 +174,9 @@ public: double getDistanceToGo(double lat, double lon, FGAIWaypoint* wp) const; int getLeg () const { return leg;}; + /** Set lead_distance_ft*/ void setLeadDistance(double speed, double bearing, FGAIWaypoint* current, FGAIWaypoint* next); + /** Set lead_distance_ft*/ void setLeadDistance(double distance_ft); double getLeadDistance( void ) const {return lead_distance_ft;} double getBearing(FGAIWaypoint* previous, FGAIWaypoint* next) const; @@ -272,6 +274,8 @@ private: void eraseLastWaypoint(); void pushBackWaypoint(FGAIWaypoint *wpt); + /**Create an arc flightplan around a center from startAngle to endAngle.*/ + void createArc(FGAIAircraft *ac, const SGGeod& center, int startAngle, int endAngle, int increment, int radius, double aElev, double aSpeed, const char* pattern); bool createLandingTaxi(FGAIAircraft *, FGAirport *apt, double radius, const std::string& fltType, const std::string& acType, const std::string& airline); void createDefaultLandingTaxi(FGAIAircraft *, FGAirport* aAirport); void createDefaultTakeoffTaxi(FGAIAircraft *, FGAirport* aAirport, FGRunway* aRunway); diff --git a/src/AIModel/AIFlightPlanCreate.cxx b/src/AIModel/AIFlightPlanCreate.cxx index 08183acae..746e4e32e 100644 --- a/src/AIModel/AIFlightPlanCreate.cxx +++ b/src/AIModel/AIFlightPlanCreate.cxx @@ -163,6 +163,30 @@ FGAIWaypoint * FGAIFlightPlan::createOnRunway(FGAIAircraft * ac, return wpt; } +void FGAIFlightPlan::createArc(FGAIAircraft *ac, const SGGeod& center, int startAngle, +int endAngle, int increment, int radius, double aElev, double aSpeed, const char* pattern) { + double trackSegmentLength = (2 * M_PI * radius) / 360.0; + double dummyAz2; + char buffer[20]; + if (endAngle > startAngle && increment<0) { + endAngle -= 360; + } + if (endAngle < startAngle && increment>0) { + endAngle += 360; + } + + for (int i = startAngle; i != endAngle; i += increment) { + SGGeod result; + SGGeodesy::direct(center, i, + radius, result, dummyAz2); + snprintf(buffer, sizeof(buffer), pattern, i); + FGAIWaypoint *wpt = createInAir(ac, buffer, result, aElev, aSpeed); + wpt->setCrossat(aElev); + wpt->setTrackLength(trackSegmentLength); + pushBackWaypoint(wpt); + } +} + FGAIWaypoint * FGAIFlightPlan::createInAir(FGAIAircraft * ac, const std::string & aName, const SGGeod & aPos, double aElev, @@ -292,7 +316,7 @@ bool FGAIFlightPlan::createTakeoffTaxi(FGAIAircraft * ac, bool firstFlight, FGTaxiNodeRef runwayNode; if (gn->getVersion() > 0) { - runwayNode = gn->findNearestNodeOnRunway(runwayTakeoff); + runwayNode = gn->findNearestNodeOnRunwayEntry(runwayTakeoff); } else { runwayNode = gn->findNearestNode(runwayTakeoff); } @@ -390,7 +414,7 @@ bool FGAIFlightPlan::createTakeoffTaxi(FGAIAircraft * ac, bool firstFlight, } double accell_point = 105.0; // Acceleration point, 105 meters into the runway, - SGGeod entryPoint = waypoints.back()->getPos(); + SGGeod entryPoint = waypoints.back()->getPos(); SGGeod runwayEnd = rwy->pointOnCenterlineDisplaced(0); double distM = SGGeodesy::distanceM(entryPoint, runwayEnd); if (distM > accell_point) { @@ -431,7 +455,8 @@ void FGAIFlightPlan::createDefaultLandingTaxi(FGAIAircraft * ac, } } -bool FGAIFlightPlan::createLandingTaxi(FGAIAircraft * ac, FGAirport * apt, +bool FGAIFlightPlan::createLandingTaxi(FGAIAircraft * ac, + FGAirport * apt, double radius, const string & fltType, const string & acType, @@ -449,9 +474,10 @@ bool FGAIFlightPlan::createLandingTaxi(FGAIAircraft * ac, FGAirport * apt, return true; } + FGRunway * rwy = apt->getRunwayByIdent(activeRunway); FGTaxiNodeRef runwayNode; if (gn->getVersion() == 1) { - runwayNode = gn->findNearestNodeOnRunway(lastWptPos); + runwayNode = gn->findNearestNodeOnRunwayExit(lastWptPos, rwy); } else { runwayNode = gn->findNearestNode(lastWptPos); } @@ -460,8 +486,9 @@ bool FGAIFlightPlan::createLandingTaxi(FGAIAircraft * ac, FGAirport * apt, // fallback mechanism for this. // Starting from gate 0 doesn't work, so don't try it FGTaxiRoute taxiRoute; - if (runwayNode && gate.isValid()) + if (runwayNode && gate.isValid()) { taxiRoute = gn->findShortestRoute(runwayNode, gate.parking()); + } if (taxiRoute.empty()) { createDefaultLandingTaxi(ac, apt); @@ -471,7 +498,8 @@ bool FGAIFlightPlan::createLandingTaxi(FGAIAircraft * ac, FGAirport * apt, FGTaxiNodeRef node; taxiRoute.first(); int size = taxiRoute.size(); - // Omit the last two waypoints, as + //FIXME find better solution than fixed number of 2 + // Omit the last two waypoints, as // those are created by createParking() // int route; for (int i = 0; i < size - 2; i++) { @@ -483,7 +511,15 @@ bool FGAIFlightPlan::createLandingTaxi(FGAIAircraft * ac, FGAirport * apt, ac->getPerformance()->vTaxi()); wpt->setRouteIndex(route); - if( !waypoints.back() || SGGeodesy::distanceM( waypoints.back()->getPos(), wpt->getPos()) > 0 ) { + // next WPT must be far enough + if (!waypoints.back() || SGGeodesy::distanceM(waypoints.back()->getPos(), wpt->getPos()) > 0 ) { + if (waypoints.back()) { + int dist = SGGeodesy::distanceM(waypoints.back()->getPos(), wpt->getPos()); + // pretty near better set speed down + if (dist<10) { + wpt->setSpeed(ac->getPerformance()->vTaxi()/2); + } + } pushBackWaypoint(wpt); } } @@ -496,7 +532,8 @@ static double accelDistance(double v0, double v1, double accel) { double t = fabs(v1 - v0) / accel; // time in seconds to change velocity // area under the v/t graph: (t * v0) + (dV / 2t) where (dV = v1 - v0) - return t * 0.5 * (v1 + v0); + SG_LOG(SG_AI, SG_BULK, "Brakingtime " << t ); + return t * 0.5 * (v1 + v0); } // find the horizontal distance to gain the specific altiude, holding @@ -519,7 +556,7 @@ static double pitchDistance(double pitchAngleDeg, double altGainM) * more likely however. * ******************************************************************/ -bool FGAIFlightPlan::createTakeOff(FGAIAircraft * ac, +bool FGAIFlightPlan::createTakeOff(FGAIAircraft * ac, bool firstFlight, FGAirport * apt, const SGGeod& pos, @@ -577,7 +614,7 @@ bool FGAIFlightPlan::createTakeOff(FGAIAircraft * ac, accell_point += distM; } } - + // distance from the runway threshold to accelerate to rotation speed. double d = accelDistance(vTaxiMetric, vRotateMetric, accelMetric) + accell_point; SGGeod rotatePoint = rwy->pointOnCenterlineDisplaced(d); @@ -716,12 +753,6 @@ bool FGAIFlightPlan::createDescent(FGAIAircraft * ac, FGAirport * apt, // To prevent absurdly steep approaches, compute the origin from where the approach should have started SGGeod origin; - if (ac->getTrafficRef()->getCallSign() == - fgGetString("/ai/track-callsign")) { - //cerr << "Reposition information: Actual distance " << distance << ". required distance " << requiredDistance << endl; - //exit(1); - } - if (distance < requiredDistance * 0.8) { reposition = true; SGGeodesy::direct(initialTarget, azimuth, @@ -730,7 +761,11 @@ bool FGAIFlightPlan::createDescent(FGAIAircraft * ac, FGAirport * apt, distance = SGGeodesy::distanceM(current, initialTarget); azimuth = SGGeodesy::courseDeg(current, initialTarget); } else { - origin = current; + // We project a new point out of line so the aircraft has room to turn to new heading. + origin = SGGeodesy::direct(current, ac->getTrueHeadingDeg(), initialTurnRadius); + distance = SGGeodesy::distanceM(origin, initialTarget); + azimuth = SGGeodesy::courseDeg(origin, initialTarget); + origin = SGGeodesy::direct(current, azimuth, initialTurnRadius); } double dAlt = 0; // = alt - (apt->getElevation() + 2000); @@ -818,16 +853,24 @@ bool FGAIFlightPlan::createDescent(FGAIAircraft * ac, FGAirport * apt, arrivalTime = newEta; time_t additionalTimeNeeded = newEta - eta; + SG_LOG(SG_AI, SG_BULK, "Additional time required " << additionalTimeNeeded << " "); + + //Number of holds + int holdsPatterns = (additionalTimeNeeded-additionalTimeNeeded%240)/240; + + additionalTimeNeeded -= holdsPatterns*240; + double distanceCovered = ((vApproach * SG_NM_TO_METER) / 3600.0) * additionalTimeNeeded; distanceOut += distanceCovered; SGGeod secondaryTarget = rwy->pointOffCenterline(-distanceOut, lateralOffset); + SGGeod secondHoldCenter = + rwy->pointOffCenterline(-2*distanceOut, lateralOffset); initialTarget = rwy->pointOnCenterline(-distanceOut); - distance = SGGeodesy::distanceM(origin, secondaryTarget); azimuth = SGGeodesy::courseDeg(origin, secondaryTarget); - + distance = SGGeodesy::distanceM(origin, secondaryTarget); double ratio = initialTurnRadius / distance; if (ratio > 1.0) ratio = 1.0; @@ -900,20 +943,22 @@ bool FGAIFlightPlan::createDescent(FGAIAircraft * ac, FGAirport * apt, currentAltitude = apt->getElevation() + 2000; } - double trackLength = (2 * M_PI * initialTurnRadius) / 360.0; - for (int i = startval; i != endval; i += increment) { - SGGeod result; - //double currentAltitude = apt->getElevation() + 2000; - - SGGeodesy::direct(secondaryTarget, i, - initialTurnRadius, result, dummyAz2); - snprintf(buffer, sizeof(buffer), "turn%03d", i); - wpt = createInAir(ac, buffer, result, currentAltitude, vDescent); - wpt->setCrossat(currentAltitude); - wpt->setTrackLength(trackLength); - //cerr << "Track Length : " << wpt->trackLength; - pushBackWaypoint(wpt); - //cerr << " Position : " << result.getLatitudeDeg() << " " << result.getLongitudeDeg() << " " << currentAltitude << endl; + + // Length of + if (holdsPatterns>0) { + // Turn into hold + createArc(ac, secondaryTarget, startval, endval, increment, initialTurnRadius, + currentAltitude, vDescent, "turnintohold%03d"); + for (int t = 0; t < holdsPatterns; t++) + { + createArc(ac, secondaryTarget, endval, endval+180, increment, initialTurnRadius, + currentAltitude, vDescent, "hold_1_%03d"); + createArc(ac, secondHoldCenter, endval+180, endval, increment, initialTurnRadius, + currentAltitude, vDescent, "hold_2_%03d"); + } + } else { + createArc(ac, secondaryTarget, startval, endval, increment, initialTurnRadius, + currentAltitude, vDescent, "turn%03d"); } @@ -1044,10 +1089,10 @@ bool FGAIFlightPlan::createLanding(FGAIAircraft * ac, FGAirport * apt, double rolloutDistance = accelDistance(vTouchdownMetric, vTaxiMetric, decelMetric); - int nPoints = (int)(rolloutDistance/30); - for (int i = 1; i < nPoints; i++) { - snprintf(buffer, sizeof(buffer), "landing%03d", i); - double t = ((double) i) / nPoints; + int nPoints = (int)(rolloutDistance/60); + for (int i = 1; i <= nPoints; i++) { + snprintf(buffer, sizeof(buffer), "rollout%03d", i); + double t = 1-pow((double) (nPoints - i), 2) / pow(nPoints, 2); coord = rwy->pointOnCenterline(touchdownDistance + (rolloutDistance * t)); double vel = (vTouchdownMetric * (1.0 - t)) + (vTaxiMetric * t); wpt = createOnRunway(ac, buffer, coord, currElev, vel); @@ -1070,7 +1115,7 @@ bool FGAIFlightPlan::createLanding(FGAIAircraft * ac, FGAirport * apt, coord = rwy->pointOnCenterline(mindist); FGTaxiNodeRef tn; if (gn->getVersion() > 0) { - tn = gn->findNearestNodeOnRunway(coord, rwy); + tn = gn->findNearestNodeOnRunwayExit(coord, rwy); } else { tn = gn->findNearestNode(coord); } @@ -1087,7 +1132,7 @@ bool FGAIFlightPlan::createLanding(FGAIAircraft * ac, FGAirport * apt, } /******************************************************************* - * CreateParking + * createParking Leg 9 * initialize the Aircraft at the parking location ******************************************************************/ bool FGAIFlightPlan::createParking(FGAIAircraft * ac, FGAirport * apt, @@ -1098,30 +1143,42 @@ bool FGAIFlightPlan::createParking(FGAIAircraft * ac, FGAirport * apt, double vTaxi = ac->getPerformance()->vTaxi(); double vTaxiReduced = vTaxi * (2.0 / 3.0); if (!gate.isValid()) { - wpt = createOnGround(ac, "END-Parking", apt->geod(), aptElev, + wpt = createOnGround(ac, "END-ParkingInvalidGate", apt->geod(), aptElev, vTaxiReduced); pushBackWaypoint(wpt); return true; } FGParking* parking = gate.parking(); - double heading = SGMiscd::normalizePeriodic(0, 360, parking->getHeading() + 180.0); + double reverseHeading = SGMiscd::normalizePeriodic(0, 360, parking->getHeading() + 180.0); double az; // unused SGGeod pos; - SGGeodesy::direct(parking->geod(), heading, 2.2 * parking->getRadius(), + SGGeodesy::direct(parking->geod(), reverseHeading, 2 * parking->getRadius(), pos, az); - wpt = createOnGround(ac, "taxiStart", pos, aptElev, vTaxiReduced); + wpt = createOnGround(ac, "parking", pos, aptElev, vTaxiReduced/2); pushBackWaypoint(wpt); - SGGeodesy::direct(parking->geod(), heading, 0.1 * parking->getRadius(), + SGGeodesy::direct(parking->geod(), reverseHeading, 1.5 * parking->getRadius(), pos, az); - wpt = createOnGround(ac, "taxiStart2", pos, aptElev, vTaxiReduced/2); + wpt = createOnGround(ac, "parking2", pos, aptElev, vTaxiReduced/3); pushBackWaypoint(wpt); - wpt = createOnGround(ac, "END-Parking", parking->geod(), aptElev, - vTaxiReduced/3); + SGGeodesy::direct(parking->geod(), reverseHeading, parking->getRadius(), + pos, az); + wpt = createOnGround(ac, "parking3", pos, aptElev, vTaxiReduced/3); + pushBackWaypoint(wpt); + + char buffer[30]; + snprintf(buffer, 30, "Parking%s", parking->getName().c_str()); + + wpt = createOnGround(ac, buffer, parking->geod(), aptElev, + 2); + pushBackWaypoint(wpt); + SGGeodesy::direct(parking->geod(), parking->getHeading(), 1, + pos, az); + wpt = createOnGround(ac, "END-Parking", pos, aptElev, vTaxiReduced/3); pushBackWaypoint(wpt); return true; } diff --git a/src/AIModel/AIFlightPlanCreatePushBack.cxx b/src/AIModel/AIFlightPlanCreatePushBack.cxx index e13ae0b18..fe388577e 100644 --- a/src/AIModel/AIFlightPlanCreatePushBack.cxx +++ b/src/AIModel/AIFlightPlanCreatePushBack.cxx @@ -118,7 +118,7 @@ bool FGAIFlightPlan::createPushBack(FGAIAircraft *ac, while (route.next(node, &rte)) { char buffer[20]; - sprintf (buffer, "pushback-%03d", (short)node->getIndex()); + snprintf (buffer, sizeof(buffer), "pushback-%03d", (short)node->getIndex()); FGAIWaypoint *wpt = createOnGround(ac, string(buffer), node->geod(), dep->getElevation(), vTaxiBackward); /* @@ -179,7 +179,7 @@ bool FGAIFlightPlan::createPushBack(FGAIAircraft *ac, SGGeodesy::direct(parking->geod(), parkingHeading, (((double)i / numSegments) * distance), pushForwardPt, az2); char buffer[20]; - sprintf(buffer, "pushforward-%03d", (short)i); + snprintf(buffer, sizeof(buffer), "pushforward-%03d", (short)i); FGAIWaypoint *wpt = createOnGround(ac, string(buffer), pushForwardPt, dep->getElevation(), vTaxiReduced); wpt->setRouteIndex(pushForwardSegment->getIndex()); diff --git a/src/Airports/dynamics.cxx b/src/Airports/dynamics.cxx index 55444b29f..1c3ac104d 100644 --- a/src/Airports/dynamics.cxx +++ b/src/Airports/dynamics.cxx @@ -301,7 +301,8 @@ bool FGAirportDynamics::hasParkings() const return !parent()->groundNetwork()->allParkings().empty(); } -ParkingAssignment FGAirportDynamics::getAvailableParking(double radius, const string & flType, +ParkingAssignment FGAirportDynamics::getAvailableParking(double radius, + const string & flType, const string & acType, const string & airline) { diff --git a/src/Airports/groundnetwork.cxx b/src/Airports/groundnetwork.cxx index 1193821b1..b827088b3 100644 --- a/src/Airports/groundnetwork.cxx +++ b/src/Airports/groundnetwork.cxx @@ -274,10 +274,27 @@ FGTaxiNodeRef FGGroundNetwork::findNearestNodeOffRunway(const SGGeod& aGeod, FGR return *node; } -FGTaxiNodeRef FGGroundNetwork::findNearestNodeOnRunway(const SGGeod & aGeod, FGRunway* aRunway) const +FGTaxiNodeRef FGGroundNetwork::findNearestNodeOnRunwayEntry(const SGGeod & aGeod) const { - SG_UNUSED(aRunway); + double d = DBL_MAX; + SGVec3d cartPos = SGVec3d::fromGeod(aGeod); + FGTaxiNodeRef result; + for (auto it = m_nodes.begin(); it != m_nodes.end(); ++it) { + if (!(*it)->getIsOnRunway()) + continue; + double localDistanceSqr = distSqr(cartPos, (*it)->cart()); + if (localDistanceSqr < d) { + SG_LOG(SG_AI, SG_BULK, "findNearestNodeOnRunway from Threshold " << localDistanceSqr); + d = localDistanceSqr; + result = *it; + } + } + return result; +} + +FGTaxiNodeRef FGGroundNetwork::findNearestNodeOnRunwayExit(const SGGeod & aGeod, FGRunway* aRunway) const +{ double d = DBL_MAX; SGVec3d cartPos = SGVec3d::fromGeod(aGeod); FGTaxiNodeRef result = 0; @@ -289,6 +306,8 @@ FGTaxiNodeRef FGGroundNetwork::findNearestNodeOnRunway(const SGGeod & aGeod, FGR if (aRunway) { double headingTowardsExit = SGGeodesy::courseDeg(aGeod, (*it)->geod()); double diff = fabs(aRunway->headingDeg() - headingTowardsExit); + SG_LOG(SG_AI, SG_BULK, "findNearestNodeOnRunwayExit " << aRunway->headingDeg() << " " + << " Diff : " << diff << " " << (*it)->getIndex()); if (diff > 10) { // Only ahead continue; @@ -301,18 +320,64 @@ FGTaxiNodeRef FGGroundNetwork::findNearestNodeOnRunway(const SGGeod & aGeod, FGR double exitHeading = SGGeodesy::courseDeg((*it)->geod(), (exitSegments.back())->geod()); diff = fabs(aRunway->headingDeg() - exitHeading); - if (diff > 80) { + SG_LOG(SG_AI, SG_BULK, "findNearestNodeOnRunwayExit2 " << diff << " " << (*it)->getIndex()); + if (diff > 70) { // Only exits going in our direction continue; } + } else { + SG_LOG(SG_AI, SG_BULK, "No Runway findNearestNodeOnRunwayExit"); } if (localDistanceSqr < d) { + SG_LOG(SG_AI, SG_BULK, "findNearestNodeOnRunwayExit3 " << localDistanceSqr << " " << (*it)->getIndex()); d = localDistanceSqr; result = *it; } } - - return result; + if(result) { + return result; + } + // Ok then fallback only exits ahead + for (it = m_nodes.begin(); it != m_nodes.end(); ++it) { + if (!(*it)->getIsOnRunway()) + continue; + double localDistanceSqr = distSqr(cartPos, (*it)->cart()); + if (aRunway) { + double headingTowardsExit = SGGeodesy::courseDeg(aGeod, (*it)->geod()); + double diff = fabs(aRunway->headingDeg() - headingTowardsExit); + SG_LOG(SG_AI, SG_BULK, "findNearestNodeOnRunwayExitFallback1 " << aRunway->headingDeg() << " " + << " Diff : " << diff << " " << (*it)->getIndex()); + if (diff > 10) { + // Only ahead + continue; + } + } + if (localDistanceSqr < d) { + SG_LOG(SG_AI, SG_BULK, "findNearestNodeOnRunwayExitFallback1 " << localDistanceSqr); + d = localDistanceSqr; + result = *it; + } + } + if(result) { + return result; + } + // Ok then fallback only exits + for (it = m_nodes.begin(); it != m_nodes.end(); ++it) { + if (!(*it)->getIsOnRunway()) + continue; + double localDistanceSqr = distSqr(cartPos, (*it)->cart()); + if (localDistanceSqr < d) { + SG_LOG(SG_AI, SG_BULK, "findNearestNodeOnRunwayExitFallback2 " << localDistanceSqr); + d = localDistanceSqr; + result = *it; + } + } + if(result) { + return result; + } else { + SG_LOG(SG_AI, SG_WARN, "No Exit found"); + return 0; + } } FGTaxiSegment *FGGroundNetwork::findOppositeSegment(unsigned int index) const diff --git a/src/Airports/groundnetwork.hxx b/src/Airports/groundnetwork.hxx index 27ae53fdf..b38424c1b 100644 --- a/src/Airports/groundnetwork.hxx +++ b/src/Airports/groundnetwork.hxx @@ -247,7 +247,9 @@ public: { return parent; } FGTaxiNodeRef findNearestNode(const SGGeod& aGeod) const; - FGTaxiNodeRef findNearestNodeOnRunway(const SGGeod& aGeod, FGRunway* aRunway = NULL) const; + FGTaxiNodeRef findNearestNodeOnRunwayEntry(const SGGeod& aGeod) const; + /**Returns the nearest node in that is in direction of runway heading. Falls back to ones behind aircraft*/ + FGTaxiNodeRef findNearestNodeOnRunwayExit(const SGGeod& aGeod, FGRunway* aRunway = NULL) const; FGTaxiNodeRef findNearestNodeOffRunway(const SGGeod& aGeod, FGRunway* aRunway, double distanceM) const; diff --git a/test_suite/test_data/YBBN.groundnet.xml b/test_suite/test_data/YBBN.groundnet.xml index bc50bea7f..849f6a317 100644 --- a/test_suite/test_data/YBBN.groundnet.xml +++ b/test_suite/test_data/YBBN.groundnet.xml @@ -1,8 +1,8 @@ 1 - Sat, 20 Feb 2021 20:19:29 GMT by FlightgearAirports 0.0.29 - unknown + Wed, 17 Nov 2021 07:35:49 GMT by FlightgearAirports 0.0.33 + PortreeKid 12435 12470 @@ -14,1214 +14,1210 @@ 12050 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -1230,48 +1226,48 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - + + + + + + + + @@ -1284,106 +1280,106 @@ - - - - - - - - - - - - + + + + + + + + - - - - + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + @@ -1392,328 +1388,328 @@ - - - - - - - - - - - - + + + + + + - - - - - - - - - - - - + + + + + + + + + + + + + + - - - - - - - - + + + + + + + + + + - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + - - + + - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + - - - - - - + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - + + + + + + + + + + + + + + - + - - - - - - - - - - - - - - - - - + + + + + + + + + + - - + + - - - - - - - - - - - + + + + + + + + + + + + + + + + - - - - + + + + + - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + @@ -1724,167 +1720,167 @@ - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + - - - - - - - - - - + + + + + + + + + + + + - + + + + - - - - - + - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - + + + + + + + + - - + + + + + + + - - - - + + + + - - - - + + + + - - - + + + - - - - - + + + + - - - - + + - - - - - - - - - + + + + + + + + + + + + + - - - + - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + @@ -1892,351 +1888,351 @@ - - - - - + + + - - - + + + + + + - - - + + + - - + + - + - + - + - - - - - - - + + - - + + + + + + - - - - + + - - - - - - + + + + - - - - - - - - - - - - + + + + + + - - - - - + + + + + + + + + + + + - - - - + + + + + + + + - + - - + - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + - - + + + - + + - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - + + + + - - - - - - - + + + + + + + + + + + + + + - + - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - + + + + + + + + + + + + + + - + + - - - - - - - - + + + + + - + + - - - - - - + + + + - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + - - - - - - + + + - + + - - - - + + + + + + + - - - + + - - @@ -2247,101 +2243,101 @@ + + - - - - - - - - - - - - + + + + + + + + + + + - - - - - - - + + + + - - + + + + - + + - - - + + + - - + + + + - - - - - - - - - - - + + - - + + + + + + + + - - - - - - + + + + + + + - - - - + + + + + - + - - - + + - + - - + @@ -2362,116 +2358,116 @@ - - - - - - - - - - - + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - + + + + - - - - - - - - - - + + + + + + + + + + @@ -2482,16 +2478,16 @@ - - + + - - + + - - + + @@ -2504,85 +2500,85 @@ - - - - + + + + - - - - - - + + + + + + - - - - - - - + + + + + + + + - - - + + - - - - + + + + - + + - - - + + + + - - - + + - - - - + + + + - - + @@ -2603,47 +2599,47 @@ + + - - - - - - + + - - - - - - - - - - - - + + + + + + + + + + + + + + + - - - + + - - + + @@ -2658,54 +2654,54 @@ + + - - - - - - - - - - - - + + + + + + + + + - + + - - - - - - + + - - - - + + + + + + - - - - - - + + + + + + + + + + - - @@ -2718,54 +2714,54 @@ + + - - - - + + + + - - - - + + - - - - + + + + - - - - - - + + + + + + - - + + @@ -2774,62 +2770,62 @@ - - - - + + + + - - - - + + + + + + - - - - - - + + + + + + - - + + - - - - + + - - - - + + + + + + - - @@ -2846,10 +2842,10 @@ + + - - @@ -2858,10 +2854,10 @@ + - @@ -2871,140 +2867,140 @@ - - - - - - - - - + + + + + + + + + + + - - + - - - - - + + + + - - + + - - + + + + - - + + - - - - - - - - - + + + + + + + - - - - - - - - - - + + + + + + + + + + + - - - - + - - - - - - - + + + + + + + + + - - - - - - - - + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + @@ -3015,10 +3011,10 @@ + + - - @@ -3027,34 +3023,34 @@ - - + + - - + + + + - - - - - - - - + + - - + + + + + + - - - - + + + + @@ -3065,40 +3061,40 @@ - - - - + + + + + - + - - + + - - + - + + - - - - - + + - - + + + + @@ -3107,147 +3103,147 @@ + + - - - - - - - - - - - - - + + + + + + + + + + + + + - - - + + - - - + - - + + + - - - - - - - - + + + + + + - - + + + - - + + - - - - - - + + - - + + + + + + - - - - + + - - + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - + + + + + + - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/test_suite/unit_tests/AI/test_groundnet.cxx b/test_suite/unit_tests/AI/test_groundnet.cxx index cab52bb61..5ef7837a3 100644 --- a/test_suite/unit_tests/AI/test_groundnet.cxx +++ b/test_suite/unit_tests/AI/test_groundnet.cxx @@ -91,7 +91,7 @@ void GroundnetTests::testShortestRoute() FGGroundNetwork* network = egph->groundNetwork(); FGParkingRef startParking = network->findParkingByName("main-apron10"); FGRunwayRef runway = egph->getRunwayByIndex(0); - FGTaxiNodeRef end = network->findNearestNodeOnRunway(runway->threshold()); + FGTaxiNodeRef end = network->findNearestNodeOnRunwayEntry(runway->threshold()); FGTaxiRoute route = network->findShortestRoute(startParking, end); CPPUNIT_ASSERT_EQUAL(true, network->exists()); CPPUNIT_ASSERT_EQUAL(29, route.size()); diff --git a/test_suite/unit_tests/AI/test_traffic.cxx b/test_suite/unit_tests/AI/test_traffic.cxx index 0d7e0be61..ad0deaa42 100644 --- a/test_suite/unit_tests/AI/test_traffic.cxx +++ b/test_suite/unit_tests/AI/test_traffic.cxx @@ -75,8 +75,10 @@ void TrafficTests::setUp() fgSetBool("/sim/terrasync/ai-data-update-now", false); fgSetBool("/sim/sound/atc/enabled", true); fgSetDouble("/instrumentation/comm[0]/frequencies/selected-mhz", 121.70); + fgSetString("/sim/multiplay/callsign", "AI-Shadow"); globals->append_data_path(SGPath::fromUtf8(FG_TEST_SUITE_DATA), false); + globals->set_download_dir(globals->get_fg_home()); // ensure EDDF has a valid ground net for parking testing FGAirport::clearAirportsCache(); @@ -129,7 +131,7 @@ void TrafficTests::testPushback() FGAISchedule* schedule = new FGAISchedule( "B737", "KLM", departureAirport->getId(), "G-BLA", "ID", false, "B737", "KLM", "N", flighttype, radius, 8); - FGScheduledFlight* flight = new FGScheduledFlight("", "", departureAirport->getId(), arrivalAirport->getId(), 24, dep, arr, "WEEK", "HBR_BN_2"); + FGScheduledFlight* flight = new FGScheduledFlight("testPushback", "", departureAirport->getId(), arrivalAirport->getId(), 24, dep, arr, "WEEK", "HBR_BN_2"); schedule->assign(flight); SGSharedPtr aiAircraft = new FGAIAircraft{schedule}; @@ -146,6 +148,7 @@ void TrafficTests::testPushback() aiAircraft->setSpeed(0); aiAircraft->setBank(0); + const string flightPlanName = departureAirport->getId() + "-" + arrivalAirport->getId() + ".xml"; const double crs = SGGeodesy::courseDeg(departureAirport->geod(), arrivalAirport->geod()); // direct course @@ -184,7 +187,7 @@ void TrafficTests::testPushbackCargo() FGAISchedule* schedule = new FGAISchedule( "B737", "KLM", "EGPH", "G-BLA", "ID", false, "B737", "KLM", "N", "cargo", 24, 8); - FGScheduledFlight* flight = new FGScheduledFlight("", "", "EGPH", "EGPF", 24, dep, arr, "WEEK", "HBR_BN_2"); + FGScheduledFlight* flight = new FGScheduledFlight("testPushbackCargo", "", "EGPH", "EGPF", 24, dep, arr, "WEEK", "HBR_BN_2"); schedule->assign(flight); SGSharedPtr aiAircraft = new FGAIAircraft{schedule}; @@ -249,7 +252,7 @@ void TrafficTests::testChangeRunway() FGAISchedule* schedule = new FGAISchedule( "B737", "KLM", departureAirport->getId(), "G-BLA", "ID", false, "B737", "KLM", "N", flighttype, radius, 8); - FGScheduledFlight* flight = new FGScheduledFlight("", "", departureAirport->getId(), arrivalAirport->getId(), 24, dep, arr, "WEEK", "HBR_BN_2"); + FGScheduledFlight* flight = new FGScheduledFlight("testChangeRunway", "", departureAirport->getId(), arrivalAirport->getId(), 24, dep, arr, "WEEK", "HBR_BN_2"); schedule->assign(flight); SGSharedPtr aiAircraft = new FGAIAircraft{schedule}; @@ -307,7 +310,7 @@ void TrafficTests::testPushforward() FGAISchedule* schedule = new FGAISchedule( "B737", "KLM", departureAirport->getId(), "G-BLA", "ID", false, "B737", "KLM", "N", flighttype, radius, 8); - FGScheduledFlight* flight = new FGScheduledFlight("", "", departureAirport->getId(), arrivalAirport->getId(), 24, dep, arr, "WEEK", "HBR_BN_2"); + FGScheduledFlight* flight = new FGScheduledFlight("testPushforward", "", departureAirport->getId(), arrivalAirport->getId(), 24, dep, arr, "WEEK", "HBR_BN_2"); schedule->assign(flight); SGSharedPtr aiAircraft = new FGAIAircraft{schedule}; @@ -364,7 +367,7 @@ void TrafficTests::testPushforwardSpeedy() FGAISchedule* schedule = new FGAISchedule( "B737", "KLM", departureAirport->getId(), "G-BLA", "ID", false, "B737", "KLM", "N", flighttype, radius, 8); - FGScheduledFlight* flight = new FGScheduledFlight("", "", departureAirport->getId(), arrivalAirport->getId(), 24, dep, arr, "WEEK", "HBR_BN_2"); + FGScheduledFlight* flight = new FGScheduledFlight("testPushforwardSpeedy", "", departureAirport->getId(), arrivalAirport->getId(), 24, dep, arr, "WEEK", "HBR_BN_2"); schedule->assign(flight); SGSharedPtr aiAircraft = new FGAIAircraft{schedule}; @@ -422,7 +425,7 @@ void TrafficTests::testPushforwardParkYBBN() FGAISchedule* schedule = new FGAISchedule( "B737", "KLM", departureAirport->getId(), "G-BLA", "ID", false, "B737", "KLM", "N", flighttype, radius, 8); - FGScheduledFlight* flight = new FGScheduledFlight("gaParkYSSY", "", departureAirport->getId(), arrivalAirport->getId(), 24, dep, arr, "WEEK", "HBR_BN_2"); + FGScheduledFlight* flight = new FGScheduledFlight("testPushforwardParkYBBN", "", departureAirport->getId(), arrivalAirport->getId(), 24, dep, arr, "WEEK", "HBR_BN_2"); schedule->assign(flight); SGSharedPtr aiAircraft = new FGAIAircraft{schedule}; @@ -500,10 +503,10 @@ void TrafficTests::testPushforwardParkYBBNRepeatGa() FGAISchedule* schedule = new FGAISchedule( "B737", "KLM", departureAirport->getId(), "G-BLA", "TST_BN_1", false, "B737", "KLM", "N", flighttype, radius, 8); - FGScheduledFlight* flight = new FGScheduledFlight("gaParkYSSY", "VFR", departureAirport->getId(), arrivalAirport->getId(), 24, dep, arr, "WEEK", "TST_BN_1"); + FGScheduledFlight* flight = new FGScheduledFlight("testPushforwardParkYBBNRepeatGa", "VFR", departureAirport->getId(), arrivalAirport->getId(), 24, dep, arr, "WEEK", "TST_BN_1"); schedule->assign(flight); - FGScheduledFlight* returnFlight = new FGScheduledFlight("gaParkYSSY", "", arrivalAirport->getId(), departureAirport->getId(), 24, arr, ret, "WEEK", "TST_BN_1"); + FGScheduledFlight* returnFlight = new FGScheduledFlight("testPushforwardParkYBBNRepeatGa", "", arrivalAirport->getId(), departureAirport->getId(), 24, arr, ret, "WEEK", "TST_BN_1"); schedule->assign(returnFlight); SGSharedPtr aiAircraft = new FGAIAircraft{schedule}; @@ -554,6 +557,102 @@ void TrafficTests::testPushforwardParkYBBNRepeatGa() CPPUNIT_ASSERT_EQUAL(true, (aiAircraft->getDie() || aiAircraft->GetFlightPlan()->getCurrentWaypoint()->getName() == "park")); } +void TrafficTests::testPushforwardParkYBBNRepeatGaDelayed() +{ + FGAirportRef departureAirport = FGAirport::getByIdent("YBBN"); + + FGAirportRef arrivalAirport = FGAirport::getByIdent("YSSY"); + + fgSetString("/sim/presets/airport-id", arrivalAirport->getId()); + + // Time to depart + std::string dep = getTimeString(120); + // Time to arrive + std::string arr = getTimeString(3260); + // Time to arrive back + std::string ret = getTimeString(6460); + + const int radius = 8.0; + const int cruiseAltFt = 32000; + const int cruiseSpeedKnots = 80; + const char* flighttype = "ga"; + + FGAISchedule* schedule = new FGAISchedule( + "B737", "KLM", departureAirport->getId(), "G-BLA", "TST_BN_1", false, "B737", "KLM", "N", flighttype, radius, 8); + FGScheduledFlight* flight = new FGScheduledFlight("testPushforwardParkYBBNRepeatGaDelayed", "VFR", departureAirport->getId(), arrivalAirport->getId(), 24, dep, arr, "WEEK", "TST_BN_1"); + schedule->assign(flight); + + FGScheduledFlight* returnFlight = new FGScheduledFlight("testPushforwardParkYBBNRepeatGaDelayed", "", arrivalAirport->getId(), departureAirport->getId(), 24, arr, ret, "WEEK", "TST_BN_1"); + schedule->assign(returnFlight); + + SGSharedPtr aiAircraft = new FGAIAircraft{schedule}; + + const SGGeod position = departureAirport->geod(); + FGTestApi::setPositionAndStabilise(position); + + aiAircraft->setPerformance("ga", ""); + aiAircraft->setCompany("KLM"); + aiAircraft->setAcType("B737"); + aiAircraft->setSpeed(0); + aiAircraft->setBank(0); + + const string flightPlanName = departureAirport->getId() + "-" + arrivalAirport->getId() + ".xml"; + + const double crs = SGGeodesy::courseDeg(departureAirport->geod(), arrivalAirport->geod()); // direct course + time_t departureTime = globals->get_time_params()->get_cur_time(); + departureTime = departureTime + 90; + + + std::unique_ptr fp(new FGAIFlightPlan(aiAircraft, + flightPlanName, crs, departureTime, + departureAirport, arrivalAirport, true, radius, + cruiseAltFt, // cruise alt + position.getLatitudeDeg(), + position.getLongitudeDeg(), + cruiseSpeedKnots, flighttype, + aiAircraft->getAcType(), + aiAircraft->getCompany())); + CPPUNIT_ASSERT_EQUAL(fp->isValidPlan(), true); + aiAircraft->FGAIBase::setFlightPlan(std::move(fp)); + globals->get_subsystem()->attach(aiAircraft); + + FGAirport* departure = aiAircraft->getTrafficRef()->getDepartureAirport(); + FGAirportDynamicsRef departureDynamics = departure->getDynamics(); + ActiveRunway* activeDepartureRunway = departureDynamics->getApproachController()->getRunway("01"); + time_t newDeparture = activeDepartureRunway->requestTimeSlot(aiAircraft->GetFlightPlan()->getStartTime()); +// See that the wait queue is filled + for (size_t i = 0; i < 100; i++) + { + newDeparture = activeDepartureRunway->requestTimeSlot(newDeparture); + } + + FGAirport* arrival = aiAircraft->getTrafficRef()->getArrivalAirport(); + FGAirportDynamicsRef arrivalDynamics = arrival->getDynamics(); + ActiveRunway* activeYSSYRunway = arrivalDynamics->getApproachController()->getRunway("16R"); + time_t newArrival = activeYSSYRunway->requestTimeSlot(aiAircraft->GetFlightPlan()->getStartTime()); +// See that the wait queue is filled + for (size_t i = 0; i < 100; i++) + { + newArrival = activeYSSYRunway->requestTimeSlot(newArrival); + } + + aiAircraft = flyAI(aiAircraft, "flight_ga_YSSY_YBBN_park_repeat" + std::to_string(departureTime)); + + int shortestDistance = 10000; + const FGParkingList& parkings(arrivalAirport->groundNetwork()->allParkings()); + FGParkingList::const_iterator it; + FGParking* nearestParking = 0; + for (it = parkings.begin(); it != parkings.end(); ++it) { + int currentDistance = !nearestParking ? 9999 : SGGeodesy::distanceM(nearestParking->geod(), (*it)->geod()); + if (currentDistance < shortestDistance) { + nearestParking = (*it); + shortestDistance = currentDistance; + } + } + + CPPUNIT_ASSERT_EQUAL(true, (aiAircraft->getDie() || aiAircraft->GetFlightPlan()->getCurrentWaypoint()->getName() == "park")); +} + void TrafficTests::testPushforwardParkYBBNRepeatGate() { FGAirportRef departureAirport = FGAirport::getByIdent("YBBN"); @@ -569,7 +668,7 @@ void TrafficTests::testPushforwardParkYBBNRepeatGate() // Time to arrive back std::string ret = getTimeString(6460); - const int radius = 20.0; + const int radius = 32.0; const int cruiseAltFt = 32000; const int cruiseSpeedKnots = 80; const char* flighttype = "gate"; @@ -658,7 +757,7 @@ FGAIAircraft * TrafficTests::flyAI(SGSharedPtr aiAircraft, std::st char fname [160]; time_t t = time(0); // get time now - snprintf (fname, sizeof(fname), "%lld.csv", (long long) t); + snprintf (fname, sizeof(fname), "%ld.csv", t); SGPath p = SGPath::desktop() / (testname + fname); sg_ofstream csvFile; csvFile.open(p); diff --git a/test_suite/unit_tests/AI/test_traffic.hxx b/test_suite/unit_tests/AI/test_traffic.hxx index d8cf25ddb..16aa9cbeb 100644 --- a/test_suite/unit_tests/AI/test_traffic.hxx +++ b/test_suite/unit_tests/AI/test_traffic.hxx @@ -42,6 +42,7 @@ class TrafficTests : public CppUnit::TestFixture CPPUNIT_TEST(testPushforwardSpeedy); CPPUNIT_TEST(testPushforwardParkYBBN); CPPUNIT_TEST(testPushforwardParkYBBNRepeatGa); + CPPUNIT_TEST(testPushforwardParkYBBNRepeatGaDelayed); CPPUNIT_TEST(testPushforwardParkYBBNRepeatGate); CPPUNIT_TEST_SUITE_END(); public: @@ -60,6 +61,7 @@ public: void testPushforwardSpeedy(); void testPushforwardParkYBBN(); void testPushforwardParkYBBNRepeatGa(); + void testPushforwardParkYBBNRepeatGaDelayed(); void testPushforwardParkYBBNRepeatGate(); private: long currentWorldTime;