src/AIModel/AIAircraft.cxx

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