FlightPlan: DISCON works with pointAlongRoute

Add some tests for the default GPS / route-manager handling of
DISCON in flight-plans. Especially check behaviour when sequencing
a DISCON, and resuming with a Direct-to after one. 

This requires a corresponding FGData update to the default GPS Nasal.

src/Navaids/routePath.cxx

@@ -632,7 +632,7 @@ public:
   void computeDynamicPosition(int index)
   {
     auto previous(previousValidWaypoint(index));
-    if ((previous == waypoints.end()) || !previous->posValid);
+    if ((previous == waypoints.end()) || !previous->posValid)
     {
       SG_LOG(SG_NAVAID, SG_WARN, "couldn't compute position for dynamic waypoint: no preceeding valid waypoint");
       return;
@@ -1183,6 +1183,8 @@ SGGeod RoutePath::positionForDistanceFrom(int index, double distanceM) const
       // we want to look at index n-1 (so, 3), and see if this makes
       // distance positive. We need to offset by distance from 3 -> 4,
       // which is waypoint 4's path distance.
+      
+      // note pathDistanceM is 0 for skipped waypoints, so this works out
       distanceM += d->waypoints[index].pathDistanceM;
       --index;
     }
@@ -1201,14 +1203,23 @@ SGGeod RoutePath::positionForDistanceFrom(int index, double distanceM) const
     }
   }
   
-    if ((index + 1) >= sz) {
+  auto nextIt = d->nextValidWaypoint(index);
+  if (nextIt == d->waypoints.end()) {
     // past route end, just return final position
     return d->waypoints[sz - 1].pos;
   }
   
+  // this is important so we start from a valid WP if we're
+  // working either side of a DISCON
+  auto curIt = d->previousValidWaypoint(nextIt);
+  if (curIt == d->waypoints.end()) {
+    SG_LOG(SG_NAVAID, SG_WARN, "Couldn't find valid preceeding waypoint " << index);
+    return nextIt->pos;
+  }
+  
+  const WayptData& wpt = *curIt;
+  const WayptData& next = *nextIt;
   
-    const WayptData& wpt(d->waypoints[index]);
-    const WayptData& next(d->waypoints[index+1]);
   if (next.wpt->type() == "via") {
     return positionAlongVia(static_cast<Via*>(next.wpt.get()), index, distanceM);
   }

test_suite/unit_tests/Navaids/test_flightplan.cxx

@@ -405,6 +405,7 @@ void FlightplanTests::testBasicDiscontinuity()
     
     const SGGeod balenPos = fp1->legAtIndex(3)->waypoint()->position();
     const SGGeod murenPos = fp1->legAtIndex(4)->waypoint()->position();
+    const auto crs = SGGeodesy::courseDeg(balenPos, murenPos);
     
     // total distance should not change
     fp1->insertWayptAtIndex(new Discontinuity(fp1), 4); // betwee BALEN and MUREN
@@ -420,6 +421,20 @@ void FlightplanTests::testBasicDiscontinuity()
     CPPUNIT_ASSERT_DOUBLES_EQUAL(fp1->legAtIndex(5)->distanceNm(),
                                  SGGeodesy::distanceNm(balenPos, murenPos), 0.1);
     
+    // ensure that pointAlongRoute works correctly going into the DISCON
+    
+    
+    const auto pos1 = fp1->pointAlongRoute(3, 20.0);
+    const auto validP1 = SGGeodesy::direct(balenPos, crs, 20.0 * SG_NM_TO_METER);
+    
+    CPPUNIT_ASSERT(SGGeodesy::distanceM(pos1, validP1) < 500.0);
+    
+    const auto pos2 = fp1->pointAlongRoute(5, -10.0);
+    const auto crs2 = SGGeodesy::courseDeg(murenPos, balenPos);
+    const auto validP2 = SGGeodesy::direct(murenPos, crs2, 10.0 * SG_NM_TO_METER);
+    
+    CPPUNIT_ASSERT(SGGeodesy::distanceM(pos2, validP2) < 500.0);
+    
     // remove the discontinuity
     fp1->deleteIndex(4);
     CPPUNIT_ASSERT_DOUBLES_EQUAL(fp1->legAtIndex(4)->courseDeg(),

test_suite/unit_tests/Navaids/test_routeManager.cxx

@@ -279,6 +279,83 @@ void RouteManagerTests::testDirectToLegOnFlightplanAndResume()
     CPPUNIT_ASSERT_EQUAL(std::string{"leg"}, std::string{gpsNode->getStringValue("mode")});
 }
 
+void RouteManagerTests::testSequenceDiscontinuityAndResume()
+{
+    FGTestApi::setUp::logPositionToKML("rm_seq_discon_resume_leg");
+
+    FlightPlanRef fp1 = makeTestFP("LIRF", "16R", "LEBL", "07L",
+                                    "GITRI BALEN MUREN TOSNU");
+    
+    
+    fp1->insertWayptAtIndex(new Discontinuity(fp1), 3);
+
+     auto rm = globals->get_subsystem<FGRouteMgr>();
+     rm->setFlightPlan(fp1);
+
+     auto gpsNode = globals->get_props()->getNode("instrumentation/gps", true);
+   //  auto rmNode = globals->get_props()->getNode("autopilot/route-manager", true);
+
+     CPPUNIT_ASSERT(!strcmp("obs", gpsNode->getStringValue("mode")));
+     rm->activate();
+     
+     CPPUNIT_ASSERT(fp1->isActive());
+
+    auto balenLeg = fp1->legAtIndex(2);
+    auto pos = fp1->pointAlongRoute(2, -8.0); // 8nm before BALEN
+    
+    
+    fp1->setCurrentIndex(2);
+    
+     FGTestApi::setPosition(pos);
+     FGTestApi::runForTime(10.0); // let the GPS stabilize
+
+    auto pilot = SGSharedPtr<FGTestApi::TestPilot>(new FGTestApi::TestPilot);
+    pilot->resetAtPosition(pos);
+    pilot->setCourseTrue(270);
+    pilot->setSpeedKts(250);
+    pilot->flyGPSCourse(m_gps);
+    
+    bool ok = FGTestApi::runForTimeWithCheck(180.0, [gpsNode] () {
+        return (gpsNode->getStringValue("mode") == std::string{"obs"});
+    });
+    
+    CPPUNIT_ASSERT(ok);
+    CPPUNIT_ASSERT_EQUAL(2, fp1->currentIndex()); // shouldn't sequence
+    
+    FGTestApi::runForTime(30.0);
+    
+    
+    // 2nm before MUREN : this should be on the GC course from BALEN
+    auto pos2 = fp1->pointAlongRoute(4, -6.0);
+    FGTestApi::setPosition(pos2);
+    FGTestApi::runForTime(2.0); // let the GPS stabilize
+
+    // initiate a direct-to the next real WP
+    const auto murenPos = fp1->legAtIndex(4)->waypoint()->position();
+   gpsNode->setStringValue("scratch/ident", "MUREN");
+   gpsNode->setDoubleValue("scratch/longitude-deg", murenPos.getLongitudeDeg());
+   gpsNode->setDoubleValue("scratch/latitude-deg", murenPos.getLatitudeDeg());
+   gpsNode->setStringValue("command", "direct");
+   CPPUNIT_ASSERT_EQUAL(std::string{"dto"}, std::string{gpsNode->getStringValue("mode")});
+
+    pilot->resetAtPosition(pos2);
+    pilot->flyGPSCourse(m_gps);
+    
+    ok = FGTestApi::runForTimeWithCheck(600.0, [fp1] () {
+        if (fp1->currentIndex() == 5) {
+            return true;
+        }
+        return false;
+    });
+    
+    CPPUNIT_ASSERT(ok);
+    CPPUNIT_ASSERT_EQUAL(std::string{"leg"}, std::string{gpsNode->getStringValue("mode")});
+    CPPUNIT_ASSERT_EQUAL(5, fp1->currentIndex());
+    
+    FGTestApi::runForTime(30.0);
+
+}
+
 void RouteManagerTests::testDefaultApproach()
 {
     

test_suite/unit_tests/Navaids/test_routeManager.hxx

@@ -38,6 +38,8 @@ class RouteManagerTests : public CppUnit::TestFixture
     CPPUNIT_TEST(testDefaultApproach);
     CPPUNIT_TEST(testDirectToLegOnFlightplanAndResume);
     CPPUNIT_TEST(testHoldFromNasal);
+    CPPUNIT_TEST(testSequenceDiscontinuityAndResume);
+    
     CPPUNIT_TEST_SUITE_END();
 
    // void setPositionAndStabilise(FGNavRadio* r, const SGGeod& g);
@@ -57,6 +59,7 @@ public:
     void testDefaultApproach();
     void testDirectToLegOnFlightplanAndResume();
     void testHoldFromNasal();
+    void testSequenceDiscontinuityAndResume();
 private:
     GPS* m_gps = nullptr;
 };