diff --git a/src/Navaids/routePath.cxx b/src/Navaids/routePath.cxx index 24eaa93c6..dc5335e71 100644 --- a/src/Navaids/routePath.cxx +++ b/src/Navaids/routePath.cxx @@ -368,12 +368,20 @@ public: legCourseValid = true; return; } + + SGGeod previousPos = previous->pos; + // use correct location for runway, otherwise use threshold + // and get weird courses + if (previous->wpt->type() == "runway") { + FGRunway* rwy = static_cast<RunwayWaypt*>(previous->wpt.get())->runway(); + previousPos = rwy->end(); + } if (posValid) { - legCourseTrue = SGGeodesy::courseDeg(previous->pos, pos); + legCourseTrue = SGGeodesy::courseDeg(previousPos, pos); legCourseValid = true; } else if (isCourseConstrained()) { - double magVar = magVarFor(previous->pos); + double magVar = magVarFor(previousPos); legCourseTrue = wpt->headingRadialDeg() + magVar; legCourseValid = true; } // of pos not valid @@ -418,6 +426,7 @@ public: turnExitAngle = next.legCourseTrue - rwy->headingDeg(); legCourseTrue = rwy->headingDeg(); flyOver = true; + // fall through } else { legCourseValid = true; legCourseTrue = next.legCourseTrue; @@ -463,9 +472,14 @@ public: theta = copysign(theta, turnExitAngle); turnExitAngle += theta; - // move by the distance to compensate - double d = turnRadius * 2.0 * sin(theta * SG_DEGREES_TO_RADIANS); - turnExitPos = SGGeodesy::direct(turnExitPos, next.legCourseTrue, d); + double p = copysign(90, turnExitAngle); + const double offsetAngle = legCourseTrue + turnExitAngle - p; + SGGeod tc2 = SGGeodesy::direct(turnExitCenter, + offsetAngle, + turnRadius * 2.0); + + + turnExitPos = SGGeodesy::direct(tc2, next.legCourseTrue + p , turnRadius); overflightCompensationAngle = -theta; // sign of angles will differ, so compute distances seperately @@ -576,23 +590,28 @@ public: SGGeod pointAlongExitPath(double distanceM) const { - double theta = (distanceM / turnRadius) * SG_RADIANS_TO_DEGREES; - double p = copysign(90, turnExitAngle); + double basicTurnDistance = pathDistanceForTurnAngle(turnExitAngle); + + if (distanceM > basicTurnDistance) { + assert(flyOver); + assert(overflightCompensationAngle != 0.0); + double p = copysign(90, turnExitAngle); - if (flyOver && (overflightCompensationAngle != 0.0)) { - // figure out if we're in the compensation section - if (theta > turnExitAngle) { - // compute the compensation turn center - twice the turn radius - // from turnCenter - SGGeod tc2 = SGGeodesy::direct(turnExitCenter, - legCourseTrue - overflightCompensationAngle - p, - turnRadius * 2.0); - theta = copysign(theta - turnExitAngle, overflightCompensationAngle); - return SGGeodesy::direct(tc2, - legCourseTrue - overflightCompensationAngle + theta + p, turnRadius); - } + double distanceAlongCompensationTurn = distanceM - basicTurnDistance; + double theta = (distanceAlongCompensationTurn / turnRadius) * SG_RADIANS_TO_DEGREES; + + // compute the compensation turn center - twice the turn radius + // from turnCenter + SGGeod tc2 = SGGeodesy::direct(turnExitCenter, + legCourseTrue + turnExitAngle - p, + turnRadius * 2.0); + + theta = copysign(theta, overflightCompensationAngle); + return SGGeodesy::direct(tc2, + legCourseTrue + turnExitAngle + theta + p, turnRadius); } - + + double theta = (distanceM / turnRadius) * SG_RADIANS_TO_DEGREES; theta = copysign(theta, turnExitAngle); double inboundCourse = legCourseTrue + (flyOver ? 0.0 : turnExitAngle); return pointOnExitTurnFromHeading(inboundCourse + theta); diff --git a/test_suite/FGTestApi/TestPilot.cxx b/test_suite/FGTestApi/TestPilot.cxx index fa8171277..49d591fcd 100644 --- a/test_suite/FGTestApi/TestPilot.cxx +++ b/test_suite/FGTestApi/TestPilot.cxx @@ -273,4 +273,10 @@ bool TestPilot::isOnHeading(double heading) const return fabs(hdgDelta) < 0.5; } +double TestPilot::trueCourseDeg() const +{ + return _trueCourseDeg; +} + + } // of namespace diff --git a/test_suite/FGTestApi/TestPilot.hxx b/test_suite/FGTestApi/TestPilot.hxx index 6087c8f7c..7558c213d 100644 --- a/test_suite/FGTestApi/TestPilot.hxx +++ b/test_suite/FGTestApi/TestPilot.hxx @@ -63,6 +63,7 @@ public: bool isOnHeading(double heading) const; + double trueCourseDeg() const; private: enum class LateralMode { diff --git a/test_suite/unit_tests/Instrumentation/test_gps.cxx b/test_suite/unit_tests/Instrumentation/test_gps.cxx index 29823c155..bd823a1c1 100644 --- a/test_suite/unit_tests/Instrumentation/test_gps.cxx +++ b/test_suite/unit_tests/Instrumentation/test_gps.cxx @@ -1717,3 +1717,70 @@ void GPSTests::testCourseLegIntermediateWaypoint() }); CPPUNIT_ASSERT(ok); } + +// Test to check the situation where you have two legs forming a straight line +// with the current waypoint being the third waypoint +void GPSTests::testFlyOverMaxInterceptAngle() +{ + //FGTestApi::setUp::logPositionToKML("gps_fly_over_max_intercept_angle"); + auto rm = globals->get_subsystem<FGRouteMgr>(); + auto fp = FlightPlan::create(); + rm->setFlightPlan(fp); + + FGTestApi::setUp::populateFPWithoutNasal(fp, "EGAA", "25", "EGPH", "06", "LISBO BLACA"); + + // takes the place of the Nasal delegates + auto testDelegate = new TestFPDelegate; + testDelegate->thePlan = fp; + CPPUNIT_ASSERT(rm->activate()); + fp->addDelegate(testDelegate); + + FGTestApi::writeFlightPlanToKML(fp); + + SGGeod initPos = fp->pointAlongRouteNorm(1, -0.15); + FGTestApi::writePointToKML("Start pos", initPos); + + auto gps = setupStandardGPS(); + FGTestApi::setPositionAndStabilise(initPos); + + auto gpsNode = globals->get_props()->getNode("instrumentation/gps"); + gpsNode->setBoolValue("config/delegate-sequencing", true); + gpsNode->setStringValue("command", "leg"); + + auto gpsErrorNmNode = gpsNode->getNode("wp/wp[1]/course-error-nm"); + + fp->setCurrentIndex(1); // BLACA + + auto pilot = SGSharedPtr<FGTestApi::TestPilot>(new FGTestApi::TestPilot); + pilot->resetAtPosition(initPos); + pilot->setCourseTrue(fp->legAtIndex(1)->courseDeg()); + pilot->setSpeedKts(280); + pilot->flyGPSCourse(gps); + + bool ok = FGTestApi::runForTimeWithCheck(1200.0, [&fp]() { + return fp->currentIndex() == 2; + }); + CPPUNIT_ASSERT(ok); + + FGPositioned::TypeFilter fixFilter(FGPositioned::FIX); + auto lisbo = FGPositioned::findClosestWithIdent("LISBO", fp->departureAirport()->geod(), &fixFilter); + auto blaca = FGPositioned::findClosestWithIdent("BLACA", fp->departureAirport()->geod(), &fixFilter); + + const double crsToBlaca = SGGeodesy::courseDeg(lisbo->geod(), blaca->geod()); + + ok = FGTestApi::runForTimeWithCheck(1200.0, [pilot, crsToBlaca, gpsErrorNmNode]() + { + const double heading = pilot->trueCourseDeg(); + if (heading < (crsToBlaca - 45.0)) { + CPPUNIT_FAIL("Turned too far on intercept"); + } + + // check if we're near the leg + if (fabs(gpsErrorNmNode->getDoubleValue()) > 0.05) { + return false; + } + + return pilot->isOnHeading(crsToBlaca); + }); + CPPUNIT_ASSERT(ok); +} diff --git a/test_suite/unit_tests/Instrumentation/test_gps.hxx b/test_suite/unit_tests/Instrumentation/test_gps.hxx index 03986d2f7..0f7e5da1a 100644 --- a/test_suite/unit_tests/Instrumentation/test_gps.hxx +++ b/test_suite/unit_tests/Instrumentation/test_gps.hxx @@ -57,7 +57,8 @@ class GPSTests : public CppUnit::TestFixture CPPUNIT_TEST(testFinalLegCourse); CPPUNIT_TEST(testCourseLegIntermediateWaypoint); CPPUNIT_TEST(testExceedFlyByMaxAngleTurn); - + CPPUNIT_TEST(testFlyOverMaxInterceptAngle); + CPPUNIT_TEST_SUITE_END(); void setPositionAndStabilise(GPS* gps, const SGGeod& g); @@ -94,6 +95,7 @@ public: void testFinalLegCourse(); void testCourseLegIntermediateWaypoint(); void testExceedFlyByMaxAngleTurn(); + void testFlyOverMaxInterceptAngle(); }; #endif // _FG_GPS_UNIT_TESTS_HXX diff --git a/test_suite/unit_tests/Navaids/test_flightplan.cxx b/test_suite/unit_tests/Navaids/test_flightplan.cxx index e80c7dbae..a4b96c16d 100644 --- a/test_suite/unit_tests/Navaids/test_flightplan.cxx +++ b/test_suite/unit_tests/Navaids/test_flightplan.cxx @@ -359,7 +359,7 @@ void FlightplanTests::testRoutePathTrivialFlightPlan() rtepath.distanceForIndex(leg); } - CPPUNIT_ASSERT_DOUBLES_EQUAL(19.68, fp1->totalDistanceNm(), 0.1); + CPPUNIT_ASSERT_DOUBLES_EQUAL(19.5, fp1->totalDistanceNm(), 0.1); } void FlightplanTests::testRoutePathVec()