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