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