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