diff --git a/src/AIModel/AIAircraft.cxx b/src/AIModel/AIAircraft.cxx index 44a384629..ae716e755 100644 --- a/src/AIModel/AIAircraft.cxx +++ b/src/AIModel/AIAircraft.cxx @@ -255,9 +255,10 @@ void FGAIAircraft::ClimbTo(double alt_ft ) { void FGAIAircraft::TurnTo(double heading) { if( fabs(heading) < 0.1 ) { - SG_LOG(SG_AI, SG_WARN, "Heading reset"); + SG_LOG(SG_AI, SG_WARN, "Heading reset to zero"); } tgt_heading = heading; + // SG_LOG(SG_AI, SG_BULK, "Turn tgt_heading to " << tgt_heading); hdg_lock = true; } @@ -397,8 +398,11 @@ void FGAIAircraft::ProcessFlightPlan( double dt, time_t now ) { } } - if (next) { + if (next && !curr->contains("END") && !curr->contains("PushBackPointlegend")) { fp->setLeadDistance(tgt_speed, tgt_heading, curr, next); + } else { + // If we are ending in a parking + fp->setLeadDistance(1); } // Calculate a target altitude for any leg in which at least one waypoint is in the air. @@ -768,11 +772,12 @@ void FGAIAircraft::handleFirstWaypoint() { //TODO fp should handle this fp->IncrementWaypoint(eraseWaypoints); - if (!(fp->getNextWaypoint()) && trafficRef) + if (!(fp->getNextWaypoint()) && trafficRef) { if (!loadNextLeg()) { setDie(true); return; } + } prev = fp->getPreviousWaypoint(); //first waypoint SG_LOG(SG_AI, SG_BULK, "Previous WP \t" << prev->getName()); @@ -785,7 +790,11 @@ void FGAIAircraft::handleFirstWaypoint() { setLatitude(prev->getLatitude()); setLongitude(prev->getLongitude()); - setSpeed(prev->getSpeed()); + if(fp->getLeg()==1) { + setSpeed(0); + } else { + setSpeed(prev->getSpeed()); + } setAltitude(prev->getAltitude()); if (prev->getSpeed() > 0.0) @@ -798,8 +807,11 @@ 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 (next) { + // If we are ending in a parking + if (next && !curr->contains("END") && !curr->contains("PushBackPointlegend")) { fp->setLeadDistance(speed, hdg, curr, next); + } else { + fp->setLeadDistance(1); } if (curr->getCrossat() > -1000.0) //use a calculated descent/climb rate @@ -828,7 +840,6 @@ void FGAIAircraft::handleFirstWaypoint() { prevSpeed = 0; } - /** * Check Execution time (currently once every 100 ms) * Add a bit of randomization to prevent the execution of all flight plans @@ -855,18 +866,35 @@ bool FGAIAircraft::leadPointReached(FGAIWaypoint* curr, FGAIWaypoint* next, int double dist_to_go_m = fp->getDistanceToGo(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr); // Leaddistance should be ft double lead_dist = fp->getLeadDistance(); - // experimental: Use fabs, because speed can be negative (I hope) during push_back. - if ((dist_to_go_m < fabs(10.0* speed)) && (speed < 0) && (tgt_speed < 0) && fp->getCurrentWaypoint()->contains("PushBackPoint")) { - tgt_speed = -(dist_to_go_m / 10.0); - if (tgt_speed > -0.5) { - tgt_speed = -0.5; + 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")) { +// 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); + 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); 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); + 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); + fp->getPreviousWaypoint()->setSpeed(tgt_speed); + } + } if (lead_dist < fabs(2*speed)) { //don't skip over the waypoint @@ -903,15 +931,22 @@ bool FGAIAircraft::leadPointReached(FGAIWaypoint* curr, FGAIWaypoint* next, int if ((dist_to_go_m < lead_dist) || ((dist_to_go_m > prev_dist_to_go) && (bearing > (minBearing * 1.1))) ) { - SG_LOG(SG_AI, SG_BULK, "Leadpoint reached " << bearing << "\t" << nextBearing); + SG_LOG(SG_AI, SG_BULK, "Leadpoint reached Bearing : " << bearing << "\tNext Bearing : " << nextBearing); minBearing = 360; speedFraction = 1.0; prev_dist_to_go = HUGE_VAL; return true; } else { - if (prev_dist_to_go == dist_to_go_m) { + if (prev_dist_to_go == dist_to_go_m && fabs(groundTargetSpeed)>0) { //FIXME must be suppressed when parked - SG_LOG(SG_AI, SG_WARN, "Aircraft " << _callsign << " stuck. Speed " << speed); + SG_LOG(SG_AI, SG_BULK, "Aircraft " << _callsign << " stuck. Speed " << speed); + stuckCounter++; + if (stuckCounter>AI_STUCK_LIMIT) { + SG_LOG(SG_AI, SG_WARN, "Stuck flight " << _callsign << " killed" ); + setDie(true); + } + } else { + stuckCounter = 0; } prev_dist_to_go = dist_to_go_m; return false; @@ -966,6 +1001,8 @@ 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() ); + //FIXME Heading Error should be reset time_t nextDeparture = trafficRef->getDepartureTime(); // make sure to wait at least 20 minutes at parking to prevent "nervous" taxi behavior @@ -985,11 +1022,7 @@ bool FGAIAircraft::handleAirportEndPoints(FGAIWaypoint* prev, time_t now) { * @param curr */ void FGAIAircraft::controlHeading(FGAIWaypoint* curr) { - double calc_bearing = fp->getBearing(pos, curr); - if (speed < 0) { - calc_bearing +=180; - SG_NORMALIZE_RANGE(calc_bearing, 0.0, 360.0); - } + 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; @@ -1020,7 +1053,12 @@ void FGAIAircraft::controlSpeed(FGAIWaypoint* curr, FGAIWaypoint* next) { if (fabs(speed_diff) > 10) { prevSpeed = speed; if (next) { - fp->setLeadDistance(speed, tgt_heading, curr, next); + if (next && !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 + fp->setLeadDistance(1); + } } } } @@ -1100,19 +1138,21 @@ void FGAIAircraft::updateHeading(double dt) { if (roll != 0.0) { // If on ground, calculate heading change directly if (onGround()) { - double headingDiff = fabs(hdg-tgt_heading); - - if (headingDiff > 180) { - headingDiff = fabs(headingDiff - 360); - } + const double headingDiff = SGMiscd::normalizePeriodic(-180, 180, hdg-tgt_heading); + // When pushback behind us we still want to move but ... groundTargetSpeed = tgt_speed * cos(headingDiff * SG_DEGREES_TO_RADIANS); - if (sign(groundTargetSpeed) != sign(tgt_speed)) { - if (fabs(speed) < 2 ) { + if (sign(groundTargetSpeed) != sign(tgt_speed) && fabs(tgt_speed) > 0) { + if (fabs(speed) < 2 && fp->isActive(globals->get_time_params()->get_cur_time())) { // This seems to happen in case there is a change from forward to pushback. // which should never happen. - SG_LOG(SG_AI, SG_BULK, "Oh dear we're stuck. Speed is " << speed ); + 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" ); + setDie(true); + } } // Negative Cosinus means angle > 90° groundTargetSpeed = 0.21 * sign(tgt_speed); // to prevent speed getting stuck in 'negative' mode @@ -1120,12 +1160,18 @@ void FGAIAircraft::updateHeading(double dt) { // Only update the target values when we're not moving because otherwise we might introduce an enormous target change rate while waiting a the gate, or holding. if (speed != 0) { - if (headingDiff > 30.0) { + if (fabs(headingDiff) > 30.0) { // invert if pushed backward - headingChangeRate += 10.0 * dt * sign(roll); + if( sign(headingChangeRate) == sign(headingDiff)) { + // left/right change + headingChangeRate = 10.0 * dt * sign(headingDiff) * -1; + } else { + headingChangeRate -= 10.0 * dt * sign(headingDiff); + } // Clamp the maximum steering rate to 30 degrees per second, // But only do this when the heading error is decreasing. + // FIXME if ((headingDiff < headingError)) { if (headingChangeRate > 30) headingChangeRate = 30; @@ -1134,18 +1180,29 @@ void FGAIAircraft::updateHeading(double dt) { } } else { if (speed != 0) { - if (fabs(headingChangeRate) > headingDiff) + if( sign(headingChangeRate) == sign(headingDiff)) { + // left/right change + headingChangeRate = 3 * dt * sign(headingDiff) * -1; + } else { + headingChangeRate -= 3 * dt * sign(headingDiff); + } + /* + if (headingChangeRate > headingDiff || + headingChangeRate < headingDiff) { headingChangeRate = headingDiff*sign(roll); - else + } + else { headingChangeRate += dt * sign(roll); + } + */ } } } hdg += headingChangeRate * dt * sqrt(fabs(speed) / 15); - SG_NORMALIZE_RANGE(headingDiff, 0.0, 360.0); headingError = headingDiff; + // SG_LOG(SG_AI, SG_BULK, "Headingerror " << headingError ); if (fabs(headingError) < 1.0) { hdg = tgt_heading; } @@ -1262,8 +1319,6 @@ void FGAIAircraft::handleATCRequests(double dt) if (!this->getTrafficRef()) { return; } - time_t startTime = this->getTrafficRef()->getDepartureTime(); /* is unused. */ - time_t now = globals->get_time_params()->get_cur_time(); /* is unused. */ //TODO implement NullController for having no ATC to save the conditionals if (controller) { @@ -1491,9 +1546,9 @@ void FGAIAircraft::dumpCSVHeader(std::ofstream& o) { o << "Lat\t"; o << "Lon\t"; o << "Callsign\t"; - o << "heading change rate\t"; - o << "headingErr\t"; o << "headingDiff\t"; + o << "headingChangeRate\t"; + o << "headingError\t"; o << "hdg\t"; o << "tgt_heading\t"; o << "tgt_speed\t"; @@ -1504,9 +1559,7 @@ void FGAIAircraft::dumpCSVHeader(std::ofstream& o) { o << "groundTargetSpeed\t"; o << "getVerticalSpeedFPM\t"; o << "getTrueHeadingDeg\t"; - o << "Bearing\t"; - o << "headingChangeRate\t"; - o << "headingError\t"; + o << "bearingToWP\t"; o << "Name\t"; o << "WP Lat\t"; @@ -1519,25 +1572,23 @@ void FGAIAircraft::dumpCSVHeader(std::ofstream& o) { o << "Leg\t"; o << "Num WP\t"; o << "Leaddistance\t"; - o << "no_roll"; + o << "no_roll\t"; + o << "roll\t"; + o << "stuckCounter"; o << endl; } void FGAIAircraft::dumpCSV(std::ofstream& o, int lineIndex) { - double headingDiff = fabs(hdg-tgt_heading); - - if (headingDiff > 180) { - headingDiff = fabs(headingDiff - 360); - } + 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 << headingChangeRate << "\t"; - o << headingError << "\t"; o << headingDiff << "\t"; + o << headingChangeRate << "\t"; + o << headingError << "\t"; o << hdg << "\t"; o << tgt_heading << "\t"; o << tgt_speed << "\t"; @@ -1550,8 +1601,6 @@ void FGAIAircraft::dumpCSV(std::ofstream& o, int lineIndex) { o << round(this->getVerticalSpeedFPM()) << "\t"; o << this->getTrueHeadingDeg() << "\t"; FGAIFlightPlan* fp = this->GetFlightPlan(); - o << headingChangeRate << "\t"; - o << headingError << "\t"; FGAIWaypoint* currentWP = this->GetFlightPlan()->getCurrentWaypoint(); if (currentWP) { o << this->GetFlightPlan()->getBearing(this->getGeodPos(), this->GetFlightPlan()->getCurrentWaypoint()) << "\t"; @@ -1565,16 +1614,18 @@ void FGAIAircraft::dumpCSV(std::ofstream& o, int lineIndex) { double dist_to_go_m = fp->getDistanceToGo(pos.getLatitudeDeg(), pos.getLongitudeDeg(), currentWP); o << dist_to_go_m << "\t"; } else { - o << "\t\t\t\t\t\t\t\t"; + o << "No WP\t\t\t\t\t\t\t\t"; } if (fp->isValidPlan()) { o << fp->getLeg() << "\t"; o << fp->getNrOfWayPoints() << "\t"; o << fp->getLeadDistance() << "\t"; } else { - o << "NotValid\t\t"; + o << "FP NotValid\t\t"; } - o << this->onGround(); + o << this->onGround() << "\t"; + o << roll << "\t"; + o << stuckCounter; o << endl; } @@ -1587,4 +1638,4 @@ std::string FGAIAircraft::getTimeString(int timeOffset) tm* timeinfo = gmtime(&rawtime); strftime(ret, 11, "%w/%H:%M:%S", timeinfo); return ret; -} +} \ No newline at end of file diff --git a/src/AIModel/AIAircraft.hxx b/src/AIModel/AIAircraft.hxx index 8db5682e5..86b027153 100644 --- a/src/AIModel/AIAircraft.hxx +++ b/src/AIModel/AIAircraft.hxx @@ -128,6 +128,7 @@ private: double headingError; double minBearing; double speedFraction; + /**Zero if FP is not active*/ double groundTargetSpeed; double groundOffset; @@ -172,6 +173,9 @@ private: std::string transponderCode; int spinCounter; + /**Kills a flight when it's stuck */ + const int AI_STUCK_LIMIT = 100; + int stuckCounter; double prevSpeed; double prev_dist_to_go; diff --git a/src/AIModel/AIFlightPlan.cxx b/src/AIModel/AIFlightPlan.cxx index e4726f26b..6ad2d57f3 100644 --- a/src/AIModel/AIFlightPlan.cxx +++ b/src/AIModel/AIFlightPlan.cxx @@ -457,6 +457,9 @@ void FGAIFlightPlan::setLeadDistance(double speed, double bearing, //lead_distance = turn_radius * sin(leadInAngle * SG_DEGREES_TO_RADIANS); lead_distance = turn_radius * tan((leadInAngle * SG_DEGREES_TO_RADIANS)/2); + if (lead_distance > 1000) { + SG_LOG(SG_AI, SG_BULK, "Excessive leaddistance possible direction change " << lead_distance << " leadInAngle " << leadInAngle << " inbound " << inbound << " outbound " << outbound); + } /* if ((lead_distance > (3*turn_radius)) && (current->on_ground == false)) { SG_LOG(SG_AI, SG_ALERT, "Warning: Lead-in distance is large. Inbound = " << inbound @@ -531,18 +534,19 @@ void FGAIFlightPlan::addWaypoint(FGAIWaypoint* wpt) void FGAIFlightPlan::pushBackWaypoint(FGAIWaypoint *wpt) { - const size_t pos = std::distance(waypoints.cbegin(), wpt_iterator); - - if (!waypoints.empty()) { - const double dist = SGGeodesy::distanceM( waypoints.back()->getPos(), wpt->getPos()); - if ( dist < 0.5 ) { - SG_LOG(SG_AI, SG_DEV_ALERT, "Double WP : \t" << wpt->getName() << " not added "); + size_t pos = wpt_iterator - waypoints.begin(); + if (waypoints.size()>0) { + double dist = SGGeodesy::distanceM( waypoints.back()->getPos(), wpt->getPos()); + if( dist == 0 ) { + SG_LOG(SG_AI, SG_DEBUG, "Double WP : \t" << wpt->getName() << " not added "); + } else { + waypoints.push_back(wpt); + SG_LOG(SG_AI, SG_BULK, "Added WP : \t" << setprecision(12) << wpt->getName() << "\t" << wpt->getPos() << "\t" << wpt->getSpeed()); } - } - + } else { waypoints.push_back(wpt); - SG_LOG(SG_AI, SG_BULK, "Added WP : \t" << wpt->getName() << "\t" << wpt->getPos() << "\t" << wpt->getSpeed()); - + SG_LOG(SG_AI, SG_BULK, "Added WP : \t" << setprecision(12) << wpt->getName() << "\t" << wpt->getPos() << "\t" << wpt->getSpeed()); + } // std::vector::push_back invalidates waypoints // so we should restore wpt_iterator after push_back // (or it could be an index in the vector) diff --git a/src/AIModel/AIFlightPlanCreate.cxx b/src/AIModel/AIFlightPlanCreate.cxx index 0dcd86e71..0c8c31233 100644 --- a/src/AIModel/AIFlightPlanCreate.cxx +++ b/src/AIModel/AIFlightPlanCreate.cxx @@ -417,7 +417,7 @@ void FGAIFlightPlan::createDefaultLandingTaxi(FGAIAircraft * ac, pushBackWaypoint(wpt); if (gate.isValid()) { - wpt = createOnGround(ac, "ENDtaxi", gate.parking()->geod(), airportElev, + wpt = createOnGround(ac, "END-taxi", gate.parking()->geod(), airportElev, ac->getPerformance()->vTaxi()); pushBackWaypoint(wpt); } @@ -468,14 +468,16 @@ bool FGAIFlightPlan::createLandingTaxi(FGAIAircraft * ac, FGAirport * apt, // int route; for (int i = 0; i < size - 2; i++) { taxiRoute.next(node, &route); - char buffer[10]; - snprintf(buffer, 10, "%d", node->getIndex()); + char buffer[16]; + snprintf(buffer, 16, "landingtaxi-%d", node->getIndex()); FGAIWaypoint *wpt = createOnGround(ac, buffer, node->geod(), apt->getElevation(), ac->getPerformance()->vTaxi()); wpt->setRouteIndex(route); - pushBackWaypoint(wpt); + if( !waypoints.back() || SGGeodesy::distanceM( waypoints.back()->getPos(), wpt->getPos()) > 0 ) { + pushBackWaypoint(wpt); + } } SG_LOG(SG_AI, SG_BULK, "Created taxi from " << runwayNode->getIndex() << " to " << gate.parking()->ident() << " at " << apt->getId()); @@ -1113,11 +1115,11 @@ bool FGAIFlightPlan::createParking(FGAIAircraft * ac, FGAirport * apt, SGGeodesy::direct(parking->geod(), heading, 0.1 * parking->getRadius(), pos, az); - wpt = createOnGround(ac, "taxiStart2", pos, aptElev, vTaxiReduced); + wpt = createOnGround(ac, "taxiStart2", pos, aptElev, vTaxiReduced/2); pushBackWaypoint(wpt); wpt = createOnGround(ac, "END-Parking", parking->geod(), aptElev, - vTaxiReduced); + vTaxiReduced/3); pushBackWaypoint(wpt); return true; } diff --git a/src/AIModel/AIFlightPlanCreatePushBack.cxx b/src/AIModel/AIFlightPlanCreatePushBack.cxx index 29967cf7b..e13ae0b18 100644 --- a/src/AIModel/AIFlightPlanCreatePushBack.cxx +++ b/src/AIModel/AIFlightPlanCreatePushBack.cxx @@ -43,7 +43,8 @@ using std::string; // TODO: Use James Turner's createOnGround functions. bool FGAIFlightPlan::createPushBack(FGAIAircraft *ac, - bool firstFlight, FGAirport *dep, + bool firstFlight, + FGAirport *dep, double radius, const string& fltType, const string& aircraftType, @@ -59,32 +60,34 @@ bool FGAIFlightPlan::createPushBack(FGAIAircraft *ac, if (!(dep->getDynamics()->getGroundController()->exists())) { //cerr << "Push Back fallback" << endl; + SG_LOG(SG_AI, SG_DEV_WARN, "No groundcontroller createPushBackFallBack at " << dep->getId()); createPushBackFallBack(ac, firstFlight, dep, radius, fltType, aircraftType, airline); return true; } - // establish the parking position / gate if required - if (firstFlight) { - // if the airport has no parking positions defined, don't log - // the warning below. - if (!dep->getDynamics()->hasParkings()) { - return false; - } - + if (firstFlight || !dep->getDynamics()->hasParking(gate.parking())) { + // establish the parking position / gate + // if the airport has no parking positions defined, don't log + // the warning below. + if (!dep->getDynamics()->hasParkings()) { + return false; + } gate = dep->getDynamics()->getAvailableParking(radius, fltType, - aircraftType, airline); + aircraftType, airline); if (!gate.isValid()) { SG_LOG(SG_AI, SG_DEV_WARN, "Could not find parking for a " << - aircraftType << - " of flight type " << fltType << - " of airline " << airline << - " at airport " << dep->getId()); + aircraftType << + " of flight type " << fltType << + " of airline " << airline << + " at airport " << dep->getId()); return false; } } + if (!gate.isValid()) { + SG_LOG(SG_AI, SG_DEV_WARN, "Gate " << gate.parking()->ident() << " not valid createPushBackFallBack at " << dep->getId()); createPushBackFallBack(ac, firstFlight, dep, radius, fltType, aircraftType, airline); return true; @@ -95,23 +98,28 @@ bool FGAIFlightPlan::createPushBack(FGAIAircraft *ac, FGParking *parking = gate.parking(); if (parking && parking->getPushBackPoint() != nullptr) { FGTaxiRoute route = groundNet->findShortestRoute(parking, parking->getPushBackPoint(), false); - SG_LOG(SG_AI, SG_BULK, "Creating Pushback : \t" << parking->getPushBackPoint()->getIndex()); + SG_LOG(SG_AI, SG_BULK, "Creating Pushback from " << parking->ident() << " to " << parking->getPushBackPoint()->getIndex()); int size = route.size(); if (size < 2) { - SG_LOG(SG_AI, SG_DEV_WARN, "Push back route from gate " << parking->ident() << " has only " << size << " nodes."); - SG_LOG(SG_AI, SG_DEV_WARN, "Using " << parking->getPushBackPoint()); + SG_LOG(SG_AI, SG_DEV_WARN, "Push back route from gate " << parking->ident() << " has only " << size << " nodes.\n" << "Using " << parking->getPushBackPoint()); } route.first(); FGTaxiNodeRef node; int rte; + if (waypoints.size()>0) { + // This will be a parking from a previous leg which still contains the forward speed + waypoints.back()->setSpeed(vTaxiBackward); + } + + while (route.next(node, &rte)) { char buffer[20]; - snprintf (buffer, sizeof(buffer), "pushback-%d4", (short)node->getIndex()); - FGAIWaypoint *wpt = createOnGround(ac, string(buffer), node->geod(), dep->getElevation(), -vTaxiBackward); + sprintf (buffer, "pushback-%03d", (short)node->getIndex()); + FGAIWaypoint *wpt = createOnGround(ac, string(buffer), node->geod(), dep->getElevation(), vTaxiBackward); /* if (previous) { @@ -143,11 +151,11 @@ bool FGAIFlightPlan::createPushBack(FGAIAircraft *ac, SG_LOG(SG_AI, SG_DEV_WARN, "Gate " << parking->ident() << " at " << dep->getId() << " doesn't seem to have pushforward routes associated with it."); - FGAIWaypoint *wpt = createOnGround(ac, string("park"), dep->geod(), dep->getElevation(), vTaxiReduced); + FGAIWaypoint *wpt = createOnGround(ac, string("park"), parking->geod(), dep->getElevation(), vTaxiReduced); pushBackWaypoint(wpt); SGGeod coord; - SGGeodesy::direct(dep->geod(), parking->getHeading(), 2.0, coord, az2); + SGGeodesy::direct(parking->geod(), parking->getHeading(), 2.0, coord, az2); wpt = createOnGround(ac, string("taxiStart"), coord, dep->getElevation(), vTaxiReduced); pushBackWaypoint(wpt); return true; @@ -158,16 +166,20 @@ bool FGAIFlightPlan::createPushBack(FGAIAircraft *ac, double parkingHeading = parking->getHeading(); - SG_LOG(SG_AI, SG_BULK, "Creating Pushforward : \t" << pushForwardSegment->getEnd()->getIndex() << " Length : \t" << distance); - - int numSegments = distance/2.0; + SG_LOG(SG_AI, SG_BULK, "Creating Pushforward from ID " << pushForwardSegment->getEnd()->getIndex() << " Length : \t" << distance); +// Add the parking if on first leg and not repeat + if (waypoints.size() == 0) { + pushBackWaypoint( createOnGround(ac, parking->getName(), parking->geod(), dep->getElevation(), vTaxiReduced)); + } +// Make sure we have at least three WPs + int numSegments = distance>15?(distance/5.0):3; for (int i = 1; i < numSegments; i++) { SGGeod pushForwardPt; SGGeodesy::direct(parking->geod(), parkingHeading, (((double)i / numSegments) * distance), pushForwardPt, az2); char buffer[20]; - snprintf(buffer, sizeof(buffer), "pushforward-%d4", (short)i); + sprintf(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 95564e4df..9234d00c7 100644 --- a/src/Airports/dynamics.cxx +++ b/src/Airports/dynamics.cxx @@ -287,6 +287,14 @@ FGParking* FGAirportDynamics::innerGetAvailableParking(double radius, const stri return candidates.front(); } +bool FGAirportDynamics::hasParking(FGParking* parking) const +{ + return std::find(parent()->groundNetwork()->allParkings().begin(), + parent()->groundNetwork()->allParkings().end(), + parking) + != parent()->groundNetwork()->allParkings().end(); +} + bool FGAirportDynamics::hasParkings() const { return !parent()->groundNetwork()->allParkings().empty(); diff --git a/src/Airports/dynamics.hxx b/src/Airports/dynamics.hxx index 7327c3bc7..c05edcb23 100644 --- a/src/Airports/dynamics.hxx +++ b/src/Airports/dynamics.hxx @@ -116,6 +116,8 @@ public: std::string& runway, double heading ); + bool hasParking(FGParking* parking) const; + bool hasParkings() const; /** diff --git a/src/Airports/groundnetwork.cxx b/src/Airports/groundnetwork.cxx index b5de65855..d4d590149 100644 --- a/src/Airports/groundnetwork.cxx +++ b/src/Airports/groundnetwork.cxx @@ -301,7 +301,7 @@ FGTaxiNodeRef FGGroundNetwork::findNearestNodeOnRunway(const SGGeod & aGeod, FGR double exitHeading = SGGeodesy::courseDeg((*it)->geod(), (exitSegments.back())->geod()); diff = fabs(aRunway->headingDeg() - exitHeading); - if (diff > 10) { + if (diff > 80) { // Only exits going in our direction continue; } diff --git a/test_suite/test_data/YSSY.groundnet.xml b/test_suite/test_data/YSSY.groundnet.xml index 9018c4bb5..b85e3136d 100644 --- a/test_suite/test_data/YSSY.groundnet.xml +++ b/test_suite/test_data/YSSY.groundnet.xml @@ -1,8 +1,8 @@ 1 - Sun, 09 May 2021 19:33:40 GMT by FlightgearAirports 0.0.31 - unknown + Wed, 11 Aug 2021 18:28:41 GMT by FlightgearAirports 0.0.33 + PortreeKid 13510 11840 @@ -17,8 +17,8 @@ 12470 - - + + @@ -30,83 +30,83 @@ - - - - - + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - + + + + + + + + + + - - - - - - - - - - + + + + + + + + + + - + @@ -174,7 +174,7 @@ - + @@ -182,7 +182,7 @@ - + @@ -1386,7 +1386,7 @@ - + @@ -1528,6 +1528,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -4597,7 +4632,7 @@ - + @@ -4813,5 +4848,74 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ 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..d5ae731cc 100644 --- a/test_suite/unit_tests/AI/test_groundnet.cxx +++ b/test_suite/unit_tests/AI/test_groundnet.cxx @@ -94,7 +94,7 @@ void GroundnetTests::testShortestRoute() FGTaxiNodeRef end = network->findNearestNodeOnRunway(runway->threshold()); FGTaxiRoute route = network->findShortestRoute(startParking, end); CPPUNIT_ASSERT_EQUAL(true, network->exists()); - CPPUNIT_ASSERT_EQUAL(29, route.size()); + CPPUNIT_ASSERT_EQUAL(25, route.size()); } /** diff --git a/test_suite/unit_tests/AI/test_traffic.cxx b/test_suite/unit_tests/AI/test_traffic.cxx index 364b89dce..ebce1dce5 100644 --- a/test_suite/unit_tests/AI/test_traffic.cxx +++ b/test_suite/unit_tests/AI/test_traffic.cxx @@ -148,7 +148,7 @@ void TrafficTests::testPushback() const double crs = SGGeodesy::courseDeg(departureAirport->geod(), arrivalAirport->geod()); // direct course time_t departureTime = globals->get_time_params()->get_cur_time(); - departureTime = departureTime - 90; + departureTime = departureTime + 90; std::unique_ptr fp(new FGAIFlightPlan(aiAircraft, flightPlanName, crs, departureTime, @@ -164,7 +164,7 @@ void TrafficTests::testPushback() aiAircraft->FGAIBase::setFlightPlan(std::move(fp)); globals->get_subsystem()->attach(aiAircraft); - aiAircraft = flyAI(aiAircraft, "flight_EGPH_EGPF_" + std::to_string(departureTime)); + aiAircraft = flyAI(aiAircraft, "flight_testPushback_EGPH_EGPF_" + std::to_string(departureTime)); } void TrafficTests::testPushbackCargo() @@ -205,7 +205,8 @@ void TrafficTests::testPushbackCargo() const double crs = SGGeodesy::courseDeg(egph->geod(), egpf->geod()); // direct course time_t departureTime = globals->get_time_params()->get_cur_time(); - departureTime = departureTime - 90; + departureTime = departureTime + 90; + std::unique_ptr fp(new FGAIFlightPlan(aiAircraft, flightPlanName, crs, departureTime, @@ -264,7 +265,8 @@ void TrafficTests::testChangeRunway() const double crs = SGGeodesy::courseDeg(departureAirport->geod(), arrivalAirport->geod()); // direct course time_t departureTime = globals->get_time_params()->get_cur_time(); - departureTime = departureTime - 90; + departureTime = departureTime + 90; + std::unique_ptr fp(new FGAIFlightPlan(aiAircraft, flightPlanName, crs, departureTime, @@ -321,7 +323,8 @@ void TrafficTests::testPushforward() const double crs = SGGeodesy::courseDeg(departureAirport->geod(), arrivalAirport->geod()); // direct course time_t departureTime = globals->get_time_params()->get_cur_time(); - departureTime = departureTime - 90; + departureTime = departureTime + 90; + std::unique_ptr fp(new FGAIFlightPlan(aiAircraft, flightPlanName, crs, departureTime, @@ -377,7 +380,8 @@ void TrafficTests::testPushforwardSpeedy() const double crs = SGGeodesy::courseDeg(departureAirport->geod(), arrivalAirport->geod()); // direct course time_t departureTime = globals->get_time_params()->get_cur_time(); - departureTime = departureTime - 90; + departureTime = departureTime + 90; + std::unique_ptr fp(new FGAIFlightPlan(aiAircraft, flightPlanName, crs, departureTime, @@ -434,7 +438,8 @@ void TrafficTests::testPushforwardParkYBBN() const double crs = SGGeodesy::courseDeg(departureAirport->geod(), arrivalAirport->geod()); // direct course time_t departureTime = globals->get_time_params()->get_cur_time(); - departureTime = departureTime - 90; + departureTime = departureTime + 90; + std::unique_ptr fp(new FGAIFlightPlan(aiAircraft, flightPlanName, crs, departureTime, @@ -514,7 +519,8 @@ void TrafficTests::testPushforwardParkYBBNRepeatGa() const double crs = SGGeodesy::courseDeg(departureAirport->geod(), arrivalAirport->geod()); // direct course time_t departureTime = globals->get_time_params()->get_cur_time(); - departureTime = departureTime - 90; + departureTime = departureTime + 90; + std::unique_ptr fp(new FGAIFlightPlan(aiAircraft, flightPlanName, crs, departureTime, @@ -589,7 +595,8 @@ void TrafficTests::testPushforwardParkYBBNRepeatGate() const double crs = SGGeodesy::courseDeg(departureAirport->geod(), arrivalAirport->geod()); // direct course time_t departureTime = globals->get_time_params()->get_cur_time(); - departureTime = departureTime - 90; + departureTime = departureTime + 90; + std::unique_ptr fp(new FGAIFlightPlan(aiAircraft, flightPlanName, crs, departureTime,