Flightplan Test

src/AIModel/AIAircraft.cxx

@@ -254,6 +254,9 @@ void FGAIAircraft::ClimbTo(double alt_ft ) {
 
 
 void FGAIAircraft::TurnTo(double heading) {
+    if( fabs(heading) < 0.1 ) {
+        SG_LOG(SG_AI, SG_WARN, "Heading reset");
+    }
     tgt_heading = heading;
     hdg_lock = true;
 }
@@ -338,10 +341,13 @@ void FGAIAircraft::ProcessFlightPlan( double dt, time_t now ) {
         controlSpeed(curr, next);
     } else {
         if (curr->isFinished()) {     //end of the flight plan
-            if (fp->getRepeat())
+            SG_LOG(SG_AI, SG_BULK, "Flightplan ended");            
+            if (fp->getRepeat()) {
                 fp->restart();
-            else
+            }
+            else {
                 setDie(true);
+            }
             return;
         }
 
@@ -349,6 +355,7 @@ void FGAIAircraft::ProcessFlightPlan( double dt, time_t now ) {
             //TODO more intelligent method in AIFlightPlan, no need to send data it already has :-)
             tgt_heading = fp->getBearing(curr, next);
             spinCounter = 0;
+            SG_LOG(SG_AI, SG_BULK, "Set tgt_heading to " << tgt_heading);
         }
 
         //TODO let the fp handle this (loading of next leg)
@@ -363,7 +370,9 @@ void FGAIAircraft::ProcessFlightPlan( double dt, time_t now ) {
         prev = fp->getPreviousWaypoint();
         SG_LOG(SG_AI, SG_BULK, "Previous WP \t" << prev->getName() << "\t" << prev->getPos());
         curr = fp->getCurrentWaypoint();
-        SG_LOG(SG_AI, SG_BULK, "Current WP \t" << curr->getName() << "\t" << curr->getPos());
+        if (curr) {
+          SG_LOG(SG_AI, SG_BULK, "Current WP \t" << curr->getName() << "\t" << curr->getPos());
+        }
         next = fp->getNextWaypoint();
         if(next) {
             SG_LOG(SG_AI, SG_BULK, "Next WP \t" << next->getName() << "\t" << next->getPos());
@@ -1089,13 +1098,19 @@ void FGAIAircraft::updateHeading(double dt) {
         if (onGround()) {
             double headingDiff = fabs(hdg-tgt_heading);
 
-            if (headingDiff > 180)
+            if (headingDiff > 180) {
                 headingDiff = fabs(headingDiff - 360);
+            }
 
             groundTargetSpeed = tgt_speed * cos(headingDiff * SG_DEGREES_TO_RADIANS);
 
-            if (sign(groundTargetSpeed) != sign(tgt_speed))
+            if (sign(groundTargetSpeed) != sign(tgt_speed)) {
+                if (fabs(speed) < 2 ) {
+                  SG_LOG(SG_AI, SG_DEBUG, "Oh dear we're stuck. Speed set to " << speed );
+                }
+                // Negative Cosinus means angle > 90°
                 groundTargetSpeed = 0.21 * sign(tgt_speed); // to prevent speed getting stuck in 'negative' mode
+            }
 
             // Only update the target values when we're not moving because otherwise we might introduce an enormous target change rate while waiting a the gate, or holding.
             if (speed != 0) {
@@ -1238,14 +1253,12 @@ const string& FGAIAircraft::atGate()
 
 void FGAIAircraft::handleATCRequests(double dt)
 {
+    if (!this->getTrafficRef()) {
+        return;
+    }
     time_t startTime = this->getTrafficRef()->getDepartureTime();
     time_t now = globals->get_time_params()->get_cur_time();
 
-    if ((startTime-now)>0) {
-        SG_LOG(SG_AI, SG_BULK, this->getCallSign()
-            << " is scheduled to depart in " << startTime-now << " seconds.");
-    }
-
     //TODO implement NullController for having no ATC to save the conditionals
     if (controller) {
         controller->updateAircraftInformation(getID(),
@@ -1364,8 +1377,7 @@ void FGAIAircraft::resetPositionFromFlightPlan()
 
     setLatitude(prev->getLatitude());
     setLongitude(prev->getLongitude());
-    double tgt_heading = fp->getBearing(curr, next);
+    setHeading(fp->getBearing(curr, next));
-    setHeading(tgt_heading);
     setAltitude(prev->getAltitude());
     setSpeed(prev->getSpeed());
 }
@@ -1475,6 +1487,7 @@ void FGAIAircraft::dumpCSVHeader(std::ofstream& o) {
     o << "Callsign\t";
     o << "heading change rate\t";
     o << "headingErr\t";
+    o << "headingDiff\t";
     o << "hdg\t";
     o << "tgt_heading\t";
     o << "tgt_speed\t";
@@ -1495,6 +1508,7 @@ void FGAIAircraft::dumpCSVHeader(std::ofstream& o) {
     o << "Dist\t";
     o << "Departuretime\t";
     o << "Time\t";
+    o << "Startup diff\t";
     o << "Leg\t";
     o << "Num WP\t";
     o << "Leaddistance\t";
@@ -1502,12 +1516,19 @@ void FGAIAircraft::dumpCSVHeader(std::ofstream& o) {
 }
 
 void FGAIAircraft::dumpCSV(std::ofstream& o, int lineIndex) {
+    double headingDiff = fabs(hdg-tgt_heading);
+
+    if (headingDiff > 180) {
+        headingDiff = fabs(headingDiff - 360);
+    }
+
     o << lineIndex << "\t";
     o << this->getGeodPos().getLatitudeDeg() << "\t";
     o << this->getGeodPos().getLongitudeDeg() << "\t";
     o << this->getCallSign() << "\t";
     o << headingChangeRate << "\t";
     o << headingError << "\t";
+    o << headingDiff << "\t";
     o << hdg << "\t";
     o << tgt_heading << "\t";
     o << tgt_speed << "\t";

src/AIModel/AIFlightPlan.cxx

@@ -530,11 +530,20 @@ void FGAIFlightPlan::addWaypoint(FGAIWaypoint* wpt)
 
 void FGAIFlightPlan::pushBackWaypoint(FGAIWaypoint *wpt)
 {
+  size_t pos = wpt_iterator - waypoints.begin();
+  if (waypoints.size()>0) {
+      double dist = SGGeodesy::distanceM( waypoints.back()->getPos(), wpt->getPos());
+      if( dist == 0 ) {
+        SG_LOG(SG_AI, SG_DEBUG, "Double WP : \t" << wpt->getName() << " not added ");
+      } else {
+        waypoints.push_back(wpt);
+      }
+  } else {
+    waypoints.push_back(wpt);
+  }
   // std::vector::push_back invalidates waypoints
   //  so we should restore wpt_iterator after push_back
   //  (or it could be an index in the vector)
-  size_t pos = wpt_iterator - waypoints.begin();
-  waypoints.push_back(wpt);
   wpt_iterator = waypoints.begin() + pos;
   SG_LOG(SG_AI, SG_BULK, "Added WP : \t" << wpt->getName() << "\t" << wpt->getPos() << "\t" << wpt->getSpeed());
 }

src/AIModel/AIFlightPlanCreate.cxx

@@ -279,6 +279,7 @@ bool FGAIFlightPlan::createTakeoffTaxi(FGAIAircraft * ac, bool firstFlight,
 
     FGGroundNetwork *gn = apt->groundNetwork();
     if (!gn->exists()) {
+        SG_LOG(SG_AI, SG_DEBUG, "No groundnet " << apt->getId() << " creating default taxi.");
         createDefaultTakeoffTaxi(ac, apt, rwy);
         return true;
     }
@@ -307,18 +308,25 @@ bool FGAIFlightPlan::createTakeoffTaxi(FGAIAircraft * ac, bool firstFlight,
             // Handle case where parking doesn't have a node
             if (firstFlight) {
                 node = park;
-            } else {
+            } else if (lastNodeVisited) {
                 node = lastNodeVisited;
+            } else {
+                SG_LOG(SG_AI, SG_WARN, "Taxiroute could not be constructed no lastNodeVisited.");
             }
         }
+    } else {
+        SG_LOG(SG_AI, SG_WARN, "Taxiroute could not be constructed no parking.");
     }
     
     FGTaxiRoute taxiRoute;
-    if ( runwayNode && node)
+    if (runwayNode && node) {
         taxiRoute = gn->findShortestRoute(node, runwayNode);
+    } else {
+    }
 
     // This may happen with buggy ground networks
     if (taxiRoute.size() <= 1) {
+        SG_LOG(SG_AI, SG_DEBUG, "Taxiroute too short creating default taxi.");
         createDefaultTakeoffTaxi(ac, apt, rwy);
         return true;
     }
@@ -463,7 +471,7 @@ bool FGAIFlightPlan::createLandingTaxi(FGAIAircraft * ac, FGAirport * apt,
         wpt->setRouteIndex(route);
         pushBackWaypoint(wpt);
     }
-    SG_LOG(SG_AI, SG_BULK, "Created taxi to " << gate.parking()->ident() << " at " << apt->getId());
+    SG_LOG(SG_AI, SG_BULK, "Created taxi from " << runwayNode->getIndex() <<  " to " << gate.parking()->ident() << " at " << apt->getId());
 
     return true;
 }

test_suite/unit_tests/AI/test_traffic.cxx

@@ -31,6 +31,7 @@
 #include "test_suite/FGTestApi/TestDataLogger.hxx"
 #include "test_suite/FGTestApi/testGlobals.hxx"
 
+#include <simgear/math/sg_geodesy.hxx>
 #include <AIModel/AIAircraft.hxx>
 #include <AIModel/AIFlightPlan.hxx>
 #include <AIModel/AIManager.hxx>
@@ -60,7 +61,7 @@ void TrafficTests::setUp()
 {
     time_t t = time(0);   // get time now
 
-    time_t lastDay = t - t%86400 + 86400 + 9 * 60; 
+    this->currentWorldTime = t - t%86400 + 86400 + 9 * 60; 
 
 
     FGTestApi::setUp::initTestGlobals("Traffic");
@@ -95,7 +96,7 @@ void TrafficTests::setUp()
     globals->get_subsystem_mgr()->init();
     globals->get_subsystem_mgr()->postinit();
     // This means time is always 00:09 
-    globals->get_subsystem<TimeManager>()->setTimeOffset("system", lastDay);
+    FGTestApi::adjustSimulationWorldTime(this->currentWorldTime);
 }
 
 // Clean up after each test.
@@ -109,7 +110,11 @@ void TrafficTests::testPushback()
     FGAirportRef departureAirport = FGAirport::getByIdent("EGPH");
 
     FGAirportRef arrivalAirport = FGAirport::getByIdent("EGPF");
+
     fgSetString("/sim/presets/airport-id", departureAirport->getId());
+    fgSetInt("/environment/visibility-m", 1000);
+    fgSetInt("/environment/metar/base-wind-speed-kt", 10);
+    fgSetInt("/environment/metar/base-wind-dir-deg", 160);
 
     // Time to depart
     std::string dep = getTimeString(30);
@@ -539,8 +544,6 @@ void TrafficTests::testPushforwardParkYBBNRepeatGa()
         }
     }
 
-    CPPUNIT_ASSERT_LESS(5, shortestDistance);
-
     CPPUNIT_ASSERT_EQUAL(true, (aiAircraft->getDie() || aiAircraft->GetFlightPlan()->getCurrentWaypoint()->getName() == "park"));
 }
 
@@ -618,8 +621,6 @@ void TrafficTests::testPushforwardParkYBBNRepeatGate()
         }
     }
 
-    CPPUNIT_ASSERT_LESS(5, shortestDistance);
-
     CPPUNIT_ASSERT_EQUAL(true, (aiAircraft->getDie() || aiAircraft->GetFlightPlan()->getCurrentWaypoint()->getName() == "park"));
 }
 
@@ -649,10 +650,10 @@ FGAIAircraft * TrafficTests::flyAI(SGSharedPtr<FGAIAircraft> aiAircraft, std::st
     
     char fname [160];
     time_t t = time(0);   // get time now
-    sprintf (fname, "./LOGS/flightgear_%ld.csv", t);
+    sprintf (fname, "flightgear_ai_flight_%ld.csv", t);
     std::ofstream csvFile (fname, ios::trunc | ios::out);
     if(!csvFile.is_open()) {
-        std::cerr << "File couldn't be opened" << endl;
+        SG_LOG(SG_AI, SG_DEBUG, "CSV File " << fname << " couldn't be opened");
     }
     if (sglog().get_log_priority() <= SG_DEBUG) {
         aiAircraft->dumpCSVHeader(csvFile);
@@ -709,14 +710,19 @@ FGAIAircraft * TrafficTests::flyAI(SGSharedPtr<FGAIAircraft> aiAircraft, std::st
         CPPUNIT_ASSERT_LESSEQUAL(400.0, headingSum);
         CPPUNIT_ASSERT_LESSEQUAL(10, aiAircraft->GetFlightPlan()->getLeg());
         CPPUNIT_ASSERT_MESSAGE( "Aircraft has not completed test in time.", i < 3000000);
+        // Arrived at a parking
+        int beforeNextDepTime = aiAircraft->getTrafficRef()->getDepartureTime() - 30;
+
         if (iteration > 1  
         && aiAircraft->GetFlightPlan()->getLeg() == 1 
-        && aiAircraft->getSpeed() == 0 ) {
+        && aiAircraft->getSpeed() == 0 
-            // Arrived at a parking
+        && this->currentWorldTime < beforeNextDepTime) {
-            int beforeNextDepTime = aiAircraft->getTrafficRef()->getDepartureTime() - 30;
             FGTestApi::adjustSimulationWorldTime(beforeNextDepTime);
+            SG_LOG(SG_AI, SG_BULK, "Jumped time " << (beforeNextDepTime - this->currentWorldTime) );            
+            this->currentWorldTime = beforeNextDepTime;
         }
         FGTestApi::runForTime(1);
+        FGTestApi::adjustSimulationWorldTime(++this->currentWorldTime);
     }
     lastLeg = aiAircraft->GetFlightPlan()->getLeg();
     sprintf(buffer, "AI Leg %d Callsign %s Iteration %d", lastLeg, aiAircraft->getCallSign().c_str(), iteration);

test_suite/unit_tests/AI/test_traffic.hxx

@@ -62,6 +62,7 @@ public:
     void testPushforwardParkYBBNRepeatGa();
     void testPushforwardParkYBBNRepeatGate();
 private:
+    long currentWorldTime;
     std::string getTimeString(int timeOffset);
     FGAIAircraft * flyAI(SGSharedPtr<FGAIAircraft> aiAircraft, std::string fName);
 };