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