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