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