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