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