From: James Turner Date: Wed, 17 Dec 2014 08:57:28 +0000 (+0000) Subject: Fix route-path bugs: X-Git-Url: https://git.mxchange.org/?a=commitdiff_plain;h=7317aff22d393a5aaef6b5e84571fb86a465c002;p=flightgear.git Fix route-path bugs: - accurate fly-over / fly-by computations - parse additional LevelD XML elements - path vector contains curves for turns Remove dead code. --- diff --git a/src/GUI/WaypointList.cxx b/src/GUI/WaypointList.cxx index 3687c58b7..68ba544bd 100644 --- a/src/GUI/WaypointList.cxx +++ b/src/GUI/WaypointList.cxx @@ -481,8 +481,8 @@ void WaypointList::drawRow(int dx, int dy, int rowIndex, int y, buffer[0] = 0; } } else if (rowIndex > 0) { - double courseDeg = path.computeTrackForIndex(rowIndex); - double distanceM = path.computeDistanceForIndex(rowIndex); + double courseDeg = path.trackForIndex(rowIndex); + double distanceM = path.distanceForIndex(rowIndex); ::snprintf(buffer, 128 - count, "%03.0f %5.1fnm", courseDeg, distanceM * SG_METER_TO_NM); } @@ -491,11 +491,18 @@ void WaypointList::drawRow(int dx, int dy, int rowIndex, int y, x += 100 + PUSTR_LGAP; if (wp->altitudeRestriction() != RESTRICT_NONE) { + char aboveAtBelow = ' '; + if (wp->altitudeRestriction() == RESTRICT_ABOVE) { + aboveAtBelow = 'A'; + } else if (wp->altitudeRestriction() == RESTRICT_BELOW) { + aboveAtBelow = 'B'; + } + int altHundredFt = (wp->altitudeFt() + 50) / 100; // round to nearest 100ft if (altHundredFt < 100) { - count = ::snprintf(buffer, 128, "%d'", altHundredFt * 100); + count = ::snprintf(buffer, 128, "%d'%c", altHundredFt * 100, aboveAtBelow); } else { // display as a flight-level - count = ::snprintf(buffer, 128, "FL%d", altHundredFt); + count = ::snprintf(buffer, 128, "FL%d%c", altHundredFt, aboveAtBelow); } f->drawString(buffer, x, yy); diff --git a/src/Instrumentation/gps.cxx b/src/Instrumentation/gps.cxx index cf897e254..bc2139fc0 100644 --- a/src/Instrumentation/gps.cxx +++ b/src/Instrumentation/gps.cxx @@ -736,7 +736,7 @@ void GPS::computeTurnData() _turnStartBearing = _desiredCourse; // compute next leg course RoutePath path(_route); - double crs = path.computeTrackForIndex(_route->currentIndex() + 1); + double crs = path.trackForIndex(_route->currentIndex() + 1); // compute offset bearing _turnAngle = crs - _turnStartBearing; @@ -805,7 +805,7 @@ void GPS::updateRouteData() if (leg->waypoint()->flag(WPT_MISS)) continue; - totalDistance += leg->distanceAlongRoute(); + totalDistance += leg->distanceNm(); } _routeDistanceNm->setDoubleValue(totalDistance * SG_METER_TO_NM); diff --git a/src/Instrumentation/rnav_waypt_controller.cxx b/src/Instrumentation/rnav_waypt_controller.cxx index d487e2b3f..cefc93348 100644 --- a/src/Instrumentation/rnav_waypt_controller.cxx +++ b/src/Instrumentation/rnav_waypt_controller.cxx @@ -82,8 +82,9 @@ bool geocRadialIntersection(const SGGeoc& a, double r1, const SGGeoc& b, double } */ - double ang1 = SGMiscd::normalizeAngle2(crs13-crs12); - double ang2 = SGMiscd::normalizeAngle2(crs21-crs23); + // normalise to -pi .. pi range + double ang1 = SGMiscd::normalizeAngle(crs13-crs12); + double ang2 = SGMiscd::normalizeAngle(crs21-crs23); if ((sin(ang1) == 0.0) && (sin(ang2) == 0.0)) { SG_LOG(SG_INSTR, SG_WARN, "geocRadialIntersection: infinity of intersections"); @@ -91,7 +92,14 @@ bool geocRadialIntersection(const SGGeoc& a, double r1, const SGGeoc& b, double } if ((sin(ang1)*sin(ang2))<0.0) { - SG_LOG(SG_INSTR, SG_WARN, "geocRadialIntersection: intersection ambiguous"); + SG_LOG(SG_INSTR, SG_INFO, "deg crs12:" << crs12 * SG_RADIANS_TO_DEGREES); + SG_LOG(SG_INSTR, SG_INFO, "deg crs21:" << crs21 * SG_RADIANS_TO_DEGREES); + + SG_LOG(SG_INSTR, SG_INFO, "crs13 - crs12 in deg:" << (crs13 - crs12) * SG_RADIANS_TO_DEGREES); + SG_LOG(SG_INSTR, SG_INFO, "crs21 - crs23 in deg:" << (crs21 - crs23) * SG_RADIANS_TO_DEGREES); + + SG_LOG(SG_INSTR, SG_WARN, "geocRadialIntersection: intersection ambiguous:" + << ang1 << " " << ang2 << " sin1 " << sin(ang1) << " sin2 " << sin(ang2)); return false; } diff --git a/src/Navaids/FlightPlan.cxx b/src/Navaids/FlightPlan.cxx index 3b2fe6870..c89c1d118 100644 --- a/src/Navaids/FlightPlan.cxx +++ b/src/Navaids/FlightPlan.cxx @@ -1190,10 +1190,9 @@ void FlightPlan::rebuildLegData() double totalDistanceIncludingMissed = 0.0; RoutePath path(this); - int lastLeg = static_cast(_legs.size()) - 1; - for (int l=0; l_courseDeg = path.computeTrackForIndex(l); - _legs[l]->_pathDistance = path.computeDistanceForIndex(l) * SG_METER_TO_NM; + for (unsigned int l=0; l<_legs.size(); ++l) { + _legs[l]->_courseDeg = path.trackForIndex(l); + _legs[l]->_pathDistance = path.distanceForIndex(l) * SG_METER_TO_NM; _legs[l]->_distanceAlongPath = totalDistanceIncludingMissed; // omit misseed-approach waypoints from total distance calculation @@ -1204,14 +1203,6 @@ void FlightPlan::rebuildLegData() totalDistanceIncludingMissed += _legs[l]->_pathDistance; } // of legs iteration -// set some data on the final leg - if (lastLeg > 0) { - // keep the same course as the final leg, when passing the final - // waypoint - _legs[lastLeg]->_courseDeg = _legs[lastLeg - 1]->_courseDeg; - _legs[lastLeg]->_pathDistance = 0.0; - _legs[lastLeg]->_distanceAlongPath = totalDistanceIncludingMissed; - } } SGGeod FlightPlan::pointAlongRoute(int aIndex, double aOffsetNm) const diff --git a/src/Navaids/LevelDXML.cxx b/src/Navaids/LevelDXML.cxx index a370add49..6f28c34a9 100644 --- a/src/Navaids/LevelDXML.cxx +++ b/src/Navaids/LevelDXML.cxx @@ -70,6 +70,7 @@ void NavdataVisitor::startElement(const char* name, const XMLAttributes &atts) _altRestrict = RESTRICT_NONE; _altitude = 0.0; _overflightWaypt = false; // default to Fly-by + _courseFlag = false; // default to heading } else if (tag == "Approach") { _ident = atts.getValue("Name"); _waypoints.clear(); @@ -194,8 +195,10 @@ void NavdataVisitor::endElement(const char* name) _holdRighthanded = (_text == "Right"); } else if (tag == "Hld_td_value") { _holdTD = atof(_text.c_str()); + } else if (tag == "Hdg_Crs") { + _courseFlag = atoi(_text.c_str()); } else if (tag == "Hdg_Crs_value") { - _course = atof(_text.c_str()); + _courseOrHeading = atof(_text.c_str()); } else if (tag == "DMEtoIntercept") { _dmeDistance = atof(_text.c_str()); } else if (tag == "RadialtoIntercept") { @@ -203,6 +206,11 @@ void NavdataVisitor::endElement(const char* name) } else if (tag == "Flytype") { // values are 'Fly-by' and 'Fly-over' _overflightWaypt = (_text == "Fly-over"); + } else if ((tag == "AltitudeCons") || + (tag == "BankLimit") || + (tag == "Sp_Turn")) + { + // ignored but don't warn } else { SG_LOG(SG_IO, SG_INFO, "unrecognized Level-D XML element:" << tag); } @@ -242,16 +250,16 @@ Waypt* NavdataVisitor::buildWaypoint(RouteBase* owner) wp = new ATCVectors(owner, _airport); } else if ((_wayptType == "Intc") || (_wayptType == "VorRadialIntc")) { SGGeod pos(SGGeod::fromDeg(_longitude, _latitude)); - wp = new RadialIntercept(owner, _wayptName, pos, _course, _radial); + wp = new RadialIntercept(owner, _wayptName, pos, _courseOrHeading, _radial); } else if (_wayptType == "DmeIntc") { SGGeod pos(SGGeod::fromDeg(_longitude, _latitude)); - wp = new DMEIntercept(owner, _wayptName, pos, _course, _dmeDistance); + wp = new DMEIntercept(owner, _wayptName, pos, _courseOrHeading, _dmeDistance); } else if (_wayptType == "ConstHdgtoAlt") { - wp = new HeadingToAltitude(owner, _wayptName, _course); + wp = new HeadingToAltitude(owner, _wayptName, _courseOrHeading); } else if (_wayptType == "PBD") { SGGeod pos(SGGeod::fromDeg(_longitude, _latitude)), pos2; double az2; - SGGeodesy::direct(pos, _course, _dmeDistance, pos2, az2); + SGGeodesy::direct(pos, _courseOrHeading, _dmeDistance, pos2, az2); wp = new BasicWaypt(pos2, _wayptName, owner); } else { SG_LOG(SG_NAVAID, SG_ALERT, "implement waypoint type:" << _wayptType); diff --git a/src/Navaids/LevelDXML.hxx b/src/Navaids/LevelDXML.hxx index e3336e074..78e0c8134 100644 --- a/src/Navaids/LevelDXML.hxx +++ b/src/Navaids/LevelDXML.hxx @@ -59,7 +59,8 @@ private: bool _holdRighthanded; bool _holdDistance; // true, TD is distance in nm; false, TD is time in seconds - double _course, _radial, _dmeDistance; + double _courseOrHeading, _radial, _dmeDistance; + bool _courseFlag; }; } diff --git a/src/Navaids/routePath.cxx b/src/Navaids/routePath.cxx index 904154ecb..4146fa1d0 100644 --- a/src/Navaids/routePath.cxx +++ b/src/Navaids/routePath.cxx @@ -28,6 +28,7 @@ static double sqr(const double x) return x * x; } +// http://williams.best.vwh.net/avform.htm#POINTDME double pointsKnownDistanceFromGC(const SGGeoc& a, const SGGeoc&b, const SGGeoc& d, double dist) { double A = SGGeodesy::courseRad(a, d) - SGGeodesy::courseRad(a, b); @@ -71,26 +72,559 @@ lat=atan((sin(lat1)*cos(lat2)*sin(lon-lon2) return lat; } +static double magVarFor(const SGGeod& geod) +{ + double jd = globals->get_time_params()->getJD(); + return sgGetMagVar(geod, jd) * SG_RADIANS_TO_DEGREES; +} + +class WayptData +{ +public: + explicit WayptData(WayptRef w) : + wpt(w), + hasEntry(false), + posValid(false), + legCourseValid(false), + turnAngle(0.0), + turnRadius(0.0), + pathDistanceM(0.0), + overflightCompensationAngle(0.0), + flyOver(w->flag(WPT_OVERFLIGHT)) + { + } + + void initPass0() + { + const std::string& ty(wpt->type()); + if (wpt->flag(WPT_DYNAMIC)) { + if ((ty == "hdgToAlt") || (ty == "radialIntercept") || (ty == "dmeIntercept")) { + legCourse = wpt->headingRadialDeg(); + legCourseValid = true; + } + + // presumtpion is that we always overfly such a waypoint + if (ty == "hdgToAlt") { + flyOver = true; + } + } else { + pos = wpt->position(); + posValid = true; + + if ((ty == "runway") || (ty == "hold")) { + legCourse = wpt->headingRadialDeg(); + legCourseValid = true; + } + + if (ty == "runway") { + FGRunway* rwy = static_cast(wpt.get())->runway(); + turnExitPos = rwy->end(); + } + } // of static waypt + } + + // compute leg courses for all static legs (both ends are fixed) + void initPass1(const WayptData& previous, WayptData* next) + { + if (wpt->type() == "vectors") { + // relying on the fact vectors will be followed by a static fix/wpt + if (next && next->posValid) { + posValid = true; + pos = next->pos; + } + } + + if (posValid && !legCourseValid && previous.posValid) { + // we can compute leg course now + legCourse = SGGeodesy::courseDeg(previous.pos, pos); + legCourseValid = true; + } + } + + void computeTurn(double radiusM, const WayptData& previous, const WayptData& next) + { + assert(legCourseValid && next.legCourseValid); + + turnAngle = next.legCourse - legCourse; + SG_NORMALIZE_RANGE(turnAngle, -180.0, 180.0); + turnRadius = radiusM; + + double p = copysign(90.0, turnAngle); + + if (flyOver) { + turnEntryPos = pos; + turnCenter = SGGeodesy::direct(pos, legCourse + p, turnRadius); + // use the leg course + turnExitPos = SGGeodesy::direct(turnCenter, next.legCourse - p, turnRadius); + + if (!next.wpt->flag(WPT_DYNAMIC)) { + // distance perpendicular to next leg course, after turning + // through turnAngle + double xtk = turnRadius * (1 - cos(turnAngle * SG_DEGREES_TO_RADIANS)); + + // compensation angle to turn back on course + double theta = acos((turnRadius - (xtk * 0.5)) / turnRadius) * SG_RADIANS_TO_DEGREES; + theta = copysign(theta, turnAngle); + turnAngle += theta; + + // move by the distance to compensate + double d = turnRadius * 2.0 * sin(theta * SG_DEGREES_TO_RADIANS); + turnExitPos = SGGeodesy::direct(turnExitPos, next.legCourse, d); + + overflightCompensationAngle = -theta; + } + } else { + hasEntry = true; + + double halfAngle = turnAngle * 0.5; + + double turnCenterOffset = turnRadius / cos(halfAngle * SG_DEGREES_TO_RADIANS); + turnCenter = SGGeodesy::direct(pos, legCourse + halfAngle + p, turnCenterOffset); + + double entryExitDistanceAlongPath = turnRadius * tan(fabs(halfAngle) * SG_DEGREES_TO_RADIANS); + + turnEntryPos = SGGeodesy::direct(pos, legCourse, -entryExitDistanceAlongPath); + turnExitPos = SGGeodesy::direct(pos, next.legCourse, entryExitDistanceAlongPath); + } + } + + double turnDistanceM() const + { + return (fabs(turnAngle * SG_DEGREES_TO_RADIANS) / SG_PI * 2.0) * turnRadius; + } + + void turnEntryPath(SGGeodVec& path) const + { + assert(!flyOver); + if (fabs(turnAngle) < 0.5 ) { + path.push_back(pos); + return; + } + + double halfAngle = turnAngle * 0.5; + int steps = std::max(SGMiscd::roundToInt(fabs(halfAngle) / 3.0), 1); + double stepIncrement = halfAngle / steps; + double h = legCourse; + SGGeod p = turnEntryPos; + double stepDist = (fabs(stepIncrement) / 360.0) * SGMiscd::twopi() * turnRadius; + + for (int s=0; s WayptDataVec; + +class PerformanceBracket +{ +public: + PerformanceBracket(double atOrBelow, double climb, double descent, double speed, bool isMach = false) : + atOrBelowAltitudeFt(atOrBelow), + climbRateFPM(climb), + descentRateFPM(descent), + speedIASOrMach(speed), + speedIsMach(isMach) + { } + + double atOrBelowAltitudeFt; + double climbRateFPM; + double descentRateFPM; + double speedIASOrMach; + bool speedIsMach; +}; + +typedef std::vector PerformanceBracketVec; + +bool isDescentWaypoint(const WayptRef& wpt) +{ + return (wpt->flag(WPT_APPROACH) && !wpt->flag(WPT_MISS)) || wpt->flag(WPT_ARRIVAL); +} + +class RoutePath::RoutePathPrivate +{ +public: + WayptDataVec waypoints; + PerformanceBracketVec perf; + + + PerformanceBracketVec::const_iterator + findPerformanceBracket(double altFt) const + { + PerformanceBracketVec::const_iterator r; + PerformanceBracketVec::const_iterator result = perf.begin(); + + for (r = perf.begin(); r != perf.end(); ++r) { + if (r->atOrBelowAltitudeFt > altFt) { + break; + } + + result = r; + } // of brackets iteration + + return result; + } + + void computeDynamicPosition(int index) + { + const WayptData& previous(waypoints[index-1]); + WayptRef wpt = waypoints[index].wpt; + + assert(previous.posValid); + const std::string& ty(wpt->type()); + if (ty == "hdgToAlt") { + HeadingToAltitude* h = (HeadingToAltitude*) wpt.get(); + + double altFt = computeVNAVAltitudeFt(index - 1); + double altChange = h->altitudeFt() - altFt; + PerformanceBracketVec::const_iterator it = findPerformanceBracket(altFt); + double speedMSec = groundSpeedForAltitude(altFt) * SG_KT_TO_MPS; + double timeToChangeSec; + + if (isDescentWaypoint(wpt)) { + timeToChangeSec = (altChange / it->descentRateFPM) * 60.0; + } else { + timeToChangeSec = (altChange / it->climbRateFPM) * 60.0; + } + + double distanceM = timeToChangeSec * speedMSec; + double hdg = h->headingDegMagnetic() + magVarFor(previous.pos); + waypoints[index].pos = SGGeodesy::direct(previous.turnExitPos, hdg, distanceM); + waypoints[index].posValid = true; + } else if (ty == "radialIntercept") { + // start from previous.turnExit + RadialIntercept* i = (RadialIntercept*) wpt.get(); + + SGGeoc prevGc = SGGeoc::fromGeod(previous.turnExitPos); + SGGeoc navid = SGGeoc::fromGeod(wpt->position()); + SGGeoc rGc; + double magVar = magVarFor(previous.pos); + + double radial = i->radialDegMagnetic() + magVar; + double track = i->courseDegMagnetic() + magVar; + bool ok = geocRadialIntersection(prevGc, track, navid, radial, rGc); + if (!ok) { + SG_LOG(SG_NAVAID, SG_WARN, "couldn't compute interception for radial:" + << previous.turnExitPos << " / " << track << "/" << wpt->position() + << "/" << radial); + waypoints[index].pos = wpt->position(); // horrible fallback + } else { + waypoints[index].pos = SGGeod::fromGeoc(rGc); + } + + waypoints[index].posValid = true; + } else if (ty == "dmeIntercept") { + DMEIntercept* di = (DMEIntercept*) wpt.get(); + + SGGeoc prevGc = SGGeoc::fromGeod(previous.turnExitPos); + SGGeoc navid = SGGeoc::fromGeod(wpt->position()); + double distRad = di->dmeDistanceNm() * SG_NM_TO_RAD; + SGGeoc rGc; + + SGGeoc bPt; + double crs = di->courseDegMagnetic() + magVarFor(wpt->position()); + SGGeodesy::advanceRadM(prevGc, crs, 100 * SG_NM_TO_RAD, bPt); + + double dNm = pointsKnownDistanceFromGC(prevGc, bPt, navid, distRad); + if (dNm < 0.0) { + SG_LOG(SG_NAVAID, SG_WARN, "dmeIntercept failed"); + waypoints[index].pos = wpt->position(); // horrible fallback + } else { + waypoints[index].pos = SGGeodesy::direct(previous.turnExitPos, crs, dNm * SG_NM_TO_METER); + } + + waypoints[index].posValid = true; + } else if (ty == "vectors") { + waypoints[index].legCourse = SGGeodesy::courseDeg(previous.turnExitPos, waypoints[index].pos); + waypoints[index].legCourseValid = true; + // no turn data + } + } + + double computeVNAVAltitudeFt(int index) + { + WayptRef w = waypoints[index].wpt; + if ((w->flag(WPT_APPROACH) && !w->flag(WPT_MISS)) || w->flag(WPT_ARRIVAL)) { + // descent + int next = findNextKnownAltitude(index); + if (next < 0) { + return 0.0; + } + + double fixedAlt = altitudeForIndex(next); + double distanceM = distanceBetweenIndices(index, next); + double speedMSec = groundSpeedForAltitude(fixedAlt) * SG_KT_TO_MPS; + double minutes = (distanceM / speedMSec) / 60.0; + + PerformanceBracketVec::const_iterator it = findPerformanceBracket(fixedAlt); + return fixedAlt + (it->descentRateFPM * minutes); + + } else { + // climb + int prev = findPreceedingKnownAltitude(index); + if (prev < 0) { + return 0.0; + } + + double fixedAlt = altitudeForIndex(prev); + double distanceM = distanceBetweenIndices(prev, index); + double speedMSec = groundSpeedForAltitude(fixedAlt) * SG_KT_TO_MPS; + double minutes = (distanceM / speedMSec) / 60.0; + + PerformanceBracketVec::const_iterator it = findPerformanceBracket(fixedAlt); + return fixedAlt + (it->climbRateFPM * minutes); + } + + } + + int findPreceedingKnownAltitude(int index) const + { + const WayptData& w(waypoints[index]); + if (w.wpt->altitudeRestriction() == RESTRICT_AT) { + return index; + } + + // principal base case is runways. + const std::string& ty(w.wpt->type()); + if (ty == "runway") { + return index; // runway always has a known elevation + } + + if (index == 0) { + SG_LOG(SG_NAVAID, SG_WARN, "findPreceedingKnownAltitude: no preceeding altitude value found"); + return -1; + } + + // recurse earlier in the route + return findPreceedingKnownAltitude(index - 1); + } + + int findNextKnownAltitude(int index) const + { + if (index >= waypoints.size()) { + SG_LOG(SG_NAVAID, SG_WARN, "findNextKnownAltitude: no next altitude value found"); + return -1; + } + + const WayptData& w(waypoints[index]); + if (w.wpt->altitudeRestriction() == RESTRICT_AT) { + return index; + } + + // principal base case is runways. + const std::string& ty(w.wpt->type()); + if (ty == "runway") { + return index; // runway always has a known elevation + } + + if (index == waypoints.size() - 1) { + SG_LOG(SG_NAVAID, SG_WARN, "findNextKnownAltitude: no next altitude value found"); + return -1; + } + + return findNextKnownAltitude(index + 1); + } + + double altitudeForIndex(int index) const + { + const WayptData& w(waypoints[index]); + if (w.wpt->altitudeRestriction() != RESTRICT_NONE) { + return w.wpt->altitudeFt(); + } + + const std::string& ty(w.wpt->type()); + if (ty == "runway") { + FGRunway* rwy = static_cast(w.wpt.get())->runway(); + return rwy->threshold().getElevationFt(); + } + + SG_LOG(SG_NAVAID, SG_WARN, "altitudeForIndex: waypoint has no explicit altitude"); + return 0.0; + } + + double groundSpeedForAltitude(double altitude) const + { + // FIXME +#if 0 + if (0) { + PerformanceBracketVec::const_iterator it = findPerformanceBracket(altitude); + double mach; + + if (it->speedIsMach) { + mach = it->speedIASOrMach; // easy + } else { + const double Cs_0 = 661.4786; // speed of sound at sea level, knots + const double P_0 = 29.92126; + const double P = P_0 * pow(, ); + // convert IAS (which we will treat as CAS) to Mach based on altitude + } + + double oatK; + double Cs = sqrt(SG_gamma * SG_R_m2_p_s2_p_K * oatK); + + double tas = mach * Cs; + +#if 0 + P_0= 29.92126 "Hg = 1013.25 mB = 2116.2166 lbs/ft^2 + P= P_0*(1-6.8755856*10^-6*PA)^5.2558797, pressure altitude, PA<36,089.24ft + CS= 38.967854*sqrt(T+273.15) where T is the (static/true) OAT in Celsius. + + DP=P_0*((1 + 0.2*(IAS/CS_0)^2)^3.5 -1) + M=(5*( (DP/P + 1)^(2/7) -1) )^0.5 (*) + TAS= M*CS +#endif + } +#endif + + return 250.0; + } + + double distanceBetweenIndices(int from, int to) const + { + double total = 0.0; + + for (int i=from+1; i<= to; ++i) { + total += waypoints[i].pathDistanceM; + } + + return total; + } + + void initPerfData() + { + // assume category C/D aircraft for now + perf.push_back(PerformanceBracket(4000, 1800, 1800, 180)); + perf.push_back(PerformanceBracket(10000, 1800, 1800, 230)); + perf.push_back(PerformanceBracket(18000, 1200, 1800, 270)); + perf.push_back(PerformanceBracket(60000, 800, 1200, 0.85, true /* is Mach */)); + } + +}; // of RoutePathPrivate class + RoutePath::RoutePath(const flightgear::WayptVec& wpts) : - _waypts(wpts) + d(new RoutePathPrivate) { + WayptVec::const_iterator it; + for (it = wpts.begin(); it != wpts.end(); ++it) { + d->waypoints.push_back(WayptData(*it)); + } commonInit(); } -RoutePath::RoutePath(const flightgear::FlightPlan* fp) +RoutePath::RoutePath(const flightgear::FlightPlan* fp) : + d(new RoutePathPrivate) { for (int l=0; lnumLegs(); ++l) { - _waypts.push_back(fp->legAtIndex(l)->waypoint()); + d->waypoints.push_back(WayptData(fp->legAtIndex(l)->waypoint())); } commonInit(); } void RoutePath::commonInit() { - _pathClimbFPM = 1200; - _pathDescentFPM = 800; - _pathIAS = 190; - _pathTurnRate = 3.0; // 3 deg/sec = 180def/min = standard rate turn + _pathTurnRate = 3.0; // 3 deg/sec = 180deg/min = standard rate turn + + d->initPerfData(); + + WayptDataVec::iterator it; + for (it = d->waypoints.begin(); it != d->waypoints.end(); ++it) { + it->initPass0(); + } + + for (unsigned int i=1; iwaypoints.size(); ++i) { + WayptData* nextPtr = ((i + 1) < d->waypoints.size()) ? &d->waypoints[i+1] : 0; + d->waypoints[i].initPass1(d->waypoints[i-1], nextPtr); + } + + for (unsigned int i=1; iwaypoints.size(); ++i) { + d->computeDynamicPosition(i); + + const WayptData& prev(d->waypoints[i-1]); + + double alt = 0.0; // FIXME + double gs = d->groundSpeedForAltitude(alt); + double radiusM = ((360.0 / _pathTurnRate) * gs * SG_KT_TO_MPS) / SGMiscd::twopi(); + + if (i < (d->waypoints.size() - 1)) { + WayptData& next(d->waypoints[i+1]); + if (!next.legCourseValid && next.posValid) { + // compute leg course now our own position is valid + next.legCourse = SGGeodesy::courseDeg(d->waypoints[i].pos, next.pos); + next.legCourseValid = true; + } + + if (next.legCourseValid) { + d->waypoints[i].computeTurn(radiusM, prev, next); + } else { + // next waypoint has indeterminate course. Let's create a sharp turn + // this can happen when the following point is ATC vectors, for example. + d->waypoints[i].turnEntryPos = d->waypoints[i].pos; + d->waypoints[i].turnExitPos = d->waypoints[i].pos; + } + } else { + // final waypt, fix up some data + d->waypoints[i].turnEntryPos = d->waypoints[i].pos; + d->waypoints[i].turnExitPos = d->waypoints[i].pos; + } + + // now turn is computed, can resolve distances + d->waypoints[i].pathDistanceM = computeDistanceForIndex(i); + } } SGGeodVec RoutePath::pathForIndex(int index) const @@ -99,24 +633,21 @@ SGGeodVec RoutePath::pathForIndex(int index) const return SGGeodVec(); // no path for first waypoint } - if (_waypts[index]->type() == "vectors") { + const WayptData& w(d->waypoints[index]); + const std::string& ty(w.wpt->type()); + if (ty == "vectors") { return SGGeodVec(); // empty } - if (_waypts[index]->type() == "hold") { - return pathForHold((Hold*) _waypts[index].get()); + if (ty== "hold") { + return pathForHold((Hold*) d->waypoints[index].wpt.get()); } - + SGGeodVec r; - SGGeod from, to; - if (!computedPositionForIndex(index-1, from)) { - return SGGeodVec(); - } + d->waypoints[index - 1].turnExitPath(r); - r.push_back(from); - if (!computedPositionForIndex(index, to)) { - return SGGeodVec(); - } + SGGeod from = d->waypoints[index - 1].turnExitPos, + to = w.turnEntryPos; // compute rounding offset, we want to round towards the direction of travel // which depends on the east/west sign of the longitude change @@ -125,12 +656,17 @@ SGGeodVec RoutePath::pathForIndex(int index) const interpolateGreatCircle(from, to, r); } - r.push_back(to); + if (w.flyOver) { + r.push_back(w.pos); + } else { + // flyBy + w.turnEntryPath(r); + } - if (_waypts[index]->type() == "runway") { + if (ty == "runway") { // runways get an extra point, at the end. this is particularly // important so missed approach segments draw correctly - FGRunway* rwy = static_cast(_waypts[index].get())->runway(); + FGRunway* rwy = static_cast(w.wpt.get())->runway(); r.push_back(rwy->end()); } @@ -163,13 +699,7 @@ void RoutePath::interpolateGreatCircle(const SGGeod& aFrom, const SGGeod& aTo, S SGGeod RoutePath::positionForIndex(int index) const { - SGGeod r; - bool ok = computedPositionForIndex(index, r); - if (!ok) { - return SGGeod(); - } - - return r; + return d->waypoints[index].pos; } SGGeodVec RoutePath::pathForHold(Hold* hold) const @@ -177,14 +707,16 @@ SGGeodVec RoutePath::pathForHold(Hold* hold) const int turnSteps = 16; double hdg = hold->inboundRadial(); double turnDelta = 180.0 / turnSteps; + double altFt = 0.0; // FIXME + double gsKts = d->groundSpeedForAltitude(altFt); SGGeodVec r; double az2; double stepTime = turnDelta / _pathTurnRate; // in seconds - double stepDist = _pathIAS * (stepTime / 3600.0) * SG_NM_TO_METER; + double stepDist = gsKts * (stepTime / 3600.0) * SG_NM_TO_METER; double legDist = hold->isDistance() ? hold->timeOrDistance() - : _pathIAS * (hold->timeOrDistance() / 3600.0); + : gsKts * (hold->timeOrDistance() / 3600.0); legDist *= SG_NM_TO_METER; if (hold->isLeftHanded()) { @@ -210,244 +742,48 @@ SGGeodVec RoutePath::pathForHold(Hold* hold) const return r; } -bool RoutePath::computedPositionForIndex(int index, SGGeod& r) const -{ - if ((index < 0) || (index >= (int) _waypts.size())) { - throw sg_range_exception("waypt index out of range", - "RoutePath::computedPositionForIndex"); - } - - WayptRef w = _waypts[index]; - if (!w->flag(WPT_DYNAMIC)) { - r = w->position(); - return true; - } - - if (w->type() == "radialIntercept") { - // radial intersection along track - SGGeod prev; - if (!computedPositionForIndex(index - 1, prev)) { - return false; - } - - SGGeoc prevGc = SGGeoc::fromGeod(prev); - SGGeoc navid = SGGeoc::fromGeod(w->position()); - SGGeoc rGc; - double magVar = magVarFor(prev); - - RadialIntercept* i = (RadialIntercept*) w.get(); - double radial = i->radialDegMagnetic() + magVar; - double track = i->courseDegMagnetic() + magVar; - bool ok = geocRadialIntersection(prevGc, track, navid, radial, rGc); - if (!ok) { - return false; - } - - r = SGGeod::fromGeoc(rGc); - return true; - } else if (w->type() == "dmeIntercept") { - // find the point along the DME track, from prev, that is the correct distance - // from the DME - SGGeod prev; - if (!computedPositionForIndex(index - 1, prev)) { - return false; - } - - DMEIntercept* di = (DMEIntercept*) w.get(); - - SGGeoc prevGc = SGGeoc::fromGeod(prev); - SGGeoc navid = SGGeoc::fromGeod(w->position()); - double distRad = di->dmeDistanceNm() * SG_NM_TO_RAD; - SGGeoc rGc; - - SGGeoc bPt; - double crs = di->courseDegMagnetic() + magVarFor(prev); - SGGeodesy::advanceRadM(prevGc, crs, 100 * SG_NM_TO_RAD, bPt); - - double dNm = pointsKnownDistanceFromGC(prevGc, bPt, navid, distRad); - if (dNm < 0.0) { - return false; - } - - double az2; - SGGeodesy::direct(prev, crs, dNm * SG_NM_TO_METER, r, az2); - return true; - } else if (w->type() == "hdgToAlt") { - HeadingToAltitude* h = (HeadingToAltitude*) w.get(); - double climb = h->altitudeFt() - computeAltitudeForIndex(index - 1); - double d = distanceForClimb(climb); - - SGGeod prevPos; - if (!computedPositionForIndex(index - 1, prevPos)) { - return false; - } - - double hdg = h->headingDegMagnetic() + magVarFor(prevPos); - - double az2; - SGGeodesy::direct(prevPos, hdg, d * SG_NM_TO_METER, r, az2); - return true; - } else if (w->type() == "vectors"){ - // return position of next point (which is presumably static fix/wpt) - // however, a missed approach might end with VECTORS, so tolerate that case - if (index + 1 >= _waypts.size()) { - SG_LOG(SG_NAVAID, SG_INFO, "route ends with VECTORS, no position"); - return false; - } - - WayptRef nextWp = _waypts[index+1]; - if (nextWp->flag(WPT_DYNAMIC)) { - SG_LOG(SG_NAVAID, SG_INFO, "dynamic WP following VECTORS, no position"); - return false; - } - - r = nextWp->position(); - return true; - } else if (w->type() == "hold") { - r = w->position(); - return true; - } - - SG_LOG(SG_NAVAID, SG_INFO, "RoutePath::computedPositionForIndex: unhandled type:" << w->type()); - return false; -} - double RoutePath::computeDistanceForIndex(int index) const { - if ((index < 0) || (index >= (int) _waypts.size())) { + if ((index < 0) || (index >= (int) d->waypoints.size())) { throw sg_range_exception("waypt index out of range", "RoutePath::computeDistanceForIndex"); } - if (index + 1 >= (int) _waypts.size()) { - // final waypoint, distance is 0 + if (index == 0) { + // first waypoint, distance is 0 return 0.0; } - WayptRef w = _waypts[index], - nextWp = _waypts[index+1]; - - // common case, both waypoints are static - if (!w->flag(WPT_DYNAMIC) && !nextWp->flag(WPT_DYNAMIC)) { - return SGGeodesy::distanceM(w->position(), nextWp->position()); + double dist = SGGeodesy::distanceM(d->waypoints[index-1].turnExitPos, + d->waypoints[index].turnEntryPos); + if (d->waypoints[index-1].flyOver) { + // all the turn distance counts towards this leg + dist += d->waypoints[index-1].turnDistanceM(); + } else { + // add half of turn distance + dist += d->waypoints[index-1].turnDistanceM() * 0.5; } - - SGGeod wPos, nextPos; - bool ok = computedPositionForIndex(index, wPos), - nextOk = computedPositionForIndex(index + 1, nextPos); - if (ok && nextOk) { - return SGGeodesy::distanceM(wPos, nextPos); + if (!d->waypoints[index].flyOver) { + // add half turn distance + dist += d->waypoints[index].turnDistanceM() * 0.5; } - SG_LOG(SG_NAVAID, SG_INFO, "RoutePath::computeDistanceForIndex: unhandled arrangement:" - << w->type() << " followed by " << nextWp->type()); - return 0.0; + return dist; } -double RoutePath::computeAltitudeForIndex(int index) const +double RoutePath::trackForIndex(int index) const { - if ((index < 0) || (index >= (int) _waypts.size())) { - throw sg_range_exception("waypt index out of range", - "RoutePath::computeAltitudeForIndex"); - } - - WayptRef w = _waypts[index]; - if (w->altitudeRestriction() != RESTRICT_NONE) { - return w->altitudeFt(); // easy! - } - - if (w->type() == "runway") { - FGRunway* rwy = static_cast(w.get())->runway(); - return rwy->threshold().getElevationFt(); - } else if ((w->type() == "hold") || (w->type() == "vectors")) { - // pretend we don't change altitude in holds/vectoring - return computeAltitudeForIndex(index - 1); - } - - double prevAlt = computeAltitudeForIndex(index - 1); -// find distance to previous, and hence climb/descent - SGGeod pos, prevPos; - - if (!computedPositionForIndex(index, pos) || - !computedPositionForIndex(index - 1, prevPos)) - { - SG_LOG(SG_NAVAID, SG_WARN, "unable to compute position for waypoints"); - throw sg_range_exception("unable to compute position for waypoints"); - } - - double d = SGGeodesy::distanceNm(prevPos, pos); - double tMinutes = (d / _pathIAS) * 60.0; // (nm / knots) * 60 = time in minutes - - double deltaFt; // change in altitude in feet - if (w->flag(WPT_ARRIVAL) && !w->flag(WPT_MISS)) { - deltaFt = -_pathDescentFPM * tMinutes; - } else { - deltaFt = _pathClimbFPM * tMinutes; - } - - return prevAlt + deltaFt; + return d->waypoints[index].legCourse; } -double RoutePath::computeTrackForIndex(int index) const +double RoutePath::distanceForIndex(int index) const { - if ((index < 0) || (index >= (int) _waypts.size())) { - throw sg_range_exception("waypt index out of range", - "RoutePath::computeTrackForIndex"); - } - - WayptRef w = _waypts[index]; - if (w->type() == "radialIntercept") { - RadialIntercept* r = (RadialIntercept*) w.get(); - return r->courseDegMagnetic(); - } else if (w->type() == "dmeIntercept") { - DMEIntercept* d = (DMEIntercept*) w.get(); - return d->courseDegMagnetic(); - } else if (w->type() == "hdgToAlt") { - HeadingToAltitude* h = (HeadingToAltitude*) w.get(); - return h->headingDegMagnetic(); - } else if (w->type() == "hold") { - Hold* h = (Hold*) w.get(); - return h->inboundRadial(); - } else if (w->type() == "vectors") { - SG_LOG(SG_NAVAID, SG_WARN, "asked for track from VECTORS"); - throw sg_range_exception("asked for track from vectors waypt"); - } else if (w->type() == "runway") { - FGRunway* rwy = static_cast(w.get())->runway(); - return rwy->headingDeg(); - } - -// final waypoint, use inbound course - if (index + 1 >= _waypts.size()) { - return computeTrackForIndex(index - 1); - } - -// course based upon current and next pos - SGGeod pos, nextPos; - if (!computedPositionForIndex(index, pos) || - !computedPositionForIndex(index + 1, nextPos)) - { - SG_LOG(SG_NAVAID, SG_WARN, "unable to compute position for waypoints"); - throw sg_range_exception("unable to compute position for waypoints"); - } - - return SGGeodesy::courseDeg(pos, nextPos); + return d->waypoints[index].pathDistanceM; } -double RoutePath::distanceForClimb(double climbFt) const +double RoutePath::distanceBetweenIndices(int from, int to) const { - double t = 0.0; // in seconds - if (climbFt > 0.0) { - t = (climbFt / _pathClimbFPM) * 60.0; - } else if (climbFt < 0.0) { - t = (climbFt / _pathDescentFPM) * 60.0; - } - - return _pathIAS * (t / 3600.0); + return d->distanceBetweenIndices(from, to); } -double RoutePath::magVarFor(const SGGeod& geod) const -{ - double jd = globals->get_time_params()->getJD(); - return sgGetMagVar(geod, jd) * SG_RADIANS_TO_DEGREES; -} diff --git a/src/Navaids/routePath.hxx b/src/Navaids/routePath.hxx index ddc15693e..421dd9820 100644 --- a/src/Navaids/routePath.hxx +++ b/src/Navaids/routePath.hxx @@ -44,33 +44,27 @@ public: SGGeod positionForIndex(int index) const; - double computeDistanceForIndex(int index) const; - double computeTrackForIndex(int index) const; - + double trackForIndex(int index) const; + + double distanceForIndex(int index) const; + + double distanceBetweenIndices(int from, int to) const; private: - void commonInit(); + class RoutePathPrivate; - class PathCtx; + void commonInit(); + double computeDistanceForIndex(int index) const; + SGGeodVec pathForHold(flightgear::Hold* hold) const; - bool computedPositionForIndex(int index, SGGeod& pos) const; - double computeAltitudeForIndex(int index) const; void interpolateGreatCircle(const SGGeod& aFrom, const SGGeod& aTo, SGGeodVec& r) const; - /** - * Find the distance (in Nm) to climb/descend a height in feet - */ - double distanceForClimb(double climbFt) const; - double magVarFor(const SGGeod& gd) const; + RoutePathPrivate* d; + - flightgear::WayptVec _waypts; - - int _pathClimbFPM; ///< climb-rate to use for pathing - int _pathDescentFPM; ///< descent rate to use (feet-per-minute) - int _pathIAS; ///< IAS (knots) to use for pathing double _pathTurnRate; ///< degrees-per-second, defaults to 3, i.e 180 in a minute }; diff --git a/src/Navaids/waypoint.hxx b/src/Navaids/waypoint.hxx index 377049ab0..f0ed8b890 100644 --- a/src/Navaids/waypoint.hxx +++ b/src/Navaids/waypoint.hxx @@ -272,7 +272,9 @@ public: double radialDegMagnetic() const { return _radial; } - + + virtual double headingRadialDeg() const + { return courseDegMagnetic(); } private: std::string _ident; SGGeod _pos;