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