diff --git a/src/AIModel/AIAircraft.cxx b/src/AIModel/AIAircraft.cxx index f53464ce8..a5f30cfcc 100644 --- a/src/AIModel/AIAircraft.cxx +++ b/src/AIModel/AIAircraft.cxx @@ -46,6 +46,7 @@ extern double fgIsFinite(double x); #include "performancedata.hxx" #include "performancedb.hxx" #include +#include 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) - { - // Oops! FIXME + if (!curr) { + SG_LOG(SG_AI, SG_WARN, "No current WP" << next->getName()); 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; +} \ No newline at end of file diff --git a/src/AIModel/AIAircraft.hxx b/src/AIModel/AIAircraft.hxx index 2288e7b32..4c4bb7ea6 100644 --- a/src/AIModel/AIAircraft.hxx +++ b/src/AIModel/AIAircraft.hxx @@ -24,6 +24,7 @@ #include "AIBaseAircraft.hxx" #include +#include 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(); diff --git a/test_suite/unit_tests/AI/test_traffic.cxx b/test_suite/unit_tests/AI/test_traffic.cxx index 7f178c2e7..a3e37d257 100644 --- a/test_suite/unit_tests/AI/test_traffic.cxx +++ b/test_suite/unit_tests/AI/test_traffic.cxx @@ -21,9 +21,11 @@ #include "test_traffic.hxx" +#include #include #include #include +#include #include "test_suite/FGTestApi/NavDataCache.hxx" #include "test_suite/FGTestApi/TestDataLogger.hxx" @@ -41,6 +43,8 @@ #include #include +#include + #include #include
@@ -148,7 +152,6 @@ void TrafficTests::testPushback() globals->get_subsystem()->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()->attach(aiAircraft); - aiAircraft = flyAI(aiAircraft, "flight_cargo_EDPH_" + std::to_string(departureTime)); - - CPPUNIT_ASSERT_EQUAL(5, aiAircraft->GetFlightPlan()->getLeg()); + aiAircraft = flyAI(aiAircraft, "flight_cargo_EGPH_EGPF_" + std::to_string(departureTime)); } void TrafficTests::testChangeRunway() @@ -268,9 +269,7 @@ void TrafficTests::testChangeRunway() aiAircraft->FGAIBase::setFlightPlan(std::move(fp)); globals->get_subsystem()->attach(aiAircraft); - aiAircraft = flyAI(aiAircraft, "flight_runway_EGPH_" + std::to_string(departureTime)); - - CPPUNIT_ASSERT_EQUAL(5, aiAircraft->GetFlightPlan()->getLeg()); + aiAircraft = flyAI(aiAircraft, "flight_runway_EGPH_EGPF_" + std::to_string(departureTime)); } @@ -329,8 +328,6 @@ void TrafficTests::testPushforward() globals->get_subsystem()->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()->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_EQUAL(0, shortestDistance); - // CPPUNIT_ASSERT_EQUAL(nearestParking->getHeading(), lastHeading); + CPPUNIT_ASSERT_LESS(5, shortestDistance); + + 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 - if (aiAircraft->GetFlightPlan()->getLeg() == 9) { - this->dump(aiAircraft); - } - if (aiAircraft->GetFlightPlan()->getLeg() != lastLeg) { + // Leg has been incremented + if (aiAircraft->GetFlightPlan()->getLeg() != lastLeg ) { + // The current WP is really in our new leg 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]; diff --git a/test_suite/unit_tests/AI/test_traffic.hxx b/test_suite/unit_tests/AI/test_traffic.hxx index 711e7febf..679720252 100644 --- a/test_suite/unit_tests/AI/test_traffic.hxx +++ b/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); };