src/AIModel/AIAircraft.cxx

   1 // FGAIAircraft - FGAIBase-derived class creates an AI airplane
   2 //
   3 // Written by David Culp, started October 2003.
   4 //
   5 // Copyright (C) 2003  David P. Culp - davidculp2@comcast.net
   6 //
   7 // This program is free software; you can redistribute it and/or
   8 // modify it under the terms of the GNU General Public License as
   9 // published by the Free Software Foundation; either version 2 of the
  10 // License, or (at your option) any later version.
  11 //
  12 // This program is distributed in the hope that it will be useful, but
  13 // WITHOUT ANY WARRANTY; without even the implied warranty of
  14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  15 // General Public License for more details.
  16 //
  17 // You should have received a copy of the GNU General Public License
  18 // along with this program; if not, write to the Free Software
  19 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  20
  21 #ifdef HAVE_CONFIG_H
  22 #  include <config.h>
  23 #endif
  24
  25 #include <Main/fg_props.hxx>
  26 #include <Main/globals.hxx>
  27 #include <Viewer/viewer.hxx>
  28 #include <Scenery/scenery.hxx>
  29 #include <Scenery/tilemgr.hxx>
  30 #include <Airports/dynamics.hxx>
  31 #include <Airports/simple.hxx>
  32
  33 #include <string>
  34 #include <math.h>
  35 #include <time.h>
  36
  37 #ifdef _MSC_VER
  38 #  include <float.h>
  39 #  define finite _finite
  40 #elif defined(__sun) || defined(sgi)
  41 #  include <ieeefp.h>
  42 #endif
  43
  44 using std::string;
  45
  46 #include "AIAircraft.hxx"
  47 #include "performancedata.hxx"
  48 #include "performancedb.hxx"
  49 #include <signal.h>
  50
  51 //#include <Airports/trafficcontroller.hxx>
  52
  53 FGAIAircraft::FGAIAircraft(FGAISchedule *ref) :
  54      /* HOT must be disabled for AI Aircraft,
  55       * otherwise traffic detection isn't working as expected.*/
  56      FGAIBase(otAircraft, false)
  57 {
  58     trafficRef = ref;
  59     if (trafficRef) {
  60         groundOffset = trafficRef->getGroundOffset();
  61         setCallSign(trafficRef->getCallSign());
  62     }
  63     else
  64         groundOffset = 0;
  65
  66     fp              = 0;
  67     controller      = 0;
  68     prevController  = 0;
  69     towerController = 0;
  70     dt_count = 0;
  71     dt_elev_count = 0;
  72     use_perf_vs = true;
  73
  74     no_roll = false;
  75     tgt_speed = 0;
  76     speed = 0;
  77     groundTargetSpeed = 0;
  78
  79     // set heading and altitude locks
  80     hdg_lock = false;
  81     alt_lock = false;
  82     roll = 0;
  83     headingChangeRate = 0.0;
  84     headingError = 0;
  85     minBearing = 360;
  86     speedFraction =1.0;
  87
  88     holdPos = false;
  89     needsTaxiClearance = false;
  90     _needsGroundElevation = true;
  91
  92     _performance = 0; //TODO initialize to JET_TRANSPORT from PerformanceDB
  93     dt = 0;
  94     takeOffStatus = 0;
  95 }
  96
  97
  98 FGAIAircraft::~FGAIAircraft() {
  99     //delete fp;
 100     if (controller)
 101         controller->signOff(getID());
 102 }
 103
 104
 105 void FGAIAircraft::readFromScenario(SGPropertyNode* scFileNode) {
 106     if (!scFileNode)
 107         return;
 108
 109     FGAIBase::readFromScenario(scFileNode);
 110
 111     setPerformance(scFileNode->getStringValue("class", "jet_transport"));
 112     setFlightPlan(scFileNode->getStringValue("flightplan"),
 113                   scFileNode->getBoolValue("repeat", false));
 114     setCallSign(scFileNode->getStringValue("callsign"));
 115 }
 116
 117
 118 void FGAIAircraft::bind() {
 119     FGAIBase::bind();
 120
 121     tie("controls/gear/gear-down",
 122         SGRawValueMethods<FGAIAircraft,bool>(*this,
 123                 &FGAIAircraft::_getGearDown));
 124     tie("transponder-id",
 125         SGRawValueMethods<FGAIAircraft,const char*>(*this,
 126                 &FGAIAircraft::_getTransponderCode));
 127 }
 128
 129 void FGAIAircraft::update(double dt) {
 130     FGAIBase::update(dt);
 131     Run(dt);
 132     Transform();
 133 }
 134
 135 void FGAIAircraft::setPerformance(const std::string& acclass) {
 136      static PerformanceDB perfdb; //TODO make it a global service
 137      setPerformance(perfdb.getDataFor(acclass));
 138   }
 139
 140
 141  void FGAIAircraft::setPerformance(PerformanceData *ps) {
 142      _performance = ps;
 143   }
 144
 145
 146  void FGAIAircraft::Run(double dt) {
 147       FGAIAircraft::dt = dt;
 148
 149      bool outOfSight = false,
 150         flightplanActive = true;
 151      updatePrimaryTargetValues(flightplanActive, outOfSight); // target hdg, alt, speed
 152      if (outOfSight) {
 153         return;
 154      }
 155
 156      if (!flightplanActive) {
 157         groundTargetSpeed = 0;
 158      }
 159
 160      handleATCRequests(); // ATC also has a word to say
 161      updateSecondaryTargetValues(); // target roll, vertical speed, pitch
 162      updateActualState();
 163     // We currently have one situation in which an AIAircraft object is used that is not attached to the
 164     // AI manager. In this particular case, the AIAircraft is used to shadow the user's aircraft's behavior in the AI world.
 165     // Since we perhaps don't want a radar entry of our own aircraft, the following conditional should probably be adequate
 166     // enough
 167      if (manager)
 168         UpdateRadar(manager);
 169      checkVisibility();
 170   }
 171
 172 void FGAIAircraft::checkVisibility()
 173 {
 174   double visibility_meters = fgGetDouble("/environment/visibility-m");
 175   FGViewer* vw = globals->get_current_view();
 176   invisible = (SGGeodesy::distanceM(vw->getPosition(), pos) > visibility_meters);
 177 }
 178
 179
 180
 181 void FGAIAircraft::AccelTo(double speed) {
 182     tgt_speed = speed;
 183     //assertSpeed(speed);
 184     if (!isStationary())
 185         _needsGroundElevation = true;
 186 }
 187
 188
 189 void FGAIAircraft::PitchTo(double angle) {
 190     tgt_pitch = angle;
 191     alt_lock = false;
 192 }
 193
 194
 195 void FGAIAircraft::RollTo(double angle) {
 196     tgt_roll = angle;
 197     hdg_lock = false;
 198 }
 199
 200
 201 void FGAIAircraft::YawTo(double angle) {
 202     tgt_yaw = angle;
 203 }
 204
 205
 206 void FGAIAircraft::ClimbTo(double alt_ft ) {
 207     tgt_altitude_ft = alt_ft;
 208     alt_lock = true;
 209 }
 210
 211
 212 void FGAIAircraft::TurnTo(double heading) {
 213     tgt_heading = heading;
 214     hdg_lock = true;
 215 }
 216
 217
 218 double FGAIAircraft::sign(double x) {
 219     if (x == 0.0)
 220         return x;
 221     else
 222         return x/fabs(x);
 223 }
 224
 225
 226 void FGAIAircraft::setFlightPlan(const std::string& flightplan, bool repeat) {
 227     if (!flightplan.empty()) {
 228         FGAIFlightPlan* fp = new FGAIFlightPlan(flightplan);
 229         fp->setRepeat(repeat);
 230         SetFlightPlan(fp);
 231     }
 232 }
 233
 234
 235 void FGAIAircraft::SetFlightPlan(FGAIFlightPlan *f) {
 236     delete fp;
 237     fp = f;
 238 }
 239
 240
 241 void FGAIAircraft::ProcessFlightPlan( double dt, time_t now ) {
 242
 243     // the one behind you
 244     FGAIWaypoint* prev = 0;
 245     // the one ahead
 246     FGAIWaypoint* curr = 0;
 247     // the next plus 1
 248     FGAIWaypoint* next = 0;
 249
 250     prev = fp->getPreviousWaypoint();
 251     curr = fp->getCurrentWaypoint();
 252     next = fp->getNextWaypoint();
 253
 254     dt_count += dt;
 255
 256     ///////////////////////////////////////////////////////////////////////////
 257     // Initialize the flightplan
 258     //////////////////////////////////////////////////////////////////////////
 259     if (!prev) {
 260         handleFirstWaypoint();
 261         return;
 262     }                            // end of initialization
 263     if (! fpExecutable(now))
 264           return;
 265     dt_count = 0;
 266
 267     double distanceToDescent;
 268     if(reachedEndOfCruise(distanceToDescent)) {
 269         if (!loadNextLeg(distanceToDescent)) {
 270             setDie(true);
 271             return;
 272         }
 273         prev = fp->getPreviousWaypoint();
 274         curr = fp->getCurrentWaypoint();
 275         next = fp->getNextWaypoint();
 276     }
 277     if (!curr)
 278     {
 279         // Oops! FIXME
 280         return;
 281     }
 282
 283     if (! leadPointReached(curr)) {
 284         controlHeading(curr);
 285         controlSpeed(curr, next);
 286
 287             /*
 288             if (speed < 0) {
 289                 cerr << getCallSign()
 290                      << ": verifying lead distance to waypoint : "
 291                      << fp->getCurrentWaypoint()->name << " "
 292                      << fp->getLeadDistance() << ". Distance to go "
 293                      << (fp->getDistanceToGo(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr))
 294                      << ". Target speed = "
 295                      << tgt_speed
 296                      << ". Current speed = "
 297                      << speed
 298                      << ". Minimum Bearing " << minBearing
 299                      << endl;
 300             } */
 301     } else {
 302         if (curr->isFinished())      //end of the flight plan
 303         {
 304             if (fp->getRepeat())
 305                 fp->restart();
 306             else
 307                 setDie(true);
 308             return;
 309         }
 310
 311         if (next) {
 312             //TODO more intelligent method in AIFlightPlan, no need to send data it already has :-)
 313             tgt_heading = fp->getBearing(curr, next);
 314             spinCounter = 0;
 315         }
 316
 317         //TODO let the fp handle this (loading of next leg)
 318         fp->IncrementWaypoint( trafficRef != 0 );
 319         if  ( ((!(fp->getNextWaypoint()))) && (trafficRef != 0) )
 320             if (!loadNextLeg()) {
 321                 setDie(true);
 322                 return;
 323             }
 324
 325         prev = fp->getPreviousWaypoint();
 326         curr = fp->getCurrentWaypoint();
 327         next = fp->getNextWaypoint();
 328
 329         // Now that we have incremented the waypoints, excute some traffic manager specific code
 330         if (trafficRef) {
 331             //TODO isn't this best executed right at the beginning?
 332             if (! aiTrafficVisible()) {
 333                 setDie(true);
 334                 return;
 335             }
 336
 337             if (! handleAirportEndPoints(prev, now)) {
 338                 setDie(true);
 339                 return;
 340             }
 341
 342             announcePositionToController();
 343
 344         }
 345
 346         if (next) {
 347             fp->setLeadDistance(tgt_speed, tgt_heading, curr, next);
 348         }
 349
 350
 351         if (!(prev->getOn_ground()))  // only update the tgt altitude from flightplan if not on the ground
 352         {
 353             tgt_altitude_ft = prev->getAltitude();
 354             if (curr->getCrossat() > -1000.0) {
 355                 use_perf_vs = false;
 356                 // Distance to go in meters
 357                 double vert_dist_ft = curr->getCrossat() - altitude_ft;
 358                 double err_dist     = prev->getCrossat() - altitude_ft;
 359                 double dist_m       = fp->getDistanceToGo(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr);
 360                 tgt_vs = calcVerticalSpeed(vert_dist_ft, dist_m, speed, err_dist);
 361
 362                 checkTcas();
 363                 tgt_altitude_ft = curr->getCrossat();
 364             } else {
 365                 use_perf_vs = true;
 366             }
 367         }
 368         AccelTo(prev->getSpeed());
 369         hdg_lock = alt_lock = true;
 370         no_roll = prev->getOn_ground();
 371     }
 372 }
 373
 374 double FGAIAircraft::calcVerticalSpeed(double vert_ft, double dist_m, double speed, double err)
 375 {
 376     // err is negative when we passed too high
 377     double vert_m = vert_ft * SG_FEET_TO_METER;
 378     //double err_m  = err     * SG_FEET_TO_METER;
 379     //double angle = atan2(vert_m, dist_m);
 380     double speedMs = (speed * SG_NM_TO_METER) / 3600;
 381     //double vs = cos(angle) * speedMs; // Now in m/s
 382     double vs = 0;
 383     //cerr << "Error term = " << err_m << endl;
 384     if (dist_m) {
 385         vs = ((vert_m) / dist_m) * speedMs;
 386     }
 387     // Convert to feet per minute
 388     vs *= (SG_METER_TO_FEET * 60);
 389     //if (getCallSign() == "LUFTHANSA2002")
 390     //if (fabs(vs) > 100000) {
 391 //     if (getCallSign() == "LUFTHANSA2057") {
 392 //         cerr << getCallSign() << " " << fp->getPreviousWaypoint()->getName() << ". Alt = " << altitude_ft <<  " vertical dist = " << vert_m << " horiz dist = " << dist_m << " << angle  = " << angle * SG_RADIANS_TO_DEGREES << " vs " << vs << " horizontal speed " << speed << "Previous crossAT " << fp->getPreviousWaypoint()->getCrossat() << endl;
 393 //     //= (curr->getCrossat() - altitude_ft) / (fp->getDistanceToGo(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr)
 394 //     //                     / 6076.0 / speed*60.0);
 395 //         //raise(SIGSEGV);
 396 //     }
 397     return vs;
 398 }
 399
 400 void FGAIAircraft::assertSpeed(double speed)
 401 {
 402     if ((speed < -50) || (speed > 1000)) {
 403         cerr << getCallSign() << " "
 404             << "Previous waypoint " << fp->getPreviousWaypoint()->getName() << " "
 405             << "Departure airport " << trafficRef->getDepartureAirport() << " "
 406             << "Leg " << fp->getLeg() <<  " "
 407             << "target_speed << " << tgt_speed <<  " "
 408             << "speedFraction << " << speedFraction << " "
 409             << "Currecnt speed << " << speed << " "
 410             << endl;
 411        //raise(SIGSEGV);
 412     }
 413 }
 414
 415
 416
 417 void FGAIAircraft::checkTcas(void)
 418 {
 419     if (props->getIntValue("tcas/threat-level",0)==3)
 420     {
 421         int RASense = props->getIntValue("tcas/ra-sense",0);
 422         if ((RASense>0)&&(tgt_vs<4000))
 423             // upward RA: climb!
 424             tgt_vs = 4000;
 425         else
 426         if (RASense<0)
 427         {
 428             // downward RA: descend!
 429             if (altitude_ft < 1000)
 430             {
 431                 // too low: level off
 432                 if (tgt_vs>0)
 433                     tgt_vs = 0;
 434             }
 435             else
 436             {
 437                 if (tgt_vs >- 4000)
 438                     tgt_vs = -4000;
 439             }
 440         }
 441     }
 442 }
 443
 444 void FGAIAircraft::initializeFlightPlan() {
 445 }
 446
 447
 448 bool FGAIAircraft::_getGearDown() const {
 449     return _performance->gearExtensible(this);
 450 }
 451
 452
 453 const char * FGAIAircraft::_getTransponderCode() const {
 454   return transponderCode.c_str();
 455 }
 456
 457 // NOTE: Check whether the new (delayed leg increment code has any effect on this code.
 458 // Probably not, because it should only be executed after we have already passed the leg incrementing waypoint.
 459
 460 bool FGAIAircraft::loadNextLeg(double distance) {
 461
 462     int leg;
 463     if ((leg = fp->getLeg())  == 9) {
 464         if (!trafficRef->next()) {
 465             return false;
 466         }
 467         setCallSign(trafficRef->getCallSign());
 468         leg = 0;
 469         fp->setLeg(leg);
 470     }
 471
 472     FGAirport *dep = trafficRef->getDepartureAirport();
 473     FGAirport *arr = trafficRef->getArrivalAirport();
 474     if (!(dep && arr)) {
 475         setDie(true);
 476
 477     } else {
 478         double cruiseAlt = trafficRef->getCruiseAlt() * 100;
 479
 480         fp->create (this,
 481                     dep,
 482                     arr,
 483                     leg+1,
 484                     cruiseAlt,
 485                     trafficRef->getSpeed(),
 486                     _getLatitude(),
 487                     _getLongitude(),
 488                     false,
 489                     trafficRef->getRadius(),
 490                     trafficRef->getFlightType(),
 491                     acType,
 492                     company,
 493                     distance);
 494        //cerr << "created  leg " << leg << " for " << trafficRef->getCallSign() << endl;
 495     }
 496     return true;
 497 }
 498
 499
 500 // Note: This code is copied from David Luff's AILocalTraffic
 501 // Warning - ground elev determination is CPU intensive
 502 // Either this function or the logic of how often it is called
 503 // will almost certainly change.
 504
 505 void FGAIAircraft::getGroundElev(double dt) {
 506     dt_elev_count += dt;
 507
 508     if (!needGroundElevation())
 509         return;
 510     // Update minimally every three secs, but add some randomness
 511     // to prevent all AI objects doing this in synchrony
 512     if (dt_elev_count < (3.0) + (rand() % 10))
 513         return;
 514
 515     dt_elev_count = 0;
 516
 517     // Only do the proper hitlist stuff if we are within visible range of the viewer.
 518     if (!invisible) {
 519         double visibility_meters = fgGetDouble("/environment/visibility-m");
 520         FGViewer* vw = globals->get_current_view();
 521
 522         if (SGGeodesy::distanceM(vw->getPosition(), pos) > visibility_meters) {
 523             return;
 524         }
 525
 526         double range = 500.0;
 527         if (globals->get_tile_mgr()->schedule_scenery(pos, range, 5.0))
 528         {
 529             double alt;
 530             if (getGroundElevationM(SGGeod::fromGeodM(pos, 20000), alt, 0))
 531             {
 532                 tgt_altitude_ft = alt * SG_METER_TO_FEET;
 533                 if (isStationary())
 534                 {
 535                     // aircraft is stationary and we obtained altitude for this spot - we're done.
 536                     _needsGroundElevation = false;
 537                 }
 538             }
 539         }
 540     }
 541 }
 542
 543
 544 void FGAIAircraft::doGroundAltitude() {
 545     if ((fp->getLeg() == 7) && ((altitude_ft -  tgt_altitude_ft) > 5)) {
 546         tgt_vs = -500;
 547     } else {
 548         if ((fabs(altitude_ft - (tgt_altitude_ft+groundOffset)) > 1000.0)||
 549             (isStationary()))
 550             altitude_ft = (tgt_altitude_ft + groundOffset);
 551         else
 552             altitude_ft += 0.1 * ((tgt_altitude_ft+groundOffset) - altitude_ft);
 553         tgt_vs = 0;
 554     }
 555 }
 556
 557
 558 void FGAIAircraft::announcePositionToController() {
 559     if (trafficRef) {
 560         int leg = fp->getLeg();
 561
 562         // Note that leg has been incremented after creating the current leg, so we should use
 563         // leg numbers here that are one higher than the number that is used to create the leg
 564         // NOTE: As of July, 30, 2011, the post-creation leg updating is no longer happening.
 565         // Leg numbers are updated only once the aircraft passes the last waypoint created for that legm so I should probably just use
 566         // the original leg numbers here!
 567         switch (leg) {
 568           case 1:              // Startup and Push back
 569             if (trafficRef->getDepartureAirport()->getDynamics())
 570                 controller = trafficRef->getDepartureAirport()->getDynamics()->getStartupController();
 571             break;
 572         case 2:              // Taxiing to runway
 573             if (trafficRef->getDepartureAirport()->getDynamics()->getGroundNetwork()->exists())
 574                 controller = trafficRef->getDepartureAirport()->getDynamics()->getGroundNetwork();
 575             break;
 576         case 3:              //Take off tower controller
 577             if (trafficRef->getDepartureAirport()->getDynamics()) {
 578                 controller = trafficRef->getDepartureAirport()->getDynamics()->getTowerController();
 579                 towerController = 0;
 580             } else {
 581                 cerr << "Error: Could not find Dynamics at airport : " << trafficRef->getDepartureAirport()->getId() << endl;
 582             }
 583             break;
 584         case 6:
 585              if (trafficRef->getDepartureAirport()->getDynamics()) {
 586                  controller = trafficRef->getArrivalAirport()->getDynamics()->getApproachController();
 587               }
 588               break;
 589         case 8:              // Taxiing for parking
 590             if (trafficRef->getArrivalAirport()->getDynamics()->getGroundNetwork()->exists())
 591                 controller = trafficRef->getArrivalAirport()->getDynamics()->getGroundNetwork();
 592             break;
 593         default:
 594             controller = 0;
 595             break;
 596         }
 597
 598         if ((controller != prevController) && (prevController != 0)) {
 599             prevController->signOff(getID());
 600         }
 601         prevController = controller;
 602         if (controller) {
 603             controller->announcePosition(getID(), fp, fp->getCurrentWaypoint()->getRouteIndex(),
 604                                          _getLatitude(), _getLongitude(), hdg, speed, altitude_ft,
 605                                          trafficRef->getRadius(), leg, this);
 606         }
 607     }
 608 }
 609
 610 void FGAIAircraft::scheduleForATCTowerDepartureControl(int state) {
 611     if (!takeOffStatus) {
 612         int leg = fp->getLeg();
 613         if (trafficRef) {
 614             if (trafficRef->getDepartureAirport()->getDynamics()) {
 615                 towerController = trafficRef->getDepartureAirport()->getDynamics()->getTowerController();
 616             } else {
 617                 cerr << "Error: Could not find Dynamics at airport : " << trafficRef->getDepartureAirport()->getId() << endl;
 618             }
 619             if (towerController) {
 620                 towerController->announcePosition(getID(), fp, fp->getCurrentWaypoint()->getRouteIndex(),
 621                                                    _getLatitude(), _getLongitude(), hdg, speed, altitude_ft,
 622                                                     trafficRef->getRadius(), leg, this);
 623                 //cerr << "Scheduling " << trafficRef->getCallSign() << " for takeoff " << endl;
 624             }
 625         }
 626     }
 627     takeOffStatus = state;
 628 }
 629
 630 // Process ATC instructions and report back
 631
 632 void FGAIAircraft::processATC(FGATCInstruction instruction) {
 633     if (instruction.getCheckForCircularWait()) {
 634         // This is not exactly an elegant solution,
 635         // but at least it gives me a chance to check
 636         // if circular waits are resolved.
 637         // For now, just take the offending aircraft
 638         // out of the scene
 639         setDie(true);
 640         // a more proper way should be - of course - to
 641         // let an offending aircraft take an evasive action
 642         // for instance taxi back a little bit.
 643     }
 644     //cerr << "Processing ATC instruction (not Implimented yet)" << endl;
 645     if (instruction.getHoldPattern   ()) {}
 646
 647     // Hold Position
 648     if (instruction.getHoldPosition  ()) {
 649         if (!holdPos) {
 650             holdPos = true;
 651         }
 652         AccelTo(0.0);
 653     } else {
 654         if (holdPos) {
 655             //if (trafficRef)
 656             //  cerr << trafficRef->getCallSign() << " Resuming Taxi." << endl;
 657             holdPos = false;
 658         }
 659         // Change speed Instruction. This can only be excecuted when there is no
 660         // Hold position instruction.
 661         if (instruction.getChangeSpeed   ()) {
 662             //  if (trafficRef)
 663             //cerr << trafficRef->getCallSign() << " Changing Speed " << endl;
 664             AccelTo(instruction.getSpeed());
 665         } else {
 666             if (fp) AccelTo(fp->getPreviousWaypoint()->getSpeed());
 667         }
 668     }
 669     if (instruction.getChangeHeading ()) {
 670         hdg_lock = false;
 671         TurnTo(instruction.getHeading());
 672     } else {
 673         if (fp) {
 674             hdg_lock = true;
 675         }
 676     }
 677     if (instruction.getChangeAltitude()) {}
 678
 679 }
 680
 681
 682 void FGAIAircraft::handleFirstWaypoint() {
 683     bool eraseWaypoints;         //TODO YAGNI
 684     headingError = 0;
 685     if (trafficRef) {
 686         eraseWaypoints = true;
 687     } else {
 688         eraseWaypoints = false;
 689     }
 690
 691     FGAIWaypoint* prev = 0; // the one behind you
 692     FGAIWaypoint* curr = 0; // the one ahead
 693     FGAIWaypoint* next = 0;// the next plus 1
 694
 695     spinCounter = 0;
 696
 697     //TODO fp should handle this
 698     fp->IncrementWaypoint(eraseWaypoints);
 699     if (!(fp->getNextWaypoint()) && trafficRef)
 700         if (!loadNextLeg()) {
 701             setDie(true);
 702             return;
 703         }
 704
 705     prev = fp->getPreviousWaypoint();   //first waypoint
 706     curr = fp->getCurrentWaypoint();    //second waypoint
 707     next = fp->getNextWaypoint();       //third waypoint (might not exist!)
 708
 709     setLatitude(prev->getLatitude());
 710     setLongitude(prev->getLongitude());
 711     setSpeed(prev->getSpeed());
 712     setAltitude(prev->getAltitude());
 713
 714     if (prev->getSpeed() > 0.0)
 715         setHeading(fp->getBearing(prev->getLatitude(), prev->getLongitude(), curr));
 716     else
 717         setHeading(fp->getBearing(curr->getLatitude(), curr->getLongitude(), prev));
 718
 719     // If next doesn't exist, as in incrementally created flightplans for
 720     // AI/Trafficmanager created plans,
 721     // Make sure lead distance is initialized otherwise
 722     if (next)
 723         fp->setLeadDistance(speed, hdg, curr, next);
 724
 725     if (curr->getCrossat() > -1000.0) //use a calculated descent/climb rate
 726     {
 727         use_perf_vs = false;
 728         //tgt_vs = (curr->getCrossat() - prev->getAltitude())
 729         //         / (fp->getDistanceToGo(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr)
 730         //            / 6076.0 / prev->getSpeed()*60.0);
 731         double vert_dist_ft = curr->getCrossat() - altitude_ft;
 732         double err_dist     = prev->getCrossat() - altitude_ft;
 733         double dist_m       = fp->getDistanceToGo(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr);
 734         tgt_vs = calcVerticalSpeed(vert_dist_ft, dist_m, speed, err_dist);
 735         checkTcas();
 736         tgt_altitude_ft = curr->getCrossat();
 737     } else {
 738         use_perf_vs = true;
 739         tgt_altitude_ft = prev->getAltitude();
 740     }
 741     alt_lock = hdg_lock = true;
 742     no_roll = prev->getOn_ground();
 743     if (no_roll) {
 744         Transform();             // make sure aip is initialized.
 745         getGroundElev(60.1);     // make sure it's executed first time around, so force a large dt value
 746         doGroundAltitude();
 747         _needsGroundElevation = true; // check ground elevation again (maybe scenery wasn't available yet)
 748     }
 749     // Make sure to announce the aircraft's position
 750     announcePositionToController();
 751     prevSpeed = 0;
 752 }
 753
 754
 755 /**
 756  * Check Execution time (currently once every 100 ms)
 757  * Add a bit of randomization to prevent the execution of all flight plans
 758  * in synchrony, which can add significant periodic framerate flutter.
 759  *
 760  * @param now
 761  * @return
 762  */
 763 bool FGAIAircraft::fpExecutable(time_t now) {
 764     double rand_exec_time = (rand() % 100) / 100;
 765     return (dt_count > (0.1+rand_exec_time)) && (fp->isActive(now));
 766 }
 767
 768
 769 /**
 770  * Check to see if we've reached the lead point for our next turn
 771  *
 772  * @param curr
 773  * @return
 774  */
 775 bool FGAIAircraft::leadPointReached(FGAIWaypoint* curr) {
 776     double dist_to_go = fp->getDistanceToGo(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr);
 777
 778     //cerr << "2" << endl;
 779     double lead_dist = fp->getLeadDistance();
 780     // experimental: Use fabs, because speed can be negative (I hope) during push_back.
 781     if ((dist_to_go < fabs(10.0* speed)) && (speed < 0) && (tgt_speed < 0) && fp->getCurrentWaypoint()->contains("PushBackPoint")) {
 782           tgt_speed = -(dist_to_go / 10.0);
 783           if (tgt_speed > -0.5) {
 784                 tgt_speed = -0.5;
 785           }
 786           //assertSpeed(tgt_speed);
 787           if (fp->getPreviousWaypoint()->getSpeed() < tgt_speed) {
 788               fp->getPreviousWaypoint()->setSpeed(tgt_speed);
 789           }
 790     }
 791     if (lead_dist < fabs(2*speed)) {
 792       //don't skip over the waypoint
 793       lead_dist = fabs(2*speed);
 794       //cerr << "Extending lead distance to " << lead_dist << endl;
 795     }
 796
 797     //prev_dist_to_go = dist_to_go;
 798     //if (dist_to_go < lead_dist)
 799     //     cerr << trafficRef->getCallSign() << " Distance : "
 800     //          << dist_to_go << ": Lead distance "
 801     //          << lead_dist << " " << curr->name
 802     //          << " Ground target speed " << groundTargetSpeed << endl;
 803     double bearing = 0;
 804      // don't do bearing calculations for ground traffic
 805        bearing = getBearing(fp->getBearing(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr));
 806        if (bearing < minBearing) {
 807             minBearing = bearing;
 808             if (minBearing < 10) {
 809                  minBearing = 10;
 810             }
 811             if ((minBearing < 360.0) && (minBearing > 10.0)) {
 812                 speedFraction = 0.5 + (cos(minBearing *SG_DEGREES_TO_RADIANS) * 0.5);
 813             } else {
 814                 speedFraction = 1.0;
 815             }
 816        }
 817     if (trafficRef) {
 818          //cerr << "Tracking callsign : \"" << fgGetString("/ai/track-callsign") << "\"" << endl;
 819          //if (trafficRef->getCallSign() == fgGetString("/ai/track-callsign")) {
 820               //cerr << trafficRef->getCallSign() << " " << tgt_altitude_ft << " " << _getSpeed() << " "
 821               //     << _getAltitude() << " "<< _getLatitude() << " " << _getLongitude() << " " << dist_to_go << " " << lead_dist << " " << curr->getName() << " " << vs << " " << //tgt_vs << " " << bearing << " " << minBearing << " " << speedFraction << endl;
 822          //}
 823      }
 824     if ((dist_to_go < lead_dist) ||
 825         ((dist_to_go > prev_dist_to_go) && (bearing > (minBearing * 1.1))) ) {
 826         minBearing = 360;
 827         speedFraction = 1.0;
 828         prev_dist_to_go = HUGE_VAL;
 829         return true;
 830     } else {
 831         prev_dist_to_go = dist_to_go;
 832         return false;
 833     }
 834 }
 835
 836
 837 bool FGAIAircraft::aiTrafficVisible()
 838 {
 839     SGVec3d cartPos = SGVec3d::fromGeod(pos);
 840     const double d2 = (TRAFFICTOAIDISTTODIE * SG_NM_TO_METER) *
 841         (TRAFFICTOAIDISTTODIE * SG_NM_TO_METER);
 842     return (distSqr(cartPos, globals->get_aircraft_positon_cart()) < d2);
 843 }
 844
 845
 846 /**
 847  * Handle release of parking gate, once were taxiing. Also ensure service time at the gate
 848  * in the case of an arrival.
 849  *
 850  * @param prev
 851  * @return
 852  */
 853
 854 //TODO the trafficRef is the right place for the method
 855 bool FGAIAircraft::handleAirportEndPoints(FGAIWaypoint* prev, time_t now) {
 856     // prepare routing from one airport to another
 857     FGAirport * dep = trafficRef->getDepartureAirport();
 858     FGAirport * arr = trafficRef->getArrivalAirport();
 859
 860     if (!( dep && arr))
 861         return false;
 862
 863     // This waypoint marks the fact that the aircraft has passed the initial taxi
 864     // departure waypoint, so it can release the parking.
 865     //cerr << trafficRef->getCallSign() << " has passed waypoint " << prev->name << " at speed " << speed << endl;
 866     //cerr << "Passing waypoint : " << prev->getName() << endl;
 867     if (prev->contains("PushBackPoint")) {
 868         dep->getDynamics()->releaseParking(fp->getGate());
 869         AccelTo(0.0);
 870         //setTaxiClearanceRequest(true);
 871     }
 872     if (prev->contains("legend")) {
 873         fp->incrementLeg();
 874     }
 875     if (prev->contains(string("DepartureHold"))) {
 876         //cerr << "Passing point DepartureHold" << endl;
 877         scheduleForATCTowerDepartureControl(1);
 878     }
 879     if (prev->contains(string("Accel"))) {
 880         takeOffStatus = 3;
 881     }
 882     //if (prev->contains(string("landing"))) {
 883     //    if (speed < _performance->vTaxi() * 2) {
 884     //        fp->shortenToFirst(2, "legend");
 885     //    }
 886     //}
 887     //if (prev->contains(string("final"))) {
 888     //
 889     //     cerr << getCallSign() << " "
 890     //        << fp->getPreviousWaypoint()->getName()
 891     //        << ". Alt = " << altitude_ft
 892     //        << " vs " << vs
 893     //        << " horizontal speed " << speed
 894     //        << "Previous crossAT " << fp->getPreviousWaypoint()->getCrossat()
 895     //        << "Airport elevation" << getTrafficRef()->getArrivalAirport()->getElevation()
 896     //        << "Altitude difference " << (altitude_ft -  fp->getPreviousWaypoint()->getCrossat()) << endl;
 897     //q}
 898     // This is the last taxi waypoint, and marks the the end of the flight plan
 899     // so, the schedule should update and wait for the next departure time.
 900     if (prev->contains("END")) {
 901         time_t nextDeparture = trafficRef->getDepartureTime();
 902         // make sure to wait at least 20 minutes at parking to prevent "nervous" taxi behavior
 903         if (nextDeparture < (now+1200)) {
 904             nextDeparture = now + 1200;
 905         }
 906         fp->setTime(nextDeparture);
 907     }
 908
 909     return true;
 910 }
 911
 912
 913 /**
 914  * Check difference between target bearing and current heading and correct if necessary.
 915  *
 916  * @param curr
 917  */
 918 void FGAIAircraft::controlHeading(FGAIWaypoint* curr) {
 919     double calc_bearing = fp->getBearing(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr);
 920     //cerr << "Bearing = " << calc_bearing << endl;
 921     if (speed < 0) {
 922         calc_bearing +=180;
 923         if (calc_bearing > 360)
 924             calc_bearing -= 360;
 925     }
 926
 927     if (finite(calc_bearing)) {
 928         double hdg_error = calc_bearing - tgt_heading;
 929         if (fabs(hdg_error) > 0.01) {
 930             TurnTo( calc_bearing );
 931         }
 932
 933     } else {
 934         cerr << "calc_bearing is not a finite number : "
 935         << "Speed " << speed
 936         << "pos : " << pos.getLatitudeDeg() << ", " << pos.getLongitudeDeg()
 937         << ", waypoint: " << curr->getLatitude() << ", " << curr->getLongitude() << endl;
 938         cerr << "waypoint name: '" << curr->getName() << "'" << endl;
 939         //exit(1);                 // FIXME
 940     }
 941 }
 942
 943
 944 /**
 945  * Update the lead distance calculation if speed has changed sufficiently
 946  * to prevent spinning (hopefully);
 947  *
 948  * @param curr
 949  * @param next
 950  */
 951 void FGAIAircraft::controlSpeed(FGAIWaypoint* curr, FGAIWaypoint* next) {
 952     double speed_diff = speed - prevSpeed;
 953
 954     if (fabs(speed_diff) > 10) {
 955         prevSpeed = speed;
 956         //assertSpeed(speed);
 957         if (next) {
 958             fp->setLeadDistance(speed, tgt_heading, curr, next);
 959         }
 960     }
 961 }
 962
 963
 964 /**
 965  * Update target values (heading, alt, speed) depending on flight plan or control properties
 966  */
 967 void FGAIAircraft::updatePrimaryTargetValues(bool& flightplanActive, bool& aiOutOfSight) {
 968     if (fp)                      // AI object has a flightplan
 969     {
 970         //TODO make this a function of AIBase
 971         time_t now = time(NULL) + fgGetLong("/sim/time/warp");
 972         //cerr << "UpateTArgetValues() " << endl;
 973         ProcessFlightPlan(dt, now);
 974
 975         // Do execute Ground elev for inactive aircraft, so they
 976         // Are repositioned to the correct ground altitude when the user flies within visibility range.
 977         // In addition, check whether we are out of user range, so this aircraft
 978         // can be deleted.
 979         if (onGround()) {
 980                 Transform();     // make sure aip is initialized.
 981                 getGroundElev(dt);
 982                 doGroundAltitude();
 983                 // Transform();
 984                 pos.setElevationFt(altitude_ft);
 985         }
 986         if (trafficRef) {
 987            //cerr << trafficRef->getRegistration() << " Setting altitude to " << altitude_ft;
 988             aiOutOfSight = !aiTrafficVisible();
 989             if (aiOutOfSight) {
 990                 setDie(true);
 991                 //cerr << trafficRef->getRegistration() << " is set to die " << endl;
 992                 aiOutOfSight = true;
 993                 return;
 994             }
 995         }
 996         timeElapsed = now - fp->getStartTime();
 997         flightplanActive = fp->isActive(now);
 998     } else {
 999         // no flight plan, update target heading, speed, and altitude
1000         // from control properties.  These default to the initial
1001         // settings in the config file, but can be changed "on the
1002         // fly".
1003         string lat_mode = props->getStringValue("controls/flight/lateral-mode");
1004         if ( lat_mode == "roll" ) {
1005             double angle
1006             = props->getDoubleValue("controls/flight/target-roll" );
1007             RollTo( angle );
1008         } else {
1009             double angle
1010             = props->getDoubleValue("controls/flight/target-hdg" );
1011             TurnTo( angle );
1012         }
1013
1014         string lon_mode
1015         = props->getStringValue("controls/flight/longitude-mode");
1016         if ( lon_mode == "alt" ) {
1017             double alt = props->getDoubleValue("controls/flight/target-alt" );
1018             ClimbTo( alt );
1019         } else {
1020             double angle
1021             = props->getDoubleValue("controls/flight/target-pitch" );
1022             PitchTo( angle );
1023         }
1024
1025         AccelTo( props->getDoubleValue("controls/flight/target-spd" ) );
1026     }
1027 }
1028
1029 void FGAIAircraft::updatePosition() {
1030     // convert speed to degrees per second
1031     double speed_north_deg_sec = cos( hdg * SGD_DEGREES_TO_RADIANS )
1032                                  * speed * 1.686 / ft_per_deg_lat;
1033     double speed_east_deg_sec  = sin( hdg * SGD_DEGREES_TO_RADIANS )
1034                                  * speed * 1.686 / ft_per_deg_lon;
1035
1036     // set new position
1037     pos.setLatitudeDeg( pos.getLatitudeDeg() + speed_north_deg_sec * dt);
1038     pos.setLongitudeDeg( pos.getLongitudeDeg() + speed_east_deg_sec * dt);
1039 }
1040
1041
1042 void FGAIAircraft::updateHeading() {
1043     // adjust heading based on current bank angle
1044     if (roll == 0.0)
1045         roll = 0.01;
1046
1047     if (roll != 0.0) {
1048         // double turnConstant;
1049         //if (no_roll)
1050         //  turnConstant = 0.0088362;
1051         //else
1052         //  turnConstant = 0.088362;
1053         // If on ground, calculate heading change directly
1054         if (onGround()) {
1055             double headingDiff = fabs(hdg-tgt_heading);
1056 //            double bank_sense = 0.0;
1057         /*
1058         double diff = fabs(hdg - tgt_heading);
1059         if (diff > 180)
1060             diff = fabs(diff - 360);
1061
1062         double sum = hdg + diff;
1063         if (sum > 360.0)
1064             sum -= 360.0;
1065         if (fabs(sum - tgt_heading) < 1.0) {
1066             bank_sense = 1.0;    // right turn
1067         } else {
1068             bank_sense = -1.0;   // left turn
1069         }*/
1070             if (headingDiff > 180)
1071                 headingDiff = fabs(headingDiff - 360);
1072             double sum = hdg + headingDiff;
1073             if (sum > 360.0)
1074                 sum -= 360.0;
1075             if (fabs(sum - tgt_heading) > 0.0001) {
1076 //                bank_sense = -1.0;
1077             } else {
1078 //                bank_sense = 1.0;
1079             }
1080             //if (trafficRef)
1081             //  cerr << trafficRef->getCallSign() << " Heading "
1082             //         << hdg << ". Target " << tgt_heading <<  ". Diff " << fabs(sum - tgt_heading) << ". Speed " << speed << endl;
1083             //if (headingDiff > 60) {
1084             groundTargetSpeed = tgt_speed; // * cos(headingDiff * SG_DEGREES_TO_RADIANS);
1085             //assertSpeed(groundTargetSpeed);
1086                 //groundTargetSpeed = tgt_speed - tgt_speed * (headingDiff/180);
1087             //} else {
1088             //    groundTargetSpeed = tgt_speed;
1089             //}
1090             if (sign(groundTargetSpeed) != sign(tgt_speed))
1091                 groundTargetSpeed = 0.21 * sign(tgt_speed); // to prevent speed getting stuck in 'negative' mode
1092             //assertSpeed(groundTargetSpeed);
1093             // Only update the target values when we're not moving because otherwise we might introduce an enormous target change rate while waiting a the gate, or holding.
1094             if (speed != 0) {
1095                 if (headingDiff > 30.0) {
1096                     // invert if pushed backward
1097                     headingChangeRate += 10.0 * dt * sign(roll);
1098
1099                     // Clamp the maximum steering rate to 30 degrees per second,
1100                     // But only do this when the heading error is decreasing.
1101                     if ((headingDiff < headingError)) {
1102                         if (headingChangeRate > 30)
1103                             headingChangeRate = 30;
1104                         else if (headingChangeRate < -30)
1105                             headingChangeRate = -30;
1106                     }
1107                 } else {
1108                     if (speed != 0) {
1109                         if (fabs(headingChangeRate) > headingDiff)
1110                             headingChangeRate = headingDiff*sign(roll);
1111                         else
1112                             headingChangeRate += dt * sign(roll);
1113                     }
1114                 }
1115             }
1116             if (trafficRef)
1117                 //cerr << trafficRef->getCallSign() << " Heading "
1118                 //     << hdg << ". Target " << tgt_heading <<  ". Diff " << fabs(sum - tgt_heading) << ". Speed " << speed << "Heading change rate : " << headingChangeRate << " bacnk sence " << bank_sense << endl;
1119             hdg += headingChangeRate * dt * sqrt(fabs(speed) / 15);
1120             headingError = headingDiff;
1121             if (fabs(headingError) < 1.0) {
1122                 hdg = tgt_heading;
1123             }
1124         } else {
1125             if (fabs(speed) > 1.0) {
1126                 turn_radius_ft = 0.088362 * speed * speed
1127                                  / tan( fabs(roll) / SG_RADIANS_TO_DEGREES );
1128             } else {
1129                 // Check if turn_radius_ft == 0; this might lead to a division by 0.
1130                 turn_radius_ft = 1.0;
1131             }
1132             double turn_circum_ft = SGD_2PI * turn_radius_ft;
1133             double dist_covered_ft = speed * 1.686 * dt;
1134             double alpha = dist_covered_ft / turn_circum_ft * 360.0;
1135             hdg += alpha * sign(roll);
1136         }
1137         while ( hdg > 360.0 ) {
1138             hdg -= 360.0;
1139             spinCounter++;
1140         }
1141         while ( hdg < 0.0) {
1142             hdg += 360.0;
1143             spinCounter--;
1144         }
1145     }
1146 }
1147
1148
1149 void FGAIAircraft::updateBankAngleTarget() {
1150     // adjust target bank angle if heading lock engaged
1151     if (hdg_lock) {
1152         double bank_sense = 0.0;
1153         double diff = fabs(hdg - tgt_heading);
1154         if (diff > 180)
1155             diff = fabs(diff - 360);
1156
1157         double sum = hdg + diff;
1158         if (sum > 360.0)
1159             sum -= 360.0;
1160         if (fabs(sum - tgt_heading) < 1.0) {
1161             bank_sense = 1.0;    // right turn
1162         } else {
1163             bank_sense = -1.0;   // left turn
1164         }
1165         if (diff < _performance->maximumBankAngle()) {
1166             tgt_roll = diff * bank_sense;
1167         } else {
1168             tgt_roll = _performance->maximumBankAngle() * bank_sense;
1169         }
1170         if ((fabs((double) spinCounter) > 1) && (diff > _performance->maximumBankAngle())) {
1171             tgt_speed *= 0.999;  // Ugly hack: If aircraft get stuck, they will continually spin around.
1172             // The only way to resolve this is to make them slow down.
1173         }
1174     }
1175 }
1176
1177
1178 void FGAIAircraft::updateVerticalSpeedTarget() {
1179     // adjust target Altitude, based on ground elevation when on ground
1180     if (onGround()) {
1181         getGroundElev(dt);
1182         doGroundAltitude();
1183     } else if (alt_lock) {
1184         // find target vertical speed
1185         if (use_perf_vs) {
1186             if (altitude_ft < tgt_altitude_ft) {
1187                 tgt_vs = tgt_altitude_ft - altitude_ft;
1188                 if (tgt_vs > _performance->climbRate())
1189                     tgt_vs = _performance->climbRate();
1190             } else {
1191                 tgt_vs = tgt_altitude_ft - altitude_ft;
1192                 if (tgt_vs  < (-_performance->descentRate()))
1193                     tgt_vs = -_performance->descentRate();
1194             }
1195         } else {
1196             double vert_dist_ft = fp->getCurrentWaypoint()->getCrossat() - altitude_ft;
1197             double err_dist     = 0; //prev->getCrossat() - altitude_ft;
1198             double dist_m       = fp->getDistanceToGo(pos.getLatitudeDeg(), pos.getLongitudeDeg(), fp->getCurrentWaypoint());
1199             tgt_vs = calcVerticalSpeed(vert_dist_ft, dist_m, speed, err_dist);
1200             //cerr << "Target vs before : " << tgt_vs;
1201 /*            double max_vs = 10*(tgt_altitude_ft - altitude_ft);
1202             double min_vs = 100;
1203             if (tgt_altitude_ft < altitude_ft)
1204                 min_vs = -100.0;
1205             if ((fabs(tgt_altitude_ft - altitude_ft) < 1500.0)
1206                     && (fabs(max_vs) < fabs(tgt_vs)))
1207                 tgt_vs = max_vs;
1208
1209             if (fabs(tgt_vs) < fabs(min_vs))
1210                 tgt_vs = min_vs;*/
1211             //cerr << "target vs : after " << tgt_vs << endl;
1212         }
1213     } //else
1214     //    tgt_vs = 0.0;
1215     checkTcas();
1216 }
1217
1218 void FGAIAircraft::updatePitchAngleTarget() {
1219     // if on ground and above vRotate -> initial rotation
1220     if (onGround() && (speed > _performance->vRotate()))
1221         tgt_pitch = 8.0; // some rough B737 value
1222
1223     //TODO pitch angle on approach and landing
1224
1225     // match pitch angle to vertical speed
1226     else if (tgt_vs > 0) {
1227         tgt_pitch = tgt_vs * 0.005;
1228     } else {
1229         tgt_pitch = tgt_vs * 0.002;
1230     }
1231 }
1232
1233 string FGAIAircraft::atGate() {
1234      string tmp("");
1235      if (fp->getLeg() < 3) {
1236          if (trafficRef) {
1237              if (fp->getGate() > 0) {
1238                  FGParking *park =
1239                      trafficRef->getDepartureAirport()->getDynamics()->getParking(fp->getGate());
1240                  tmp = park->getName();
1241              }
1242          }
1243      }
1244      return tmp;
1245 }
1246
1247 void FGAIAircraft::handleATCRequests() {
1248     //TODO implement NullController for having no ATC to save the conditionals
1249     if (controller) {
1250         controller->updateAircraftInformation(getID(),
1251                                               pos.getLatitudeDeg(),
1252                                               pos.getLongitudeDeg(),
1253                                               hdg,
1254                                               speed,
1255                                               altitude_ft, dt);
1256         processATC(controller->getInstruction(getID()));
1257     }
1258     if (towerController) {
1259         towerController->updateAircraftInformation(getID(),
1260                                               pos.getLatitudeDeg(),
1261                                               pos.getLongitudeDeg(),
1262                                               hdg,
1263                                               speed,
1264                                               altitude_ft, dt);
1265     }
1266 }
1267
1268 void FGAIAircraft::updateActualState() {
1269     //update current state
1270     //TODO have a single tgt_speed and check speed limit on ground on setting tgt_speed
1271     updatePosition();
1272
1273     if (onGround())
1274         speed = _performance->actualSpeed(this, groundTargetSpeed, dt, holdPos);
1275     else
1276         speed = _performance->actualSpeed(this, (tgt_speed *speedFraction), dt, false);
1277     //assertSpeed(speed);
1278     updateHeading();
1279     roll = _performance->actualBankAngle(this, tgt_roll, dt);
1280
1281     // adjust altitude (meters) based on current vertical speed (fpm)
1282     altitude_ft += vs / 60.0 * dt;
1283     pos.setElevationFt(altitude_ft);
1284
1285     vs = _performance->actualVerticalSpeed(this, tgt_vs, dt);
1286     pitch = _performance->actualPitch(this, tgt_pitch, dt);
1287 }
1288
1289 void FGAIAircraft::updateSecondaryTargetValues() {
1290     // derived target state values
1291     updateBankAngleTarget();
1292     updateVerticalSpeedTarget();
1293     updatePitchAngleTarget();
1294
1295     //TODO calculate wind correction angle (tgt_yaw)
1296 }
1297
1298
1299 bool FGAIAircraft::reachedEndOfCruise(double &distance) {
1300     FGAIWaypoint* curr = fp->getCurrentWaypoint();
1301     if (curr->getName() == string("BOD")) {
1302         double dist = fp->getDistanceToGo(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr);
1303         double descentSpeed = (getPerformance()->vDescent() * SG_NM_TO_METER) / 3600.0;     // convert from kts to meter/s
1304         double descentRate  = (getPerformance()->descentRate() * SG_FEET_TO_METER) / 60.0;  // convert from feet/min to meter/s
1305
1306         double verticalDistance  = ((altitude_ft - 2000.0) - trafficRef->getArrivalAirport()->getElevation()) *SG_FEET_TO_METER;
1307         double descentTimeNeeded = verticalDistance / descentRate;
1308         double distanceCovered   = descentSpeed * descentTimeNeeded;
1309
1310         //cerr << "Tracking  : " << fgGetString("/ai/track-callsign");
1311         if (trafficRef->getCallSign() == fgGetString("/ai/track-callsign")) {
1312             cerr << "Checking for end of cruise stage for :" << trafficRef->getCallSign() << endl;
1313             cerr << "Descent rate      : " << descentRate << endl;
1314             cerr << "Descent speed     : " << descentSpeed << endl;
1315             cerr << "VerticalDistance  : " << verticalDistance << ". Altitude : " << altitude_ft << ". Elevation " << trafficRef->getArrivalAirport()->getElevation() << endl;
1316             cerr << "DecentTimeNeeded  : " << descentTimeNeeded << endl;
1317             cerr << "DistanceCovered   : " << distanceCovered   << endl;
1318         }
1319         //cerr << "Distance = " << distance << endl;
1320         distance = distanceCovered;
1321         if (dist < distanceCovered) {
1322               if (trafficRef->getCallSign() == fgGetString("/ai/track-callsign")) {
1323                    //exit(1);
1324               }
1325               return true;
1326         } else {
1327               return false;
1328         }
1329     } else {
1330          return false;
1331     }
1332 }
1333
1334 void FGAIAircraft::resetPositionFromFlightPlan()
1335 {
1336     // the one behind you
1337     FGAIWaypoint* prev = 0;
1338     // the one ahead
1339     FGAIWaypoint* curr = 0;
1340     // the next plus 1
1341     FGAIWaypoint* next = 0;
1342
1343     prev = fp->getPreviousWaypoint();
1344     curr = fp->getCurrentWaypoint();
1345     next = fp->getNextWaypoint();
1346
1347     setLatitude(prev->getLatitude());
1348     setLongitude(prev->getLongitude());
1349     double tgt_heading = fp->getBearing(curr, next);
1350     setHeading(tgt_heading);
1351     setAltitude(prev->getAltitude());
1352     setSpeed(prev->getSpeed());
1353 }
1354
1355 double FGAIAircraft::getBearing(double crse)
1356 {
1357   double hdgDiff = fabs(hdg-crse);
1358   if (hdgDiff > 180)
1359       hdgDiff = fabs(hdgDiff - 360);
1360   return hdgDiff;
1361 }
1362
1363 time_t FGAIAircraft::checkForArrivalTime(string wptName) {
1364      FGAIWaypoint* curr = 0;
1365      curr = fp->getCurrentWaypoint();
1366
1367      double tracklength = fp->checkTrackLength(wptName);
1368      if (tracklength > 0.1) {
1369           tracklength += fp->getDistanceToGo(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr);
1370      } else {
1371          return 0;
1372      }
1373      time_t now = time(NULL) + fgGetLong("/sim/time/warp");
1374      time_t arrivalTime = fp->getArrivalTime();
1375
1376      time_t ete = tracklength / ((speed * SG_NM_TO_METER) / 3600.0);
1377      time_t secondsToGo = arrivalTime - now;
1378      if (trafficRef->getCallSign() == fgGetString("/ai/track-callsign")) {
1379           cerr << "Checking arrival time: ete " << ete << ". Time to go : " << secondsToGo << ". Track length = " << tracklength << endl;
1380      }
1381      return (ete - secondsToGo); // Positive when we're too slow...
1382 }