src/AIModel/AIFlightPlan.cxx

   1 // // FGAIFlightPlan - class for loading and storing  AI flight plans
   2 // Written by David Culp, started May 2004
   3 // - davidculp2@comcast.net
   4 //
   5 // This program is free software; you can redistribute it and/or
   6 // modify it under the terms of the GNU General Public License as
   7 // published by the Free Software Foundation; either version 2 of the
   8 // License, or (at your option) any later version.
   9 //
  10 // This program is distributed in the hope that it will be useful, but
  11 // WITHOUT ANY WARRANTY; without even the implied warranty of
  12 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  13 // General Public License for more details.
  14 //
  15 // You should have received a copy of the GNU General Public License
  16 // along with this program; if not, write to the Free Software
  17 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  18
  19 #ifdef HAVE_CONFIG_H
  20 #  include <config.h>
  21 #endif
  22
  23 #include <iostream>
  24
  25 #include <simgear/misc/sg_path.hxx>
  26 #include <simgear/debug/logstream.hxx>
  27 #include <simgear/route/waypoint.hxx>
  28 #include <simgear/math/sg_geodesy.hxx>
  29 #include <simgear/structure/exception.hxx>
  30 #include <simgear/constants.h>
  31 #include <simgear/props/props.hxx>
  32 #include <simgear/props/props_io.hxx>
  33
  34 #include <Main/globals.hxx>
  35 #include <Main/fg_props.hxx>
  36 #include <Main/fg_init.hxx>
  37 #include <Airports/simple.hxx>
  38 #include <Airports/runways.hxx>
  39 #include <Airports/groundnetwork.hxx>
  40
  41 #include <Environment/environment_mgr.hxx>
  42 #include <Environment/environment.hxx>
  43
  44 #include "AIFlightPlan.hxx"
  45 #include "AIAircraft.hxx"
  46
  47 using std::cerr;
  48
  49 FGAIWaypoint::FGAIWaypoint() {
  50   latitude    = 0;
  51   longitude   = 0;
  52   altitude    = 0;
  53   speed       = 0;
  54   crossat     = 0;
  55   finished    = 0;
  56   gear_down   = 0;
  57   flaps_down  = 0;
  58   on_ground   = 0;
  59   routeIndex  = 0;
  60   time_sec    = 0;
  61   trackLength = 0;
  62 }
  63
  64 bool FGAIWaypoint::contains(string target) {
  65     size_t found = name.find(target);
  66     if (found == string::npos)
  67         return false;
  68     else
  69         return true;
  70 }
  71
  72 FGAIFlightPlan::FGAIFlightPlan()
  73 {
  74     sid = 0;
  75     repeat = false;
  76     distance_to_go = 0;
  77     lead_distance = 0;
  78     start_time = 0;
  79     arrivalTime = 0;
  80     leg = 10;
  81     gateId = 0;
  82     lastNodeVisited = 0;
  83     taxiRoute = 0;
  84     wpt_iterator = waypoints.begin();
  85     isValid = true;
  86 }
  87
  88 FGAIFlightPlan::FGAIFlightPlan(const string& filename)
  89 {
  90   int i;
  91   sid = 0;
  92   start_time = 0;
  93   leg = 10;
  94   gateId = 0;
  95   taxiRoute = 0;
  96   SGPath path( globals->get_fg_root() );
  97   path.append( ("/AI/FlightPlans/" + filename).c_str() );
  98   SGPropertyNode root;
  99   repeat = false;
 100
 101   try {
 102       readProperties(path.str(), &root);
 103   } catch (const sg_exception &) {
 104       SG_LOG(SG_GENERAL, SG_ALERT,
 105        "Error reading AI flight plan: " << path.str());
 106        // cout << path.str() << endl;
 107      return;
 108   }
 109
 110   SGPropertyNode * node = root.getNode("flightplan");
 111   for (i = 0; i < node->nChildren(); i++) {
 112      //cout << "Reading waypoint " << i << endl;
 113      FGAIWaypoint* wpt = new FGAIWaypoint;
 114      SGPropertyNode * wpt_node = node->getChild(i);
 115      wpt->setName       (wpt_node->getStringValue("name", "END"     ));
 116      wpt->setLatitude   (wpt_node->getDoubleValue("lat", 0          ));
 117      wpt->setLongitude  (wpt_node->getDoubleValue("lon", 0          ));
 118      wpt->setAltitude   (wpt_node->getDoubleValue("alt", 0          ));
 119      wpt->setSpeed      (wpt_node->getDoubleValue("ktas", 0         ));
 120      wpt->setCrossat    (wpt_node->getDoubleValue("crossat", -10000 ));
 121      wpt->setGear_down  (wpt_node->getBoolValue("gear-down", false  ));
 122      wpt->setFlaps_down (wpt_node->getBoolValue("flaps-down", false ));
 123      wpt->setOn_ground  (wpt_node->getBoolValue("on-ground", false  ));
 124      wpt->setTime_sec   (wpt_node->getDoubleValue("time-sec", 0     ));
 125      wpt->setTime       (wpt_node->getStringValue("time", ""        ));
 126
 127      if (wpt->getName() == "END") wpt->setFinished(true);
 128      else wpt->setFinished(false);
 129
 130      pushBackWaypoint( wpt );
 131    }
 132
 133   wpt_iterator = waypoints.begin();
 134   isValid = true;
 135   //cout << waypoints.size() << " waypoints read." << endl;
 136 }
 137
 138
 139 // This is a modified version of the constructor,
 140 // Which not only reads the waypoints from a
 141 // Flight plan file, but also adds the current
 142 // Position computed by the traffic manager, as well
 143 // as setting speeds and altitude computed by the
 144 // traffic manager.
 145 FGAIFlightPlan::FGAIFlightPlan(FGAIAircraft *ac,
 146                                const std::string& p,
 147                                double course,
 148                                time_t start,
 149                                FGAirport *dep,
 150                                FGAirport *arr,
 151                                bool firstLeg,
 152                                double radius,
 153                                double alt,
 154                                double lat,
 155                                double lon,
 156                                double speed,
 157                                const string& fltType,
 158                                const string& acType,
 159                                const string& airline)
 160 {
 161   sid = 0;
 162   repeat = false;
 163   leg = 10;
 164   gateId=0;
 165   taxiRoute = 0;
 166   start_time = start;
 167   bool useInitialWayPoint = true;
 168   bool useCurrentWayPoint = false;
 169   SGPath path( globals->get_fg_root() );
 170   path.append( "/AI/FlightPlans" );
 171   path.append( p );
 172
 173   SGPropertyNode root;
 174   isValid = true;
 175   // This is a bit of a hack:
 176   // Normally the value of course will be used to evaluate whether
 177   // or not a waypoint will be used for midair initialization of
 178   // an AI aircraft. However, if a course value of 999 will be passed
 179   // when an update request is received, which will by definition always be
 180   // on the ground and should include all waypoints.
 181   if (course == 999)
 182     {
 183       useInitialWayPoint = false;
 184       useCurrentWayPoint = true;
 185     }
 186
 187   if (path.exists())
 188     {
 189       try
 190         {
 191           readProperties(path.str(), &root);
 192
 193           SGPropertyNode * node = root.getNode("flightplan");
 194
 195           //pushBackWaypoint( init_waypoint );
 196           for (int i = 0; i < node->nChildren(); i++) {
 197             //cout << "Reading waypoint " << i << endl;
 198             FGAIWaypoint* wpt = new FGAIWaypoint;
 199             SGPropertyNode * wpt_node = node->getChild(i);
 200             wpt->setName       (wpt_node->getStringValue("name", "END"     ));
 201             wpt->setLatitude   (wpt_node->getDoubleValue("lat", 0          ));
 202             wpt->setLongitude  (wpt_node->getDoubleValue("lon", 0          ));
 203             wpt->setAltitude   (wpt_node->getDoubleValue("alt", 0          ));
 204             wpt->setSpeed      (wpt_node->getDoubleValue("ktas", 0         ));
 205             wpt->setCrossat    (wpt_node->getDoubleValue("crossat", -10000 ));
 206             wpt->setGear_down  (wpt_node->getBoolValue("gear-down", false  ));
 207             wpt->setFlaps_down (wpt_node->getBoolValue("flaps-down", false ));
 208
 209             if (wpt->getName() == "END") wpt->setFinished(true);
 210             else wpt->setFinished(false);
 211             pushBackWaypoint(wpt);
 212           } // of node loop
 213           wpt_iterator = waypoints.begin();
 214         } catch (const sg_exception &e) {
 215       SG_LOG(SG_GENERAL, SG_WARN, "Error reading AI flight plan: " <<
 216         e.getMessage() << " from " << e.getOrigin());
 217     }
 218   } else {
 219       // cout << path.str() << endl;
 220       // cout << "Trying to create this plan dynamically" << endl;
 221       // cout << "Route from " << dep->id << " to " << arr->id << endl;
 222       time_t now = time(NULL) + fgGetLong("/sim/time/warp");
 223       time_t timeDiff = now-start;
 224       leg = 1;
 225
 226       if ((timeDiff > 60) && (timeDiff < 1500))
 227         leg = 2;
 228       //else if ((timeDiff >= 1200) && (timeDiff < 1500)) {
 229         //leg = 3;
 230         //ac->setTakeOffStatus(2);
 231       //}
 232       else if ((timeDiff >= 1500) && (timeDiff < 2000))
 233         leg = 4;
 234       else if (timeDiff >= 2000)
 235         leg = 5;
 236       /*
 237       if (timeDiff >= 2000)
 238           leg = 5;
 239       */
 240       SG_LOG(SG_GENERAL, SG_INFO, "Route from " << dep->getId() << " to " << arr->getId() << ". Set leg to : " << leg << " " << ac->getTrafficRef()->getCallSign());
 241       wpt_iterator = waypoints.begin();
 242       bool dist = 0;
 243       isValid = create(ac, dep,arr, leg, alt, speed, lat, lon,
 244              firstLeg, radius, fltType, acType, airline, dist);
 245       wpt_iterator = waypoints.begin();
 246       //cerr << "after create: " << (*wpt_iterator)->name << endl;
 247       //leg++;
 248       // Now that we have dynamically created a flight plan,
 249       // we need to add some code that pops any waypoints already past.
 250       //return;
 251     }
 252   /*
 253     waypoint* init_waypoint   = new waypoint;
 254     init_waypoint->name       = string("initial position");
 255     init_waypoint->latitude   = entity->latitude;
 256     init_waypoint->longitude  = entity->longitude;
 257     init_waypoint->altitude   = entity->altitude;
 258     init_waypoint->speed      = entity->speed;
 259     init_waypoint->crossat    = - 10000;
 260     init_waypoint->gear_down  = false;
 261     init_waypoint->flaps_down = false;
 262     init_waypoint->finished   = false;
 263
 264     wpt_vector_iterator i = waypoints.begin();
 265     while (i != waypoints.end())
 266     {
 267       //cerr << "Checking status of each waypoint: " << (*i)->name << endl;
 268        SGWayPoint first(init_waypoint->longitude,
 269                        init_waypoint->latitude,
 270                        init_waypoint->altitude);
 271       SGWayPoint curr ((*i)->longitude,
 272                        (*i)->latitude,
 273                        (*i)->altitude);
 274       double crse, crsDiff;
 275       double dist;
 276       curr.CourseAndDistance(first, &crse, &dist);
 277
 278       dist *= SG_METER_TO_NM;
 279
 280       // We're only interested in the absolute value of crsDiff
 281       // wich should fall in the 0-180 deg range.
 282       crsDiff = fabs(crse-course);
 283       if (crsDiff > 180)
 284         crsDiff = 360-crsDiff;
 285       // These are the three conditions that we consider including
 286       // in our flight plan:
 287       // 1) current waypoint is less then 100 miles away OR
 288       // 2) curren waypoint is ahead of us, at any distance
 289
 290       if ((dist > 20.0) && (crsDiff > 90.0) && ((*i)->name != string ("EOF")))
 291         {
 292           //useWpt = false;
 293           // Once we start including waypoints, we have to continue, even though
 294           // one of the following way point would suffice.
 295           // so once is the useWpt flag is set to true, we cannot reset it to false.
 296           //cerr << "Discarding waypoint: " << (*i)->name
 297           //   << ": Course difference = " << crsDiff
 298           //  << "Course = " << course
 299           // << "crse   = " << crse << endl;
 300         }
 301       else
 302         useCurrentWayPoint = true;
 303
 304       if (useCurrentWayPoint)
 305         {
 306           if ((dist > 100.0) && (useInitialWayPoint))
 307             {
 308               //pushBackWaypoint(init_waypoint);;
 309               waypoints.insert(i, init_waypoint);
 310               //cerr << "Using waypoint : " << init_waypoint->name <<  endl;
 311             }
 312           //if (useInitialWayPoint)
 313           // {
 314           //    (*i)->speed = dist; // A hack
 315           //  }
 316           //pushBackWaypoint( wpt );
 317           //cerr << "Using waypoint : " << (*i)->name
 318           //  << ": course diff : " << crsDiff
 319           //   << "Course = " << course
 320           //   << "crse   = " << crse << endl
 321           //    << "distance      : " << dist << endl;
 322           useInitialWayPoint = false;
 323           i++;
 324         }
 325       else
 326         {
 327           //delete wpt;
 328           delete *(i);
 329           i = waypoints.erase(i);
 330           }
 331
 332         }
 333   */
 334   //for (i = waypoints.begin(); i != waypoints.end(); i++)
 335   //  cerr << "Using waypoint : " << (*i)->name << endl;
 336   //wpt_iterator = waypoints.begin();
 337   //cout << waypoints.size() << " waypoints read." << endl;
 338 }
 339
 340
 341
 342
 343 FGAIFlightPlan::~FGAIFlightPlan()
 344 {
 345   deleteWaypoints();
 346   delete taxiRoute;
 347 }
 348
 349
 350 FGAIWaypoint* const FGAIFlightPlan::getPreviousWaypoint( void ) const
 351 {
 352   if (wpt_iterator == waypoints.begin()) {
 353     return 0;
 354   } else {
 355     wpt_vector_iterator prev = wpt_iterator;
 356     return *(--prev);
 357   }
 358 }
 359
 360 FGAIWaypoint* const FGAIFlightPlan::getCurrentWaypoint( void ) const
 361 {
 362   return *wpt_iterator;
 363 }
 364
 365 FGAIWaypoint* const FGAIFlightPlan::getNextWaypoint( void ) const
 366 {
 367   wpt_vector_iterator i = waypoints.end();
 368   i--;  // end() points to one element after the last one.
 369   if (wpt_iterator == i) {
 370     return 0;
 371   } else {
 372     wpt_vector_iterator next = wpt_iterator;
 373     return *(++next);
 374   }
 375 }
 376
 377 void FGAIFlightPlan::IncrementWaypoint(bool eraseWaypoints )
 378 {
 379   if (eraseWaypoints)
 380     {
 381       if (wpt_iterator == waypoints.begin())
 382         wpt_iterator++;
 383       else
 384         {
 385           delete *(waypoints.begin());
 386           waypoints.erase(waypoints.begin());
 387           wpt_iterator = waypoints.begin();
 388           wpt_iterator++;
 389         }
 390     }
 391   else
 392     wpt_iterator++;
 393
 394 }
 395
 396 void FGAIFlightPlan::DecrementWaypoint(bool eraseWaypoints )
 397 {
 398     if (eraseWaypoints)
 399     {
 400         if (wpt_iterator == waypoints.end())
 401             wpt_iterator--;
 402         else
 403         {
 404             delete *(waypoints.end());
 405             waypoints.erase(waypoints.end());
 406             wpt_iterator = waypoints.end();
 407             wpt_iterator--;
 408         }
 409     }
 410     else
 411         wpt_iterator--;
 412 }
 413
 414 void FGAIFlightPlan::eraseLastWaypoint()
 415 {
 416     delete (waypoints.back());
 417     waypoints.pop_back();;
 418     wpt_iterator = waypoints.begin();
 419     wpt_iterator++;
 420 }
 421
 422
 423
 424
 425 // gives distance in feet from a position to a waypoint
 426 double FGAIFlightPlan::getDistanceToGo(double lat, double lon, FGAIWaypoint* wp) const{
 427   return SGGeodesy::distanceM(SGGeod::fromDeg(lon, lat),
 428       SGGeod::fromDeg(wp->getLongitude(), wp->getLatitude()));
 429 }
 430
 431 // sets distance in feet from a lead point to the current waypoint
 432 void FGAIFlightPlan::setLeadDistance(double speed, double bearing,
 433                                      FGAIWaypoint* current, FGAIWaypoint* next){
 434   double turn_radius;
 435   // Handle Ground steering
 436   // At a turn rate of 30 degrees per second, it takes 12 seconds to do a full 360 degree turn
 437   // So, to get an estimate of the turn radius, calculate the cicumference of the circle
 438   // we travel on. Get the turn radius by dividing by PI (*2).
 439   if (speed < 0.5) {
 440         lead_distance = 0.5;
 441         return;
 442   }
 443   if (speed < 25) {
 444        turn_radius = ((360/30)*fabs(speed)) / (2*M_PI);
 445   } else
 446       turn_radius = 0.1911 * speed * speed; // an estimate for 25 degrees bank
 447
 448   double inbound = bearing;
 449   double outbound = getBearing(current, next);
 450   leadInAngle = fabs(inbound - outbound);
 451   if (leadInAngle > 180.0)
 452     leadInAngle = 360.0 - leadInAngle;
 453   //if (leadInAngle < 30.0) // To prevent lead_dist from getting so small it is skipped
 454   //  leadInAngle = 30.0;
 455
 456   //lead_distance = turn_radius * sin(leadInAngle * SG_DEGREES_TO_RADIANS);
 457   lead_distance = turn_radius * tan((leadInAngle * SG_DEGREES_TO_RADIANS)/2);
 458   /*
 459   if ((lead_distance > (3*turn_radius)) && (current->on_ground == false)) {
 460       // cerr << "Warning: Lead-in distance is large. Inbound = " << inbound
 461       //      << ". Outbound = " << outbound << ". Lead in angle = " << leadInAngle  << ". Turn radius = " << turn_radius << endl;
 462        lead_distance = 3 * turn_radius;
 463        return;
 464   }
 465   if ((leadInAngle > 90) && (current->on_ground == true)) {
 466       lead_distance = turn_radius * tan((90 * SG_DEGREES_TO_RADIANS)/2);
 467       return;
 468   }*/
 469 }
 470
 471 void FGAIFlightPlan::setLeadDistance(double distance_ft){
 472   lead_distance = distance_ft;
 473 }
 474
 475
 476 double FGAIFlightPlan::getBearing(FGAIWaypoint* first, FGAIWaypoint* second) const{
 477   return getBearing(first->getLatitude(), first->getLongitude(), second);
 478 }
 479
 480
 481 double FGAIFlightPlan::getBearing(double lat, double lon, FGAIWaypoint* wp) const{
 482   return SGGeodesy::courseDeg(SGGeod::fromDeg(lon, lat),
 483       SGGeod::fromDeg(wp->getLongitude(), wp->getLatitude()));
 484 }
 485
 486 void FGAIFlightPlan::deleteWaypoints()
 487 {
 488   for (wpt_vector_iterator i = waypoints.begin(); i != waypoints.end();i++)
 489     delete (*i);
 490   waypoints.clear();
 491 }
 492
 493 // Delete all waypoints except the last,
 494 // which we will recycle as the first waypoint in the next leg;
 495 void FGAIFlightPlan::resetWaypoints()
 496 {
 497   if (waypoints.begin() == waypoints.end())
 498     return;
 499   else
 500     {
 501       FGAIWaypoint *wpt = new FGAIWaypoint;
 502       wpt_vector_iterator i = waypoints.end();
 503       i--;
 504       wpt->setName        ( (*i)->getName()       );
 505       wpt->setLatitude    ( (*i)->getLatitude()   );
 506       wpt->setLongitude   ( (*i)->getLongitude()  );
 507       wpt->setAltitude    ( (*i)->getAltitude()   );
 508       wpt->setSpeed       ( (*i)->getSpeed()      );
 509       wpt->setCrossat     ( (*i)->getCrossat()    );
 510       wpt->setGear_down   ( (*i)->getGear_down()  );
 511       wpt->setFlaps_down  ( (*i)->getFlaps_down() );
 512       wpt->setFinished    ( false                 );
 513       wpt->setOn_ground   ( (*i)->getOn_ground()  );
 514       //cerr << "Recycling waypoint " << wpt->name << endl;
 515       deleteWaypoints();
 516       pushBackWaypoint(wpt);
 517     }
 518 }
 519
 520 void FGAIFlightPlan::pushBackWaypoint(FGAIWaypoint *wpt)
 521 {
 522   // std::vector::push_back invalidates waypoints
 523   //  so we should restore wpt_iterator after push_back
 524   //  (or it could be an index in the vector)
 525   size_t pos = wpt_iterator - waypoints.begin();
 526   waypoints.push_back(wpt);
 527   wpt_iterator = waypoints.begin() + pos;
 528 }
 529
 530 // Start flightplan over from the beginning
 531 void FGAIFlightPlan::restart()
 532 {
 533   wpt_iterator = waypoints.begin();
 534 }
 535
 536
 537 void FGAIFlightPlan::deleteTaxiRoute()
 538 {
 539   delete taxiRoute;
 540   taxiRoute = 0;
 541 }
 542
 543
 544 int FGAIFlightPlan::getRouteIndex(int i) {
 545   if ((i > 0) && (i < (int)waypoints.size())) {
 546     return waypoints[i]->getRouteIndex();
 547   }
 548   else
 549     return 0;
 550 }
 551
 552
 553 double FGAIFlightPlan::checkTrackLength(string wptName) {
 554     // skip the first two waypoints: first one is behind, second one is partially done;
 555     double trackDistance = 0;
 556     wpt_vector_iterator wptvec = waypoints.begin();
 557     wptvec++;
 558     wptvec++;
 559     while ((wptvec != waypoints.end()) && (!((*wptvec)->contains(wptName)))) {
 560            trackDistance += (*wptvec)->getTrackLength();
 561            wptvec++;
 562     }
 563     if (wptvec == waypoints.end()) {
 564         trackDistance = 0; // name not found
 565     }
 566     return trackDistance;
 567 }
 568
 569 void FGAIFlightPlan::shortenToFirst(unsigned int number, string name)
 570 {
 571     while (waypoints.size() > number + 3) {
 572         eraseLastWaypoint();
 573     }
 574     (waypoints.back())->setName((waypoints.back())->getName() + name);
 575 }