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