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