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

461
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;
@ -102,7 +107,7 @@ FGAIAircraft::FGAIAircraft(FGAISchedule* ref) : /* HOT must be disabled for AI A
trackCache.remainingLength = 0; trackCache.remainingLength = 0;
trackCache.startWptName = "-"; trackCache.startWptName = "-";
tcasThreatNode = props->getNode("tcas/threat-level", true); tcasThreatNode = props->getNode("tcas/threat-level", true);
tcasRANode = props->getNode("tcas/ra-sense", true); tcasRANode = props->getNode("tcas/ra-sense", true);
_searchOrder = ModelSearchOrder::PREFER_AI; _searchOrder = ModelSearchOrder::PREFER_AI;
@ -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);
@ -272,7 +290,7 @@ void FGAIAircraft::setFlightPlan(const std::string& flightplan, bool repeat)
// this is the case for Nasal-scripted aircraft // this is the case for Nasal-scripted aircraft
return; return;
} }
std::unique_ptr<FGAIFlightPlan> plan(new FGAIFlightPlan(flightplan)); std::unique_ptr<FGAIFlightPlan> plan(new FGAIFlightPlan(flightplan));
if (plan->isValidPlan()) { if (plan->isValidPlan()) {
plan->setRepeat(repeat); plan->setRepeat(repeat);
@ -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
@ -389,14 +407,15 @@ void FGAIAircraft::ProcessFlightPlan( double dt, time_t now ) {
announcePositionToController(); announcePositionToController();
if (fp && props) { if (fp && props) {
props->getChild("arrival-time-sec", 0, true)->setIntValue(fp->getArrivalTime()); props->getChild("arrival-time-sec", 0, true)->setIntValue(fp->getArrivalTime());
} }
} }
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
fp->setLeadDistance(1); fp->setLeadDistance(1);
} }
@ -407,10 +426,10 @@ void FGAIAircraft::ProcessFlightPlan( double dt, time_t now ) {
if (curr->getCrossat() > -1000.0) { if (curr->getCrossat() > -1000.0) {
use_perf_vs = false; use_perf_vs = false;
double dist_m = fp->getDistanceToGo(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr); double dist_m = fp->getDistanceToGo(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr);
double vert_dist_ft = curr->getCrossat() - altitude_ft; double vert_dist_ft = curr->getCrossat() - altitude_ft;
double err_dist = prev->getCrossat() - altitude_ft; double err_dist = prev->getCrossat() - altitude_ft;
tgt_vs = calcVerticalSpeed(vert_dist_ft, dist_m, speed, err_dist); tgt_vs = calcVerticalSpeed(vert_dist_ft, dist_m, speed, err_dist);
tgt_altitude_ft = curr->getCrossat(); tgt_altitude_ft = curr->getCrossat();
checkTcas(); checkTcas();
} else { } else {
use_perf_vs = true; use_perf_vs = true;
@ -422,7 +441,7 @@ void FGAIAircraft::ProcessFlightPlan( double dt, time_t now ) {
// Altitude restriction // Altitude restriction
use_perf_vs = false; use_perf_vs = false;
double dist_m = fp->getDistanceToGo(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr); double dist_m = fp->getDistanceToGo(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr);
double vert_dist_ft = curr->getCrossat() - altitude_ft; double vert_dist_ft = curr->getCrossat() - altitude_ft;
double err_dist = - altitude_ft; double err_dist = - altitude_ft;
tgt_vs = calcVerticalSpeed(vert_dist_ft, dist_m, speed, err_dist); tgt_vs = calcVerticalSpeed(vert_dist_ft, dist_m, speed, err_dist);
tgt_altitude_ft = curr->getCrossat(); tgt_altitude_ft = curr->getCrossat();
@ -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)
@ -525,14 +555,15 @@ const char * FGAIAircraft::_getTransponderCode() const {
} }
// NOTE: Check whether the new (delayed leg increment code has any effect on this code. // NOTE: Check whether the new (delayed leg increment code has any effect on this code.
// 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;
@ -598,7 +632,7 @@ void FGAIAircraft::getGroundElev(double dt) {
// Only do the proper hitlist stuff if we are within visible range of the viewer. // Only do the proper hitlist stuff if we are within visible range of the viewer.
if (!invisible) { if (!invisible) {
double visibility_meters = fgGetDouble("/environment/visibility-m"); double visibility_meters = fgGetDouble("/environment/visibility-m");
if (SGGeodesy::distanceM(globals->get_view_position(), pos) > visibility_meters) { if (SGGeodesy::distanceM(globals->get_view_position(), pos) > visibility_meters) {
return; return;
} }
@ -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;
@ -639,47 +672,47 @@ void FGAIAircraft::announcePositionToController() {
if (!trafficRef) { if (!trafficRef) {
return; return;
} }
int leg = fp->getLeg(); int leg = fp->getLeg();
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
// leg numbers here that are one higher than the number that is used to create the leg // leg numbers here that are one higher than the number that is used to create the leg
// NOTE: As of July, 30, 2011, the post-creation leg updating is no longer happening. // NOTE: As of July, 30, 2011, the post-creation leg updating is no longer happening.
// 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(),
@ -717,10 +750,10 @@ void FGAIAircraft::scheduleForATCTowerDepartureControl(int state) {
void FGAIAircraft::processATC(const FGATCInstruction& instruction) { void FGAIAircraft::processATC(const FGATCInstruction& instruction) {
if (instruction.getCheckForCircularWait()) { if (instruction.getCheckForCircularWait()) {
// This is not exactly an elegant solution, // This is not exactly an elegant solution,
// but at least it gives me a chance to check // but at least it gives me a chance to check
// if circular waits are resolved. // if circular waits are resolved.
// For now, just take the offending aircraft // For now, just take the offending aircraft
// out of the scene // out of the scene
setDie(true); setDie(true);
// a more proper way should be - of course - to // a more proper way should be - of course - to
@ -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());
@ -810,7 +843,7 @@ void FGAIAircraft::handleFirstWaypoint() {
// If next doesn't exist, as in incrementally created flightplans for // If next doesn't exist, as in incrementally created flightplans for
// AI/Trafficmanager created plans, // AI/Trafficmanager created plans,
// Make sure lead distance is initialized otherwise // Make sure lead distance is initialized otherwise
// If we are ending in a parking // If we are ending in a parking
if (next && !curr->contains("END") && !curr->contains("PushBackPointlegend")) { if (next && !curr->contains("END") && !curr->contains("PushBackPointlegend")) {
fp->setLeadDistance(speed, hdg, curr, next); fp->setLeadDistance(speed, hdg, curr, next);
} else { } else {
@ -861,7 +894,7 @@ bool FGAIAircraft::fpExecutable(time_t now) {
* Check to see if we've reached the lead point for our next turn * Check to see if we've reached the lead point for our next turn
* *
* @param curr the WP we are currently targeting at. * @param curr the WP we are currently targeting at.
* @param next the WP that will follow. Used to detect passed WPs (heading diff curr/next > 120°) * @param next the WP that will follow. Used to detect passed WPs (heading diff curr/next > 120°)
* @param nextTurnAngle to detect sharp corners * @param nextTurnAngle to detect sharp corners
* @return * @return
*/ */
@ -871,20 +904,20 @@ bool FGAIAircraft::leadPointReached(FGAIWaypoint* curr, FGAIWaypoint* next, int
double lead_distance_m = fp->getLeadDistance() * SG_FEET_TO_METER; double lead_distance_m = fp->getLeadDistance() * SG_FEET_TO_METER;
const double arrivalDist = fabs(10.0*fp->getCurrentWaypoint()->getSpeed()); const double arrivalDist = fabs(10.0*fp->getCurrentWaypoint()->getSpeed());
// arrive at pushback end // arrive at pushback end
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);
} }
} }
// arrive at parking // arrive at parking
if ((dist_to_go_m < arrivalDist) && (speed > 0) && (tgt_speed > 0) && fp->getCurrentWaypoint()->contains("END")) { if ((dist_to_go_m < arrivalDist) && (speed > 0) && (tgt_speed > 0) && fp->getCurrentWaypoint()->contains("END")) {
tgt_speed = (dist_to_go_m / 10.0); tgt_speed = (dist_to_go_m / 10.0);
@ -894,18 +927,18 @@ 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);
} }
double bearing = 0; double bearing = 0;
// don't do bearing calculations for ground traffic // don't do bearing calculations for ground traffic
bearing = getBearing(fp->getBearing(pos, curr)); bearing = getBearing(fp->getBearing(pos, curr));
@ -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);
if (fgIsFinite(calc_bearing)) { // when moving forward we want to aim for an average heading
double hdg_error = calc_bearing - tgt_heading; if(next&& speed>0) {
if (fabs(hdg_error) > 0.01) { const double calcNextBearing = fp->getBearing(pos, next);
TurnTo( calc_bearing ); 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 { } else {
cerr << "calc_bearing is not a finite number : " if (fgIsFinite(calc_bearing)) {
<< "Speed " << speed double hdg_error = calc_bearing - tgt_heading;
<< "pos : " << pos.getLatitudeDeg() << ", " << pos.getLongitudeDeg() if (fabs(hdg_error) > 0.01) {
<< ", waypoint: " << curr->getLatitude() << ", " << curr->getLongitude() << endl; TurnTo( calc_bearing );
cerr << "waypoint name: '" << curr->getName() << "'" << endl; }
} 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() << "'" );;
}
} }
} }
@ -1059,7 +1112,7 @@ void FGAIAircraft::controlSpeed(FGAIWaypoint* curr, FGAIWaypoint* next) {
if (!curr->contains("END") && !curr->contains("PushBackPointlegend")) { if (!curr->contains("END") && !curr->contains("PushBackPointlegend")) {
fp->setLeadDistance(speed, tgt_heading, curr, next); fp->setLeadDistance(speed, tgt_heading, curr, next);
} else { } else {
// If we are ending in a parking the heading will be a // If we are ending in a parking the heading will be a
fp->setLeadDistance(1); fp->setLeadDistance(1);
} }
} }
@ -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);
} }
} }
@ -1189,7 +1242,7 @@ void FGAIAircraft::updateHeading(double dt) {
headingChangeRate -= 3 * dt * sign(headingDiff); headingChangeRate -= 3 * dt * sign(headingDiff);
} }
/* /*
if (headingChangeRate > headingDiff || if (headingChangeRate > headingDiff ||
headingChangeRate < headingDiff) { headingChangeRate < headingDiff) {
headingChangeRate = headingDiff*sign(roll); headingChangeRate = headingDiff*sign(roll);
} }
@ -1291,10 +1344,10 @@ void FGAIAircraft::updateVerticalSpeedTarget(double dt) {
void FGAIAircraft::updatePitchAngleTarget() { void FGAIAircraft::updatePitchAngleTarget() {
// if on ground and above vRotate -> initial rotation // if on ground and above vRotate -> initial rotation
if (onGround() && (speed > _performance->vRotate())) if (onGround() && (speed > _performance->vRotate()))
tgt_pitch = 8.0; // some rough B737 value tgt_pitch = 8.0; // some rough B737 value
//TODO pitch angle on approach and landing //TODO pitch angle on approach and landing
// match pitch angle to vertical speed // match pitch angle to vertical speed
else if (tgt_vs > 0) { else if (tgt_vs > 0) {
tgt_pitch = tgt_vs * 0.005; tgt_pitch = tgt_vs * 0.005;
@ -1310,7 +1363,7 @@ const string& FGAIAircraft::atGate()
return fp->getParkingGate()->ident(); return fp->getParkingGate()->ident();
} }
} }
static const string empty; static const string empty;
return empty; return empty;
} }
@ -1381,7 +1434,7 @@ bool FGAIAircraft::reachedEndOfCruise(double &distance) {
// return true (=done) here, so we don't just get stuck on this forever // return true (=done) here, so we don't just get stuck on this forever
return true; return true;
} }
if (curr->getName() == string("BOD")) { if (curr->getName() == string("BOD")) {
// Sentry: FLIGHTGEAR-893 // Sentry: FLIGHTGEAR-893
if (!trafficRef->getArrivalAirport()) { if (!trafficRef->getArrivalAirport()) {
@ -1390,30 +1443,26 @@ bool FGAIAircraft::reachedEndOfCruise(double &distance) {
// return 'done' here, we are broken // return 'done' here, we are broken
return true; return true;
} }
double dist = fp->getDistanceToGo(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr); double dist = fp->getDistanceToGo(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr);
double descentSpeed = (getPerformance()->vDescent() * SG_NM_TO_METER) / 3600.0; // convert from kts to meter/s double descentSpeed = (getPerformance()->vDescent() * SG_NM_TO_METER) / 3600.0; // convert from kts to meter/s
double descentRate = (getPerformance()->descentRate() * SG_FEET_TO_METER) / 60.0; // convert from feet/min to meter/s double descentRate = (getPerformance()->descentRate() * SG_FEET_TO_METER) / 60.0; // convert from feet/min to meter/s
double verticalDistance = ((altitude_ft - 2000.0) - trafficRef->getArrivalAirport()->getElevation()) *SG_FEET_TO_METER; double verticalDistance = ((altitude_ft - 2000.0) - trafficRef->getArrivalAirport()->getElevation()) *SG_FEET_TO_METER;
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 {
@ -1448,7 +1497,7 @@ void FGAIAircraft::resetPositionFromFlightPlan()
* Returns a normalised bearing * Returns a normalised bearing
*/ */
double FGAIAircraft::getBearing(double crse) double FGAIAircraft::getBearing(double crse)
{ {
double hdgDiff = fabs(hdg-crse); double hdgDiff = fabs(hdg-crse);
if (hdgDiff > 180) if (hdgDiff > 180)
@ -1477,27 +1526,35 @@ time_t FGAIAircraft::checkForArrivalTime(const string& wptName) {
} }
time_t now = globals->get_time_params()->get_cur_time(); time_t now = globals->get_time_params()->get_cur_time();
time_t arrivalTime = fp->getArrivalTime(); time_t arrivalTime = fp->getArrivalTime();
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;
double maxDelta = maxDeltaSec * dt; double maxDelta = maxDeltaSec * dt;
// if delta is > maxDelta, use maxDelta, but with the sign of delta. // if delta is > maxDelta, use maxDelta, but with the sign of delta.
return (fabs(delta) < maxDelta) ? delta : copysign(maxDelta, delta); return (fabs(delta) < maxDelta) ? delta : copysign(maxDelta, delta);
} }
// Drive various properties in a semi-realistic fashion. // Drive various properties in a semi-realistic fashion.
// Note that we assume that the properties are set at // Note that we assume that the properties are set at
// a waypoint rather than in the leg before. So we need // a waypoint rather than in the leg before. So we need
// to use the previous waypoint (i.e. the one just passed) // to use the previous waypoint (i.e. the one just passed)
// rather than the current one (i.e. the next one on the route) // rather than the current one (i.e. the next one on the route)
void FGAIAircraft::updateModelProperties(double dt) void FGAIAircraft::updateModelProperties(double dt)
@ -1505,26 +1562,26 @@ void FGAIAircraft::updateModelProperties(double dt)
if ((!fp) || (!fp->getPreviousWaypoint())) { if ((!fp) || (!fp->getPreviousWaypoint())) {
return; return;
} }
double targetGearPos = fp->getPreviousWaypoint()->getGear_down() ? 1.0 : 0.0; double targetGearPos = fp->getPreviousWaypoint()->getGear_down() ? 1.0 : 0.0;
double gearPos = GearPos(); double gearPos = GearPos();
if (gearPos != targetGearPos) { if (gearPos != targetGearPos) {
gearPos = gearPos + limitRateOfChange(gearPos, targetGearPos, 0.1, dt); gearPos = gearPos + limitRateOfChange(gearPos, targetGearPos, 0.1, dt);
if (gearPos < 0.001) { if (gearPos < 0.001) {
gearPos = 0.0; gearPos = 0.0;
} else if (gearPos > 0.999) { } else if (gearPos > 0.999) {
gearPos = 1.0; gearPos = 1.0;
} }
GearPos(gearPos); GearPos(gearPos);
} }
double targetFlapsPos = fp->getPreviousWaypoint()->getFlaps(); double targetFlapsPos = fp->getPreviousWaypoint()->getFlaps();
double flapsPos = FlapsPos(); double flapsPos = FlapsPos();
if (flapsPos != targetFlapsPos) { if (flapsPos != targetFlapsPos) {
flapsPos = flapsPos + limitRateOfChange(flapsPos, targetFlapsPos, 0.05, dt); flapsPos = flapsPos + limitRateOfChange(flapsPos, targetFlapsPos, 0.05, dt);
if (flapsPos < 0.001) { if (flapsPos < 0.001) {
flapsPos = 0.0; flapsPos = 0.0;
} else if (flapsPos > 0.999) { } else if (flapsPos > 0.999) {
flapsPos = 1.0; flapsPos = 1.0;
@ -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();
if (currentWP) { FGAIWaypoint* nextWP = fp->getNextWaypoint();
o << this->GetFlightPlan()->getBearing(this->getGeodPos(), this->GetFlightPlan()->getCurrentWaypoint()) << "\t"; if (currentWP) {
o << currentWP->getName() << "\t"; (*o) << fp->getBearing(this->getGeodPos(), currentWP) << "\t";
o << currentWP->getPos().getLatitudeDeg() << "\t"; (*o) << currentWP->getName() << "\t";
o << currentWP->getPos().getLongitudeDeg() << "\t"; (*o) << currentWP->getPos().getLatitudeDeg() << "\t";
(*o) << currentWP->getPos().getLongitudeDeg() << "\t";
}
if (nextWP) {
(*o) << nextWP->getPos().getLatitudeDeg() << "\t";
(*o) << nextWP->getPos().getLongitudeDeg() << "\t";
} else {
(*o) << "\t\t";
}
if (currentWP) {
(*o) << SGGeodesy::distanceM(this->getGeodPos(), currentWP->getPos()) << "\t";
(*o) << fp->getStartTime() << "\t";
(*o) << 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);
(*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 {
(*o) << "No WP\t\t\t\t\t\t\t\t";
}
if (fp->isValidPlan()) {
(*o) << fp->getLeadDistance() * SG_FEET_TO_METER << "\t";
(*o) << fp->getLeg() << "\t";
(*o) << fp->getNrOfWayPoints() << "\t";
} else {
(*o) << "FP NotValid\t\t";
}
} }
if (nextWP) { (*o) << this->onGround() << "\t";
o << nextWP->getPos().getLatitudeDeg() << "\t"; (*o) << roll << "\t";
o << nextWP->getPos().getLongitudeDeg() << "\t"; (*o) << repositioned << "\t";
} else { (*o) << stuckCounter;
o << "\t\t"; (*o) << endl;
}
if (currentWP) {
o << SGGeodesy::distanceM(this->getGeodPos(), currentWP->getPos()) << "\t";
o << this->GetFlightPlan()->getStartTime() << "\t";
o << globals->get_time_params()->get_cur_time() << "\t";
o << this->GetFlightPlan()->getStartTime() - globals->get_time_params()->get_cur_time() << "\t";
double dist_to_go_m = fp->getDistanceToGo(pos.getLatitudeDeg(), pos.getLongitudeDeg(), currentWP);
o << dist_to_go_m << "\t";
} else {
o << "No WP\t\t\t\t\t\t\t\t";
}
if (fp->isValidPlan()) {
o << fp->getLeadDistance() * SG_FEET_TO_METER << "\t";
o << fp->getLeg() << "\t";
o << fp->getNrOfWayPoints() << "\t";
} else {
o << "FP NotValid\t\t";
}
o << this->onGround() << "\t";
o << roll << "\t";
o << stuckCounter;
o << endl;
} }
#if 0 #if 0

44
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,7 +74,8 @@ 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);
void RollTo(double angle); void RollTo(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; };
@ -107,8 +128,6 @@ public:
inline double airspeed() const { return props->getFloatValue("velocities/airspeed-kt");}; inline double airspeed() const { return props->getFloatValue("velocities/airspeed-kt");};
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);
@ -116,9 +135,9 @@ public:
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.
@ -211,7 +231,8 @@ 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;
};

24
src/AIModel/AIFlightPlan.cxx
View file

@ -432,23 +432,30 @@ 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
// we travel on. Get the turn radius by dividing by PI (*2). // we travel on. Get the turn radius by dividing by PI (*2).
// FIXME Why when going backwards? No fabs // FIXME Why when going backwards? No fabs
if (speed < 0.5) { if (speed < 0.5) {
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,9 +466,12 @@ 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);
} }
/* /*
if ((lead_distance_ft > (3*turn_radius)) && (current->on_ground == false)) { if ((lead_distance_ft > (3*turn_radius)) && (current->on_ground == false)) {

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

151
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);
} }
@ -390,7 +414,7 @@ bool FGAIFlightPlan::createTakeoffTaxi(FGAIAircraft * ac, bool firstFlight,
} }
double accell_point = 105.0; double accell_point = 105.0;
// Acceleration point, 105 meters into the runway, // Acceleration point, 105 meters into the runway,
SGGeod entryPoint = waypoints.back()->getPos(); SGGeod entryPoint = waypoints.back()->getPos();
SGGeod runwayEnd = rwy->pointOnCenterlineDisplaced(0); SGGeod runwayEnd = rwy->pointOnCenterlineDisplaced(0);
double distM = SGGeodesy::distanceM(entryPoint, runwayEnd); double distM = SGGeodesy::distanceM(entryPoint, runwayEnd);
if (distM > accell_point) { if (distM > accell_point) {
@ -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,7 +498,8 @@ bool FGAIFlightPlan::createLandingTaxi(FGAIAircraft * ac, FGAirport * apt,
FGTaxiNodeRef node; FGTaxiNodeRef node;
taxiRoute.first(); taxiRoute.first();
int size = taxiRoute.size(); int size = taxiRoute.size();
// Omit the last two waypoints, as //FIXME find better solution than fixed number of 2
// Omit the last two waypoints, as
// those are created by createParking() // those are created by createParking()
// int route; // int route;
for (int i = 0; i < size - 2; i++) { for (int i = 0; i < size - 2; i++) {
@ -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,7 +532,8 @@ 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)
return t * 0.5 * (v1 + v0); SG_LOG(SG_AI, SG_BULK, "Brakingtime " << t );
return t * 0.5 * (v1 + v0);
} }
// find the horizontal distance to gain the specific altiude, holding // find the horizontal distance to gain the specific altiude, holding
@ -519,7 +556,7 @@ static double pitchDistance(double pitchAngleDeg, double altGainM)
* more likely however. * more likely however.
* *
******************************************************************/ ******************************************************************/
bool FGAIFlightPlan::createTakeOff(FGAIAircraft * ac, bool FGAIFlightPlan::createTakeOff(FGAIAircraft * ac,
bool firstFlight, bool firstFlight,
FGAirport * apt, FGAirport * apt,
const SGGeod& pos, const SGGeod& pos,
@ -577,7 +614,7 @@ bool FGAIFlightPlan::createTakeOff(FGAIAircraft * ac,
accell_point += distM; accell_point += distM;
} }
} }
// distance from the runway threshold to accelerate to rotation speed. // distance from the runway threshold to accelerate to rotation speed.
double d = accelDistance(vTaxiMetric, vRotateMetric, accelMetric) + accell_point; double d = accelDistance(vTaxiMetric, vRotateMetric, accelMetric) + accell_point;
SGGeod rotatePoint = rwy->pointOnCenterlineDisplaced(d); SGGeod rotatePoint = rwy->pointOnCenterlineDisplaced(d);
@ -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) { // Length of
SGGeod result; if (holdsPatterns>0) {
//double currentAltitude = apt->getElevation() + 2000; // Turn into hold
createArc(ac, secondaryTarget, startval, endval, increment, initialTurnRadius,
SGGeodesy::direct(secondaryTarget, i, currentAltitude, vDescent, "turnintohold%03d");
initialTurnRadius, result, dummyAz2); for (int t = 0; t < holdsPatterns; t++)
snprintf(buffer, sizeof(buffer), "turn%03d", i); {
wpt = createInAir(ac, buffer, result, currentAltitude, vDescent); createArc(ac, secondaryTarget, endval, endval+180, increment, initialTurnRadius,
wpt->setCrossat(currentAltitude); currentAltitude, vDescent, "hold_1_%03d");
wpt->setTrackLength(trackLength); createArc(ac, secondHoldCenter, endval+180, endval, increment, initialTurnRadius,
//cerr << "Track Length : " << wpt->trackLength; currentAltitude, vDescent, "hold_2_%03d");
pushBackWaypoint(wpt); }
//cerr << " Position : " << result.getLatitudeDeg() << " " << result.getLongitudeDeg() << " " << currentAltitude << endl; } 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)
{ {

75
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;