fly/flightgear
1
0
Fork 0
Code Activity

AI Fixes

Browse source 
* Fix turns (left-turn-bug)
* Better parking apporach
* CSV Logging via property
* ft vs m bug in setLeadDistance
* Split Runway Entry/Exit calculation
* Prototype wait pattern
This commit is contained in:
portree_kid 2022-02-06 21:11:58 +01:00
parent cacd650f27
commit 82863d3f3e
13 changed files with 3092 additions and 2759 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

345
src/AIModel/AIAircraft.cxx
View file

@ -28,6 +28,7 @@
#include <simgear/timing/sg_time.hxx> #include <simgear/timing/sg_time.hxx>
#include <simgear/structure/exception.hxx> #include <simgear/structure/exception.hxx>
#include <simgear/io/iostreams/sgstream.hxx>
#include <string> #include <string>
#include <cmath> #include <cmath>
@ -57,12 +58,16 @@ FGAIAircraft::FGAIAircraft(FGAISchedule* ref) : /* HOT must be disabled for AI A
_performance(nullptr) _performance(nullptr)
{ {
trafficRef = ref; trafficRef = ref;
csvFile = std::make_unique<sg_ofstream>();
if (trafficRef) { if (trafficRef) {
groundOffset = trafficRef->getGroundOffset(); groundOffset = trafficRef->getGroundOffset();
setCallSign(trafficRef->getCallSign()); setCallSign(trafficRef->getCallSign());
tracked = getCallSign() == fgGetString("/ai/track-callsign")
|| fgGetString("/ai/track-callsign") == "ALL";
} }
else else {
groundOffset = 0; groundOffset = 0;
}
fp = 0; fp = 0;
controller = 0; controller = 0;
@ -141,6 +146,8 @@ void FGAIAircraft::readFromScenario(SGPropertyNode* scFileNode) {
setFlightPlan(scFileNode->getStringValue("flightplan"), setFlightPlan(scFileNode->getStringValue("flightplan"),
scFileNode->getBoolValue("repeat", false)); scFileNode->getBoolValue("repeat", false));
setCallSign(scFileNode->getStringValue("callsign")); setCallSign(scFileNode->getStringValue("callsign"));
tracked = getCallSign() == fgGetString("/ai/track-callsign")
|| fgGetString("/ai/track-callsign") == "ALL";
} }
@ -156,6 +163,17 @@ void FGAIAircraft::update(double dt) {
FGAIBase::update(dt); FGAIBase::update(dt);
Run(dt); Run(dt);
Transform(); Transform();
if (tracked && !csvFile->is_open()) {
char fname [160];
time_t t = time(0); // get time now
snprintf (fname, sizeof(fname), "%s_%ld.csv", getCallSign().c_str(), t);
SGPath p = globals->get_download_dir() / fname;
csvFile->open(p);
dumpCSVHeader(csvFile);
}
if (tracked && csvFile->is_open()) {
dumpCSV(csvFile, csvIndex++);
}
} }
void FGAIAircraft::unbind() void FGAIAircraft::unbind()
@ -250,7 +268,7 @@ void FGAIAircraft::ClimbTo(double alt_ft ) {
void FGAIAircraft::TurnTo(double heading) { void FGAIAircraft::TurnTo(double heading) {
if( fabs(heading) < 0.1 ) { if( fabs(heading) < 0.1 ) {
SG_LOG(SG_AI, SG_WARN, "Heading reset to zero"); SG_LOG(SG_AI, SG_WARN, getCallSign() << "|Heading reset to zero");
} }
tgt_heading = heading; tgt_heading = heading;
// SG_LOG(SG_AI, SG_BULK, "Turn tgt_heading to " << tgt_heading); // SG_LOG(SG_AI, SG_BULK, "Turn tgt_heading to " << tgt_heading);
@ -319,12 +337,12 @@ void FGAIAircraft::ProcessFlightPlan( double dt, time_t now ) {
return; return;
} }
prev = fp->getPreviousWaypoint(); prev = fp->getPreviousWaypoint();
SG_LOG(SG_AI, SG_BULK, "Previous WP \t" << prev->getName()); SG_LOG(SG_AI, SG_BULK, getCallSign() << "|Previous WP \t" << prev->getName());
curr = fp->getCurrentWaypoint(); curr = fp->getCurrentWaypoint();
SG_LOG(SG_AI, SG_BULK, "Current WP \t" << curr->getName()); SG_LOG(SG_AI, SG_BULK, getCallSign() << "|Current WP \t" << curr->getName());
next = fp->getNextWaypoint(); next = fp->getNextWaypoint();
if( next ) { if( next ) {
SG_LOG(SG_AI, SG_BULK, "Next WP \t" << next->getName()); SG_LOG(SG_AI, SG_BULK, getCallSign() << "|Next WP \t" << next->getName());
} }
} }
if (!curr) { if (!curr) {
@ -333,11 +351,11 @@ void FGAIAircraft::ProcessFlightPlan( double dt, time_t now ) {
} }
if (!leadPointReached(curr, next, nextTurnAngle)) { if (!leadPointReached(curr, next, nextTurnAngle)) {
controlHeading(curr); controlHeading(curr, nullptr);
controlSpeed(curr, next); controlSpeed(curr, next);
} else { } else {
if (curr->isFinished()) { //end of the flight plan if (curr->isFinished()) { //end of the flight plan
SG_LOG(SG_AI, SG_BULK, "Flightplan ended"); SG_LOG(SG_AI, SG_BULK, getCallSign() << "|Flightplan ended");
if (fp->getRepeat()) { if (fp->getRepeat()) {
fp->restart(); fp->restart();
} }
@ -364,14 +382,14 @@ void FGAIAircraft::ProcessFlightPlan( double dt, time_t now ) {
} }
prev = fp->getPreviousWaypoint(); prev = fp->getPreviousWaypoint();
SG_LOG(SG_AI, SG_BULK, "Previous WP \t" << prev->getName() << "\t" << prev->getPos()); SG_LOG(SG_AI, SG_BULK, getCallSign() << "|Previous WP \t" << prev->getName() << "\t" << prev->getPos());
curr = fp->getCurrentWaypoint(); curr = fp->getCurrentWaypoint();
if (curr) { if (curr) {
SG_LOG(SG_AI, SG_BULK, "Current WP \t" << curr->getName() << "\t" << curr->getPos()); SG_LOG(SG_AI, SG_BULK, getCallSign() << "|Current WP \t" << curr->getName() << "\t" << curr->getPos());
} }
next = fp->getNextWaypoint(); next = fp->getNextWaypoint();
if(next) { if(next) {
SG_LOG(SG_AI, SG_BULK, "Next WP \t" << next->getName() << "\t" << next->getPos()); SG_LOG(SG_AI, SG_BULK, getCallSign() << "|Next WP \t" << next->getName() << "\t" << next->getPos());
} }
// Now that we have incremented the waypoints, excute some traffic manager specific code // Now that we have incremented the waypoints, excute some traffic manager specific code
@ -394,6 +412,7 @@ void FGAIAircraft::ProcessFlightPlan( double dt, time_t now ) {
} }
if (next && !curr->contains("END") && !curr->contains("PushBackPointlegend")) { if (next && !curr->contains("END") && !curr->contains("PushBackPointlegend")) {
SG_LOG(SG_AI, SG_BULK, getCallSign() << "|Setting Leaddistance");
fp->setLeadDistance(tgt_speed, tgt_heading, curr, next); fp->setLeadDistance(tgt_speed, tgt_heading, curr, next);
} else { } else {
// If we are ending in a parking // If we are ending in a parking
@ -474,22 +493,33 @@ void FGAIAircraft::clearATCController()
towerController = 0; towerController = 0;
} }
bool FGAIAircraft::isBlockedBy(FGAIAircraft* other) {
if(!other) {
return false;
}
int azimuth = SGGeodesy::courseDeg(getGeodPos(), other->getGeodPos());
const double headingDiff = SGMiscd::normalizePeriodic(-180, 180, hdg-azimuth);
SG_LOG(SG_AI, SG_BULK, getCallSign() << " blocked by " << other->getCallSign() << azimuth << " Heading " << hdg << " Diff" << headingDiff);
return false;
}
#if 0 #if 0
void FGAIAircraft::assertSpeed(double speed) void FGAIAircraft::assertSpeed(double speed)
{ {
if ((speed < -50) || (speed > 1000)) { if ((speed < -50) || (speed > 1000)) {
cerr << getCallSign() << " " SG_LOG(SG_AI, SG_DEBUG, getCallSign() << " "
<< "Previous waypoint " << fp->getPreviousWaypoint()->getName() << " " << "Previous waypoint " << fp->getPreviousWaypoint()->getName() << " "
<< "Departure airport " << trafficRef->getDepartureAirport() << " " << "Departure airport " << trafficRef->getDepartureAirport() << " "
<< "Leg " << fp->getLeg() << " " << "Leg " << fp->getLeg() << " "
<< "target_speed << " << tgt_speed << " " << "target_speed << " << tgt_speed << " "
<< "speedFraction << " << speedFraction << " " << "speedFraction << " << speedFraction << " "
<< "Currecnt speed << " << speed << " " << "Currecnt speed << " << speed << " ");
<< endl;
} }
} }
#endif #endif
void FGAIAircraft::checkTcas(void) void FGAIAircraft::checkTcas(void)
{ {
if (tcasThreatNode && tcasThreatNode->getIntValue()==3) if (tcasThreatNode && tcasThreatNode->getIntValue()==3)
@ -528,11 +558,12 @@ const char * FGAIAircraft::_getTransponderCode() const {
// Probably not, because it should only be executed after we have already passed the leg incrementing waypoint. // Probably not, because it should only be executed after we have already passed the leg incrementing waypoint.
bool FGAIAircraft::loadNextLeg(double distance) { bool FGAIAircraft::loadNextLeg(double distance) {
int leg; int leg;
if ((leg = fp->getLeg()) == 9) { if ((leg = fp->getLeg()) == AILeg::PARKING) {
FGAirport *oldArr = trafficRef->getArrivalAirport(); FGAirport *oldArr = trafficRef->getArrivalAirport();
if (!trafficRef->next()) { if (!trafficRef->next()) {
SG_LOG(SG_AI, SG_BULK, getCallSign() << "|No following flight killing");
//FIXME I'm on leg 9 and don't even reach parking. //FIXME I'm on leg 9 and don't even reach parking.
return false; return false;
} }
@ -544,6 +575,8 @@ bool FGAIAircraft::loadNextLeg(double distance) {
repositioned = false; repositioned = false;
} }
setCallSign(trafficRef->getCallSign()); setCallSign(trafficRef->getCallSign());
tracked = getCallSign() == fgGetString("/ai/track-callsign")
|| fgGetString("/ai/track-callsign") == "ALL";
leg = 0; leg = 0;
fp->setLeg(leg); fp->setLeg(leg);
} }
@ -556,6 +589,7 @@ bool FGAIAircraft::loadNextLeg(double distance) {
} else { } else {
double cruiseAlt = trafficRef->getCruiseAlt() * 100; double cruiseAlt = trafficRef->getCruiseAlt() * 100;
SG_LOG(SG_AI, SG_BULK, getCallSign() << "|Loading Leg " << leg+1);
bool ok = fp->create (this, bool ok = fp->create (this,
dep, dep,
arr, arr,
@ -572,7 +606,7 @@ bool FGAIAircraft::loadNextLeg(double distance) {
distance); distance);
if (!ok) { if (!ok) {
SG_LOG(SG_AI, SG_WARN, "Failed to create waypoints for leg:" << leg+1); SG_LOG(SG_AI, SG_WARN, getCallSign() << "|Failed to create waypoints for leg:" << leg+1);
} }
} }
return true; return true;
@ -620,7 +654,6 @@ void FGAIAircraft::getGroundElev(double dt) {
} }
} }
void FGAIAircraft::doGroundAltitude() { void FGAIAircraft::doGroundAltitude() {
if ((fp->getLeg() == 7) && ((altitude_ft - tgt_altitude_ft) > 5)) { if ((fp->getLeg() == 7) && ((altitude_ft - tgt_altitude_ft) > 5)) {
tgt_vs = -500; tgt_vs = -500;
@ -644,7 +677,7 @@ void FGAIAircraft::announcePositionToController() {
if (!fp->getCurrentWaypoint()) { if (!fp->getCurrentWaypoint()) {
// http://code.google.com/p/flightgear-bugs/issues/detail?id=1153 // http://code.google.com/p/flightgear-bugs/issues/detail?id=1153
// throw an exception so this aircraft gets killed by the AIManager. // throw an exception so this aircraft gets killed by the AIManager.
throw sg_exception("bad AI flight plan"); throw sg_exception("bad AI flight plan. No current WP");
} }
// Note that leg has been incremented after creating the current leg, so we should use // Note that leg has been incremented after creating the current leg, so we should use
@ -653,33 +686,33 @@ void FGAIAircraft::announcePositionToController() {
// Leg numbers are updated only once the aircraft passes the last waypoint created for that legm so I should probably just use // Leg numbers are updated only once the aircraft passes the last waypoint created for that legm so I should probably just use
// the original leg numbers here! // the original leg numbers here!
switch (leg) { switch (leg) {
case 1: // Startup and Push back case AILeg::STARTUP_PUSHBACK: // Startup and Push back
if (trafficRef->getDepartureAirport()->getDynamics()) if (trafficRef->getDepartureAirport()->getDynamics())
controller = trafficRef->getDepartureAirport()->getDynamics()->getStartupController(); controller = trafficRef->getDepartureAirport()->getDynamics()->getStartupController();
break; break;
case 2: // Taxiing to runway case AILeg::TAXI: // Taxiing to runway
if (trafficRef->getDepartureAirport()->getDynamics()->getGroundController()->exists()) if (trafficRef->getDepartureAirport()->getDynamics()->getGroundController()->exists())
controller = trafficRef->getDepartureAirport()->getDynamics()->getGroundController(); controller = trafficRef->getDepartureAirport()->getDynamics()->getGroundController();
break; break;
case 3: //Take off tower controller case AILeg::TAKEOFF: //Take off tower controller
if (trafficRef->getDepartureAirport()->getDynamics()) { if (trafficRef->getDepartureAirport()->getDynamics()) {
controller = trafficRef->getDepartureAirport()->getDynamics()->getTowerController(); controller = trafficRef->getDepartureAirport()->getDynamics()->getTowerController();
towerController = 0; towerController = 0;
} else { } else {
cerr << "Error: Could not find Dynamics at airport : " << trafficRef->getDepartureAirport()->getId() << endl; SG_LOG(SG_AI, SG_BULK, "Error: Could not find Dynamics at airport : " << trafficRef->getDepartureAirport()->getId());
} }
break; break;
case 6: case AILeg::APPROACH:
if (trafficRef->getArrivalAirport()->getDynamics()) { if (trafficRef->getArrivalAirport()->getDynamics()) {
controller = trafficRef->getArrivalAirport()->getDynamics()->getApproachController(); controller = trafficRef->getArrivalAirport()->getDynamics()->getApproachController();
} }
break; break;
case 8: // Taxiing for parking case AILeg::PARKING_TAXI: // Taxiing for parking
if (trafficRef->getArrivalAirport()->getDynamics()->getGroundController()->exists()) if (trafficRef->getArrivalAirport()->getDynamics()->getGroundController()->exists())
controller = trafficRef->getArrivalAirport()->getDynamics()->getGroundController(); controller = trafficRef->getArrivalAirport()->getDynamics()->getGroundController();
break; break;
default: default:
controller = 0; controller = nullptr;
break; break;
} }
@ -701,7 +734,7 @@ void FGAIAircraft::scheduleForATCTowerDepartureControl(int state) {
if (trafficRef->getDepartureAirport()->getDynamics()) { if (trafficRef->getDepartureAirport()->getDynamics()) {
towerController = trafficRef->getDepartureAirport()->getDynamics()->getTowerController(); towerController = trafficRef->getDepartureAirport()->getDynamics()->getTowerController();
} else { } else {
cerr << "Error: Could not find Dynamics at airport : " << trafficRef->getDepartureAirport()->getId() << endl; SG_LOG(SG_AI, SG_WARN, "Could not find Dynamics at airport : " << trafficRef->getDepartureAirport()->getId());
} }
if (towerController) { if (towerController) {
towerController->announcePosition(getID(), fp.get(), fp->getCurrentWaypoint()->getRouteIndex(), towerController->announcePosition(getID(), fp.get(), fp->getCurrentWaypoint()->getRouteIndex(),
@ -783,12 +816,12 @@ void FGAIAircraft::handleFirstWaypoint() {
} }
prev = fp->getPreviousWaypoint(); //first waypoint prev = fp->getPreviousWaypoint(); //first waypoint
SG_LOG(SG_AI, SG_BULK, "Previous WP \t" << prev->getName()); SG_LOG(SG_AI, SG_BULK, getCallSign() << "|Previous WP \t" << prev->getName());
curr = fp->getCurrentWaypoint(); //second waypoint curr = fp->getCurrentWaypoint(); //second waypoint
SG_LOG(SG_AI, SG_BULK, "Current WP \t" << curr->getName()); SG_LOG(SG_AI, SG_BULK, getCallSign() << "|Current WP \t" << curr->getName());
next = fp->getNextWaypoint(); //third waypoint (might not exist!) next = fp->getNextWaypoint(); //third waypoint (might not exist!)
if( next ) { if( next ) {
SG_LOG(SG_AI, SG_BULK, "Next WP \t" << next->getName()); SG_LOG(SG_AI, SG_BULK, getCallSign() << "|Next WP \t" << next->getName());
} }
setLatitude(prev->getLatitude()); setLatitude(prev->getLatitude());
@ -874,14 +907,14 @@ bool FGAIAircraft::leadPointReached(FGAIWaypoint* curr, FGAIWaypoint* next, int
if ((dist_to_go_m < arrivalDist) && (speed < 0) && (tgt_speed < 0) && fp->getCurrentWaypoint()->contains("PushBackPoint")) { if ((dist_to_go_m < arrivalDist) && (speed < 0) && (tgt_speed < 0) && fp->getCurrentWaypoint()->contains("PushBackPoint")) {
// tgt_speed = -(dist_to_go_m / 10.0); // tgt_speed = -(dist_to_go_m / 10.0);
tgt_speed = -std::sqrt((pow(arrivalDist,2)-pow(arrivalDist-dist_to_go_m,2))); tgt_speed = -std::sqrt((pow(arrivalDist,2)-pow(arrivalDist-dist_to_go_m,2)));
SG_LOG(SG_AI, SG_BULK, "tgt_speed " << tgt_speed); SG_LOG(SG_AI, SG_BULK, getCallSign() << "|tgt_speed " << tgt_speed);
if (tgt_speed > -1) { if (tgt_speed > -1) {
// Speed is int and cannot go below 1 knot // Speed is int and cannot go below 1 knot
tgt_speed = -1; tgt_speed = -1;
} }
if (fp->getPreviousWaypoint()->getSpeed() < tgt_speed) { if (fp->getPreviousWaypoint()->getSpeed() < tgt_speed) {
SG_LOG(SG_AI, SG_BULK, "Set speed of WP from " << fp->getPreviousWaypoint()->getSpeed() << " to " << tgt_speed); SG_LOG(SG_AI, SG_BULK, getCallSign() << "|Set speed of WP from " << fp->getPreviousWaypoint()->getSpeed() << " to " << tgt_speed);
fp->getPreviousWaypoint()->setSpeed(tgt_speed); fp->getPreviousWaypoint()->setSpeed(tgt_speed);
} }
} }
@ -894,14 +927,14 @@ bool FGAIAircraft::leadPointReached(FGAIWaypoint* curr, FGAIWaypoint* next, int
} }
if (fp->getPreviousWaypoint()->getSpeed() < tgt_speed) { if (fp->getPreviousWaypoint()->getSpeed() < tgt_speed) {
SG_LOG(SG_AI, SG_BULK, "Set speed of WP from " << fp->getPreviousWaypoint()->getSpeed() << " to " << tgt_speed); SG_LOG(SG_AI, SG_BULK, getCallSign() << "|Set speed of WP from " << fp->getPreviousWaypoint()->getSpeed() << " to " << tgt_speed);
fp->getPreviousWaypoint()->setSpeed(tgt_speed); fp->getPreviousWaypoint()->setSpeed(tgt_speed);
} }
} }
if (lead_distance_m < fabs(2*speed) * SG_FEET_TO_METER) { if (lead_distance_m < fabs(2*speed) * SG_FEET_TO_METER) {
//don't skip over the waypoint //don't skip over the waypoint
SG_LOG(SG_AI, SG_BULK, "Set lead_distance_m due to speed " << lead_distance_m << " to " << fabs(2*speed) * SG_FEET_TO_METER); SG_LOG(SG_AI, SG_BULK, getCallSign() << "|Set lead_distance_m due to speed " << lead_distance_m << " to " << fabs(2*speed) * SG_FEET_TO_METER);
lead_distance_m = fabs(2*speed) * SG_FEET_TO_METER; lead_distance_m = fabs(2*speed) * SG_FEET_TO_METER;
fp->setLeadDistance(lead_distance_m * SG_METER_TO_FEET); fp->setLeadDistance(lead_distance_m * SG_METER_TO_FEET);
} }
@ -916,7 +949,7 @@ bool FGAIAircraft::leadPointReached(FGAIWaypoint* curr, FGAIWaypoint* next, int
if (onGround() && fabs(nextTurnAngle) > 50 ) { if (onGround() && fabs(nextTurnAngle) > 50 ) {
//don't skip over the waypoint //don't skip over the waypoint
const int multiplicator = 4; const int multiplicator = 4;
SG_LOG(SG_AI, SG_BULK, "Set lead_distance_m due to next turn angle " << lead_distance_m << " to " << fabs(multiplicator*speed) * SG_FEET_TO_METER << " dist_to_go_m " << dist_to_go_m << " Next turn angle : " << fabs(nextTurnAngle) ); SG_LOG(SG_AI, SG_BULK, getCallSign() << "|Set lead_distance_m due to next turn angle " << lead_distance_m << " to " << fabs(multiplicator*speed) * SG_FEET_TO_METER << " dist_to_go_m " << dist_to_go_m << " Next turn angle : " << fabs(nextTurnAngle) );
lead_distance_m = fabs(multiplicator*speed) * SG_FEET_TO_METER; lead_distance_m = fabs(multiplicator*speed) * SG_FEET_TO_METER;
fp->setLeadDistance(lead_distance_m * SG_METER_TO_FEET); fp->setLeadDistance(lead_distance_m * SG_METER_TO_FEET);
} }
@ -934,18 +967,20 @@ bool FGAIAircraft::leadPointReached(FGAIWaypoint* curr, FGAIWaypoint* next, int
if ((dist_to_go_m < lead_distance_m) || if ((dist_to_go_m < lead_distance_m) ||
((dist_to_go_m > prev_dist_to_go) && (bearing > (minBearing * 1.1))) ) { ((dist_to_go_m > prev_dist_to_go) && (bearing > (minBearing * 1.1))) ) {
SG_LOG(SG_AI, SG_BULK, "Leadpoint reached Bearing : " << bearing << "\tNext Bearing : " << nextBearing << " Next Turn Angle : " << fabs(nextTurnAngle)); SG_LOG(SG_AI, SG_BULK, getCallSign() << "|Leadpoint reached Bearing : " << bearing << "\tNext Bearing : " << nextBearing << " Next Turn Angle : " << fabs(nextTurnAngle));
minBearing = 360; minBearing = 360;
speedFraction = 1.0; speedFraction = 1.0;
prev_dist_to_go = HUGE_VAL; prev_dist_to_go = HUGE_VAL;
return true; return true;
} else { } else {
if (prev_dist_to_go == dist_to_go_m && fabs(groundTargetSpeed)>0) { if (prev_dist_to_go == dist_to_go_m
&& fabs(groundTargetSpeed)>0
&& this->atGate().empty() ) {
//FIXME must be suppressed when parked //FIXME must be suppressed when parked
SG_LOG(SG_AI, SG_BULK, "Aircraft " << _callsign << " stuck. Speed " << speed); SG_LOG(SG_AI, SG_BULK, getCallSign() << "|Aircraft stuck. Speed " << speed);
stuckCounter++; stuckCounter++;
if (stuckCounter>AI_STUCK_LIMIT) { if (stuckCounter>AI_STUCK_LIMIT) {
SG_LOG(SG_AI, SG_WARN, "Stuck flight " << _callsign << " killed" ); SG_LOG(SG_AI, SG_WARN, getCallSign() << "|Stuck flight killed on leg " << fp->getLeg() );
setDie(true); setDie(true);
} }
} else { } else {
@ -1004,7 +1039,7 @@ bool FGAIAircraft::handleAirportEndPoints(FGAIWaypoint* prev, time_t now) {
// This is the last taxi waypoint, and marks the the end of the flight plan // This is the last taxi waypoint, and marks the the end of the flight plan
// so, the schedule should update and wait for the next departure time. // so, the schedule should update and wait for the next departure time.
if (prev->contains("END")) { if (prev->contains("END")) {
SG_LOG(SG_AI, SG_BULK, "Reached " << prev->getName() ); SG_LOG(SG_AI, SG_BULK, getCallSign() << "|Reached " << prev->getName() );
//FIXME Heading Error should be reset //FIXME Heading Error should be reset
time_t nextDeparture = trafficRef->getDepartureTime(); time_t nextDeparture = trafficRef->getDepartureTime();
@ -1024,21 +1059,39 @@ bool FGAIAircraft::handleAirportEndPoints(FGAIWaypoint* prev, time_t now) {
* *
* @param curr * @param curr
*/ */
void FGAIAircraft::controlHeading(FGAIWaypoint* curr) { void FGAIAircraft::controlHeading(FGAIWaypoint* curr,FGAIWaypoint* next) {
const double calc_bearing = speed < 0?SGMiscd::normalizePeriodic(0, 360, fp->getBearing(pos, curr) + 180.0):fp->getBearing(pos, curr); const double calc_bearing = speed < 0?SGMiscd::normalizePeriodic(0, 360, fp->getBearing(pos, curr) + 180.0):fp->getBearing(pos, curr);
// when moving forward we want to aim for an average heading
if(next&& speed>0) {
const double calcNextBearing = fp->getBearing(pos, next);
if (fgIsFinite(calc_bearing) && fgIsFinite(calcNextBearing)) {
double averageHeading = calc_bearing + (calcNextBearing-calc_bearing)/2;
averageHeading = SGMiscd::normalizePeriodic(0, 360, averageHeading);
double hdg_error = averageHeading - tgt_heading;
if (fabs(hdg_error) > 0.01) {
TurnTo( averageHeading );
}
} else {
SG_LOG(SG_AI, SG_WARN, "calc_bearing is not a finite number : "
<< "Speed " << speed
<< "pos : " << pos.getLatitudeDeg() << ", " << pos.getLongitudeDeg()
<< ", waypoint: " << curr->getLatitude() << ", " << curr->getLongitude() );
SG_LOG(SG_AI, SG_WARN, "waypoint name: '" << curr->getName() << "'" );;
}
} else {
if (fgIsFinite(calc_bearing)) { if (fgIsFinite(calc_bearing)) {
double hdg_error = calc_bearing - tgt_heading; double hdg_error = calc_bearing - tgt_heading;
if (fabs(hdg_error) > 0.01) { if (fabs(hdg_error) > 0.01) {
TurnTo( calc_bearing ); TurnTo( calc_bearing );
} }
} else { } else {
cerr << "calc_bearing is not a finite number : " SG_LOG(SG_AI, SG_WARN, "calc_bearing is not a finite number : "
<< "Speed " << speed << "Speed " << speed
<< "pos : " << pos.getLatitudeDeg() << ", " << pos.getLongitudeDeg() << "pos : " << pos.getLatitudeDeg() << ", " << pos.getLongitudeDeg()
<< ", waypoint: " << curr->getLatitude() << ", " << curr->getLongitude() << endl; << ", waypoint: " << curr->getLatitude() << ", " << curr->getLongitude() );
cerr << "waypoint name: '" << curr->getName() << "'" << endl; SG_LOG(SG_AI, SG_WARN, "waypoint name: '" << curr->getName() << "'" );;
}
} }
} }
@ -1153,7 +1206,7 @@ void FGAIAircraft::updateHeading(double dt) {
SG_LOG(SG_AI, SG_BULK, "Oh dear " << _callsign << " might get stuck aka next point is behind us. Speed is " << speed ); SG_LOG(SG_AI, SG_BULK, "Oh dear " << _callsign << " might get stuck aka next point is behind us. Speed is " << speed );
stuckCounter++; stuckCounter++;
if (stuckCounter>AI_STUCK_LIMIT) { if (stuckCounter>AI_STUCK_LIMIT) {
SG_LOG(SG_AI, SG_WARN, "Stuck flight " << _callsign << " killed" ); SG_LOG(SG_AI, SG_WARN, "Stuck flight " << _callsign << " killed on leg " << fp->getLeg() << " because point behind");
setDie(true); setDie(true);
} }
} }
@ -1399,21 +1452,17 @@ bool FGAIAircraft::reachedEndOfCruise(double &distance) {
double descentTimeNeeded = verticalDistance / descentRate; double descentTimeNeeded = verticalDistance / descentRate;
double distanceCovered = descentSpeed * descentTimeNeeded; double distanceCovered = descentSpeed * descentTimeNeeded;
if (trafficRef->getCallSign() != "" && if (tracked) {
trafficRef->getCallSign() == fgGetString("/ai/track-callsign")) { SG_LOG(SG_AI, SG_DEBUG, "Checking for end of cruise stage for :" << trafficRef->getCallSign());
cerr << "Checking for end of cruise stage for :" << trafficRef->getCallSign() << endl; SG_LOG(SG_AI, SG_DEBUG, "Descent rate : " << descentRate);
cerr << "Descent rate : " << descentRate << endl; SG_LOG(SG_AI, SG_DEBUG, "Descent speed : " << descentSpeed);
cerr << "Descent speed : " << descentSpeed << endl; SG_LOG(SG_AI, SG_DEBUG, "VerticalDistance : " << verticalDistance << ". Altitude : " << altitude_ft << ". Elevation " << trafficRef->getArrivalAirport()->getElevation());
cerr << "VerticalDistance : " << verticalDistance << ". Altitude : " << altitude_ft << ". Elevation " << trafficRef->getArrivalAirport()->getElevation() << endl; SG_LOG(SG_AI, SG_DEBUG, "DecentTimeNeeded : " << descentTimeNeeded);
cerr << "DecentTimeNeeded : " << descentTimeNeeded << endl; SG_LOG(SG_AI, SG_DEBUG, "DistanceCovered : " << distanceCovered);
cerr << "DistanceCovered : " << distanceCovered << endl;
} }
distance = distanceCovered; distance = distanceCovered;
if (dist < distanceCovered) { if (dist < distanceCovered) {
if (trafficRef->getCallSign() == fgGetString("/ai/track-callsign")) {
//exit(1);
}
SG_LOG(SG_AI, SG_BULK, "End Of Cruise"); SG_LOG(SG_AI, SG_BULK, "End Of Cruise");
return true; return true;
} else { } else {
@ -1480,12 +1529,20 @@ time_t FGAIAircraft::checkForArrivalTime(const string& wptName) {
time_t ete = tracklength / ((speed * SG_NM_TO_METER) / 3600.0); time_t ete = tracklength / ((speed * SG_NM_TO_METER) / 3600.0);
time_t secondsToGo = arrivalTime - now; time_t secondsToGo = arrivalTime - now;
if (trafficRef->getCallSign() == fgGetString("/ai/track-callsign")) { if (tracked) {
cerr << "Checking arrival time: ete " << ete << ". Time to go : " << secondsToGo << ". Track length = " << tracklength << endl; SG_LOG(SG_AI, SG_BULK, "Checking arrival time: ete " << ete << ". Time to go : " << secondsToGo << ". Track length = " << tracklength);
} }
return (ete - secondsToGo); // Positive when we're too slow... return (ete - secondsToGo); // Positive when we're too slow...
} }
time_t FGAIAircraft::calcDeparture() {
time_t departure = this->checkForArrivalTime("DepartureHold");
if(!departure) {
departure = globals->get_time_params()->get_cur_time();
}
return departure;
}
double limitRateOfChange(double cur, double target, double maxDeltaSec, double dt) double limitRateOfChange(double cur, double target, double maxDeltaSec, double dt)
{ {
double delta = target - cur; double delta = target - cur;
@ -1542,103 +1599,119 @@ void FGAIAircraft::updateModelProperties(double dt)
TaxiLight(fp->getPreviousWaypoint()->getTaxiLight()); TaxiLight(fp->getPreviousWaypoint()->getTaxiLight());
} }
void FGAIAircraft::dumpCSVHeader(std::ofstream& o) { void FGAIAircraft::dumpCSVHeader(std::unique_ptr<sg_ofstream> &o) {
o << "Index\t"; (*o) << "Index\t";
o << "Lat\t"; (*o) << "Lat\t";
o << "Lon\t"; (*o) << "Lon\t";
o << "Callsign\t"; (*o) << "Callsign\t";
o << "headingDiff\t"; (*o) << "headingDiff\t";
o << "headingChangeRate\t"; (*o) << "headingChangeRate\t";
o << "headingError\t"; (*o) << "headingError\t";
o << "hdg\t"; (*o) << "hdg\t";
o << "tgt_heading\t"; (*o) << "tgt_heading\t";
o << "tgt_speed\t"; (*o) << "tgt_speed\t";
o << "minBearing\t"; (*o) << "minBearing\t";
o << "speedFraction\t"; (*o) << "speedFraction\t";
o << "groundOffset\t"; (*o) << "groundOffset\t";
o << "speed\t"; (*o) << "speed\t";
o << "groundTargetSpeed\t"; (*o) << "groundTargetSpeed\t";
o << "getVerticalSpeedFPM\t"; (*o) << "getVerticalSpeedFPM\t";
o << "getTrueHeadingDeg\t"; (*o) << "getTrueHeadingDeg\t";
o << "bearingToWP\t"; (*o) << "bearingToWP\t";
o << "Name\t"; (*o) << "Name\t";
o << "WP Lat\t"; (*o) << "WP Lat\t";
o << "WP Lon\t"; (*o) << "WP Lon\t";
o << "Dist\t"; (*o) << "Dist\t";
o << "Next Lat\t"; (*o) << "Next Lat\t";
o << "Next Lon\t"; (*o) << "Next Lon\t";
o << "Departuretime\t"; (*o) << "Departuretime\t";
o << "Time\t"; (*o) << "Time\t";
o << "Startup diff\t"; (*o) << "Startup diff\t";
o << "dist_to_go_m\t"; (*o) << "Departure\t";
o << "Leaddistance\t"; (*o) << "Arrival\t";
o << "Leg\t"; (*o) << "dist_to_go_m\t";
o << "Num WP\t"; (*o) << "leadInAngle\t";
o << "no_roll\t"; (*o) << "Leaddistance\t";
o << "roll\t"; (*o) << "Leg\t";
o << "stuckCounter"; (*o) << "Num WP\t";
o << endl; (*o) << "no_roll\t";
(*o) << "roll\t";
(*o) << "repositioned\t";
(*o) << "stuckCounter";
(*o) << endl;
} }
void FGAIAircraft::dumpCSV(std::ofstream& o, int lineIndex) { void FGAIAircraft::dumpCSV(std::unique_ptr<sg_ofstream> &o, int lineIndex) {
const double headingDiff = SGMiscd::normalizePeriodic(-180, 180, hdg-tgt_heading); const double headingDiff = SGMiscd::normalizePeriodic(-180, 180, hdg-tgt_heading);
o << lineIndex << "\t"; (*o) << lineIndex << "\t";
o << setprecision(12); (*o) << setprecision(12);
o << this->getGeodPos().getLatitudeDeg() << "\t"; (*o) << this->getGeodPos().getLatitudeDeg() << "\t";
o << this->getGeodPos().getLongitudeDeg() << "\t"; (*o) << this->getGeodPos().getLongitudeDeg() << "\t";
o << this->getCallSign() << "\t"; (*o) << this->getCallSign() << "\t";
o << headingDiff << "\t"; (*o) << headingDiff << "\t";
o << headingChangeRate << "\t"; (*o) << headingChangeRate << "\t";
o << headingError << "\t"; (*o) << headingError << "\t";
o << hdg << "\t"; (*o) << hdg << "\t";
o << tgt_heading << "\t"; (*o) << tgt_heading << "\t";
o << tgt_speed << "\t"; (*o) << tgt_speed << "\t";
o << minBearing << "\t"; (*o) << minBearing << "\t";
o << speedFraction << "\t"; (*o) << speedFraction << "\t";
o << groundOffset << "\t"; (*o) << groundOffset << "\t";
o << round(this->getSpeed()) << "\t"; (*o) << round(this->getSpeed()) << "\t";
o << groundTargetSpeed << "\t"; (*o) << groundTargetSpeed << "\t";
o << round(this->getVerticalSpeedFPM()) << "\t"; (*o) << round(this->getVerticalSpeedFPM()) << "\t";
o << this->getTrueHeadingDeg() << "\t"; (*o) << this->getTrueHeadingDeg() << "\t";
FGAIFlightPlan* fp = this->GetFlightPlan(); FGAIFlightPlan* fp = this->GetFlightPlan();
FGAIWaypoint* currentWP = this->GetFlightPlan()->getCurrentWaypoint(); if(fp) {
FGAIWaypoint* nextWP = this->GetFlightPlan()->getNextWaypoint(); FGAIWaypoint* currentWP = fp->getCurrentWaypoint();
FGAIWaypoint* nextWP = fp->getNextWaypoint();
if (currentWP) { if (currentWP) {
o << this->GetFlightPlan()->getBearing(this->getGeodPos(), this->GetFlightPlan()->getCurrentWaypoint()) << "\t"; (*o) << fp->getBearing(this->getGeodPos(), currentWP) << "\t";
o << currentWP->getName() << "\t"; (*o) << currentWP->getName() << "\t";
o << currentWP->getPos().getLatitudeDeg() << "\t"; (*o) << currentWP->getPos().getLatitudeDeg() << "\t";
o << currentWP->getPos().getLongitudeDeg() << "\t"; (*o) << currentWP->getPos().getLongitudeDeg() << "\t";
} }
if (nextWP) { if (nextWP) {
o << nextWP->getPos().getLatitudeDeg() << "\t"; (*o) << nextWP->getPos().getLatitudeDeg() << "\t";
o << nextWP->getPos().getLongitudeDeg() << "\t"; (*o) << nextWP->getPos().getLongitudeDeg() << "\t";
} else { } else {
o << "\t\t"; (*o) << "\t\t";
} }
if (currentWP) { if (currentWP) {
o << SGGeodesy::distanceM(this->getGeodPos(), currentWP->getPos()) << "\t"; (*o) << SGGeodesy::distanceM(this->getGeodPos(), currentWP->getPos()) << "\t";
o << this->GetFlightPlan()->getStartTime() << "\t"; (*o) << fp->getStartTime() << "\t";
o << globals->get_time_params()->get_cur_time() << "\t"; (*o) << globals->get_time_params()->get_cur_time() << "\t";
o << this->GetFlightPlan()->getStartTime() - globals->get_time_params()->get_cur_time() << "\t"; (*o) << fp->getStartTime() - globals->get_time_params()->get_cur_time() << "\t";
(*o) << (fp->departureAirport().get()?fp->departureAirport().get()->getId():"") << "\t";
(*o) << (fp->arrivalAirport().get()?fp->arrivalAirport().get()->getId():"") << "\t";
if(nextWP) {
double dist_to_go_m = fp->getDistanceToGo(pos.getLatitudeDeg(), pos.getLongitudeDeg(), currentWP); double dist_to_go_m = fp->getDistanceToGo(pos.getLatitudeDeg(), pos.getLongitudeDeg(), currentWP);
o << dist_to_go_m << "\t"; (*o) << dist_to_go_m << "\t";
double inbound = bearing;
double outbound = fp->getBearing(currentWP, nextWP);
double leadInAngle = fabs(inbound - outbound);
if (leadInAngle > 180.0) leadInAngle = 360.0 - leadInAngle;
(*o) << leadInAngle << "\t";
}
} else { } else {
o << "No WP\t\t\t\t\t\t\t\t"; (*o) << "No WP\t\t\t\t\t\t\t\t";
} }
if (fp->isValidPlan()) { if (fp->isValidPlan()) {
o << fp->getLeadDistance() * SG_FEET_TO_METER << "\t"; (*o) << fp->getLeadDistance() * SG_FEET_TO_METER << "\t";
o << fp->getLeg() << "\t"; (*o) << fp->getLeg() << "\t";
o << fp->getNrOfWayPoints() << "\t"; (*o) << fp->getNrOfWayPoints() << "\t";
} else { } else {
o << "FP NotValid\t\t"; (*o) << "FP NotValid\t\t";
} }
o << this->onGround() << "\t"; }
o << roll << "\t"; (*o) << this->onGround() << "\t";
o << stuckCounter; (*o) << roll << "\t";
o << endl; (*o) << repositioned << "\t";
(*o) << stuckCounter;
(*o) << endl;
} }
#if 0 #if 0

38
src/AIModel/AIAircraft.hxx
View file

@ -32,6 +32,23 @@ class FGAIFlightPlan;
class FGATCController; class FGATCController;
class FGATCInstruction; class FGATCInstruction;
class FGAIWaypoint; class FGAIWaypoint;
class sg_ofstream;
namespace AILeg
{
enum Type
{
STARTUP_PUSHBACK = 1,
TAXI = 2,
TAKEOFF = 3,
CLIMB = 4,
CRUISE = 5,
APPROACH = 6,
LANDING = 7,
PARKING_TAXI = 8,
PARKING = 9
};
}
class FGAIAircraft : public FGAIBaseAircraft { class FGAIAircraft : public FGAIBaseAircraft {
@ -57,6 +74,7 @@ public:
FGAIFlightPlan* GetFlightPlan() const { return fp.get(); }; FGAIFlightPlan* GetFlightPlan() const { return fp.get(); };
void ProcessFlightPlan( double dt, time_t now ); void ProcessFlightPlan( double dt, time_t now );
time_t checkForArrivalTime(const std::string& wptName); time_t checkForArrivalTime(const std::string& wptName);
time_t calcDeparture();
void AccelTo(double speed); void AccelTo(double speed);
void PitchTo(double angle); void PitchTo(double angle);
@ -90,6 +108,9 @@ public:
void setTrafficRef(FGAISchedule *ref) { trafficRef = ref; }; void setTrafficRef(FGAISchedule *ref) { trafficRef = ref; };
void resetTakeOffStatus() { takeOffStatus = 0;}; void resetTakeOffStatus() { takeOffStatus = 0;};
void setTakeOffStatus(int status) { takeOffStatus = status; }; void setTakeOffStatus(int status) { takeOffStatus = status; };
int getTakeOffStatus() { return takeOffStatus; };
void setTakeOffSlot(time_t timeSlot) { takeOffTimeSlot = timeSlot;};
time_t getTakeOffSlot(){return takeOffTimeSlot;};
void scheduleForATCTowerDepartureControl(int state); void scheduleForATCTowerDepartureControl(int state);
const std::string& GetTransponderCode() { return transponderCode; }; const std::string& GetTransponderCode() { return transponderCode; };
@ -108,17 +129,15 @@ public:
const std::string& atGate(); const std::string& atGate();
std::string acwakecategory; std::string acwakecategory;
int getTakeOffStatus() { return takeOffStatus; };
void checkTcas(); void checkTcas();
double calcVerticalSpeed(double vert_ft, double dist_m, double speed, double error); double calcVerticalSpeed(double vert_ft, double dist_m, double speed, double error);
FGATCController * getATCController() { return controller; }; FGATCController * getATCController() { return controller; };
void clearATCController(); void clearATCController();
void dumpCSVHeader(std::ofstream& o); bool isBlockedBy(FGAIAircraft* other);
void dumpCSV(std::ofstream& o, int lineIndex); void dumpCSVHeader(std::unique_ptr<sg_ofstream> &o);
void dumpCSV(std::unique_ptr<sg_ofstream> &o, int lineIndex);
protected: protected:
void Run(double dt); void Run(double dt);
@ -155,7 +174,8 @@ private:
bool handleAirportEndPoints(FGAIWaypoint* prev, time_t now); bool handleAirportEndPoints(FGAIWaypoint* prev, time_t now);
bool reachedEndOfCruise(double&); bool reachedEndOfCruise(double&);
bool aiTrafficVisible(void); bool aiTrafficVisible(void);
void controlHeading(FGAIWaypoint* curr); void controlHeading(FGAIWaypoint* curr,
FGAIWaypoint* next);
void controlSpeed(FGAIWaypoint* curr, FGAIWaypoint* next); void controlSpeed(FGAIWaypoint* curr, FGAIWaypoint* next);
void updatePrimaryTargetValues(double dt, bool& flightplanActive, bool& aiOutOfSight); void updatePrimaryTargetValues(double dt, bool& flightplanActive, bool& aiOutOfSight);
@ -196,7 +216,7 @@ private:
/**Kills a flight when it's stuck */ /**Kills a flight when it's stuck */
const int AI_STUCK_LIMIT = 100; const int AI_STUCK_LIMIT = 100;
int stuckCounter = 0; int stuckCounter = 0;
bool tracked = false;
/** /**
* Signals a reset to leg 1 at a different airport. * Signals a reset to leg 1 at a different airport.
* The leg loading happens at a different place than the parking loading. * The leg loading happens at a different place than the parking loading.
@ -212,6 +232,7 @@ private:
bool needsTaxiClearance = false; bool needsTaxiClearance = false;
bool _needsGroundElevation = true; bool _needsGroundElevation = true;
int takeOffStatus; // 1 = joined departure queue; 2 = Passed DepartureHold waypoint; handover control to tower; 0 = any other state. int takeOffStatus; // 1 = joined departure queue; 2 = Passed DepartureHold waypoint; handover control to tower; 0 = any other state.
time_t takeOffTimeSlot;
time_t timeElapsed; time_t timeElapsed;
PerformanceData* _performance; // the performance data for this aircraft PerformanceData* _performance; // the performance data for this aircraft
@ -235,4 +256,7 @@ private:
_controlsTargetAltitude, _controlsTargetAltitude,
_controlsTargetPitch, _controlsTargetPitch,
_controlsTargetSpeed; _controlsTargetSpeed;
std::unique_ptr<sg_ofstream> csvFile;
long csvIndex;
}; };

18
src/AIModel/AIFlightPlan.cxx
View file

@ -432,9 +432,10 @@ double FGAIFlightPlan::getDistanceToGo(double lat, double lon, FGAIWaypoint* wp)
} }
// sets distance in feet from a lead point to the current waypoint // sets distance in feet from a lead point to the current waypoint
// basically a catch radius, that triggers the advancement of WPs
void FGAIFlightPlan::setLeadDistance(double speed, double bearing, void FGAIFlightPlan::setLeadDistance(double speed, double bearing,
FGAIWaypoint* current, FGAIWaypoint* next){ FGAIWaypoint* current, FGAIWaypoint* next){
double turn_radius; double turn_radius_m;
// Handle Ground steering // Handle Ground steering
// At a turn rate of 30 degrees per second, it takes 12 seconds to do a full 360 degree turn // At a turn rate of 30 degrees per second, it takes 12 seconds to do a full 360 degree turn
// So, to get an estimate of the turn radius, calculate the cicumference of the circle // So, to get an estimate of the turn radius, calculate the cicumference of the circle
@ -444,11 +445,17 @@ void FGAIFlightPlan::setLeadDistance(double speed, double bearing,
lead_distance_ft = 0.5; lead_distance_ft = 0.5;
return; return;
} }
if (speed > 0 && speed < 0.5) {
lead_distance_ft = 5 * SG_FEET_TO_METER;
SG_LOG(SG_AI, SG_BULK, "Setting Leaddistance fixed " << (lead_distance_ft*SG_FEET_TO_METER));
return;
}
double speed_mps = speed * SG_KT_TO_MPS;
if (speed < 25) { if (speed < 25) {
turn_radius = ((360/30)*fabs(speed)) / (2*M_PI); turn_radius_m = ((360/30)*fabs(speed_mps)) / (2*M_PI);
} else { } else {
turn_radius = 0.1911 * speed * speed; // an estimate for 25 degrees bank turn_radius_m = 0.1911 * speed * speed; // an estimate for 25 degrees bank
} }
double inbound = bearing; double inbound = bearing;
@ -459,7 +466,10 @@ void FGAIFlightPlan::setLeadDistance(double speed, double bearing,
// leadInAngle = 30.0; // leadInAngle = 30.0;
//lead_distance_ft = turn_radius * sin(leadInAngle * SG_DEGREES_TO_RADIANS); //lead_distance_ft = turn_radius * sin(leadInAngle * SG_DEGREES_TO_RADIANS);
lead_distance_ft = turn_radius * tan((leadInAngle * SG_DEGREES_TO_RADIANS)/2);
double lead_distance_m = turn_radius_m * tan((leadInAngle * SG_DEGREES_TO_RADIANS)/2);
lead_distance_ft = lead_distance_m * SG_METER_TO_FEET;
SG_LOG(SG_AI, SG_BULK, "Setting Leaddistance " << (lead_distance_ft*SG_FEET_TO_METER) << " Turnradius " << turn_radius_m << " Speed " << speed_mps << " Half turn Angle " << (leadInAngle)/2);
if (lead_distance_ft > 1000) { if (lead_distance_ft > 1000) {
SG_LOG(SG_AI, SG_BULK, "Excessive leaddistance possible direction change " << lead_distance_ft << " leadInAngle " << leadInAngle << " inbound " << inbound << " outbound " << outbound); SG_LOG(SG_AI, SG_BULK, "Excessive leaddistance possible direction change " << lead_distance_ft << " leadInAngle " << leadInAngle << " inbound " << inbound << " outbound " << outbound);
} }

4
src/AIModel/AIFlightPlan.hxx
View file

@ -174,7 +174,9 @@ public:
double getDistanceToGo(double lat, double lon, FGAIWaypoint* wp) const; double getDistanceToGo(double lat, double lon, FGAIWaypoint* wp) const;
int getLeg () const { return leg;}; int getLeg () const { return leg;};
/** Set lead_distance_ft*/
void setLeadDistance(double speed, double bearing, FGAIWaypoint* current, FGAIWaypoint* next); void setLeadDistance(double speed, double bearing, FGAIWaypoint* current, FGAIWaypoint* next);
/** Set lead_distance_ft*/
void setLeadDistance(double distance_ft); void setLeadDistance(double distance_ft);
double getLeadDistance( void ) const {return lead_distance_ft;} double getLeadDistance( void ) const {return lead_distance_ft;}
double getBearing(FGAIWaypoint* previous, FGAIWaypoint* next) const; double getBearing(FGAIWaypoint* previous, FGAIWaypoint* next) const;
@ -272,6 +274,8 @@ private:
void eraseLastWaypoint(); void eraseLastWaypoint();
void pushBackWaypoint(FGAIWaypoint *wpt); void pushBackWaypoint(FGAIWaypoint *wpt);
/**Create an arc flightplan around a center from startAngle to endAngle.*/
void createArc(FGAIAircraft *ac, const SGGeod& center, int startAngle, int endAngle, int increment, int radius, double aElev, double aSpeed, const char* pattern);
bool createLandingTaxi(FGAIAircraft *, FGAirport *apt, double radius, const std::string& fltType, const std::string& acType, const std::string& airline); bool createLandingTaxi(FGAIAircraft *, FGAirport *apt, double radius, const std::string& fltType, const std::string& acType, const std::string& airline);
void createDefaultLandingTaxi(FGAIAircraft *, FGAirport* aAirport); void createDefaultLandingTaxi(FGAIAircraft *, FGAirport* aAirport);
void createDefaultTakeoffTaxi(FGAIAircraft *, FGAirport* aAirport, FGRunway* aRunway); void createDefaultTakeoffTaxi(FGAIAircraft *, FGAirport* aAirport, FGRunway* aRunway);

139
src/AIModel/AIFlightPlanCreate.cxx
View file

@ -163,6 +163,30 @@ FGAIWaypoint * FGAIFlightPlan::createOnRunway(FGAIAircraft * ac,
return wpt; return wpt;
} }
void FGAIFlightPlan::createArc(FGAIAircraft *ac, const SGGeod& center, int startAngle,
int endAngle, int increment, int radius, double aElev, double aSpeed, const char* pattern) {
double trackSegmentLength = (2 * M_PI * radius) / 360.0;
double dummyAz2;
char buffer[20];
if (endAngle > startAngle && increment<0) {
endAngle -= 360;
}
if (endAngle < startAngle && increment>0) {
endAngle += 360;
}
for (int i = startAngle; i != endAngle; i += increment) {
SGGeod result;
SGGeodesy::direct(center, i,
radius, result, dummyAz2);
snprintf(buffer, sizeof(buffer), pattern, i);
FGAIWaypoint *wpt = createInAir(ac, buffer, result, aElev, aSpeed);
wpt->setCrossat(aElev);
wpt->setTrackLength(trackSegmentLength);
pushBackWaypoint(wpt);
}
}
FGAIWaypoint * FGAIFlightPlan::createInAir(FGAIAircraft * ac, FGAIWaypoint * FGAIFlightPlan::createInAir(FGAIAircraft * ac,
const std::string & aName, const std::string & aName,
const SGGeod & aPos, double aElev, const SGGeod & aPos, double aElev,
@ -292,7 +316,7 @@ bool FGAIFlightPlan::createTakeoffTaxi(FGAIAircraft * ac, bool firstFlight,
FGTaxiNodeRef runwayNode; FGTaxiNodeRef runwayNode;
if (gn->getVersion() > 0) { if (gn->getVersion() > 0) {
runwayNode = gn->findNearestNodeOnRunway(runwayTakeoff); runwayNode = gn->findNearestNodeOnRunwayEntry(runwayTakeoff);
} else { } else {
runwayNode = gn->findNearestNode(runwayTakeoff); runwayNode = gn->findNearestNode(runwayTakeoff);
} }
@ -431,7 +455,8 @@ void FGAIFlightPlan::createDefaultLandingTaxi(FGAIAircraft * ac,
} }
} }
bool FGAIFlightPlan::createLandingTaxi(FGAIAircraft * ac, FGAirport * apt, bool FGAIFlightPlan::createLandingTaxi(FGAIAircraft * ac,
FGAirport * apt,
double radius, double radius,
const string & fltType, const string & fltType,
const string & acType, const string & acType,
@ -449,9 +474,10 @@ bool FGAIFlightPlan::createLandingTaxi(FGAIAircraft * ac, FGAirport * apt,
return true; return true;
} }
FGRunway * rwy = apt->getRunwayByIdent(activeRunway);
FGTaxiNodeRef runwayNode; FGTaxiNodeRef runwayNode;
if (gn->getVersion() == 1) { if (gn->getVersion() == 1) {
runwayNode = gn->findNearestNodeOnRunway(lastWptPos); runwayNode = gn->findNearestNodeOnRunwayExit(lastWptPos, rwy);
} else { } else {
runwayNode = gn->findNearestNode(lastWptPos); runwayNode = gn->findNearestNode(lastWptPos);
} }
@ -460,8 +486,9 @@ bool FGAIFlightPlan::createLandingTaxi(FGAIAircraft * ac, FGAirport * apt,
// fallback mechanism for this. // fallback mechanism for this.
// Starting from gate 0 doesn't work, so don't try it // Starting from gate 0 doesn't work, so don't try it
FGTaxiRoute taxiRoute; FGTaxiRoute taxiRoute;
if (runwayNode && gate.isValid()) if (runwayNode && gate.isValid()) {
taxiRoute = gn->findShortestRoute(runwayNode, gate.parking()); taxiRoute = gn->findShortestRoute(runwayNode, gate.parking());
}
if (taxiRoute.empty()) { if (taxiRoute.empty()) {
createDefaultLandingTaxi(ac, apt); createDefaultLandingTaxi(ac, apt);
@ -471,6 +498,7 @@ bool FGAIFlightPlan::createLandingTaxi(FGAIAircraft * ac, FGAirport * apt,
FGTaxiNodeRef node; FGTaxiNodeRef node;
taxiRoute.first(); taxiRoute.first();
int size = taxiRoute.size(); int size = taxiRoute.size();
//FIXME find better solution than fixed number of 2
// Omit the last two waypoints, as // Omit the last two waypoints, as
// those are created by createParking() // those are created by createParking()
// int route; // int route;
@ -483,7 +511,15 @@ bool FGAIFlightPlan::createLandingTaxi(FGAIAircraft * ac, FGAirport * apt,
ac->getPerformance()->vTaxi()); ac->getPerformance()->vTaxi());
wpt->setRouteIndex(route); wpt->setRouteIndex(route);
if( !waypoints.back() || SGGeodesy::distanceM( waypoints.back()->getPos(), wpt->getPos()) > 0 ) { // next WPT must be far enough
if (!waypoints.back() || SGGeodesy::distanceM(waypoints.back()->getPos(), wpt->getPos()) > 0 ) {
if (waypoints.back()) {
int dist = SGGeodesy::distanceM(waypoints.back()->getPos(), wpt->getPos());
// pretty near better set speed down
if (dist<10) {
wpt->setSpeed(ac->getPerformance()->vTaxi()/2);
}
}
pushBackWaypoint(wpt); pushBackWaypoint(wpt);
} }
} }
@ -496,6 +532,7 @@ static double accelDistance(double v0, double v1, double accel)
{ {
double t = fabs(v1 - v0) / accel; // time in seconds to change velocity double t = fabs(v1 - v0) / accel; // time in seconds to change velocity
// area under the v/t graph: (t * v0) + (dV / 2t) where (dV = v1 - v0) // area under the v/t graph: (t * v0) + (dV / 2t) where (dV = v1 - v0)
SG_LOG(SG_AI, SG_BULK, "Brakingtime " << t );
return t * 0.5 * (v1 + v0); return t * 0.5 * (v1 + v0);
} }
@ -716,12 +753,6 @@ bool FGAIFlightPlan::createDescent(FGAIAircraft * ac, FGAirport * apt,
// To prevent absurdly steep approaches, compute the origin from where the approach should have started // To prevent absurdly steep approaches, compute the origin from where the approach should have started
SGGeod origin; SGGeod origin;
if (ac->getTrafficRef()->getCallSign() ==
fgGetString("/ai/track-callsign")) {
//cerr << "Reposition information: Actual distance " << distance << ". required distance " << requiredDistance << endl;
//exit(1);
}
if (distance < requiredDistance * 0.8) { if (distance < requiredDistance * 0.8) {
reposition = true; reposition = true;
SGGeodesy::direct(initialTarget, azimuth, SGGeodesy::direct(initialTarget, azimuth,
@ -730,7 +761,11 @@ bool FGAIFlightPlan::createDescent(FGAIAircraft * ac, FGAirport * apt,
distance = SGGeodesy::distanceM(current, initialTarget); distance = SGGeodesy::distanceM(current, initialTarget);
azimuth = SGGeodesy::courseDeg(current, initialTarget); azimuth = SGGeodesy::courseDeg(current, initialTarget);
} else { } else {
origin = current; // We project a new point out of line so the aircraft has room to turn to new heading.
origin = SGGeodesy::direct(current, ac->getTrueHeadingDeg(), initialTurnRadius);
distance = SGGeodesy::distanceM(origin, initialTarget);
azimuth = SGGeodesy::courseDeg(origin, initialTarget);
origin = SGGeodesy::direct(current, azimuth, initialTurnRadius);
} }
double dAlt = 0; // = alt - (apt->getElevation() + 2000); double dAlt = 0; // = alt - (apt->getElevation() + 2000);
@ -818,16 +853,24 @@ bool FGAIFlightPlan::createDescent(FGAIAircraft * ac, FGAirport * apt,
arrivalTime = newEta; arrivalTime = newEta;
time_t additionalTimeNeeded = newEta - eta; time_t additionalTimeNeeded = newEta - eta;
SG_LOG(SG_AI, SG_BULK, "Additional time required " << additionalTimeNeeded << " ");
//Number of holds
int holdsPatterns = (additionalTimeNeeded-additionalTimeNeeded%240)/240;
additionalTimeNeeded -= holdsPatterns*240;
double distanceCovered = double distanceCovered =
((vApproach * SG_NM_TO_METER) / 3600.0) * additionalTimeNeeded; ((vApproach * SG_NM_TO_METER) / 3600.0) * additionalTimeNeeded;
distanceOut += distanceCovered; distanceOut += distanceCovered;
SGGeod secondaryTarget = SGGeod secondaryTarget =
rwy->pointOffCenterline(-distanceOut, lateralOffset); rwy->pointOffCenterline(-distanceOut, lateralOffset);
SGGeod secondHoldCenter =
rwy->pointOffCenterline(-2*distanceOut, lateralOffset);
initialTarget = rwy->pointOnCenterline(-distanceOut); initialTarget = rwy->pointOnCenterline(-distanceOut);
distance = SGGeodesy::distanceM(origin, secondaryTarget);
azimuth = SGGeodesy::courseDeg(origin, secondaryTarget); azimuth = SGGeodesy::courseDeg(origin, secondaryTarget);
distance = SGGeodesy::distanceM(origin, secondaryTarget);
double ratio = initialTurnRadius / distance; double ratio = initialTurnRadius / distance;
if (ratio > 1.0) if (ratio > 1.0)
ratio = 1.0; ratio = 1.0;
@ -900,20 +943,22 @@ bool FGAIFlightPlan::createDescent(FGAIAircraft * ac, FGAirport * apt,
currentAltitude = apt->getElevation() + 2000; currentAltitude = apt->getElevation() + 2000;
} }
double trackLength = (2 * M_PI * initialTurnRadius) / 360.0;
for (int i = startval; i != endval; i += increment) {
SGGeod result;
//double currentAltitude = apt->getElevation() + 2000;
SGGeodesy::direct(secondaryTarget, i, // Length of
initialTurnRadius, result, dummyAz2); if (holdsPatterns>0) {
snprintf(buffer, sizeof(buffer), "turn%03d", i); // Turn into hold
wpt = createInAir(ac, buffer, result, currentAltitude, vDescent); createArc(ac, secondaryTarget, startval, endval, increment, initialTurnRadius,
wpt->setCrossat(currentAltitude); currentAltitude, vDescent, "turnintohold%03d");
wpt->setTrackLength(trackLength); for (int t = 0; t < holdsPatterns; t++)
//cerr << "Track Length : " << wpt->trackLength; {
pushBackWaypoint(wpt); createArc(ac, secondaryTarget, endval, endval+180, increment, initialTurnRadius,
//cerr << " Position : " << result.getLatitudeDeg() << " " << result.getLongitudeDeg() << " " << currentAltitude << endl; currentAltitude, vDescent, "hold_1_%03d");
createArc(ac, secondHoldCenter, endval+180, endval, increment, initialTurnRadius,
currentAltitude, vDescent, "hold_2_%03d");
}
} else {
createArc(ac, secondaryTarget, startval, endval, increment, initialTurnRadius,
currentAltitude, vDescent, "turn%03d");
} }
@ -1044,10 +1089,10 @@ bool FGAIFlightPlan::createLanding(FGAIAircraft * ac, FGAirport * apt,
double rolloutDistance = accelDistance(vTouchdownMetric, vTaxiMetric, decelMetric); double rolloutDistance = accelDistance(vTouchdownMetric, vTaxiMetric, decelMetric);
int nPoints = (int)(rolloutDistance/30); int nPoints = (int)(rolloutDistance/60);
for (int i = 1; i < nPoints; i++) { for (int i = 1; i <= nPoints; i++) {
snprintf(buffer, sizeof(buffer), "landing%03d", i); snprintf(buffer, sizeof(buffer), "rollout%03d", i);
double t = ((double) i) / nPoints; double t = 1-pow((double) (nPoints - i), 2) / pow(nPoints, 2);
coord = rwy->pointOnCenterline(touchdownDistance + (rolloutDistance * t)); coord = rwy->pointOnCenterline(touchdownDistance + (rolloutDistance * t));
double vel = (vTouchdownMetric * (1.0 - t)) + (vTaxiMetric * t); double vel = (vTouchdownMetric * (1.0 - t)) + (vTaxiMetric * t);
wpt = createOnRunway(ac, buffer, coord, currElev, vel); wpt = createOnRunway(ac, buffer, coord, currElev, vel);
@ -1070,7 +1115,7 @@ bool FGAIFlightPlan::createLanding(FGAIAircraft * ac, FGAirport * apt,
coord = rwy->pointOnCenterline(mindist); coord = rwy->pointOnCenterline(mindist);
FGTaxiNodeRef tn; FGTaxiNodeRef tn;
if (gn->getVersion() > 0) { if (gn->getVersion() > 0) {
tn = gn->findNearestNodeOnRunway(coord, rwy); tn = gn->findNearestNodeOnRunwayExit(coord, rwy);
} else { } else {
tn = gn->findNearestNode(coord); tn = gn->findNearestNode(coord);
} }
@ -1087,7 +1132,7 @@ bool FGAIFlightPlan::createLanding(FGAIAircraft * ac, FGAirport * apt,
} }
/******************************************************************* /*******************************************************************
* CreateParking * createParking Leg 9
* initialize the Aircraft at the parking location * initialize the Aircraft at the parking location
******************************************************************/ ******************************************************************/
bool FGAIFlightPlan::createParking(FGAIAircraft * ac, FGAirport * apt, bool FGAIFlightPlan::createParking(FGAIAircraft * ac, FGAirport * apt,
@ -1098,30 +1143,42 @@ bool FGAIFlightPlan::createParking(FGAIAircraft * ac, FGAirport * apt,
double vTaxi = ac->getPerformance()->vTaxi(); double vTaxi = ac->getPerformance()->vTaxi();
double vTaxiReduced = vTaxi * (2.0 / 3.0); double vTaxiReduced = vTaxi * (2.0 / 3.0);
if (!gate.isValid()) { if (!gate.isValid()) {
wpt = createOnGround(ac, "END-Parking", apt->geod(), aptElev, wpt = createOnGround(ac, "END-ParkingInvalidGate", apt->geod(), aptElev,
vTaxiReduced); vTaxiReduced);
pushBackWaypoint(wpt); pushBackWaypoint(wpt);
return true; return true;
} }
FGParking* parking = gate.parking(); FGParking* parking = gate.parking();
double heading = SGMiscd::normalizePeriodic(0, 360, parking->getHeading() + 180.0); double reverseHeading = SGMiscd::normalizePeriodic(0, 360, parking->getHeading() + 180.0);
double az; // unused double az; // unused
SGGeod pos; SGGeod pos;
SGGeodesy::direct(parking->geod(), heading, 2.2 * parking->getRadius(), SGGeodesy::direct(parking->geod(), reverseHeading, 2 * parking->getRadius(),
pos, az); pos, az);
wpt = createOnGround(ac, "taxiStart", pos, aptElev, vTaxiReduced); wpt = createOnGround(ac, "parking", pos, aptElev, vTaxiReduced/2);
pushBackWaypoint(wpt); pushBackWaypoint(wpt);
SGGeodesy::direct(parking->geod(), heading, 0.1 * parking->getRadius(), SGGeodesy::direct(parking->geod(), reverseHeading, 1.5 * parking->getRadius(),
pos, az); pos, az);
wpt = createOnGround(ac, "taxiStart2", pos, aptElev, vTaxiReduced/2); wpt = createOnGround(ac, "parking2", pos, aptElev, vTaxiReduced/3);
pushBackWaypoint(wpt); pushBackWaypoint(wpt);
wpt = createOnGround(ac, "END-Parking", parking->geod(), aptElev, SGGeodesy::direct(parking->geod(), reverseHeading, parking->getRadius(),
vTaxiReduced/3); pos, az);
wpt = createOnGround(ac, "parking3", pos, aptElev, vTaxiReduced/3);
pushBackWaypoint(wpt);
char buffer[30];
snprintf(buffer, 30, "Parking%s", parking->getName().c_str());
wpt = createOnGround(ac, buffer, parking->geod(), aptElev,
2);
pushBackWaypoint(wpt);
SGGeodesy::direct(parking->geod(), parking->getHeading(), 1,
pos, az);
wpt = createOnGround(ac, "END-Parking", pos, aptElev, vTaxiReduced/3);
pushBackWaypoint(wpt); pushBackWaypoint(wpt);
return true; return true;
} }

4
src/AIModel/AIFlightPlanCreatePushBack.cxx
View file

@ -118,7 +118,7 @@ bool FGAIFlightPlan::createPushBack(FGAIAircraft *ac,
while (route.next(node, &rte)) while (route.next(node, &rte))
{ {
char buffer[20]; char buffer[20];
sprintf (buffer, "pushback-%03d", (short)node->getIndex()); snprintf (buffer, sizeof(buffer), "pushback-%03d", (short)node->getIndex());
FGAIWaypoint *wpt = createOnGround(ac, string(buffer), node->geod(), dep->getElevation(), vTaxiBackward); FGAIWaypoint *wpt = createOnGround(ac, string(buffer), node->geod(), dep->getElevation(), vTaxiBackward);
/* /*
@ -179,7 +179,7 @@ bool FGAIFlightPlan::createPushBack(FGAIAircraft *ac,
SGGeodesy::direct(parking->geod(), parkingHeading, SGGeodesy::direct(parking->geod(), parkingHeading,
(((double)i / numSegments) * distance), pushForwardPt, az2); (((double)i / numSegments) * distance), pushForwardPt, az2);
char buffer[20]; char buffer[20];
sprintf(buffer, "pushforward-%03d", (short)i); snprintf(buffer, sizeof(buffer), "pushforward-%03d", (short)i);
FGAIWaypoint *wpt = createOnGround(ac, string(buffer), pushForwardPt, dep->getElevation(), vTaxiReduced); FGAIWaypoint *wpt = createOnGround(ac, string(buffer), pushForwardPt, dep->getElevation(), vTaxiReduced);
wpt->setRouteIndex(pushForwardSegment->getIndex()); wpt->setRouteIndex(pushForwardSegment->getIndex());

3
src/Airports/dynamics.cxx
View file

@ -301,7 +301,8 @@ bool FGAirportDynamics::hasParkings() const
return !parent()->groundNetwork()->allParkings().empty(); return !parent()->groundNetwork()->allParkings().empty();
} }
ParkingAssignment FGAirportDynamics::getAvailableParking(double radius, const string & flType, ParkingAssignment FGAirportDynamics::getAvailableParking(double radius,
const string & flType,
const string & acType, const string & acType,
const string & airline) const string & airline)
{ {

73
src/Airports/groundnetwork.cxx
View file

@ -274,10 +274,27 @@ FGTaxiNodeRef FGGroundNetwork::findNearestNodeOffRunway(const SGGeod& aGeod, FGR
return *node; return *node;
} }
FGTaxiNodeRef FGGroundNetwork::findNearestNodeOnRunway(const SGGeod & aGeod, FGRunway* aRunway) const FGTaxiNodeRef FGGroundNetwork::findNearestNodeOnRunwayEntry(const SGGeod & aGeod) const
{ {
SG_UNUSED(aRunway); double d = DBL_MAX;
SGVec3d cartPos = SGVec3d::fromGeod(aGeod);
FGTaxiNodeRef result;
for (auto it = m_nodes.begin(); it != m_nodes.end(); ++it) {
if (!(*it)->getIsOnRunway())
continue;
double localDistanceSqr = distSqr(cartPos, (*it)->cart());
if (localDistanceSqr < d) {
SG_LOG(SG_AI, SG_BULK, "findNearestNodeOnRunway from Threshold " << localDistanceSqr);
d = localDistanceSqr;
result = *it;
}
}
return result;
}
FGTaxiNodeRef FGGroundNetwork::findNearestNodeOnRunwayExit(const SGGeod & aGeod, FGRunway* aRunway) const
{
double d = DBL_MAX; double d = DBL_MAX;
SGVec3d cartPos = SGVec3d::fromGeod(aGeod); SGVec3d cartPos = SGVec3d::fromGeod(aGeod);
FGTaxiNodeRef result = 0; FGTaxiNodeRef result = 0;
@ -289,6 +306,8 @@ FGTaxiNodeRef FGGroundNetwork::findNearestNodeOnRunway(const SGGeod & aGeod, FGR
if (aRunway) { if (aRunway) {
double headingTowardsExit = SGGeodesy::courseDeg(aGeod, (*it)->geod()); double headingTowardsExit = SGGeodesy::courseDeg(aGeod, (*it)->geod());
double diff = fabs(aRunway->headingDeg() - headingTowardsExit); double diff = fabs(aRunway->headingDeg() - headingTowardsExit);
SG_LOG(SG_AI, SG_BULK, "findNearestNodeOnRunwayExit " << aRunway->headingDeg() << " "
<< " Diff : " << diff << " " << (*it)->getIndex());
if (diff > 10) { if (diff > 10) {
// Only ahead // Only ahead
continue; continue;
@ -301,18 +320,64 @@ FGTaxiNodeRef FGGroundNetwork::findNearestNodeOnRunway(const SGGeod & aGeod, FGR
double exitHeading = SGGeodesy::courseDeg((*it)->geod(), double exitHeading = SGGeodesy::courseDeg((*it)->geod(),
(exitSegments.back())->geod()); (exitSegments.back())->geod());
diff = fabs(aRunway->headingDeg() - exitHeading); diff = fabs(aRunway->headingDeg() - exitHeading);
if (diff > 80) { SG_LOG(SG_AI, SG_BULK, "findNearestNodeOnRunwayExit2 " << diff << " " << (*it)->getIndex());
if (diff > 70) {
// Only exits going in our direction // Only exits going in our direction
continue; continue;
} }
} else {
SG_LOG(SG_AI, SG_BULK, "No Runway findNearestNodeOnRunwayExit");
} }
if (localDistanceSqr < d) { if (localDistanceSqr < d) {
SG_LOG(SG_AI, SG_BULK, "findNearestNodeOnRunwayExit3 " << localDistanceSqr << " " << (*it)->getIndex());
d = localDistanceSqr; d = localDistanceSqr;
result = *it; result = *it;
} }
} }
if(result) {
return result; return result;
}
// Ok then fallback only exits ahead
for (it = m_nodes.begin(); it != m_nodes.end(); ++it) {
if (!(*it)->getIsOnRunway())
continue;
double localDistanceSqr = distSqr(cartPos, (*it)->cart());
if (aRunway) {
double headingTowardsExit = SGGeodesy::courseDeg(aGeod, (*it)->geod());
double diff = fabs(aRunway->headingDeg() - headingTowardsExit);
SG_LOG(SG_AI, SG_BULK, "findNearestNodeOnRunwayExitFallback1 " << aRunway->headingDeg() << " "
<< " Diff : " << diff << " " << (*it)->getIndex());
if (diff > 10) {
// Only ahead
continue;
}
}
if (localDistanceSqr < d) {
SG_LOG(SG_AI, SG_BULK, "findNearestNodeOnRunwayExitFallback1 " << localDistanceSqr);
d = localDistanceSqr;
result = *it;
}
}
if(result) {
return result;
}
// Ok then fallback only exits
for (it = m_nodes.begin(); it != m_nodes.end(); ++it) {
if (!(*it)->getIsOnRunway())
continue;
double localDistanceSqr = distSqr(cartPos, (*it)->cart());
if (localDistanceSqr < d) {
SG_LOG(SG_AI, SG_BULK, "findNearestNodeOnRunwayExitFallback2 " << localDistanceSqr);
d = localDistanceSqr;
result = *it;
}
}
if(result) {
return result;
} else {
SG_LOG(SG_AI, SG_WARN, "No Exit found");
return 0;
}
} }
FGTaxiSegment *FGGroundNetwork::findOppositeSegment(unsigned int index) const FGTaxiSegment *FGGroundNetwork::findOppositeSegment(unsigned int index) const

4
src/Airports/groundnetwork.hxx
View file

@ -247,7 +247,9 @@ public:
{ return parent; } { return parent; }
FGTaxiNodeRef findNearestNode(const SGGeod& aGeod) const; FGTaxiNodeRef findNearestNode(const SGGeod& aGeod) const;
FGTaxiNodeRef findNearestNodeOnRunway(const SGGeod& aGeod, FGRunway* aRunway = NULL) const; FGTaxiNodeRef findNearestNodeOnRunwayEntry(const SGGeod& aGeod) const;
/**Returns the nearest node in that is in direction of runway heading. Falls back to ones behind aircraft*/
FGTaxiNodeRef findNearestNodeOnRunwayExit(const SGGeod& aGeod, FGRunway* aRunway = NULL) const;
FGTaxiNodeRef findNearestNodeOffRunway(const SGGeod& aGeod, FGRunway* aRunway, double distanceM) const; FGTaxiNodeRef findNearestNodeOffRunway(const SGGeod& aGeod, FGRunway* aRunway, double distanceM) const;

4958
test_suite/test_data/YBBN.groundnet.xml
View file

File diff suppressed because it is too large Load diff

2
test_suite/unit_tests/AI/test_groundnet.cxx
View file

@ -91,7 +91,7 @@ void GroundnetTests::testShortestRoute()
FGGroundNetwork* network = egph->groundNetwork(); FGGroundNetwork* network = egph->groundNetwork();
FGParkingRef startParking = network->findParkingByName("main-apron10"); FGParkingRef startParking = network->findParkingByName("main-apron10");
FGRunwayRef runway = egph->getRunwayByIndex(0); FGRunwayRef runway = egph->getRunwayByIndex(0);
FGTaxiNodeRef end = network->findNearestNodeOnRunway(runway->threshold()); FGTaxiNodeRef end = network->findNearestNodeOnRunwayEntry(runway->threshold());
FGTaxiRoute route = network->findShortestRoute(startParking, end); FGTaxiRoute route = network->findShortestRoute(startParking, end);
CPPUNIT_ASSERT_EQUAL(true, network->exists()); CPPUNIT_ASSERT_EQUAL(true, network->exists());
CPPUNIT_ASSERT_EQUAL(29, route.size()); CPPUNIT_ASSERT_EQUAL(29, route.size());

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

@ -75,8 +75,10 @@ void TrafficTests::setUp()
fgSetBool("/sim/terrasync/ai-data-update-now", false); fgSetBool("/sim/terrasync/ai-data-update-now", false);
fgSetBool("/sim/sound/atc/enabled", true); fgSetBool("/sim/sound/atc/enabled", true);
fgSetDouble("/instrumentation/comm[0]/frequencies/selected-mhz", 121.70); fgSetDouble("/instrumentation/comm[0]/frequencies/selected-mhz", 121.70);
fgSetString("/sim/multiplay/callsign", "AI-Shadow");
globals->append_data_path(SGPath::fromUtf8(FG_TEST_SUITE_DATA), false); globals->append_data_path(SGPath::fromUtf8(FG_TEST_SUITE_DATA), false);
globals->set_download_dir(globals->get_fg_home());
// ensure EDDF has a valid ground net for parking testing // ensure EDDF has a valid ground net for parking testing
FGAirport::clearAirportsCache(); FGAirport::clearAirportsCache();
@ -129,7 +131,7 @@ void TrafficTests::testPushback()
FGAISchedule* schedule = new FGAISchedule( FGAISchedule* schedule = new FGAISchedule(
"B737", "KLM", departureAirport->getId(), "G-BLA", "ID", false, "B737", "KLM", "N", flighttype, radius, 8); "B737", "KLM", departureAirport->getId(), "G-BLA", "ID", false, "B737", "KLM", "N", flighttype, radius, 8);
FGScheduledFlight* flight = new FGScheduledFlight("", "", departureAirport->getId(), arrivalAirport->getId(), 24, dep, arr, "WEEK", "HBR_BN_2"); FGScheduledFlight* flight = new FGScheduledFlight("testPushback", "", departureAirport->getId(), arrivalAirport->getId(), 24, dep, arr, "WEEK", "HBR_BN_2");
schedule->assign(flight); schedule->assign(flight);
SGSharedPtr<FGAIAircraft> aiAircraft = new FGAIAircraft{schedule}; SGSharedPtr<FGAIAircraft> aiAircraft = new FGAIAircraft{schedule};
@ -146,6 +148,7 @@ void TrafficTests::testPushback()
aiAircraft->setSpeed(0); aiAircraft->setSpeed(0);
aiAircraft->setBank(0); aiAircraft->setBank(0);
const string flightPlanName = departureAirport->getId() + "-" + arrivalAirport->getId() + ".xml"; const string flightPlanName = departureAirport->getId() + "-" + arrivalAirport->getId() + ".xml";
const double crs = SGGeodesy::courseDeg(departureAirport->geod(), arrivalAirport->geod()); // direct course const double crs = SGGeodesy::courseDeg(departureAirport->geod(), arrivalAirport->geod()); // direct course
@ -184,7 +187,7 @@ void TrafficTests::testPushbackCargo()
FGAISchedule* schedule = new FGAISchedule( FGAISchedule* schedule = new FGAISchedule(
"B737", "KLM", "EGPH", "G-BLA", "ID", false, "B737", "KLM", "N", "cargo", 24, 8); "B737", "KLM", "EGPH", "G-BLA", "ID", false, "B737", "KLM", "N", "cargo", 24, 8);
FGScheduledFlight* flight = new FGScheduledFlight("", "", "EGPH", "EGPF", 24, dep, arr, "WEEK", "HBR_BN_2"); FGScheduledFlight* flight = new FGScheduledFlight("testPushbackCargo", "", "EGPH", "EGPF", 24, dep, arr, "WEEK", "HBR_BN_2");
schedule->assign(flight); schedule->assign(flight);
SGSharedPtr<FGAIAircraft> aiAircraft = new FGAIAircraft{schedule}; SGSharedPtr<FGAIAircraft> aiAircraft = new FGAIAircraft{schedule};
@ -249,7 +252,7 @@ void TrafficTests::testChangeRunway()
FGAISchedule* schedule = new FGAISchedule( FGAISchedule* schedule = new FGAISchedule(
"B737", "KLM", departureAirport->getId(), "G-BLA", "ID", false, "B737", "KLM", "N", flighttype, radius, 8); "B737", "KLM", departureAirport->getId(), "G-BLA", "ID", false, "B737", "KLM", "N", flighttype, radius, 8);
FGScheduledFlight* flight = new FGScheduledFlight("", "", departureAirport->getId(), arrivalAirport->getId(), 24, dep, arr, "WEEK", "HBR_BN_2"); FGScheduledFlight* flight = new FGScheduledFlight("testChangeRunway", "", departureAirport->getId(), arrivalAirport->getId(), 24, dep, arr, "WEEK", "HBR_BN_2");
schedule->assign(flight); schedule->assign(flight);
SGSharedPtr<FGAIAircraft> aiAircraft = new FGAIAircraft{schedule}; SGSharedPtr<FGAIAircraft> aiAircraft = new FGAIAircraft{schedule};
@ -307,7 +310,7 @@ void TrafficTests::testPushforward()
FGAISchedule* schedule = new FGAISchedule( FGAISchedule* schedule = new FGAISchedule(
"B737", "KLM", departureAirport->getId(), "G-BLA", "ID", false, "B737", "KLM", "N", flighttype, radius, 8); "B737", "KLM", departureAirport->getId(), "G-BLA", "ID", false, "B737", "KLM", "N", flighttype, radius, 8);
FGScheduledFlight* flight = new FGScheduledFlight("", "", departureAirport->getId(), arrivalAirport->getId(), 24, dep, arr, "WEEK", "HBR_BN_2"); FGScheduledFlight* flight = new FGScheduledFlight("testPushforward", "", departureAirport->getId(), arrivalAirport->getId(), 24, dep, arr, "WEEK", "HBR_BN_2");
schedule->assign(flight); schedule->assign(flight);
SGSharedPtr<FGAIAircraft> aiAircraft = new FGAIAircraft{schedule}; SGSharedPtr<FGAIAircraft> aiAircraft = new FGAIAircraft{schedule};
@ -364,7 +367,7 @@ void TrafficTests::testPushforwardSpeedy()
FGAISchedule* schedule = new FGAISchedule( FGAISchedule* schedule = new FGAISchedule(
"B737", "KLM", departureAirport->getId(), "G-BLA", "ID", false, "B737", "KLM", "N", flighttype, radius, 8); "B737", "KLM", departureAirport->getId(), "G-BLA", "ID", false, "B737", "KLM", "N", flighttype, radius, 8);
FGScheduledFlight* flight = new FGScheduledFlight("", "", departureAirport->getId(), arrivalAirport->getId(), 24, dep, arr, "WEEK", "HBR_BN_2"); FGScheduledFlight* flight = new FGScheduledFlight("testPushforwardSpeedy", "", departureAirport->getId(), arrivalAirport->getId(), 24, dep, arr, "WEEK", "HBR_BN_2");
schedule->assign(flight); schedule->assign(flight);
SGSharedPtr<FGAIAircraft> aiAircraft = new FGAIAircraft{schedule}; SGSharedPtr<FGAIAircraft> aiAircraft = new FGAIAircraft{schedule};
@ -422,7 +425,7 @@ void TrafficTests::testPushforwardParkYBBN()
FGAISchedule* schedule = new FGAISchedule( FGAISchedule* schedule = new FGAISchedule(
"B737", "KLM", departureAirport->getId(), "G-BLA", "ID", false, "B737", "KLM", "N", flighttype, radius, 8); "B737", "KLM", departureAirport->getId(), "G-BLA", "ID", false, "B737", "KLM", "N", flighttype, radius, 8);
FGScheduledFlight* flight = new FGScheduledFlight("gaParkYSSY", "", departureAirport->getId(), arrivalAirport->getId(), 24, dep, arr, "WEEK", "HBR_BN_2"); FGScheduledFlight* flight = new FGScheduledFlight("testPushforwardParkYBBN", "", departureAirport->getId(), arrivalAirport->getId(), 24, dep, arr, "WEEK", "HBR_BN_2");
schedule->assign(flight); schedule->assign(flight);
SGSharedPtr<FGAIAircraft> aiAircraft = new FGAIAircraft{schedule}; SGSharedPtr<FGAIAircraft> aiAircraft = new FGAIAircraft{schedule};
@ -500,10 +503,10 @@ void TrafficTests::testPushforwardParkYBBNRepeatGa()
FGAISchedule* schedule = new FGAISchedule( FGAISchedule* schedule = new FGAISchedule(
"B737", "KLM", departureAirport->getId(), "G-BLA", "TST_BN_1", false, "B737", "KLM", "N", flighttype, radius, 8); "B737", "KLM", departureAirport->getId(), "G-BLA", "TST_BN_1", false, "B737", "KLM", "N", flighttype, radius, 8);
FGScheduledFlight* flight = new FGScheduledFlight("gaParkYSSY", "VFR", departureAirport->getId(), arrivalAirport->getId(), 24, dep, arr, "WEEK", "TST_BN_1"); FGScheduledFlight* flight = new FGScheduledFlight("testPushforwardParkYBBNRepeatGa", "VFR", departureAirport->getId(), arrivalAirport->getId(), 24, dep, arr, "WEEK", "TST_BN_1");
schedule->assign(flight); schedule->assign(flight);
FGScheduledFlight* returnFlight = new FGScheduledFlight("gaParkYSSY", "", arrivalAirport->getId(), departureAirport->getId(), 24, arr, ret, "WEEK", "TST_BN_1"); FGScheduledFlight* returnFlight = new FGScheduledFlight("testPushforwardParkYBBNRepeatGa", "", arrivalAirport->getId(), departureAirport->getId(), 24, arr, ret, "WEEK", "TST_BN_1");
schedule->assign(returnFlight); schedule->assign(returnFlight);
SGSharedPtr<FGAIAircraft> aiAircraft = new FGAIAircraft{schedule}; SGSharedPtr<FGAIAircraft> aiAircraft = new FGAIAircraft{schedule};
@ -554,6 +557,102 @@ void TrafficTests::testPushforwardParkYBBNRepeatGa()
CPPUNIT_ASSERT_EQUAL(true, (aiAircraft->getDie() || aiAircraft->GetFlightPlan()->getCurrentWaypoint()->getName() == "park")); CPPUNIT_ASSERT_EQUAL(true, (aiAircraft->getDie() || aiAircraft->GetFlightPlan()->getCurrentWaypoint()->getName() == "park"));
} }
void TrafficTests::testPushforwardParkYBBNRepeatGaDelayed()
{
FGAirportRef departureAirport = FGAirport::getByIdent("YBBN");
FGAirportRef arrivalAirport = FGAirport::getByIdent("YSSY");
fgSetString("/sim/presets/airport-id", arrivalAirport->getId());
// Time to depart
std::string dep = getTimeString(120);
// Time to arrive
std::string arr = getTimeString(3260);
// Time to arrive back
std::string ret = getTimeString(6460);
const int radius = 8.0;
const int cruiseAltFt = 32000;
const int cruiseSpeedKnots = 80;
const char* flighttype = "ga";
FGAISchedule* schedule = new FGAISchedule(
"B737", "KLM", departureAirport->getId(), "G-BLA", "TST_BN_1", false, "B737", "KLM", "N", flighttype, radius, 8);
FGScheduledFlight* flight = new FGScheduledFlight("testPushforwardParkYBBNRepeatGaDelayed", "VFR", departureAirport->getId(), arrivalAirport->getId(), 24, dep, arr, "WEEK", "TST_BN_1");
schedule->assign(flight);
FGScheduledFlight* returnFlight = new FGScheduledFlight("testPushforwardParkYBBNRepeatGaDelayed", "", arrivalAirport->getId(), departureAirport->getId(), 24, arr, ret, "WEEK", "TST_BN_1");
schedule->assign(returnFlight);
SGSharedPtr<FGAIAircraft> aiAircraft = new FGAIAircraft{schedule};
const SGGeod position = departureAirport->geod();
FGTestApi::setPositionAndStabilise(position);
aiAircraft->setPerformance("ga", "");
aiAircraft->setCompany("KLM");
aiAircraft->setAcType("B737");
aiAircraft->setSpeed(0);
aiAircraft->setBank(0);
const string flightPlanName = departureAirport->getId() + "-" + arrivalAirport->getId() + ".xml";
const double crs = SGGeodesy::courseDeg(departureAirport->geod(), arrivalAirport->geod()); // direct course
time_t departureTime = globals->get_time_params()->get_cur_time();
departureTime = departureTime + 90;
std::unique_ptr<FGAIFlightPlan> fp(new FGAIFlightPlan(aiAircraft,
flightPlanName, crs, departureTime,
departureAirport, arrivalAirport, true, radius,
cruiseAltFt, // cruise alt
position.getLatitudeDeg(),
position.getLongitudeDeg(),
cruiseSpeedKnots, flighttype,
aiAircraft->getAcType(),
aiAircraft->getCompany()));
CPPUNIT_ASSERT_EQUAL(fp->isValidPlan(), true);
aiAircraft->FGAIBase::setFlightPlan(std::move(fp));
globals->get_subsystem<FGAIManager>()->attach(aiAircraft);
FGAirport* departure = aiAircraft->getTrafficRef()->getDepartureAirport();
FGAirportDynamicsRef departureDynamics = departure->getDynamics();
ActiveRunway* activeDepartureRunway = departureDynamics->getApproachController()->getRunway("01");
time_t newDeparture = activeDepartureRunway->requestTimeSlot(aiAircraft->GetFlightPlan()->getStartTime());
// See that the wait queue is filled
for (size_t i = 0; i < 100; i++)
{
newDeparture = activeDepartureRunway->requestTimeSlot(newDeparture);
}
FGAirport* arrival = aiAircraft->getTrafficRef()->getArrivalAirport();
FGAirportDynamicsRef arrivalDynamics = arrival->getDynamics();
ActiveRunway* activeYSSYRunway = arrivalDynamics->getApproachController()->getRunway("16R");
time_t newArrival = activeYSSYRunway->requestTimeSlot(aiAircraft->GetFlightPlan()->getStartTime());
// See that the wait queue is filled
for (size_t i = 0; i < 100; i++)
{
newArrival = activeYSSYRunway->requestTimeSlot(newArrival);
}
aiAircraft = flyAI(aiAircraft, "flight_ga_YSSY_YBBN_park_repeat" + std::to_string(departureTime));
int shortestDistance = 10000;
const FGParkingList& parkings(arrivalAirport->groundNetwork()->allParkings());
FGParkingList::const_iterator it;
FGParking* nearestParking = 0;
for (it = parkings.begin(); it != parkings.end(); ++it) {
int currentDistance = !nearestParking ? 9999 : SGGeodesy::distanceM(nearestParking->geod(), (*it)->geod());
if (currentDistance < shortestDistance) {
nearestParking = (*it);
shortestDistance = currentDistance;
}
}
CPPUNIT_ASSERT_EQUAL(true, (aiAircraft->getDie() || aiAircraft->GetFlightPlan()->getCurrentWaypoint()->getName() == "park"));
}
void TrafficTests::testPushforwardParkYBBNRepeatGate() void TrafficTests::testPushforwardParkYBBNRepeatGate()
{ {
FGAirportRef departureAirport = FGAirport::getByIdent("YBBN"); FGAirportRef departureAirport = FGAirport::getByIdent("YBBN");
@ -569,7 +668,7 @@ void TrafficTests::testPushforwardParkYBBNRepeatGate()
// Time to arrive back // Time to arrive back
std::string ret = getTimeString(6460); std::string ret = getTimeString(6460);
const int radius = 20.0; const int radius = 32.0;
const int cruiseAltFt = 32000; const int cruiseAltFt = 32000;
const int cruiseSpeedKnots = 80; const int cruiseSpeedKnots = 80;
const char* flighttype = "gate"; const char* flighttype = "gate";
@ -658,7 +757,7 @@ FGAIAircraft * TrafficTests::flyAI(SGSharedPtr<FGAIAircraft> aiAircraft, std::st
char fname [160]; char fname [160];
time_t t = time(0); // get time now time_t t = time(0); // get time now
snprintf (fname, sizeof(fname), "%lld.csv", (long long) t); snprintf (fname, sizeof(fname), "%ld.csv", t);
SGPath p = SGPath::desktop() / (testname + fname); SGPath p = SGPath::desktop() / (testname + fname);
sg_ofstream csvFile; sg_ofstream csvFile;
csvFile.open(p); csvFile.open(p);

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

@ -42,6 +42,7 @@ class TrafficTests : public CppUnit::TestFixture
CPPUNIT_TEST(testPushforwardSpeedy); CPPUNIT_TEST(testPushforwardSpeedy);
CPPUNIT_TEST(testPushforwardParkYBBN); CPPUNIT_TEST(testPushforwardParkYBBN);
CPPUNIT_TEST(testPushforwardParkYBBNRepeatGa); CPPUNIT_TEST(testPushforwardParkYBBNRepeatGa);
CPPUNIT_TEST(testPushforwardParkYBBNRepeatGaDelayed);
CPPUNIT_TEST(testPushforwardParkYBBNRepeatGate); CPPUNIT_TEST(testPushforwardParkYBBNRepeatGate);
CPPUNIT_TEST_SUITE_END(); CPPUNIT_TEST_SUITE_END();
public: public:
@ -60,6 +61,7 @@ public:
void testPushforwardSpeedy(); void testPushforwardSpeedy();
void testPushforwardParkYBBN(); void testPushforwardParkYBBN();
void testPushforwardParkYBBNRepeatGa(); void testPushforwardParkYBBNRepeatGa();
void testPushforwardParkYBBNRepeatGaDelayed();
void testPushforwardParkYBBNRepeatGate(); void testPushforwardParkYBBNRepeatGate();
private: private:
long currentWorldTime; long currentWorldTime;