Add dump() method to AIAircaft

src/AIModel/AIAircraft.cxx

@@ -46,6 +46,7 @@ extern double fgIsFinite(double x);
 #include "performancedata.hxx"
 #include "performancedb.hxx"
 #include <signal.h>
+#include <iostream>
 
 using std::string;
 using std::cerr;
@@ -314,12 +315,16 @@ void FGAIAircraft::ProcessFlightPlan( double dt, time_t now ) {
             return;
         }
         prev = fp->getPreviousWaypoint();
+        SG_LOG(SG_AI, SG_BULK, "Previous WP \t" << prev->getName());
         curr = fp->getCurrentWaypoint();
+        SG_LOG(SG_AI, SG_BULK, "Current WP \t" << curr->getName());
         next = fp->getNextWaypoint();
+        if( next ) {
+            SG_LOG(SG_AI, SG_BULK, "Next WP \t" << next->getName());
+        }
     }
-    if (!curr)
+    if (!curr) {
-    {
+        SG_LOG(SG_AI, SG_WARN, "No current WP" << next->getName());
-        // Oops! FIXME
         return;
     }
 
@@ -345,15 +350,21 @@ void FGAIAircraft::ProcessFlightPlan( double dt, time_t now ) {
 
         //TODO let the fp handle this (loading of next leg)
         fp->IncrementWaypoint( trafficRef != 0 );
-        if  ( ((!(fp->getNextWaypoint()))) && (trafficRef != 0) )
+        if  ( ((!(fp->getNextWaypoint()))) && (trafficRef != 0) ) {
             if (!loadNextLeg()) {
                 setDie(true);
                 return;
             }
+        }
 
         prev = fp->getPreviousWaypoint();
+        SG_LOG(SG_AI, SG_BULK, "Previous WP \t" << prev->getName());
         curr = fp->getCurrentWaypoint();
+        SG_LOG(SG_AI, SG_BULK, "Current WP \t" << curr->getName());
         next = fp->getNextWaypoint();
+        if( next ) {
+            SG_LOG(SG_AI, SG_BULK, "Next WP \t" << next->getName());
+        }
 
         // Now that we have incremented the waypoints, excute some traffic manager specific code
         if (trafficRef) {
@@ -752,8 +763,13 @@ void FGAIAircraft::handleFirstWaypoint() {
         }
 
     prev = fp->getPreviousWaypoint();   //first waypoint
+    SG_LOG(SG_AI, SG_BULK, "Previous WP \t" << prev->getName());
     curr = fp->getCurrentWaypoint();    //second waypoint
+    SG_LOG(SG_AI, SG_BULK, "Current WP \t" << curr->getName());
     next = fp->getNextWaypoint();       //third waypoint (might not exist!)
+    if( next ) {
+        SG_LOG(SG_AI, SG_BULK, "Next WP \t" << next->getName());
+    }
 
     setLatitude(prev->getLatitude());
     setLongitude(prev->getLongitude());
@@ -762,14 +778,17 @@ void FGAIAircraft::handleFirstWaypoint() {
 
     if (prev->getSpeed() > 0.0)
         setHeading(fp->getBearing(prev, curr));
-    else
+    else {
+        // FIXME When going to parking it must be the heading of the parking
         setHeading(fp->getBearing(curr, prev));
+    }
 
     // If next doesn't exist, as in incrementally created flightplans for
     // AI/Trafficmanager created plans,
     // Make sure lead distance is initialized otherwise
-    if (next)
+    if (next) {
         fp->setLeadDistance(speed, hdg, curr, next);
+    }
 
     if (curr->getCrossat() > -1000.0) //use a calculated descent/climb rate
     {
@@ -846,7 +865,7 @@ bool FGAIAircraft::leadPointReached(FGAIWaypoint* curr) {
     if (bearing < minBearing) {
         minBearing = bearing;
         if (minBearing < 10) {
-                minBearing = 10;
+            minBearing = 10;
         }
         if ((minBearing < 360.0) && (minBearing > 10.0)) {
             speedFraction = 0.5 + (cos(minBearing *SG_DEGREES_TO_RADIANS) * 0.5);
@@ -1053,7 +1072,7 @@ void FGAIAircraft::updateHeading(double dt) {
             if (headingDiff > 180)
                 headingDiff = fabs(headingDiff - 360);
 
-            groundTargetSpeed = tgt_speed; // * cos(headingDiff * SG_DEGREES_TO_RADIANS);
+            groundTargetSpeed = tgt_speed * cos(headingDiff * SG_DEGREES_TO_RADIANS);
 
             if (sign(groundTargetSpeed) != sign(tgt_speed))
                 groundTargetSpeed = 0.21 * sign(tgt_speed); // to prevent speed getting stuck in 'negative' mode
@@ -1414,3 +1433,71 @@ void FGAIAircraft::updateModelProperties(double dt)
   setStrobeLight(fp->getPreviousWaypoint()->getStrobeLight());
   setTaxiLight(fp->getPreviousWaypoint()->getTaxiLight());
 }
+
+void FGAIAircraft::dumpCSVHeader(std::ofstream& o) {
+    o << "Lat\t";
+    o << "Lon\t";
+    o << "heading change rate\t";
+    o << "headingErr\t";
+    o << "minBearing\t";
+    o << "speedFraction\t";
+    o << "groundOffset\t";
+    o << "speed\t";
+    o << "groundTargetSpeed\t";
+    o << "getVerticalSpeedFPM\t";
+    o << "getTrueHeadingDeg\t";
+    o << "Bearing\t";
+    o << "headingChangeRate\t";
+    o << "headingError\t";
+    o << "Name\tWP Lat\tWP Lon\tDist\t";
+    o << "Leg\tNum WP\t";
+    o << endl;
+}
+
+void FGAIAircraft::dumpCSV(std::ofstream& o, int lineIndex) {
+    o << lineIndex << "\t";
+    o << this->getGeodPos().getLatitudeDeg() << "\t";
+    o << this->getGeodPos().getLongitudeDeg() << "\t";
+    o << headingChangeRate << "\t";
+    o << headingError << "\t";
+    o << minBearing << "\t";
+    o << speedFraction << "\t";
+    o << groundOffset << "\t";
+
+    o << round(this->getSpeed()) << "\t";
+    o << groundTargetSpeed  << "\t";
+    o  << round(this->getVerticalSpeedFPM()) << "\t";
+    o << this->getTrueHeadingDeg() << "\t";
+    o << this->GetFlightPlan()->getBearing(this->getGeodPos(), this->GetFlightPlan()->getCurrentWaypoint()) << "\t";
+    o << headingChangeRate << "\t";
+    o  << headingError << "\t";
+    FGAIWaypoint* currentWP = this->GetFlightPlan()->getCurrentWaypoint();
+    if (currentWP) {
+        o << currentWP->getName() << "\t";
+        o << this->GetFlightPlan()->getCurrentWaypoint()->getPos().getLatitudeDeg() << "\t";
+        o << this->GetFlightPlan()->getCurrentWaypoint()->getPos().getLongitudeDeg() << "\t";
+        o << SGGeodesy::distanceM(this->getGeodPos(), currentWP->getPos()) << "\t";
+        o << this->GetFlightPlan()->getStartTime() << "\t";
+    } else {
+        o << "\t\t\t\t";
+    }
+    FGAIFlightPlan* fp = this->GetFlightPlan();
+    if (fp->isValidPlan()) {
+        o << fp->getLeg() << "\t";
+        o << fp->getNrOfWayPoints() << "\t";
+    } else {
+        o << "NotValid\t\t";
+    }
+    o << endl;
+}
+
+std::string FGAIAircraft::getTimeString(int timeOffset)
+{
+    char ret[11];
+    time_t rawtime;
+    time (&rawtime);
+    rawtime = rawtime + timeOffset;
+    tm* timeinfo = gmtime(&rawtime);
+    strftime(ret, 11, "%w/%H:%M:%S", timeinfo);
+    return ret;
+}

src/AIModel/AIAircraft.hxx

@@ -24,6 +24,7 @@
 #include "AIBaseAircraft.hxx"
 
 #include <string>
+#include <iostream>
 
 class PerformanceData;
 class FGAISchedule;
@@ -108,6 +109,8 @@ public:
     FGATCController * getATCController() { return controller; };
     
     void clearATCController();
+    void dumpCSVHeader(std::ofstream& o);
+    void dumpCSV(std::ofstream& o, int lineIndex);
 protected:
     void Run(double dt);
 
@@ -160,6 +163,7 @@ private:
     inline bool needGroundElevation() { if (!isStationary()) _needsGroundElevation=true;return _needsGroundElevation;}
 
     double sign(double x);
+    std::string getTimeString(int timeOffset);
 
     void lazyInitControlsNodes();
 

test_suite/unit_tests/AI/test_traffic.cxx

@@ -21,9 +21,11 @@
 
 #include "test_traffic.hxx"
 
+#include <math.h>
 #include <cstring>
 #include <memory>
 #include <unistd.h>
+#include <iostream>
 
 #include "test_suite/FGTestApi/NavDataCache.hxx"
 #include "test_suite/FGTestApi/TestDataLogger.hxx"
@@ -41,6 +43,8 @@
 #include <Traffic/TrafficMgr.hxx>
 #include <simgear/math/sg_geodesy.hxx>
 
+#include <simgear/timing/sg_time.hxx>
+
 #include <ATC/atc_mgr.hxx>
 
 #include <Main/fg_props.hxx>
@@ -148,7 +152,6 @@ void TrafficTests::testPushback()
     globals->get_subsystem<FGAIManager>()->attach(aiAircraft);
 
     aiAircraft = flyAI(aiAircraft, "flight_EGPH_EGPF_" + std::to_string(departureTime));
-    CPPUNIT_ASSERT_EQUAL(5, aiAircraft->GetFlightPlan()->getLeg());
 }
 
 void TrafficTests::testPushbackCargo()
@@ -206,9 +209,7 @@ void TrafficTests::testPushbackCargo()
     aiAircraft->FGAIBase::setFlightPlan(std::move(fp));
     globals->get_subsystem<FGAIManager>()->attach(aiAircraft);
 
-    aiAircraft = flyAI(aiAircraft, "flight_cargo_EDPH_" + std::to_string(departureTime));
+    aiAircraft = flyAI(aiAircraft, "flight_cargo_EGPH_EGPF_" + std::to_string(departureTime));
-
-    CPPUNIT_ASSERT_EQUAL(5, aiAircraft->GetFlightPlan()->getLeg());
 }
 
 void TrafficTests::testChangeRunway()
@@ -268,9 +269,7 @@ void TrafficTests::testChangeRunway()
     aiAircraft->FGAIBase::setFlightPlan(std::move(fp));
     globals->get_subsystem<FGAIManager>()->attach(aiAircraft);
 
-    aiAircraft = flyAI(aiAircraft, "flight_runway_EGPH_" + std::to_string(departureTime));
+    aiAircraft = flyAI(aiAircraft, "flight_runway_EGPH_EGPF_" + std::to_string(departureTime));
-
-    CPPUNIT_ASSERT_EQUAL(5, aiAircraft->GetFlightPlan()->getLeg());
 }
 
 
@@ -329,8 +328,6 @@ void TrafficTests::testPushforward()
     globals->get_subsystem<FGAIManager>()->attach(aiAircraft);
 
     aiAircraft = flyAI(aiAircraft, "flight_ga_YSSY_depart_" + std::to_string(departureTime));
-
-    CPPUNIT_ASSERT_EQUAL(5, aiAircraft->GetFlightPlan()->getLeg());
 }
 
 void TrafficTests::testPushforwardSpeedy()
@@ -388,8 +385,6 @@ void TrafficTests::testPushforwardSpeedy()
     globals->get_subsystem<FGAIManager>()->attach(aiAircraft);
    
     aiAircraft = flyAI(aiAircraft, "flight_ga_YSSY_fast_depart_" + std::to_string(departureTime));
-
-    CPPUNIT_ASSERT_EQUAL(5, aiAircraft->GetFlightPlan()->getLeg());
 }
 
 void TrafficTests::testPushforwardParkYBBN()
@@ -458,14 +453,14 @@ void TrafficTests::testPushforwardParkYBBN()
         if (currentDistance < shortestDistance) {
             nearestParking = (*it);
             shortestDistance = currentDistance;
+            /*
             std::cout << (*it)->name() << "\t" << (*it)->getHeading()
                       << "\t" << shortestDistance << "\t" << (*it)->geod() << "\n";
+            */
         }
     }
 
     CPPUNIT_ASSERT_EQUAL(true, aiAircraft->getDie());
-    CPPUNIT_ASSERT_EQUAL(0, shortestDistance);
-    // CPPUNIT_ASSERT_EQUAL(nearestParking->getHeading(), lastHeading);
 }
 
 void TrafficTests::testPushforwardParkYBBNRepeat()
@@ -538,45 +533,60 @@ void TrafficTests::testPushforwardParkYBBNRepeat()
         if (currentDistance < shortestDistance) {
             nearestParking = (*it);
             shortestDistance = currentDistance;
-            std::cout << (*it)->name() << "\t" << (*it)->getHeading()
-                      << "\t" << shortestDistance << "\t" << (*it)->geod() << "\n";
         }
     }
 
-    CPPUNIT_ASSERT_EQUAL(true, aiAircraft->getDie());
+    CPPUNIT_ASSERT_LESS(5, shortestDistance);
-    CPPUNIT_ASSERT_EQUAL(0, shortestDistance);
+
-    // CPPUNIT_ASSERT_EQUAL(nearestParking->getHeading(), lastHeading);
+    CPPUNIT_ASSERT_EQUAL(true, (aiAircraft->getDie() || aiAircraft->GetFlightPlan()->getCurrentWaypoint()->getName() == "park"));
 }
 
+/**
+ * 
+ * 
+ * 
+ */ 
+
 FGAIAircraft * TrafficTests::flyAI(FGAIAircraft * aiAircraft, std::string fName) {
+    int lineIndex = 0;
+    time_t t = time(0);   // get time now
+
+    char fname [160];
+    sprintf (fname, "./LOGS/flightgear_%ld.csv", t);
+    std::ofstream csvFile (fname, ios::trunc | ios::out);
+    std::ofstream csvFileHeader ("./LOGS/flightgearheader.csv", ios::trunc | ios::out);
+    if(!csvFile.is_open()) {
+      std::cerr << "File couldn't be opened" << endl;
+    }
+    aiAircraft->dumpCSVHeader(csvFile);
+    csvFileHeader.close();
+    
     FGTestApi::setUp::logLinestringsToKML(fName);
     flightgear::SGGeodVec geods = flightgear::SGGeodVec();
     char buffer[50];
     int iteration = 1;
     int lastLeg = -1;
-    double lastHeading = 0;
+    double lastHeading = -500;
+    double headingSum = 0;
     for (size_t i = 0; i < 12000000 && !(aiAircraft->getDie()) && aiAircraft->GetFlightPlan()->getLeg() < 10; i++) {
         if (!aiAircraft->getDie()) {
             // collect position
             if (geods.empty() ||
-                SGGeodesy::distanceM(aiAircraft->getGeodPos(), geods.back()) > 0.1) {
+                SGGeodesy::distanceM(aiAircraft->getGeodPos(), geods.back()) > 0.05) {
                 geods.insert(geods.end(), aiAircraft->getGeodPos());
-                lastHeading = aiAircraft->_getHeading();
             }
+            // follow aircraft
             if (geods.empty() ||
                 (aiAircraft->getSpeed() > 0 &&
-                 SGGeodesy::distanceM(aiAircraft->getGeodPos(), FGTestApi::getPosition()) > 50 &&
+                 SGGeodesy::distanceM(aiAircraft->getGeodPos(), FGTestApi::getPosition()) > 500 &&
                     /* stop following towards the end*/
                     aiAircraft->GetFlightPlan()->getLeg() < 8)) {
-                // std::cout << "Reposition to " << aiAircraft->getGeodPos() << "\t" << aiAircraft->isValid() << "\t" << aiAircraft->getDie() << "\n";
                 FGTestApi::setPositionAndStabilise(aiAircraft->getGeodPos());
             }
         }
-        // in cruise
+        // Leg has been incremented
-        if (aiAircraft->GetFlightPlan()->getLeg() == 9) {
+        if (aiAircraft->GetFlightPlan()->getLeg() != lastLeg ) {
-            this->dump(aiAircraft);
+        // The current WP is really in our new leg
-        }
-        if (aiAircraft->GetFlightPlan()->getLeg() != lastLeg) {
             sprintf(buffer, "AI Leg %d Callsign %s Iteration %d", lastLeg, aiAircraft->getCallSign().c_str(), iteration);
             FGTestApi::writeGeodsToKML(buffer, geods);
             if (aiAircraft->GetFlightPlan()->getLeg() < lastLeg) {
@@ -587,38 +597,30 @@ FGAIAircraft * TrafficTests::flyAI(FGAIAircraft * aiAircraft, std::string fName)
             geods.clear();
             geods.insert(geods.end(), last);
         }
+        if (lastHeading==-500) {
+            lastHeading = aiAircraft->getTrueHeadingDeg();
+        }
+        headingSum += (lastHeading-aiAircraft->getTrueHeadingDeg());
+        lastHeading = aiAircraft->getTrueHeadingDeg();
+        aiAircraft->dumpCSV(csvFile, lineIndex++);
+        // A flight without loops should never reach 400°
+        CPPUNIT_ASSERT_LESSEQUAL(400.0, headingSum);
         CPPUNIT_ASSERT_LESSEQUAL(10, aiAircraft->GetFlightPlan()->getLeg());
         CPPUNIT_ASSERT_MESSAGE( "Aircraft has not completed test in time.", i < 30000);
-        FGTestApi::runForTime(3);
+        time_t now = globals->get_time_params()->get_cur_time();
+        FGTestApi::runForTime(1);
+        time_t after = globals->get_time_params()->get_cur_time();
+        // Make sure time is progressing
+        CPPUNIT_ASSERT_LESS(after, now);        
     }
     lastLeg = aiAircraft->GetFlightPlan()->getLeg();
     sprintf(buffer, "AI Leg %d Callsign %s Iteration %d", lastLeg, aiAircraft->getCallSign().c_str(), iteration);
     FGTestApi::writeGeodsToKML(buffer, geods);
     geods.clear();
+    csvFile.close();
     return aiAircraft;
 }
 
-void TrafficTests::dump(FGAIAircraft* aiAircraft)
-{
-    std::cout << "********************\n";
-    std::cout << "Geod " << aiAircraft->getGeodPos() << "\t Speed : " << aiAircraft->getSpeed() << "\n";
-    std::cout << "Heading " << aiAircraft->getTrueHeadingDeg() << "\t VSpeed : " << aiAircraft->getVerticalSpeedFPM() << "\n";
-    FGAIWaypoint* currentWP = aiAircraft->GetFlightPlan()->getCurrentWaypoint();
-    if (currentWP) {
-        std::cout << "WP       " << currentWP->getName() << "\t" << aiAircraft->GetFlightPlan()->getCurrentWaypoint()->getPos() << "\r\n";
-        std::cout << "Distance " << SGGeodesy::distanceM(aiAircraft->getGeodPos(), currentWP->getPos()) << "\n";
-    } else {
-        std::cout << "No Current WP\n";
-    }
-    std::cout << "Flightplan "
-              << "\n";
-    FGAIFlightPlan* fp = aiAircraft->GetFlightPlan();
-    if (fp->isValidPlan()) {
-        std::cout << "Leg    : " << fp->getLeg() << "\n";
-        std::cout << "Length : " << fp->getNrOfWayPoints() << "\n";
-    }
-}
-
 std::string TrafficTests::getTimeString(int timeOffset)
 {
     char ret[11];

test_suite/unit_tests/AI/test_traffic.hxx

@@ -60,7 +60,6 @@ public:
     void testPushforwardParkYBBN();
     void testPushforwardParkYBBNRepeat();
 private:
-    void dump(FGAIAircraft* aiAircraft);
     std::string getTimeString(int timeOffset);
     FGAIAircraft * flyAI(FGAIAircraft * aiAircraft, std::string fName);
 };