From: James Turner Date: Fri, 19 Dec 2014 17:01:27 +0000 (+0000) Subject: Route-path: X-Git-Url: https://git.mxchange.org/?a=commitdiff_plain;h=088572e87dd522c634933d2e4905484f59f0fb19;p=flightgear.git Route-path: * better overflight pathing based on next wpt type * tolerate procedures with bad intersections (eg KSFO OFFSH8) --- diff --git a/src/Instrumentation/rnav_waypt_controller.cxx b/src/Instrumentation/rnav_waypt_controller.cxx index cefc93348..caa6885a8 100644 --- a/src/Instrumentation/rnav_waypt_controller.cxx +++ b/src/Instrumentation/rnav_waypt_controller.cxx @@ -55,50 +55,27 @@ bool geocRadialIntersection(const SGGeoc& a, double r1, const SGGeoc& b, double // crs12=2.*pi-acos((sin(lat2)-sin(lat1)*cos(dst12))/(sin(dst12)*cos(lat1))) // crs21=acos((sin(lat1)-sin(lat2)*cos(dst12))/(sin(dst12)*cos(lat2))) // ENDIF - - - // double diffLon = b.getLongitudeRad() - a.getLongitudeRad(); - + double sinLat1 = sin(a.getLatitudeRad()); double cosLat1 = cos(a.getLatitudeRad()); - // double sinLat2 = sin(b.getLatitudeRad()); - //double cosLat2 = cos(b.getLatitudeRad()); double sinDst12 = sin(dst12); double cosDst12 = cos(dst12); double crs12 = SGGeodesy::courseRad(a, b), crs21 = SGGeodesy::courseRad(b, a); - //double degCrs12 = crs12 * SG_RADIANS_TO_DEGREES; - //double degCrs21 = crs21 * SG_RADIANS_TO_DEGREES; - - /* - if (sin(diffLon) < 0.0) { - crs12 = acos((sinLat2 - sinLat1 * cosDst12) / (sinDst12 * cosLat1)); - crs21 = SGMiscd::twopi() - acos((sinLat1 - sinLat2*cosDst12)/(sinDst12*cosLat2)); - } else { - crs12 = SGMiscd::twopi() - acos((sinLat2 - sinLat1 * cosDst12)/(sinDst12 * cosLat1)); - crs21 = acos((sinLat1 - sinLat2 * cosDst12)/(sinDst12 * cosLat2)); - } - */ - + // 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"); + SG_LOG(SG_INSTR, SG_INFO, "geocRadialIntersection: infinity of intersections"); return false; } if ((sin(ang1)*sin(ang2))<0.0) { - 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:" + SG_LOG(SG_INSTR, SG_INFO, "geocRadialIntersection: intersection ambiguous:" << ang1 << " " << ang2 << " sin1 " << sin(ang1) << " sin2 " << sin(ang2)); return false; } diff --git a/src/Navaids/routePath.cxx b/src/Navaids/routePath.cxx index 4146fa1d0..bc181b227 100644 --- a/src/Navaids/routePath.cxx +++ b/src/Navaids/routePath.cxx @@ -89,6 +89,7 @@ public: turnAngle(0.0), turnRadius(0.0), pathDistanceM(0.0), + turnPathDistanceM(0.0), overflightCompensationAngle(0.0), flyOver(w->flag(WPT_OVERFLIGHT)) { @@ -122,7 +123,16 @@ public: } } // of static waypt } - + + /** + * test if course of this leg can be adjusted or is contrained to an exact value + */ + bool isCourseConstrained() const + { + const std::string& ty(wpt->type()); + return (ty == "hdgToAlt") || (ty == "radialIntercept") || (ty == "dmeIntercept"); + } + // compute leg courses for all static legs (both ends are fixed) void initPass1(const WayptData& previous, WayptData* next) { @@ -141,7 +151,7 @@ public: } } - void computeTurn(double radiusM, const WayptData& previous, const WayptData& next) + void computeTurn(double radiusM, const WayptData& previous, WayptData& next) { assert(legCourseValid && next.legCourseValid); @@ -161,17 +171,39 @@ public: // distance perpendicular to next leg course, after turning // through turnAngle double xtk = turnRadius * (1 - cos(turnAngle * SG_DEGREES_TO_RADIANS)); + + if (next.isCourseConstrained()) { + // next leg course is constrained. We need to swing back onto the + // desired course, using a compensation turn + + // 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; - // 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; + // 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; + turnPathDistanceM = turnRadius + d; + } else { + // next leg course can be adjusted. increase the turn angle + // and modify the next leg's course accordingly. + + // hypotenuse of triangle, opposite edge has length turnRadius + turnPathDistanceM = std::min(1.0, sin(fabs(turnAngle) * SG_DEGREES_TO_RADIANS)) * turnRadius; + double nextLegDistance = SGGeodesy::distanceM(pos, next.pos) - turnPathDistanceM; + double increaseAngle = atan2(xtk, nextLegDistance) * SG_RADIANS_TO_DEGREES; + increaseAngle = copysign(increaseAngle, turnAngle); + + turnAngle += increaseAngle; + turnExitPos = SGGeodesy::direct(turnCenter, legCourse + turnAngle - p, turnRadius); + + // modify next leg course + next.legCourse = SGGeodesy::courseDeg(turnExitPos, next.pos); + + turnPathDistanceM = turnRadius; + } } } else { hasEntry = true; @@ -181,10 +213,10 @@ public: 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); + turnPathDistanceM = turnRadius * tan(fabs(halfAngle) * SG_DEGREES_TO_RADIANS); - turnEntryPos = SGGeodesy::direct(pos, legCourse, -entryExitDistanceAlongPath); - turnExitPos = SGGeodesy::direct(pos, next.legCourse, entryExitDistanceAlongPath); + turnEntryPos = SGGeodesy::direct(pos, legCourse, -turnPathDistanceM); + turnExitPos = SGGeodesy::direct(pos, next.legCourse, turnPathDistanceM); } } @@ -241,8 +273,13 @@ public: p = SGGeodesy::direct(p, h, stepDist); h += stepIncrement; } - - if (flyOver) { + + // we can use an exact check on the compensation angle, becayse we + // initialise it directly. Depending on the next element we might be + // doing compensation or adjusting the next leg's course; if we did + // adjust the course ecerything 'just works' above. + + if (flyOver && (overflightCompensationAngle != 0.0)) { // skew by compensation angle back steps = std::max(SGMiscd::roundToInt(fabs(overflightCompensationAngle) / 3.0), 1); @@ -265,6 +302,7 @@ public: SGGeod pos, turnEntryPos, turnExitPos, turnCenter; double turnAngle, turnRadius, legCourse; double pathDistanceM; + double turnPathDistanceM; // for flyBy, this is half the distance; for flyOver it's the completel distance double overflightCompensationAngle; bool flyOver; }; @@ -359,10 +397,22 @@ public: 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:" + SGGeoc navidAdjusted; + // try pulling backward along the radial in case we're too close. + // suggests bad procedure construction if this is happening! + SGGeodesy::advanceRadM(navid, radial, SG_NM_TO_METER * -10, navidAdjusted); + + // try again + ok = geocRadialIntersection(prevGc, track, navidAdjusted, 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 + waypoints[index].pos = wpt->position(); // horrible fallback + + } else { + waypoints[index].pos = SGGeod::fromGeoc(rGc); + } } else { waypoints[index].pos = SGGeod::fromGeoc(rGc); }