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/structure/exception.hxx>
#include <simgear/io/iostreams/sgstream.hxx>
#include <string>
#include <cmath>
@ -57,12 +58,16 @@ FGAIAircraft::FGAIAircraft(FGAISchedule* ref) : /* HOT must be disabled for AI A
_performance(nullptr)
{
trafficRef = ref;
csvFile = std::make_unique<sg_ofstream>();
if (trafficRef) {
groundOffset = trafficRef->getGroundOffset();
setCallSign(trafficRef->getCallSign());
tracked = getCallSign() == fgGetString("/ai/track-callsign")
|| fgGetString("/ai/track-callsign") == "ALL";
}
else
else {
groundOffset = 0;
}
fp = 0;
controller = 0;
@ -102,7 +107,7 @@ FGAIAircraft::FGAIAircraft(FGAISchedule* ref) : /* HOT must be disabled for AI A
trackCache.remainingLength = 0;
trackCache.startWptName = "-";
tcasThreatNode = props->getNode("tcas/threat-level", true);
tcasRANode = props->getNode("tcas/ra-sense", true);
_searchOrder = ModelSearchOrder::PREFER_AI;
@ -141,6 +146,8 @@ void FGAIAircraft::readFromScenario(SGPropertyNode* scFileNode) {
setFlightPlan(scFileNode->getStringValue("flightplan"),
scFileNode->getBoolValue("repeat", false));
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);
Run(dt);
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()
@ -250,7 +268,7 @@ void FGAIAircraft::ClimbTo(double alt_ft ) {
void FGAIAircraft::TurnTo(double heading) {
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;
// 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
return;
}
std::unique_ptr<FGAIFlightPlan> plan(new FGAIFlightPlan(flightplan));
if (plan->isValidPlan()) {
plan->setRepeat(repeat);
@ -319,12 +337,12 @@ void FGAIAircraft::ProcessFlightPlan( double dt, time_t now ) {
return;
}
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();
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();
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) {
@ -333,11 +351,11 @@ void FGAIAircraft::ProcessFlightPlan( double dt, time_t now ) {
}
if (!leadPointReached(curr, next, nextTurnAngle)) {
controlHeading(curr);
controlHeading(curr, nullptr);
controlSpeed(curr, next);
} else {
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()) {
fp->restart();
}
@ -364,14 +382,14 @@ void FGAIAircraft::ProcessFlightPlan( double dt, time_t now ) {
}
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();
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();
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
@ -389,14 +407,15 @@ void FGAIAircraft::ProcessFlightPlan( double dt, time_t now ) {
announcePositionToController();
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")) {
SG_LOG(SG_AI, SG_BULK, getCallSign() << "|Setting Leaddistance");
fp->setLeadDistance(tgt_speed, tgt_heading, curr, next);
} else {
// If we are ending in a parking
// If we are ending in a parking
fp->setLeadDistance(1);
}
@ -407,10 +426,10 @@ void FGAIAircraft::ProcessFlightPlan( double dt, time_t now ) {
if (curr->getCrossat() > -1000.0) {
use_perf_vs = false;
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;
tgt_vs = calcVerticalSpeed(vert_dist_ft, dist_m, speed, err_dist);
tgt_altitude_ft = curr->getCrossat();
tgt_altitude_ft = curr->getCrossat();
checkTcas();
} else {
use_perf_vs = true;
@ -422,7 +441,7 @@ void FGAIAircraft::ProcessFlightPlan( double dt, time_t now ) {
// Altitude restriction
use_perf_vs = false;
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;
tgt_vs = calcVerticalSpeed(vert_dist_ft, dist_m, speed, err_dist);
tgt_altitude_ft = curr->getCrossat();
@ -474,22 +493,33 @@ void FGAIAircraft::clearATCController()
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
void FGAIAircraft::assertSpeed(double speed)
{
if ((speed < -50) || (speed > 1000)) {
cerr << getCallSign() << " "
SG_LOG(SG_AI, SG_DEBUG, getCallSign() << " "
<< "Previous waypoint " << fp->getPreviousWaypoint()->getName() << " "
<< "Departure airport " << trafficRef->getDepartureAirport() << " "
<< "Leg " << fp->getLeg() << " "
<< "target_speed << " << tgt_speed << " "
<< "speedFraction << " << speedFraction << " "
<< "Currecnt speed << " << speed << " "
<< endl;
<< "Currecnt speed << " << speed << " ");
}
}
#endif
void FGAIAircraft::checkTcas(void)
{
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.
// 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) {
int leg;
if ((leg = fp->getLeg()) == 9) {
if ((leg = fp->getLeg()) == AILeg::PARKING) {
FGAirport *oldArr = trafficRef->getArrivalAirport();
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.
return false;
}
@ -544,6 +575,8 @@ bool FGAIAircraft::loadNextLeg(double distance) {
repositioned = false;
}
setCallSign(trafficRef->getCallSign());
tracked = getCallSign() == fgGetString("/ai/track-callsign")
|| fgGetString("/ai/track-callsign") == "ALL";
leg = 0;
fp->setLeg(leg);
}
@ -556,6 +589,7 @@ bool FGAIAircraft::loadNextLeg(double distance) {
} else {
double cruiseAlt = trafficRef->getCruiseAlt() * 100;
SG_LOG(SG_AI, SG_BULK, getCallSign() << "|Loading Leg " << leg+1);
bool ok = fp->create (this,
dep,
arr,
@ -572,7 +606,7 @@ bool FGAIAircraft::loadNextLeg(double distance) {
distance);
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;
@ -598,7 +632,7 @@ void FGAIAircraft::getGroundElev(double dt) {
// Only do the proper hitlist stuff if we are within visible range of the viewer.
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) {
return;
}
@ -620,7 +654,6 @@ void FGAIAircraft::getGroundElev(double dt) {
}
}
void FGAIAircraft::doGroundAltitude() {
if ((fp->getLeg() == 7) && ((altitude_ft - tgt_altitude_ft) > 5)) {
tgt_vs = -500;
@ -639,47 +672,47 @@ void FGAIAircraft::announcePositionToController() {
if (!trafficRef) {
return;
}
int leg = fp->getLeg();
if (!fp->getCurrentWaypoint()) {
// http://code.google.com/p/flightgear-bugs/issues/detail?id=1153
// 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
// 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
// the original leg numbers here!
switch (leg) {
case 1: // Startup and Push back
switch (leg) {
case AILeg::STARTUP_PUSHBACK: // Startup and Push back
if (trafficRef->getDepartureAirport()->getDynamics())
controller = trafficRef->getDepartureAirport()->getDynamics()->getStartupController();
break;
case 2: // Taxiing to runway
case AILeg::TAXI: // Taxiing to runway
if (trafficRef->getDepartureAirport()->getDynamics()->getGroundController()->exists())
controller = trafficRef->getDepartureAirport()->getDynamics()->getGroundController();
break;
case 3: //Take off tower controller
case AILeg::TAKEOFF: //Take off tower controller
if (trafficRef->getDepartureAirport()->getDynamics()) {
controller = trafficRef->getDepartureAirport()->getDynamics()->getTowerController();
towerController = 0;
} 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;
case 6:
case AILeg::APPROACH:
if (trafficRef->getArrivalAirport()->getDynamics()) {
controller = trafficRef->getArrivalAirport()->getDynamics()->getApproachController();
}
break;
case 8: // Taxiing for parking
case AILeg::PARKING_TAXI: // Taxiing for parking
if (trafficRef->getArrivalAirport()->getDynamics()->getGroundController()->exists())
controller = trafficRef->getArrivalAirport()->getDynamics()->getGroundController();
break;
default:
controller = 0;
controller = nullptr;
break;
}
@ -701,7 +734,7 @@ void FGAIAircraft::scheduleForATCTowerDepartureControl(int state) {
if (trafficRef->getDepartureAirport()->getDynamics()) {
towerController = trafficRef->getDepartureAirport()->getDynamics()->getTowerController();
} 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) {
towerController->announcePosition(getID(), fp.get(), fp->getCurrentWaypoint()->getRouteIndex(),
@ -717,10 +750,10 @@ void FGAIAircraft::scheduleForATCTowerDepartureControl(int state) {
void FGAIAircraft::processATC(const FGATCInstruction& instruction) {
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
// if circular waits are resolved.
// For now, just take the offending aircraft
// For now, just take the offending aircraft
// out of the scene
setDie(true);
// a more proper way should be - of course - to
@ -783,12 +816,12 @@ void FGAIAircraft::handleFirstWaypoint() {
}
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
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!)
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());
@ -810,7 +843,7 @@ void FGAIAircraft::handleFirstWaypoint() {
// If next doesn't exist, as in incrementally created flightplans for
// AI/Trafficmanager created plans,
// 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")) {
fp->setLeadDistance(speed, hdg, curr, next);
} 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
*
* @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
* @return
*/
@ -871,20 +904,20 @@ bool FGAIAircraft::leadPointReached(FGAIWaypoint* curr, FGAIWaypoint* next, int
double lead_distance_m = fp->getLeadDistance() * SG_FEET_TO_METER;
const double arrivalDist = fabs(10.0*fp->getCurrentWaypoint()->getSpeed());
// 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 = -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) {
// Speed is int and cannot go below 1 knot
tgt_speed = -1;
}
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);
}
}
}
// arrive at parking
if ((dist_to_go_m < arrivalDist) && (speed > 0) && (tgt_speed > 0) && fp->getCurrentWaypoint()->contains("END")) {
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) {
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);
}
}
}
if (lead_distance_m < fabs(2*speed) * SG_FEET_TO_METER) {
//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;
fp->setLeadDistance(lead_distance_m * SG_METER_TO_FEET);
}
double bearing = 0;
// don't do bearing calculations for ground traffic
bearing = getBearing(fp->getBearing(pos, curr));
@ -916,7 +949,7 @@ bool FGAIAircraft::leadPointReached(FGAIWaypoint* curr, FGAIWaypoint* next, int
if (onGround() && fabs(nextTurnAngle) > 50 ) {
//don't skip over the waypoint
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;
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) ||
((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;
speedFraction = 1.0;
prev_dist_to_go = HUGE_VAL;
return true;
} 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
SG_LOG(SG_AI, SG_BULK, "Aircraft " << _callsign << " stuck. Speed " << speed);
SG_LOG(SG_AI, SG_BULK, getCallSign() << "|Aircraft stuck. Speed " << speed);
stuckCounter++;
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);
}
} 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
// so, the schedule should update and wait for the next departure time.
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
time_t nextDeparture = trafficRef->getDepartureTime();
@ -1024,21 +1059,39 @@ bool FGAIAircraft::handleAirportEndPoints(FGAIWaypoint* prev, time_t now) {
*
* @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);
if (fgIsFinite(calc_bearing)) {
double hdg_error = calc_bearing - tgt_heading;
if (fabs(hdg_error) > 0.01) {
TurnTo( calc_bearing );
// when moving forward we want to aim for an average heading
if(next&& speed>0) {
const double calcNextBearing = fp->getBearing(pos, next);
if (fgIsFinite(calc_bearing) && fgIsFinite(calcNextBearing)) {
double averageHeading = calc_bearing + (calcNextBearing-calc_bearing)/2;
averageHeading = SGMiscd::normalizePeriodic(0, 360, averageHeading);
double hdg_error = averageHeading - tgt_heading;
if (fabs(hdg_error) > 0.01) {
TurnTo( averageHeading );
}
} else {
SG_LOG(SG_AI, SG_WARN, "calc_bearing is not a finite number : "
<< "Speed " << speed
<< "pos : " << pos.getLatitudeDeg() << ", " << pos.getLongitudeDeg()
<< ", waypoint: " << curr->getLatitude() << ", " << curr->getLongitude() );
SG_LOG(SG_AI, SG_WARN, "waypoint name: '" << curr->getName() << "'" );;
}
} else {
cerr << "calc_bearing is not a finite number : "
<< "Speed " << speed
<< "pos : " << pos.getLatitudeDeg() << ", " << pos.getLongitudeDeg()
<< ", waypoint: " << curr->getLatitude() << ", " << curr->getLongitude() << endl;
cerr << "waypoint name: '" << curr->getName() << "'" << endl;
if (fgIsFinite(calc_bearing)) {
double hdg_error = calc_bearing - tgt_heading;
if (fabs(hdg_error) > 0.01) {
TurnTo( calc_bearing );
}
} 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")) {
fp->setLeadDistance(speed, tgt_heading, curr, next);
} 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);
}
}
@ -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 );
stuckCounter++;
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);
}
}
@ -1189,7 +1242,7 @@ void FGAIAircraft::updateHeading(double dt) {
headingChangeRate -= 3 * dt * sign(headingDiff);
}
/*
if (headingChangeRate > headingDiff ||
if (headingChangeRate > headingDiff ||
headingChangeRate < headingDiff) {
headingChangeRate = headingDiff*sign(roll);
}
@ -1291,10 +1344,10 @@ void FGAIAircraft::updateVerticalSpeedTarget(double dt) {
void FGAIAircraft::updatePitchAngleTarget() {
// if on ground and above vRotate -> initial rotation
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
// match pitch angle to vertical speed
else if (tgt_vs > 0) {
tgt_pitch = tgt_vs * 0.005;
@ -1310,7 +1363,7 @@ const string& FGAIAircraft::atGate()
return fp->getParkingGate()->ident();
}
}
static const string 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;
}
if (curr->getName() == string("BOD")) {
// Sentry: FLIGHTGEAR-893
if (!trafficRef->getArrivalAirport()) {
@ -1390,30 +1443,26 @@ bool FGAIAircraft::reachedEndOfCruise(double &distance) {
// return 'done' here, we are broken
return true;
}
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 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 descentTimeNeeded = verticalDistance / descentRate;
double distanceCovered = descentSpeed * descentTimeNeeded;
double distanceCovered = descentSpeed * descentTimeNeeded;
if (trafficRef->getCallSign() != "" &&
trafficRef->getCallSign() == fgGetString("/ai/track-callsign")) {
cerr << "Checking for end of cruise stage for :" << trafficRef->getCallSign() << endl;
cerr << "Descent rate : " << descentRate << endl;
cerr << "Descent speed : " << descentSpeed << endl;
cerr << "VerticalDistance : " << verticalDistance << ". Altitude : " << altitude_ft << ". Elevation " << trafficRef->getArrivalAirport()->getElevation() << endl;
cerr << "DecentTimeNeeded : " << descentTimeNeeded << endl;
cerr << "DistanceCovered : " << distanceCovered << endl;
if (tracked) {
SG_LOG(SG_AI, SG_DEBUG, "Checking for end of cruise stage for :" << trafficRef->getCallSign());
SG_LOG(SG_AI, SG_DEBUG, "Descent rate : " << descentRate);
SG_LOG(SG_AI, SG_DEBUG, "Descent speed : " << descentSpeed);
SG_LOG(SG_AI, SG_DEBUG, "VerticalDistance : " << verticalDistance << ". Altitude : " << altitude_ft << ". Elevation " << trafficRef->getArrivalAirport()->getElevation());
SG_LOG(SG_AI, SG_DEBUG, "DecentTimeNeeded : " << descentTimeNeeded);
SG_LOG(SG_AI, SG_DEBUG, "DistanceCovered : " << distanceCovered);
}
distance = distanceCovered;
if (dist < distanceCovered) {
if (trafficRef->getCallSign() == fgGetString("/ai/track-callsign")) {
//exit(1);
}
SG_LOG(SG_AI, SG_BULK, "End Of Cruise");
return true;
} else {
@ -1448,7 +1497,7 @@ void FGAIAircraft::resetPositionFromFlightPlan()
* Returns a normalised bearing
*/
double FGAIAircraft::getBearing(double crse)
double FGAIAircraft::getBearing(double crse)
{
double hdgDiff = fabs(hdg-crse);
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 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;
if (trafficRef->getCallSign() == fgGetString("/ai/track-callsign")) {
cerr << "Checking arrival time: ete " << ete << ". Time to go : " << secondsToGo << ". Track length = " << tracklength << endl;
if (tracked) {
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...
}
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 delta = target - cur;
double maxDelta = maxDeltaSec * dt;
// if delta is > maxDelta, use maxDelta, but with the sign of delta.
return (fabs(delta) < maxDelta) ? delta : copysign(maxDelta, delta);
}
// Drive various properties in a semi-realistic fashion.
// Note that we assume that the properties are set at
// a waypoint rather than in the leg before. So we need
// Note that we assume that the properties are set at
// a waypoint rather than in the leg before. So we need
// to use the previous waypoint (i.e. the one just passed)
// rather than the current one (i.e. the next one on the route)
void FGAIAircraft::updateModelProperties(double dt)
@ -1505,26 +1562,26 @@ void FGAIAircraft::updateModelProperties(double dt)
if ((!fp) || (!fp->getPreviousWaypoint())) {
return;
}
double targetGearPos = fp->getPreviousWaypoint()->getGear_down() ? 1.0 : 0.0;
double gearPos = GearPos();
if (gearPos != targetGearPos) {
gearPos = gearPos + limitRateOfChange(gearPos, targetGearPos, 0.1, dt);
if (gearPos < 0.001) {
gearPos = 0.0;
} else if (gearPos > 0.999) {
gearPos = 1.0;
gearPos = 1.0;
}
GearPos(gearPos);
}
}
double targetFlapsPos = fp->getPreviousWaypoint()->getFlaps();
double flapsPos = FlapsPos();
if (flapsPos != targetFlapsPos) {
flapsPos = flapsPos + limitRateOfChange(flapsPos, targetFlapsPos, 0.05, dt);
if (flapsPos < 0.001) {
if (flapsPos < 0.001) {
flapsPos = 0.0;
} else if (flapsPos > 0.999) {
flapsPos = 1.0;
@ -1542,103 +1599,119 @@ void FGAIAircraft::updateModelProperties(double dt)
TaxiLight(fp->getPreviousWaypoint()->getTaxiLight());
}
void FGAIAircraft::dumpCSVHeader(std::ofstream& o) {
o << "Index\t";
o << "Lat\t";
o << "Lon\t";
o << "Callsign\t";
o << "headingDiff\t";
o << "headingChangeRate\t";
o << "headingError\t";
o << "hdg\t";
o << "tgt_heading\t";
o << "tgt_speed\t";
o << "minBearing\t";
o << "speedFraction\t";
o << "groundOffset\t";
o << "speed\t";
o << "groundTargetSpeed\t";
o << "getVerticalSpeedFPM\t";
o << "getTrueHeadingDeg\t";
o << "bearingToWP\t";
void FGAIAircraft::dumpCSVHeader(std::unique_ptr<sg_ofstream> &o) {
(*o) << "Index\t";
(*o) << "Lat\t";
(*o) << "Lon\t";
(*o) << "Callsign\t";
(*o) << "headingDiff\t";
(*o) << "headingChangeRate\t";
(*o) << "headingError\t";
(*o) << "hdg\t";
(*o) << "tgt_heading\t";
(*o) << "tgt_speed\t";
(*o) << "minBearing\t";
(*o) << "speedFraction\t";
(*o) << "groundOffset\t";
(*o) << "speed\t";
(*o) << "groundTargetSpeed\t";
(*o) << "getVerticalSpeedFPM\t";
(*o) << "getTrueHeadingDeg\t";
(*o) << "bearingToWP\t";
o << "Name\t";
o << "WP Lat\t";
o << "WP Lon\t";
o << "Dist\t";
o << "Next Lat\t";
o << "Next Lon\t";
o << "Departuretime\t";
o << "Time\t";
o << "Startup diff\t";
o << "dist_to_go_m\t";
o << "Leaddistance\t";
o << "Leg\t";
o << "Num WP\t";
o << "no_roll\t";
o << "roll\t";
o << "stuckCounter";
o << endl;
(*o) << "Name\t";
(*o) << "WP Lat\t";
(*o) << "WP Lon\t";
(*o) << "Dist\t";
(*o) << "Next Lat\t";
(*o) << "Next Lon\t";
(*o) << "Departuretime\t";
(*o) << "Time\t";
(*o) << "Startup diff\t";
(*o) << "Departure\t";
(*o) << "Arrival\t";
(*o) << "dist_to_go_m\t";
(*o) << "leadInAngle\t";
(*o) << "Leaddistance\t";
(*o) << "Leg\t";
(*o) << "Num WP\t";
(*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);
o << lineIndex << "\t";
o << setprecision(12);
o << this->getGeodPos().getLatitudeDeg() << "\t";
o << this->getGeodPos().getLongitudeDeg() << "\t";
o << this->getCallSign() << "\t";
o << headingDiff << "\t";
o << headingChangeRate << "\t";
o << headingError << "\t";
o << hdg << "\t";
o << tgt_heading << "\t";
o << tgt_speed << "\t";
o << minBearing << "\t";
o << speedFraction << "\t";
o << groundOffset << "\t";
(*o) << lineIndex << "\t";
(*o) << setprecision(12);
(*o) << this->getGeodPos().getLatitudeDeg() << "\t";
(*o) << this->getGeodPos().getLongitudeDeg() << "\t";
(*o) << this->getCallSign() << "\t";
(*o) << headingDiff << "\t";
(*o) << headingChangeRate << "\t";
(*o) << headingError << "\t";
(*o) << hdg << "\t";
(*o) << tgt_heading << "\t";
(*o) << tgt_speed << "\t";
(*o) << minBearing << "\t";
(*o) << speedFraction << "\t";
(*o) << groundOffset << "\t";
o << round(this->getSpeed()) << "\t";
o << groundTargetSpeed << "\t";
o << round(this->getVerticalSpeedFPM()) << "\t";
o << this->getTrueHeadingDeg() << "\t";
(*o) << round(this->getSpeed()) << "\t";
(*o) << groundTargetSpeed << "\t";
(*o) << round(this->getVerticalSpeedFPM()) << "\t";
(*o) << this->getTrueHeadingDeg() << "\t";
FGAIFlightPlan* fp = this->GetFlightPlan();
FGAIWaypoint* currentWP = this->GetFlightPlan()->getCurrentWaypoint();
FGAIWaypoint* nextWP = this->GetFlightPlan()->getNextWaypoint();
if (currentWP) {
o << this->GetFlightPlan()->getBearing(this->getGeodPos(), this->GetFlightPlan()->getCurrentWaypoint()) << "\t";
o << currentWP->getName() << "\t";
o << currentWP->getPos().getLatitudeDeg() << "\t";
o << currentWP->getPos().getLongitudeDeg() << "\t";
if(fp) {
FGAIWaypoint* currentWP = fp->getCurrentWaypoint();
FGAIWaypoint* nextWP = fp->getNextWaypoint();
if (currentWP) {
(*o) << fp->getBearing(this->getGeodPos(), currentWP) << "\t";
(*o) << currentWP->getName() << "\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 << nextWP->getPos().getLatitudeDeg() << "\t";
o << nextWP->getPos().getLongitudeDeg() << "\t";
} else {
o << "\t\t";
}
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;
(*o) << this->onGround() << "\t";
(*o) << roll << "\t";
(*o) << repositioned << "\t";
(*o) << stuckCounter;
(*o) << endl;
}
#if 0

44
src/AIModel/AIAircraft.hxx
View file

@ -32,6 +32,23 @@ class FGAIFlightPlan;
class FGATCController;
class FGATCInstruction;
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 {
@ -57,7 +74,8 @@ public:
FGAIFlightPlan* GetFlightPlan() const { return fp.get(); };
void ProcessFlightPlan( double dt, time_t now );
time_t checkForArrivalTime(const std::string& wptName);
time_t calcDeparture();
void AccelTo(double speed);
void PitchTo(double angle);
void RollTo(double angle);
@ -90,6 +108,9 @@ public:
void setTrafficRef(FGAISchedule *ref) { trafficRef = ref; };
void resetTakeOffStatus() { takeOffStatus = 0;};
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);
const std::string& GetTransponderCode() { return transponderCode; };
@ -107,8 +128,6 @@ public:
inline double airspeed() const { return props->getFloatValue("velocities/airspeed-kt");};
const std::string& atGate();
std::string acwakecategory;
int getTakeOffStatus() { return takeOffStatus; };
void checkTcas();
double calcVerticalSpeed(double vert_ft, double dist_m, double speed, double error);
@ -116,9 +135,9 @@ public:
FGATCController * getATCController() { return controller; };
void clearATCController();
void dumpCSVHeader(std::ofstream& o);
void dumpCSV(std::ofstream& o, int lineIndex);
bool isBlockedBy(FGAIAircraft* other);
void dumpCSVHeader(std::unique_ptr<sg_ofstream> &o);
void dumpCSV(std::unique_ptr<sg_ofstream> &o, int lineIndex);
protected:
void Run(double dt);
@ -155,7 +174,8 @@ private:
bool handleAirportEndPoints(FGAIWaypoint* prev, time_t now);
bool reachedEndOfCruise(double&);
bool aiTrafficVisible(void);
void controlHeading(FGAIWaypoint* curr);
void controlHeading(FGAIWaypoint* curr,
FGAIWaypoint* next);
void controlSpeed(FGAIWaypoint* curr, FGAIWaypoint* next);
void updatePrimaryTargetValues(double dt, bool& flightplanActive, bool& aiOutOfSight);
@ -196,7 +216,7 @@ private:
/**Kills a flight when it's stuck */
const int AI_STUCK_LIMIT = 100;
int stuckCounter = 0;
bool tracked = false;
/**
* Signals a reset to leg 1 at a different airport.
* The leg loading happens at a different place than the parking loading.
@ -211,7 +231,8 @@ private:
bool needsTaxiClearance = false;
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;
PerformanceData* _performance; // the performance data for this aircraft
@ -235,4 +256,7 @@ private:
_controlsTargetAltitude,
_controlsTargetPitch,
_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
// basically a catch radius, that triggers the advancement of WPs
void FGAIFlightPlan::setLeadDistance(double speed, double bearing,
FGAIWaypoint* current, FGAIWaypoint* next){
double turn_radius;
double turn_radius_m;
// Handle Ground steering
// 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
// we travel on. Get the turn radius by dividing by PI (*2).
// FIXME Why when going backwards? No fabs
if (speed < 0.5) {
lead_distance_ft = 0.5;
return;
lead_distance_ft = 0.5;
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) {
turn_radius = ((360/30)*fabs(speed)) / (2*M_PI);
turn_radius_m = ((360/30)*fabs(speed_mps)) / (2*M_PI);
} 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;
@ -459,9 +466,12 @@ void FGAIFlightPlan::setLeadDistance(double speed, double bearing,
// leadInAngle = 30.0;
//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) {
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)) {

4
src/AIModel/AIFlightPlan.hxx
View file

@ -174,7 +174,9 @@ public:
double getDistanceToGo(double lat, double lon, FGAIWaypoint* wp) const;
int getLeg () const { return leg;};
/** Set lead_distance_ft*/
void setLeadDistance(double speed, double bearing, FGAIWaypoint* current, FGAIWaypoint* next);
/** Set lead_distance_ft*/
void setLeadDistance(double distance_ft);
double getLeadDistance( void ) const {return lead_distance_ft;}
double getBearing(FGAIWaypoint* previous, FGAIWaypoint* next) const;
@ -272,6 +274,8 @@ private:
void eraseLastWaypoint();
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);
void createDefaultLandingTaxi(FGAIAircraft *, FGAirport* aAirport);
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;
}
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,
const std::string & aName,
const SGGeod & aPos, double aElev,
@ -292,7 +316,7 @@ bool FGAIFlightPlan::createTakeoffTaxi(FGAIAircraft * ac, bool firstFlight,
FGTaxiNodeRef runwayNode;
if (gn->getVersion() > 0) {
runwayNode = gn->findNearestNodeOnRunway(runwayTakeoff);
runwayNode = gn->findNearestNodeOnRunwayEntry(runwayTakeoff);
} else {
runwayNode = gn->findNearestNode(runwayTakeoff);
}
@ -390,7 +414,7 @@ bool FGAIFlightPlan::createTakeoffTaxi(FGAIAircraft * ac, bool firstFlight,
}
double accell_point = 105.0;
// Acceleration point, 105 meters into the runway,
SGGeod entryPoint = waypoints.back()->getPos();
SGGeod entryPoint = waypoints.back()->getPos();
SGGeod runwayEnd = rwy->pointOnCenterlineDisplaced(0);
double distM = SGGeodesy::distanceM(entryPoint, runwayEnd);
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,
const string & fltType,
const string & acType,
@ -449,9 +474,10 @@ bool FGAIFlightPlan::createLandingTaxi(FGAIAircraft * ac, FGAirport * apt,
return true;
}
FGRunway * rwy = apt->getRunwayByIdent(activeRunway);
FGTaxiNodeRef runwayNode;
if (gn->getVersion() == 1) {
runwayNode = gn->findNearestNodeOnRunway(lastWptPos);
runwayNode = gn->findNearestNodeOnRunwayExit(lastWptPos, rwy);
} else {
runwayNode = gn->findNearestNode(lastWptPos);
}
@ -460,8 +486,9 @@ bool FGAIFlightPlan::createLandingTaxi(FGAIAircraft * ac, FGAirport * apt,
// fallback mechanism for this.
// Starting from gate 0 doesn't work, so don't try it
FGTaxiRoute taxiRoute;
if (runwayNode && gate.isValid())
if (runwayNode && gate.isValid()) {
taxiRoute = gn->findShortestRoute(runwayNode, gate.parking());
}
if (taxiRoute.empty()) {
createDefaultLandingTaxi(ac, apt);
@ -471,7 +498,8 @@ bool FGAIFlightPlan::createLandingTaxi(FGAIAircraft * ac, FGAirport * apt,
FGTaxiNodeRef node;
taxiRoute.first();
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()
// int route;
for (int i = 0; i < size - 2; i++) {
@ -483,7 +511,15 @@ bool FGAIFlightPlan::createLandingTaxi(FGAIAircraft * ac, FGAirport * apt,
ac->getPerformance()->vTaxi());
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);
}
}
@ -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
// 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
@ -519,7 +556,7 @@ static double pitchDistance(double pitchAngleDeg, double altGainM)
* more likely however.
*
******************************************************************/
bool FGAIFlightPlan::createTakeOff(FGAIAircraft * ac,
bool FGAIFlightPlan::createTakeOff(FGAIAircraft * ac,
bool firstFlight,
FGAirport * apt,
const SGGeod& pos,
@ -577,7 +614,7 @@ bool FGAIFlightPlan::createTakeOff(FGAIAircraft * ac,
accell_point += distM;
}
}
// distance from the runway threshold to accelerate to rotation speed.
double d = accelDistance(vTaxiMetric, vRotateMetric, accelMetric) + accell_point;
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
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) {
reposition = true;
SGGeodesy::direct(initialTarget, azimuth,
@ -730,7 +761,11 @@ bool FGAIFlightPlan::createDescent(FGAIAircraft * ac, FGAirport * apt,
distance = SGGeodesy::distanceM(current, initialTarget);
azimuth = SGGeodesy::courseDeg(current, initialTarget);
} 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);
@ -818,16 +853,24 @@ bool FGAIFlightPlan::createDescent(FGAIAircraft * ac, FGAirport * apt,
arrivalTime = newEta;
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 =
((vApproach * SG_NM_TO_METER) / 3600.0) * additionalTimeNeeded;
distanceOut += distanceCovered;
SGGeod secondaryTarget =
rwy->pointOffCenterline(-distanceOut, lateralOffset);
SGGeod secondHoldCenter =
rwy->pointOffCenterline(-2*distanceOut, lateralOffset);
initialTarget = rwy->pointOnCenterline(-distanceOut);
distance = SGGeodesy::distanceM(origin, secondaryTarget);
azimuth = SGGeodesy::courseDeg(origin, secondaryTarget);
distance = SGGeodesy::distanceM(origin, secondaryTarget);
double ratio = initialTurnRadius / distance;
if (ratio > 1.0)
ratio = 1.0;
@ -900,20 +943,22 @@ bool FGAIFlightPlan::createDescent(FGAIAircraft * ac, FGAirport * apt,
currentAltitude = apt->getElevation() + 2000;
}
double trackLength = (2 * M_PI * initialTurnRadius) / 360.0;
for (int i = startval; i != endval; i += increment) {
SGGeod result;
//double currentAltitude = apt->getElevation() + 2000;
SGGeodesy::direct(secondaryTarget, i,
initialTurnRadius, result, dummyAz2);
snprintf(buffer, sizeof(buffer), "turn%03d", i);
wpt = createInAir(ac, buffer, result, currentAltitude, vDescent);
wpt->setCrossat(currentAltitude);
wpt->setTrackLength(trackLength);
//cerr << "Track Length : " << wpt->trackLength;
pushBackWaypoint(wpt);
//cerr << " Position : " << result.getLatitudeDeg() << " " << result.getLongitudeDeg() << " " << currentAltitude << endl;
// Length of
if (holdsPatterns>0) {
// Turn into hold
createArc(ac, secondaryTarget, startval, endval, increment, initialTurnRadius,
currentAltitude, vDescent, "turnintohold%03d");
for (int t = 0; t < holdsPatterns; t++)
{
createArc(ac, secondaryTarget, endval, endval+180, increment, initialTurnRadius,
currentAltitude, vDescent, "hold_1_%03d");
createArc(ac, secondHoldCenter, endval+180, endval, increment, initialTurnRadius,
currentAltitude, vDescent, "hold_2_%03d");
}
} else {
createArc(ac, secondaryTarget, startval, endval, increment, initialTurnRadius,
currentAltitude, vDescent, "turn%03d");
}
@ -1044,10 +1089,10 @@ bool FGAIFlightPlan::createLanding(FGAIAircraft * ac, FGAirport * apt,
double rolloutDistance = accelDistance(vTouchdownMetric, vTaxiMetric, decelMetric);
int nPoints = (int)(rolloutDistance/30);
for (int i = 1; i < nPoints; i++) {
snprintf(buffer, sizeof(buffer), "landing%03d", i);
double t = ((double) i) / nPoints;
int nPoints = (int)(rolloutDistance/60);
for (int i = 1; i <= nPoints; i++) {
snprintf(buffer, sizeof(buffer), "rollout%03d", i);
double t = 1-pow((double) (nPoints - i), 2) / pow(nPoints, 2);
coord = rwy->pointOnCenterline(touchdownDistance + (rolloutDistance * t));
double vel = (vTouchdownMetric * (1.0 - t)) + (vTaxiMetric * t);
wpt = createOnRunway(ac, buffer, coord, currElev, vel);
@ -1070,7 +1115,7 @@ bool FGAIFlightPlan::createLanding(FGAIAircraft * ac, FGAirport * apt,
coord = rwy->pointOnCenterline(mindist);
FGTaxiNodeRef tn;
if (gn->getVersion() > 0) {
tn = gn->findNearestNodeOnRunway(coord, rwy);
tn = gn->findNearestNodeOnRunwayExit(coord, rwy);
} else {
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
******************************************************************/
bool FGAIFlightPlan::createParking(FGAIAircraft * ac, FGAirport * apt,
@ -1098,30 +1143,42 @@ bool FGAIFlightPlan::createParking(FGAIAircraft * ac, FGAirport * apt,
double vTaxi = ac->getPerformance()->vTaxi();
double vTaxiReduced = vTaxi * (2.0 / 3.0);
if (!gate.isValid()) {
wpt = createOnGround(ac, "END-Parking", apt->geod(), aptElev,
wpt = createOnGround(ac, "END-ParkingInvalidGate", apt->geod(), aptElev,
vTaxiReduced);
pushBackWaypoint(wpt);
return true;
}
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
SGGeod pos;
SGGeodesy::direct(parking->geod(), heading, 2.2 * parking->getRadius(),
SGGeodesy::direct(parking->geod(), reverseHeading, 2 * parking->getRadius(),
pos, az);
wpt = createOnGround(ac, "taxiStart", pos, aptElev, vTaxiReduced);
wpt = createOnGround(ac, "parking", pos, aptElev, vTaxiReduced/2);
pushBackWaypoint(wpt);
SGGeodesy::direct(parking->geod(), heading, 0.1 * parking->getRadius(),
SGGeodesy::direct(parking->geod(), reverseHeading, 1.5 * parking->getRadius(),
pos, az);
wpt = createOnGround(ac, "taxiStart2", pos, aptElev, vTaxiReduced/2);
wpt = createOnGround(ac, "parking2", pos, aptElev, vTaxiReduced/3);
pushBackWaypoint(wpt);
wpt = createOnGround(ac, "END-Parking", parking->geod(), aptElev,
vTaxiReduced/3);
SGGeodesy::direct(parking->geod(), reverseHeading, parking->getRadius(),
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);
return true;
}

4
src/AIModel/AIFlightPlanCreatePushBack.cxx
View file

@ -118,7 +118,7 @@ bool FGAIFlightPlan::createPushBack(FGAIAircraft *ac,
while (route.next(node, &rte))
{
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);
/*
@ -179,7 +179,7 @@ bool FGAIFlightPlan::createPushBack(FGAIAircraft *ac,
SGGeodesy::direct(parking->geod(), parkingHeading,
(((double)i / numSegments) * distance), pushForwardPt, az2);
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);
wpt->setRouteIndex(pushForwardSegment->getIndex());

3
src/Airports/dynamics.cxx
View file

@ -301,7 +301,8 @@ bool FGAirportDynamics::hasParkings() const
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 & airline)
{

75
src/Airports/groundnetwork.cxx
View file

@ -274,10 +274,27 @@ FGTaxiNodeRef FGGroundNetwork::findNearestNodeOffRunway(const SGGeod& aGeod, FGR
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;
SGVec3d cartPos = SGVec3d::fromGeod(aGeod);
FGTaxiNodeRef result = 0;
@ -289,6 +306,8 @@ FGTaxiNodeRef FGGroundNetwork::findNearestNodeOnRunway(const SGGeod & aGeod, FGR
if (aRunway) {
double headingTowardsExit = SGGeodesy::courseDeg(aGeod, (*it)->geod());
double diff = fabs(aRunway->headingDeg() - headingTowardsExit);
SG_LOG(SG_AI, SG_BULK, "findNearestNodeOnRunwayExit " << aRunway->headingDeg() << " "
<< " Diff : " << diff << " " << (*it)->getIndex());
if (diff > 10) {
// Only ahead
continue;
@ -301,18 +320,64 @@ FGTaxiNodeRef FGGroundNetwork::findNearestNodeOnRunway(const SGGeod & aGeod, FGR
double exitHeading = SGGeodesy::courseDeg((*it)->geod(),
(exitSegments.back())->geod());
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
continue;
}
} else {
SG_LOG(SG_AI, SG_BULK, "No Runway findNearestNodeOnRunwayExit");
}
if (localDistanceSqr < d) {
SG_LOG(SG_AI, SG_BULK, "findNearestNodeOnRunwayExit3 " << localDistanceSqr << " " << (*it)->getIndex());
d = localDistanceSqr;
result = *it;
}
}
return result;
if(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

4
src/Airports/groundnetwork.hxx
View file

@ -247,7 +247,9 @@ public:
{ return parent; }
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;

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();
FGParkingRef startParking = network->findParkingByName("main-apron10");
FGRunwayRef runway = egph->getRunwayByIndex(0);
FGTaxiNodeRef end = network->findNearestNodeOnRunway(runway->threshold());
FGTaxiNodeRef end = network->findNearestNodeOnRunwayEntry(runway->threshold());
FGTaxiRoute route = network->findShortestRoute(startParking, end);
CPPUNIT_ASSERT_EQUAL(true, network->exists());
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/sound/atc/enabled", true);
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->set_download_dir(globals->get_fg_home());
// ensure EDDF has a valid ground net for parking testing
FGAirport::clearAirportsCache();
@ -129,7 +131,7 @@ void TrafficTests::testPushback()
FGAISchedule* schedule = new FGAISchedule(
"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);
SGSharedPtr<FGAIAircraft> aiAircraft = new FGAIAircraft{schedule};
@ -146,6 +148,7 @@ void TrafficTests::testPushback()
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
@ -184,7 +187,7 @@ void TrafficTests::testPushbackCargo()
FGAISchedule* schedule = new FGAISchedule(
"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);
SGSharedPtr<FGAIAircraft> aiAircraft = new FGAIAircraft{schedule};
@ -249,7 +252,7 @@ void TrafficTests::testChangeRunway()
FGAISchedule* schedule = new FGAISchedule(
"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);
SGSharedPtr<FGAIAircraft> aiAircraft = new FGAIAircraft{schedule};
@ -307,7 +310,7 @@ void TrafficTests::testPushforward()
FGAISchedule* schedule = new FGAISchedule(
"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);
SGSharedPtr<FGAIAircraft> aiAircraft = new FGAIAircraft{schedule};
@ -364,7 +367,7 @@ void TrafficTests::testPushforwardSpeedy()
FGAISchedule* schedule = new FGAISchedule(
"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);
SGSharedPtr<FGAIAircraft> aiAircraft = new FGAIAircraft{schedule};
@ -422,7 +425,7 @@ void TrafficTests::testPushforwardParkYBBN()
FGAISchedule* schedule = new FGAISchedule(
"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);
SGSharedPtr<FGAIAircraft> aiAircraft = new FGAIAircraft{schedule};
@ -500,10 +503,10 @@ void TrafficTests::testPushforwardParkYBBNRepeatGa()
FGAISchedule* schedule = new FGAISchedule(
"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);
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);
SGSharedPtr<FGAIAircraft> aiAircraft = new FGAIAircraft{schedule};
@ -554,6 +557,102 @@ void TrafficTests::testPushforwardParkYBBNRepeatGa()
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()
{
FGAirportRef departureAirport = FGAirport::getByIdent("YBBN");
@ -569,7 +668,7 @@ void TrafficTests::testPushforwardParkYBBNRepeatGate()
// Time to arrive back
std::string ret = getTimeString(6460);
const int radius = 20.0;
const int radius = 32.0;
const int cruiseAltFt = 32000;
const int cruiseSpeedKnots = 80;
const char* flighttype = "gate";
@ -658,7 +757,7 @@ FGAIAircraft * TrafficTests::flyAI(SGSharedPtr<FGAIAircraft> aiAircraft, std::st
char fname [160];
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);
sg_ofstream csvFile;
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(testPushforwardParkYBBN);
CPPUNIT_TEST(testPushforwardParkYBBNRepeatGa);
CPPUNIT_TEST(testPushforwardParkYBBNRepeatGaDelayed);
CPPUNIT_TEST(testPushforwardParkYBBNRepeatGate);
CPPUNIT_TEST_SUITE_END();
public:
@ -60,6 +61,7 @@ public:
void testPushforwardSpeedy();
void testPushforwardParkYBBN();
void testPushforwardParkYBBNRepeatGa();
void testPushforwardParkYBBNRepeatGaDelayed();
void testPushforwardParkYBBNRepeatGate();
private:
long currentWorldTime;