fly/flightgear
1
0
Fork 0
Code Activity

Add dump() method to AIAircaft

Browse source
This commit is contained in:
portree_kid 2021-03-27 09:00:22 +01:00 committed by James Turner
parent cb3f8056ff
commit 3d3ff623cd
4 changed files with 151 additions and 59 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

103
src/AIModel/AIAircraft.cxx
View file

@ -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)
{
// 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;
}

4
src/AIModel/AIAircraft.hxx
View file

@ -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();

102
test_suite/unit_tests/AI/test_traffic.cxx
View file

@ -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));
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<FGAIManager>()->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<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_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];

1
test_suite/unit_tests/AI/test_traffic.hxx
View file

@ -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);
};