From cf4801e11c5b69b107f87191584eefda3c5a9b26 Mon Sep 17 00:00:00 2001 From: PortreeKid Date: Fri, 17 Sep 2021 19:33:37 +0200 Subject: [PATCH] * Improved lead distance & handling of sharp turns * Takeoff leg respects displaced threshold --- src/AIModel/AIAircraft.cxx | 57 ++++++++++++++++++--------- src/AIModel/AIAircraft.hxx | 5 +++ src/AIModel/AIFlightPlan.cxx | 63 ++++++++++++++++-------------- src/AIModel/AIFlightPlan.hxx | 18 +++++---- src/AIModel/AIFlightPlanCreate.cxx | 22 +++++------ src/Airports/runways.cxx | 5 +++ src/Airports/runways.hxx | 7 ++++ 7 files changed, 109 insertions(+), 68 deletions(-) diff --git a/src/AIModel/AIAircraft.cxx b/src/AIModel/AIAircraft.cxx index ae716e755..4256a8ab6 100644 --- a/src/AIModel/AIAircraft.cxx +++ b/src/AIModel/AIAircraft.cxx @@ -308,7 +308,7 @@ void FGAIAircraft::ProcessFlightPlan( double dt, time_t now ) { /////////////////////////////////////////////////////////////////////////// // Initialize the flightplan ////////////////////////////////////////////////////////////////////////// - if (!prev) { + if (!prev||repositioned) { handleFirstWaypoint(); return; } // end of initialization @@ -534,10 +534,18 @@ bool FGAIAircraft::loadNextLeg(double distance) { int leg; if ((leg = fp->getLeg()) == 9) { + FGAirport *oldArr = trafficRef->getArrivalAirport(); if (!trafficRef->next()) { //FIXME I'm on leg 9 and don't even reach parking. return false; } + FGAirport *dep = trafficRef->getDepartureAirport(); + if (oldArr!=dep) { + // as though we are first leg + repositioned = true; + } else { + repositioned = false; + } setCallSign(trafficRef->getCallSign()); leg = 0; fp->setLeg(leg); @@ -865,7 +873,7 @@ bool FGAIAircraft::fpExecutable(time_t now) { bool FGAIAircraft::leadPointReached(FGAIWaypoint* curr, FGAIWaypoint* next, int nextTurnAngle) { double dist_to_go_m = fp->getDistanceToGo(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr); // Leaddistance should be ft - double lead_dist = fp->getLeadDistance(); + 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")) { @@ -896,9 +904,11 @@ bool FGAIAircraft::leadPointReached(FGAIWaypoint* curr, FGAIWaypoint* next, int } } - if (lead_dist < fabs(2*speed)) { + if (lead_distance_m < fabs(2*speed) * SG_FEET_TO_METER) { //don't skip over the waypoint - lead_dist = fabs(2*speed); + SG_LOG(SG_AI, SG_BULK, "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; @@ -908,14 +918,12 @@ bool FGAIAircraft::leadPointReached(FGAIWaypoint* curr, FGAIWaypoint* next, int if (next) { nextBearing = getBearing(fp->getBearing(pos, next)); } - double bearingDiff = fabs(bearing-nextBearing); - if (onGround() && nextTurnAngle > 30 && bearingDiff > 50) { - // Next turn is pretty sharp so we do a preturn - SG_LOG(SG_AI, SG_BULK, "Leadpoint reached due to excessive heading diff " << bearingDiff); - minBearing = 360; - speedFraction = 1.0; - prev_dist_to_go = HUGE_VAL; - return true; + 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) ); + lead_distance_m = fabs(multiplicator*speed) * SG_FEET_TO_METER; + fp->setLeadDistance(lead_distance_m * SG_METER_TO_FEET); } if (bearing < minBearing) { minBearing = bearing; @@ -929,9 +937,9 @@ bool FGAIAircraft::leadPointReached(FGAIWaypoint* curr, FGAIWaypoint* next, int } } - if ((dist_to_go_m < lead_dist) || + 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); + SG_LOG(SG_AI, SG_BULK, "Leadpoint reached Bearing : " << bearing << "\tNext Bearing : " << nextBearing << " Next Turn Angle : " << fabs(nextTurnAngle)); minBearing = 360; speedFraction = 1.0; prev_dist_to_go = HUGE_VAL; @@ -1565,13 +1573,15 @@ void FGAIAircraft::dumpCSVHeader(std::ofstream& o) { 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 << "Leaddistance\t"; o << "no_roll\t"; o << "roll\t"; o << "stuckCounter"; @@ -1588,7 +1598,7 @@ void FGAIAircraft::dumpCSV(std::ofstream& o, int lineIndex) { o << this->getCallSign() << "\t"; o << headingDiff << "\t"; o << headingChangeRate << "\t"; - o << headingError << "\t"; + o << headingError << "\t"; o << hdg << "\t"; o << tgt_heading << "\t"; o << tgt_speed << "\t"; @@ -1602,11 +1612,20 @@ void FGAIAircraft::dumpCSV(std::ofstream& o, int lineIndex) { 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 << this->GetFlightPlan()->getCurrentWaypoint()->getPos().getLatitudeDeg() << "\t"; - o << this->GetFlightPlan()->getCurrentWaypoint()->getPos().getLongitudeDeg() << "\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 << this->GetFlightPlan()->getStartTime() << "\t"; o << globals->get_time_params()->get_cur_time() << "\t"; @@ -1617,9 +1636,9 @@ void FGAIAircraft::dumpCSV(std::ofstream& o, int lineIndex) { 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"; - o << fp->getLeadDistance() << "\t"; } else { o << "FP NotValid\t\t"; } diff --git a/src/AIModel/AIAircraft.hxx b/src/AIModel/AIAircraft.hxx index 86b027153..0fbaf4e9e 100644 --- a/src/AIModel/AIAircraft.hxx +++ b/src/AIModel/AIAircraft.hxx @@ -176,6 +176,11 @@ private: /**Kills a flight when it's stuck */ const int AI_STUCK_LIMIT = 100; int stuckCounter; + /** + * Signals a reset to leg 1 at a different airport. + * The leg loading happens at a different place than the parking loading. + * */ + bool repositioned; double prevSpeed; double prev_dist_to_go; diff --git a/src/AIModel/AIFlightPlan.cxx b/src/AIModel/AIFlightPlan.cxx index 6ad2d57f3..2eda2cacb 100644 --- a/src/AIModel/AIFlightPlan.cxx +++ b/src/AIModel/AIFlightPlan.cxx @@ -105,7 +105,7 @@ FGAIFlightPlan::FGAIFlightPlan() : sid(NULL), repeat(false), distance_to_go(0), - lead_distance(0), + lead_distance_ft(0), leadInAngle(0), start_time(0), arrivalTime(0), @@ -120,7 +120,7 @@ FGAIFlightPlan::FGAIFlightPlan(const string& filename) : sid(NULL), repeat(false), distance_to_go(0), - lead_distance(0), + lead_distance_ft(0), leadInAngle(0), start_time(0), arrivalTime(0), @@ -155,7 +155,7 @@ FGAIFlightPlan::FGAIFlightPlan(FGAIAircraft *ac, sid(NULL), repeat(false), distance_to_go(0), - lead_distance(0), + lead_distance_ft(0), leadInAngle(0), start_time(start), arrivalTime(0), @@ -360,24 +360,7 @@ FGAIWaypoint* FGAIFlightPlan::getNextWaypoint( void ) const int FGAIFlightPlan::getNextTurnAngle( void ) const { - if (wpt_iterator == waypoints.end()) - return 0; - if (wpt_iterator+1 == waypoints.end()) - return 0; - if (wpt_iterator+2 == waypoints.end()) - return 0; - FGAIWaypoint* currentWP = *(wpt_iterator); - FGAIWaypoint* nextWP = *(wpt_iterator + 1); - FGAIWaypoint* afterNextWP = *(wpt_iterator + 2); - int currentBearing = this->getBearing(currentWP, nextWP); - int nextBearing = this->getBearing(nextWP, afterNextWP); - - int turnAngle = nextBearing - currentBearing; - if (turnAngle>180) { - turnAngle -= 180; - } - - return turnAngle; + return nextTurnAngle; } @@ -402,6 +385,26 @@ void FGAIFlightPlan::IncrementWaypoint(bool eraseWaypoints ) else { wpt_iterator++; } + // Calculate the angle of the next turn. + if (wpt_iterator == waypoints.end()) + return; + if (wpt_iterator == waypoints.begin()) + return; + if (wpt_iterator+1 == waypoints.end()) + return; + FGAIWaypoint* previousWP = *(wpt_iterator -1); + FGAIWaypoint* currentWP = *(wpt_iterator); + FGAIWaypoint* nextWP = *(wpt_iterator + 1); + int currentBearing = this->getBearing(previousWP, currentWP); + int nextBearing = this->getBearing(currentWP, nextWP); + + nextTurnAngle = SGMiscd::normalizePeriodic(-180, 180, nextBearing - currentBearing); + if (previousWP->getSpeed()>0&&nextWP->getSpeed()<0 || + previousWP->getSpeed()<0&&nextWP->getSpeed()>0) { + nextTurnAngle += 180; + SG_LOG(SG_AI, SG_BULK, "Add 180 to turn angle pushback end"); + } + SG_LOG(SG_AI, SG_BULK, "Calculated next turn angle " << nextTurnAngle << " " << previousWP->getName() << " " << currentWP->getName() << " Previous Speed " << previousWP->getSpeed() << " Next Speed " << nextWP->getSpeed()); } void FGAIFlightPlan::DecrementWaypoint() @@ -438,7 +441,7 @@ void FGAIFlightPlan::setLeadDistance(double speed, double bearing, // 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 = 0.5; + lead_distance_ft = 0.5; return; } @@ -455,26 +458,26 @@ void FGAIFlightPlan::setLeadDistance(double speed, double bearing, //if (leadInAngle < 30.0) // To prevent lead_dist from getting so small it is skipped // leadInAngle = 30.0; - //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); + //lead_distance_ft = turn_radius * sin(leadInAngle * SG_DEGREES_TO_RADIANS); + lead_distance_ft = turn_radius * tan((leadInAngle * SG_DEGREES_TO_RADIANS)/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); } /* - if ((lead_distance > (3*turn_radius)) && (current->on_ground == false)) { + if ((lead_distance_ft > (3*turn_radius)) && (current->on_ground == false)) { SG_LOG(SG_AI, SG_ALERT, "Warning: Lead-in distance is large. Inbound = " << inbound << ". Outbound = " << outbound << ". Lead in angle = " << leadInAngle << ". Turn radius = " << turn_radius); - lead_distance = 3 * turn_radius; + lead_distance_ft = 3 * turn_radius; return; } if ((leadInAngle > 90) && (current->on_ground == true)) { - lead_distance = turn_radius * tan((90 * SG_DEGREES_TO_RADIANS)/2); + lead_distance_ft = turn_radius * tan((90 * SG_DEGREES_TO_RADIANS)/2); return; }*/ } void FGAIFlightPlan::setLeadDistance(double distance_ft){ - lead_distance = distance_ft; + lead_distance_ft = distance_ft; } diff --git a/src/AIModel/AIFlightPlan.hxx b/src/AIModel/AIFlightPlan.hxx index 40ed22141..48e062fe7 100644 --- a/src/AIModel/AIFlightPlan.hxx +++ b/src/AIModel/AIFlightPlan.hxx @@ -175,7 +175,7 @@ public: void setLeadDistance(double speed, double bearing, FGAIWaypoint* current, FGAIWaypoint* next); void setLeadDistance(double distance_ft); - double getLeadDistance( void ) const {return lead_distance;} + double getLeadDistance( void ) const {return lead_distance_ft;} double getBearing(FGAIWaypoint* previous, FGAIWaypoint* next) const; double getBearing(const SGGeod& aPos, FGAIWaypoint* next) const; @@ -231,10 +231,10 @@ public: void setGate(const ParkingAssignment& pka); FGParking* getParkingGate(); - FGAirportRef departureAirport() const; - FGAirportRef arrivalAirport() const; + FGAirportRef departureAirport() const; + FGAirportRef arrivalAirport() const; - bool empty() const; + bool empty() const; private: FGAIFlightPlan *sid; @@ -246,12 +246,14 @@ private: wpt_vector_iterator wpt_iterator; bool repeat; - double distance_to_go; - double lead_distance; - double leadInAngle; + double distance_to_go = 0; + //FIXME ft + double lead_distance_ft = 0; + double leadInAngle = 0; + double nextTurnAngle = 0; time_t start_time; time_t arrivalTime; // For AI/ATC purposes. - int leg; + int leg = 0; ParkingAssignment gate; FGTaxiNodeRef lastNodeVisited; std::string activeRunway; diff --git a/src/AIModel/AIFlightPlanCreate.cxx b/src/AIModel/AIFlightPlanCreate.cxx index 0c8c31233..8262d058c 100644 --- a/src/AIModel/AIFlightPlanCreate.cxx +++ b/src/AIModel/AIFlightPlanCreate.cxx @@ -69,11 +69,11 @@ bool FGAIFlightPlan::create(FGAIAircraft * ac, FGAirport * dep, const string & airline, double distance) { if( legNr <= 3 ) - SG_LOG(SG_AI, SG_BULK, "Create Leg " << legNr << " " << (firstFlight?"First":"") << " Old Leg " << getLeg() << " Airport : " << dep->getId()); + SG_LOG(SG_AI, SG_BULK, "Create Leg " << legNr << " " << (firstFlight?"First":"") << " Old Leg " << getLeg() << " At Airport : " << dep->getId()); else if( legNr<= 6 ) SG_LOG(SG_AI, SG_BULK, "Create Leg " << legNr << " " << (firstFlight?"First":"") << " Old Leg " << getLeg() << " Departure Airport : " << dep->getId() << " Arrival Airport : " << arr->getId()); else - SG_LOG(SG_AI, SG_BULK, "Create Leg " << legNr << " " << (firstFlight?"First":"") << " Old Leg " << getLeg() << " Airport : " << arr->getId()); + SG_LOG(SG_AI, SG_BULK, "Create Leg " << legNr << " " << (firstFlight?"First":"") << " Old Leg " << getLeg() << " At Airport : " << arr->getId()); bool retVal = true; int currWpt = wpt_iterator - waypoints.begin(); @@ -281,7 +281,7 @@ bool FGAIFlightPlan::createTakeoffTaxi(FGAIAircraft * ac, bool firstFlight, FGRunway * rwy = apt->getRunwayByIdent(activeRunway); SG_LOG(SG_AI, SG_BULK, "Taxi to " << apt->getId() << "/" << activeRunway); assert( rwy != NULL ); - SGGeod runwayTakeoff = rwy->pointOnCenterline(5.0); + SGGeod runwayTakeoff = rwy->pointOnCenterlineDisplaced(5.0); FGGroundNetwork *gn = apt->groundNetwork(); if (!gn->exists()) { @@ -389,7 +389,7 @@ bool FGAIFlightPlan::createTakeoffTaxi(FGAIAircraft * ac, bool firstFlight, pushBackWaypoint(wpt); } // Acceleration point, 105 meters into the runway, - SGGeod accelPoint = rwy->pointOnCenterline(105.0); + SGGeod accelPoint = rwy->pointOnCenterlineDisplaced(105.0); FGAIWaypoint *wpt = createOnRunway(ac, "Accel", accelPoint, apt->getElevation(), ac->getPerformance()->vRotate()); wpt->setFlaps(0.5f); pushBackWaypoint(wpt); @@ -468,8 +468,8 @@ bool FGAIFlightPlan::createLandingTaxi(FGAIAircraft * ac, FGAirport * apt, // int route; for (int i = 0; i < size - 2; i++) { taxiRoute.next(node, &route); - char buffer[16]; - snprintf(buffer, 16, "landingtaxi-%d", node->getIndex()); + char buffer[20]; + snprintf(buffer, 20, "landingtaxi-%d-%d", node->getIndex(), i); FGAIWaypoint *wpt = createOnGround(ac, buffer, node->geod(), apt->getElevation(), ac->getPerformance()->vTaxi()); @@ -560,14 +560,14 @@ bool FGAIFlightPlan::createTakeOff(FGAIAircraft * ac, bool firstFlight, // distance from the runway threshold to accelerate to rotation speed. double d = accelDistance(vTaxiMetric, vRotateMetric, accelMetric) + ACCEL_POINT; - SGGeod rotatePoint = rwy->pointOnCenterline(d); + SGGeod rotatePoint = rwy->pointOnCenterlineDisplaced(d); wpt = createOnRunway(ac, "rotate", rotatePoint, airportElev, vRotate); wpt->setFlaps(0.5f); pushBackWaypoint(wpt); // After rotation, we still need to accelerate to the take-off speed. double t = d + accelDistance(vRotateMetric, vTakeoffMetric, accelMetric); - SGGeod takeoffPoint = rwy->pointOnCenterline(t); + SGGeod takeoffPoint = rwy->pointOnCenterlineDisplaced(t); wpt = createOnRunway(ac, "takeoff", takeoffPoint, airportElev, vTakeoff); wpt->setGear_down(true); wpt->setFlaps(0.5f); @@ -580,7 +580,7 @@ bool FGAIFlightPlan::createTakeOff(FGAIAircraft * ac, bool firstFlight, // With closely spaced waypoints on climb-out this can occur almost immediately, // so we put the waypoint further away. double gearUpDist = t + 2*vRef*SG_FEET_TO_METER + pitchDistance(INITIAL_PITCH_ANGLE, 400 * SG_FEET_TO_METER); - SGGeod gearUpPoint = rwy->pointOnCenterline(gearUpDist); + SGGeod gearUpPoint = rwy->pointOnCenterlineDisplaced(gearUpDist); wpt = createInAir(ac, "gear-up", gearUpPoint, airportElev + 400, vRef); wpt->setFlaps(0.5f); pushBackWaypoint(wpt); @@ -592,12 +592,12 @@ bool FGAIFlightPlan::createTakeOff(FGAIAircraft * ac, bool firstFlight, // be a (sometimes large) turn towards the destination, and we don't want to // commence that turn below 2000' double climbOut = t + 2*vClimbBelow10000*SG_FEET_TO_METER + pitchDistance(INITIAL_PITCH_ANGLE, 2000 * SG_FEET_TO_METER); - SGGeod climbOutPoint = rwy->pointOnCenterline(climbOut); + SGGeod climbOutPoint = rwy->pointOnCenterlineDisplaced(climbOut); wpt = createInAir(ac, "2000'", climbOutPoint, airportElev + 2000, vClimbBelow10000); pushBackWaypoint(wpt); climbOut = t + 2*vClimbBelow10000*SG_FEET_TO_METER + pitchDistance(INITIAL_PITCH_ANGLE, 2500 * SG_FEET_TO_METER); - SGGeod climbOutPoint2 = rwy->pointOnCenterline(climbOut); + SGGeod climbOutPoint2 = rwy->pointOnCenterlineDisplaced(climbOut); wpt = createInAir(ac, "2500'", climbOutPoint2, airportElev + 2500, vClimbBelow10000); pushBackWaypoint(wpt); diff --git a/src/Airports/runways.cxx b/src/Airports/runways.cxx index e57d4b603..d2b25f193 100644 --- a/src/Airports/runways.cxx +++ b/src/Airports/runways.cxx @@ -124,6 +124,11 @@ SGGeod FGRunway::threshold() const return pointOnCenterline(_displ_thresh); } +SGGeod FGRunway::pointOnCenterlineDisplaced(double aOffset) const +{ + return pointOnCenterline(_displ_thresh+aOffset); +} + void FGRunway::setReciprocalRunway(PositionedID other) { assert(_reciprocal == 0); diff --git a/src/Airports/runways.hxx b/src/Airports/runways.hxx index f669fdc6d..932ed1214 100644 --- a/src/Airports/runways.hxx +++ b/src/Airports/runways.hxx @@ -73,6 +73,13 @@ public: */ SGGeod threshold() const; + /** + * Retrieve a position on the extended centerline. Positive values + * are in the direction of the runway heading, negative values are in the + * opposited direction. 0.0 corresponds to the possibly threshold + */ + SGGeod pointOnCenterlineDisplaced(double aOffset) const; + /** * Get the 'far' end - this is equivalent to calling * pointOnCenterline(lengthFt());