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/math/sg_geodesy.hxx>
  28 #include <simgear/structure/exception.hxx>
  29 #include <simgear/constants.h>
  30 #include <simgear/props/props.hxx>
  31 #include <simgear/props/props_io.hxx>
  32
  33 #include <Main/globals.hxx>
  34 #include <Main/fg_props.hxx>
  35 #include <Main/fg_init.hxx>
  36 #include <Airports/airport.hxx>
  37 #include <Airports/dynamics.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 <Traffic/Schedule.hxx>
  45
  46 #include "AIFlightPlan.hxx"
  47 #include "AIAircraft.hxx"
  48
  49 using std::cerr;
  50 using std::string;
  51
  52 FGAIWaypoint::FGAIWaypoint() {
  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(const 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 double FGAIWaypoint::getLatitude()
  73 {
  74   return pos.getLatitudeDeg();
  75 }
  76
  77 double FGAIWaypoint::getLongitude()
  78 {
  79   return pos.getLongitudeDeg();
  80 }
  81
  82 double FGAIWaypoint::getAltitude()
  83 {
  84   return pos.getElevationFt();
  85 }
  86
  87 void FGAIWaypoint::setLatitude(double lat)
  88 {
  89   pos.setLatitudeDeg(lat);
  90 }
  91
  92 void FGAIWaypoint::setLongitude(double lon)
  93 {
  94   pos.setLongitudeDeg(lon);
  95 }
  96
  97 void FGAIWaypoint::setAltitude(double alt)
  98 {
  99   pos.setElevationFt(alt);
 100 }
 101
 102 FGAIFlightPlan::FGAIFlightPlan() :
 103     sid(NULL),
 104     repeat(false),
 105     distance_to_go(0),
 106     lead_distance(0),
 107     leadInAngle(0),
 108     start_time(0),
 109     arrivalTime(0),
 110     leg(0),
 111     lastNodeVisited(0),
 112     isValid(true)
 113 {
 114     wpt_iterator    = waypoints.begin();
 115 }
 116
 117 FGAIFlightPlan::FGAIFlightPlan(const string& filename) :
 118     sid(NULL),
 119     repeat(false),
 120     distance_to_go(0),
 121     lead_distance(0),
 122     leadInAngle(0),
 123     start_time(0),
 124     arrivalTime(0),
 125     leg(10),
 126     lastNodeVisited(0),
 127     isValid(parseProperties(filename))
 128 {
 129 }
 130
 131
 132 // This is a modified version of the constructor,
 133 // Which not only reads the waypoints from a
 134 // Flight plan file, but also adds the current
 135 // Position computed by the traffic manager, as well
 136 // as setting speeds and altitude computed by the
 137 // traffic manager.
 138 FGAIFlightPlan::FGAIFlightPlan(FGAIAircraft *ac,
 139                                const std::string& p,
 140                                double course,
 141                                time_t start,
 142                                FGAirport *dep,
 143                                FGAirport *arr,
 144                                bool firstLeg,
 145                                double radius,
 146                                double alt,
 147                                double lat,
 148                                double lon,
 149                                double speed,
 150                                const string& fltType,
 151                                const string& acType,
 152                                const string& airline) :
 153     sid(NULL),
 154     repeat(false),
 155     distance_to_go(0),
 156     lead_distance(0),
 157     leadInAngle(0),
 158     start_time(start),
 159     arrivalTime(0),
 160     leg(10),
 161     lastNodeVisited(0),
 162     isValid(false),
 163     departure(dep),
 164     arrival(arr)
 165 {
 166   if (parseProperties(p)) {
 167     isValid = true;
 168   } else {
 169     createWaypoints(ac, course, start, dep, arr, firstLeg, radius,
 170                     alt, lat, lon, speed, fltType, acType, airline);
 171   }
 172 }
 173
 174 FGAIFlightPlan::~FGAIFlightPlan()
 175 {
 176   deleteWaypoints();
 177   //delete taxiRoute;
 178 }
 179
 180 void FGAIFlightPlan::createWaypoints(FGAIAircraft *ac,
 181                                      double course,
 182                                      time_t start,
 183                                      FGAirport *dep,
 184                                      FGAirport *arr,
 185                                      bool firstLeg,
 186                                      double radius,
 187                                      double alt,
 188                                      double lat,
 189                                      double lon,
 190                                      double speed,
 191                                      const string& fltType,
 192                                      const string& acType,
 193                                      const string& airline)
 194 {
 195   time_t now = time(NULL) + fgGetLong("/sim/time/warp");
 196   time_t timeDiff = now-start;
 197   leg = 1;
 198
 199   if ((timeDiff > 60) && (timeDiff < 1500))
 200     leg = 2;
 201   //else if ((timeDiff >= 1200) && (timeDiff < 1500)) {
 202         //leg = 3;
 203   //ac->setTakeOffStatus(2);
 204   //}
 205   else if ((timeDiff >= 1500) && (timeDiff < 2000))
 206     leg = 4;
 207   else if (timeDiff >= 2000)
 208     leg = 5;
 209   /*
 210    if (timeDiff >= 2000)
 211    leg = 5;
 212    */
 213   SG_LOG(SG_AI, SG_INFO, "Route from " << dep->getId() << " to " << arr->getId() << ". Set leg to : " << leg << " " << ac->getTrafficRef()->getCallSign());
 214   wpt_iterator = waypoints.begin();
 215   bool dist = 0;
 216   isValid = create(ac, dep, arr, leg, alt, speed, lat, lon,
 217                    firstLeg, radius, fltType, acType, airline, dist);
 218   wpt_iterator = waypoints.begin();
 219 }
 220
 221 bool FGAIFlightPlan::parseProperties(const std::string& filename)
 222 {
 223   SGPath path( globals->get_fg_root() );
 224   path.append( "/AI/FlightPlans/" + filename );
 225   if (!path.exists()) {
 226     return false;
 227   }
 228
 229   SGPropertyNode root;
 230   try {
 231     readProperties(path.str(), &root);
 232   } catch (const sg_exception &e) {
 233     SG_LOG(SG_AI, SG_ALERT, "Error reading AI flight plan: " << path.str()
 234            << "message:" << e.getFormattedMessage());
 235     return false;
 236   }
 237
 238   SGPropertyNode * node = root.getNode("flightplan");
 239   for (int i = 0; i < node->nChildren(); i++) {
 240     FGAIWaypoint* wpt = new FGAIWaypoint;
 241     SGPropertyNode * wpt_node = node->getChild(i);
 242     wpt->setName       (wpt_node->getStringValue("name", "END"     ));
 243     wpt->setLatitude   (wpt_node->getDoubleValue("lat", 0          ));
 244     wpt->setLongitude  (wpt_node->getDoubleValue("lon", 0          ));
 245     wpt->setAltitude   (wpt_node->getDoubleValue("alt", 0          ));
 246     wpt->setSpeed      (wpt_node->getDoubleValue("ktas", 0         ));
 247     wpt->setCrossat    (wpt_node->getDoubleValue("crossat", -10000 ));
 248     wpt->setGear_down  (wpt_node->getBoolValue("gear-down", false  ));
 249     wpt->setFlaps_down (wpt_node->getBoolValue("flaps-down", false ));
 250     wpt->setOn_ground  (wpt_node->getBoolValue("on-ground", false  ));
 251     wpt->setTime_sec   (wpt_node->getDoubleValue("time-sec", 0     ));
 252     wpt->setTime       (wpt_node->getStringValue("time", ""        ));
 253     wpt->setFinished   ((wpt->getName() == "END"));
 254     pushBackWaypoint( wpt );
 255   }
 256   if( getLastWaypoint()->getName().compare("END") != 0  ) {
 257         SG_LOG(SG_AI, SG_ALERT, "FGAIFlightPlan::Flightplan missing END node" );
 258         return false;
 259   }
 260
 261
 262   wpt_iterator = waypoints.begin();
 263   return true;
 264 }
 265
 266 FGAIWaypoint* const FGAIFlightPlan::getPreviousWaypoint( void ) const
 267 {
 268   if (wpt_iterator == waypoints.begin()) {
 269     return 0;
 270   } else {
 271     wpt_vector_iterator prev = wpt_iterator;
 272     return *(--prev);
 273   }
 274 }
 275
 276 FGAIWaypoint* const FGAIFlightPlan::getCurrentWaypoint( void ) const
 277 {
 278   if (wpt_iterator == waypoints.end())
 279       return 0;
 280   return *wpt_iterator;
 281 }
 282
 283 FGAIWaypoint* const FGAIFlightPlan::getNextWaypoint( void ) const
 284 {
 285   wpt_vector_iterator i = waypoints.end();
 286   i--;  // end() points to one element after the last one.
 287   if (wpt_iterator == i) {
 288     return 0;
 289   } else {
 290     wpt_vector_iterator next = wpt_iterator;
 291     return *(++next);
 292   }
 293 }
 294
 295 void FGAIFlightPlan::IncrementWaypoint(bool eraseWaypoints )
 296 {
 297     if (eraseWaypoints)
 298     {
 299         if (wpt_iterator == waypoints.begin())
 300             wpt_iterator++;
 301         else
 302         if (!waypoints.empty())
 303         {
 304             delete *(waypoints.begin());
 305             waypoints.erase(waypoints.begin());
 306             wpt_iterator = waypoints.begin();
 307             wpt_iterator++;
 308         }
 309     }
 310     else
 311         wpt_iterator++;
 312 }
 313
 314 void FGAIFlightPlan::DecrementWaypoint(bool eraseWaypoints )
 315 {
 316     if (eraseWaypoints)
 317     {
 318         if (wpt_iterator == waypoints.end())
 319             wpt_iterator--;
 320         else
 321         if (!waypoints.empty())
 322         {
 323             delete *(waypoints.end()-1);
 324             waypoints.erase(waypoints.end()-1);
 325             wpt_iterator = waypoints.end();
 326             wpt_iterator--;
 327         }
 328     }
 329     else
 330         wpt_iterator--;
 331 }
 332
 333 void FGAIFlightPlan::eraseLastWaypoint()
 334 {
 335     delete (waypoints.back());
 336     waypoints.pop_back();;
 337     wpt_iterator = waypoints.begin();
 338     wpt_iterator++;
 339 }
 340
 341
 342
 343
 344 // gives distance in feet from a position to a waypoint
 345 double FGAIFlightPlan::getDistanceToGo(double lat, double lon, FGAIWaypoint* wp) const{
 346   return SGGeodesy::distanceM(SGGeod::fromDeg(lon, lat), wp->getPos());
 347 }
 348
 349 // sets distance in feet from a lead point to the current waypoint
 350 void FGAIFlightPlan::setLeadDistance(double speed, double bearing,
 351                                      FGAIWaypoint* current, FGAIWaypoint* next){
 352   double turn_radius;
 353   // Handle Ground steering
 354   // At a turn rate of 30 degrees per second, it takes 12 seconds to do a full 360 degree turn
 355   // So, to get an estimate of the turn radius, calculate the cicumference of the circle
 356   // we travel on. Get the turn radius by dividing by PI (*2).
 357   if (speed < 0.5) {
 358         lead_distance = 0.5;
 359         return;
 360   }
 361   if (speed < 25) {
 362        turn_radius = ((360/30)*fabs(speed)) / (2*M_PI);
 363   } else
 364       turn_radius = 0.1911 * speed * speed; // an estimate for 25 degrees bank
 365
 366   double inbound = bearing;
 367   double outbound = getBearing(current, next);
 368   leadInAngle = fabs(inbound - outbound);
 369   if (leadInAngle > 180.0)
 370     leadInAngle = 360.0 - leadInAngle;
 371   //if (leadInAngle < 30.0) // To prevent lead_dist from getting so small it is skipped
 372   //  leadInAngle = 30.0;
 373
 374   //lead_distance = turn_radius * sin(leadInAngle * SG_DEGREES_TO_RADIANS);
 375   lead_distance = turn_radius * tan((leadInAngle * SG_DEGREES_TO_RADIANS)/2);
 376   /*
 377   if ((lead_distance > (3*turn_radius)) && (current->on_ground == false)) {
 378       // cerr << "Warning: Lead-in distance is large. Inbound = " << inbound
 379       //      << ". Outbound = " << outbound << ". Lead in angle = " << leadInAngle  << ". Turn radius = " << turn_radius << endl;
 380        lead_distance = 3 * turn_radius;
 381        return;
 382   }
 383   if ((leadInAngle > 90) && (current->on_ground == true)) {
 384       lead_distance = turn_radius * tan((90 * SG_DEGREES_TO_RADIANS)/2);
 385       return;
 386   }*/
 387 }
 388
 389 void FGAIFlightPlan::setLeadDistance(double distance_ft){
 390   lead_distance = distance_ft;
 391 }
 392
 393
 394 double FGAIFlightPlan::getBearing(FGAIWaypoint* first, FGAIWaypoint* second) const
 395 {
 396   return SGGeodesy::courseDeg(first->getPos(), second->getPos());
 397 }
 398
 399 double FGAIFlightPlan::getBearing(const SGGeod& aPos, FGAIWaypoint* wp) const
 400 {
 401   return SGGeodesy::courseDeg(aPos, wp->getPos());
 402 }
 403
 404 void FGAIFlightPlan::deleteWaypoints()
 405 {
 406   for (wpt_vector_iterator i = waypoints.begin(); i != waypoints.end();i++)
 407     delete (*i);
 408   waypoints.clear();
 409   wpt_iterator = waypoints.begin();
 410 }
 411
 412 // Delete all waypoints except the last,
 413 // which we will recycle as the first waypoint in the next leg;
 414 void FGAIFlightPlan::resetWaypoints()
 415 {
 416   if (waypoints.begin() == waypoints.end())
 417     return;
 418   else
 419     {
 420       FGAIWaypoint *wpt = new FGAIWaypoint;
 421       wpt_vector_iterator i = waypoints.end();
 422       i--;
 423       wpt->setName        ( (*i)->getName()       );
 424       wpt->setPos         ( (*i)->getPos()        );
 425       wpt->setCrossat     ( (*i)->getCrossat()    );
 426       wpt->setGear_down   ( (*i)->getGear_down()  );
 427       wpt->setFlaps_down  ( (*i)->getFlaps_down() );
 428       wpt->setFinished    ( false                 );
 429       wpt->setOn_ground   ( (*i)->getOn_ground()  );
 430       //cerr << "Recycling waypoint " << wpt->name << endl;
 431       deleteWaypoints();
 432       pushBackWaypoint(wpt);
 433     }
 434 }
 435
 436 void FGAIFlightPlan::pushBackWaypoint(FGAIWaypoint *wpt)
 437 {
 438   // std::vector::push_back invalidates waypoints
 439   //  so we should restore wpt_iterator after push_back
 440   //  (or it could be an index in the vector)
 441   size_t pos = wpt_iterator - waypoints.begin();
 442   waypoints.push_back(wpt);
 443   wpt_iterator = waypoints.begin() + pos;
 444 }
 445
 446 // Start flightplan over from the beginning
 447 void FGAIFlightPlan::restart()
 448 {
 449   wpt_iterator = waypoints.begin();
 450 }
 451
 452 int FGAIFlightPlan::getRouteIndex(int i) {
 453   if ((i > 0) && (i < (int)waypoints.size())) {
 454     return waypoints[i]->getRouteIndex();
 455   }
 456   else
 457     return 0;
 458 }
 459
 460 double FGAIFlightPlan::checkTrackLength(const string& wptName) const {
 461     // skip the first two waypoints: first one is behind, second one is partially done;
 462     double trackDistance = 0;
 463     wpt_vector_iterator wptvec = waypoints.begin();
 464     wptvec++;
 465     wptvec++;
 466     while ((wptvec != waypoints.end()) && (!((*wptvec)->contains(wptName)))) {
 467            trackDistance += (*wptvec)->getTrackLength();
 468            wptvec++;
 469     }
 470     if (wptvec == waypoints.end()) {
 471         trackDistance = 0; // name not found
 472     }
 473     return trackDistance;
 474 }
 475
 476 void FGAIFlightPlan::shortenToFirst(unsigned int number, string name)
 477 {
 478     while (waypoints.size() > number + 3) {
 479         eraseLastWaypoint();
 480     }
 481     (waypoints.back())->setName((waypoints.back())->getName() + name);
 482 }
 483
 484 void FGAIFlightPlan::setGate(const ParkingAssignment& pka)
 485 {
 486   gate = pka;
 487 }
 488
 489 FGParking* FGAIFlightPlan::getParkingGate()
 490 {
 491   return gate.parking();
 492 }
 493
 494 FGAirportRef FGAIFlightPlan::departureAirport() const
 495 {
 496     return departure;
 497 }
 498
 499 FGAirportRef FGAIFlightPlan::arrivalAirport() const
 500 {
 501     return arrival;
 502 }