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 FGAIFlightPlan::FGAIFlightPlan()
  50 {
  51    sid = 0;
  52    wpt_iterator = waypoints.begin();
  53    isValid = true;
  54 }
  55
  56 FGAIFlightPlan::FGAIFlightPlan(const string& filename)
  57 {
  58   int i;
  59   sid = 0;
  60   start_time = 0;
  61   leg = 10;
  62   gateId = 0;
  63   taxiRoute = 0;
  64   SGPath path( globals->get_fg_root() );
  65   path.append( ("/AI/FlightPlans/" + filename).c_str() );
  66   SGPropertyNode root;
  67   repeat = false;
  68
  69   try {
  70       readProperties(path.str(), &root);
  71   } catch (const sg_exception &) {
  72       SG_LOG(SG_GENERAL, SG_ALERT,
  73        "Error reading AI flight plan: " << path.str());
  74        // cout << path.str() << endl;
  75      return;
  76   }
  77
  78   SGPropertyNode * node = root.getNode("flightplan");
  79   for (i = 0; i < node->nChildren(); i++) {
  80      //cout << "Reading waypoint " << i << endl;
  81      waypoint* wpt = new waypoint;
  82      SGPropertyNode * wpt_node = node->getChild(i);
  83      wpt->name      = wpt_node->getStringValue("name", "END");
  84      wpt->latitude  = wpt_node->getDoubleValue("lat", 0);
  85      wpt->longitude = wpt_node->getDoubleValue("lon", 0);
  86      wpt->altitude  = wpt_node->getDoubleValue("alt", 0);
  87      wpt->speed     = wpt_node->getDoubleValue("ktas", 0);
  88      wpt->crossat   = wpt_node->getDoubleValue("crossat", -10000);
  89      wpt->gear_down = wpt_node->getBoolValue("gear-down", false);
  90      wpt->flaps_down= wpt_node->getBoolValue("flaps-down", false);
  91      wpt->on_ground = wpt_node->getBoolValue("on-ground", false);
  92      wpt->time_sec   = wpt_node->getDoubleValue("time-sec", 0);
  93      wpt->time       = wpt_node->getStringValue("time", "");
  94
  95      if (wpt->name == "END") wpt->finished = true;
  96      else wpt->finished = false;
  97
  98      waypoints.push_back( wpt );
  99    }
 100
 101   wpt_iterator = waypoints.begin();
 102   isValid = true;
 103   //cout << waypoints.size() << " waypoints read." << endl;
 104 }
 105
 106
 107 // This is a modified version of the constructor,
 108 // Which not only reads the waypoints from a
 109 // Flight plan file, but also adds the current
 110 // Position computed by the traffic manager, as well
 111 // as setting speeds and altitude computed by the
 112 // traffic manager.
 113 FGAIFlightPlan::FGAIFlightPlan(FGAIAircraft *ac,
 114                                const std::string& p,
 115                                double course,
 116                                time_t start,
 117                                FGAirport *dep,
 118                                FGAirport *arr,
 119                                bool firstLeg,
 120                                double radius,
 121                                double alt,
 122                                double lat,
 123                                double lon,
 124                                double speed,
 125                                const string& fltType,
 126                                const string& acType,
 127                                const string& airline)
 128 {
 129   sid = 0;
 130   repeat = false;
 131   leg = 10;
 132   gateId=0;
 133   taxiRoute = 0;
 134   start_time = start;
 135   bool useInitialWayPoint = true;
 136   bool useCurrentWayPoint = false;
 137   SGPath path( globals->get_fg_root() );
 138   path.append( "/AI/FlightPlans" );
 139   path.append( p );
 140
 141   SGPropertyNode root;
 142   isValid = true;
 143   // This is a bit of a hack:
 144   // Normally the value of course will be used to evaluate whether
 145   // or not a waypoint will be used for midair initialization of
 146   // an AI aircraft. However, if a course value of 999 will be passed
 147   // when an update request is received, which will by definition always be
 148   // on the ground and should include all waypoints.
 149   if (course == 999)
 150     {
 151       useInitialWayPoint = false;
 152       useCurrentWayPoint = true;
 153     }
 154
 155   if (path.exists())
 156     {
 157       try
 158         {
 159           readProperties(path.str(), &root);
 160
 161           SGPropertyNode * node = root.getNode("flightplan");
 162
 163           //waypoints.push_back( init_waypoint );
 164           for (int i = 0; i < node->nChildren(); i++) {
 165             //cout << "Reading waypoint " << i << endl;
 166             waypoint* wpt = new waypoint;
 167             SGPropertyNode * wpt_node = node->getChild(i);
 168             wpt->name      = wpt_node->getStringValue("name", "END");
 169             wpt->latitude  = wpt_node->getDoubleValue("lat", 0);
 170             wpt->longitude = wpt_node->getDoubleValue("lon", 0);
 171             wpt->altitude  = wpt_node->getDoubleValue("alt", 0);
 172             wpt->speed     = wpt_node->getDoubleValue("ktas", 0);
 173             //wpt->speed     = speed;
 174             wpt->crossat   = wpt_node->getDoubleValue("crossat", -10000);
 175             wpt->gear_down = wpt_node->getBoolValue("gear-down", false);
 176             wpt->flaps_down= wpt_node->getBoolValue("flaps-down", false);
 177
 178             if (wpt->name == "END") wpt->finished = true;
 179             else wpt->finished = false;
 180             waypoints.push_back(wpt);
 181           } // of node loop
 182           wpt_iterator = waypoints.begin();
 183         } catch (const sg_exception &e) {
 184       SG_LOG(SG_GENERAL, SG_WARN, "Error reading AI flight plan: " <<
 185         e.getMessage() << " from " << e.getOrigin());
 186     }
 187   } else {
 188       // cout << path.str() << endl;
 189       // cout << "Trying to create this plan dynamically" << endl;
 190       // cout << "Route from " << dep->id << " to " << arr->id << endl;
 191       time_t now = time(NULL) + fgGetLong("/sim/time/warp");
 192       time_t timeDiff = now-start;
 193       leg = 1;
 194
 195       if ((timeDiff > 60) && (timeDiff < 1200))
 196         leg = 2;
 197       else if ((timeDiff >= 1200) && (timeDiff < 1500))
 198         leg = 3;
 199       else if ((timeDiff >= 1500) && (timeDiff < 2000))
 200         leg = 4;
 201       else if (timeDiff >= 2000)
 202         leg = 5;
 203       /*
 204       if (timeDiff >= 2000)
 205           leg = 5;
 206       */
 207       SG_LOG(SG_GENERAL, SG_INFO, "Route from " << dep->getId() << " to " << arr->getId() << ". Set leg to : " << leg << " " << ac->getTrafficRef()->getCallSign());
 208       wpt_iterator = waypoints.begin();
 209       bool dist = 0;
 210       isValid = create(ac, dep,arr, leg, alt, speed, lat, lon,
 211              firstLeg, radius, fltType, acType, airline, dist);
 212       wpt_iterator = waypoints.begin();
 213       //cerr << "after create: " << (*wpt_iterator)->name << endl;
 214       //leg++;
 215       // Now that we have dynamically created a flight plan,
 216       // we need to add some code that pops any waypoints already past.
 217       //return;
 218     }
 219   /*
 220     waypoint* init_waypoint   = new waypoint;
 221     init_waypoint->name       = string("initial position");
 222     init_waypoint->latitude   = entity->latitude;
 223     init_waypoint->longitude  = entity->longitude;
 224     init_waypoint->altitude   = entity->altitude;
 225     init_waypoint->speed      = entity->speed;
 226     init_waypoint->crossat    = - 10000;
 227     init_waypoint->gear_down  = false;
 228     init_waypoint->flaps_down = false;
 229     init_waypoint->finished   = false;
 230
 231     wpt_vector_iterator i = waypoints.begin();
 232     while (i != waypoints.end())
 233     {
 234       //cerr << "Checking status of each waypoint: " << (*i)->name << endl;
 235        SGWayPoint first(init_waypoint->longitude,
 236                        init_waypoint->latitude,
 237                        init_waypoint->altitude);
 238       SGWayPoint curr ((*i)->longitude,
 239                        (*i)->latitude,
 240                        (*i)->altitude);
 241       double crse, crsDiff;
 242       double dist;
 243       curr.CourseAndDistance(first, &crse, &dist);
 244
 245       dist *= SG_METER_TO_NM;
 246
 247       // We're only interested in the absolute value of crsDiff
 248       // wich should fall in the 0-180 deg range.
 249       crsDiff = fabs(crse-course);
 250       if (crsDiff > 180)
 251         crsDiff = 360-crsDiff;
 252       // These are the three conditions that we consider including
 253       // in our flight plan:
 254       // 1) current waypoint is less then 100 miles away OR
 255       // 2) curren waypoint is ahead of us, at any distance
 256
 257       if ((dist > 20.0) && (crsDiff > 90.0) && ((*i)->name != string ("EOF")))
 258         {
 259           //useWpt = false;
 260           // Once we start including waypoints, we have to continue, even though
 261           // one of the following way point would suffice.
 262           // so once is the useWpt flag is set to true, we cannot reset it to false.
 263           //cerr << "Discarding waypoint: " << (*i)->name
 264           //   << ": Course difference = " << crsDiff
 265           //  << "Course = " << course
 266           // << "crse   = " << crse << endl;
 267         }
 268       else
 269         useCurrentWayPoint = true;
 270
 271       if (useCurrentWayPoint)
 272         {
 273           if ((dist > 100.0) && (useInitialWayPoint))
 274             {
 275               //waypoints.push_back(init_waypoint);;
 276               waypoints.insert(i, init_waypoint);
 277               //cerr << "Using waypoint : " << init_waypoint->name <<  endl;
 278             }
 279           //if (useInitialWayPoint)
 280           // {
 281           //    (*i)->speed = dist; // A hack
 282           //  }
 283           //waypoints.push_back( wpt );
 284           //cerr << "Using waypoint : " << (*i)->name
 285           //  << ": course diff : " << crsDiff
 286           //   << "Course = " << course
 287           //   << "crse   = " << crse << endl
 288           //    << "distance      : " << dist << endl;
 289           useInitialWayPoint = false;
 290           i++;
 291         }
 292       else
 293         {
 294           //delete wpt;
 295           delete *(i);
 296           i = waypoints.erase(i);
 297           }
 298
 299         }
 300   */
 301   //for (i = waypoints.begin(); i != waypoints.end(); i++)
 302   //  cerr << "Using waypoint : " << (*i)->name << endl;
 303   //wpt_iterator = waypoints.begin();
 304   //cout << waypoints.size() << " waypoints read." << endl;
 305 }
 306
 307
 308
 309
 310 FGAIFlightPlan::~FGAIFlightPlan()
 311 {
 312   deleteWaypoints();
 313   delete taxiRoute;
 314 }
 315
 316
 317 FGAIFlightPlan::waypoint* const
 318 FGAIFlightPlan::getPreviousWaypoint( void ) const
 319 {
 320   if (wpt_iterator == waypoints.begin()) {
 321     return 0;
 322   } else {
 323     wpt_vector_iterator prev = wpt_iterator;
 324     return *(--prev);
 325   }
 326 }
 327
 328 FGAIFlightPlan::waypoint* const
 329 FGAIFlightPlan::getCurrentWaypoint( void ) const
 330 {
 331   return *wpt_iterator;
 332 }
 333
 334 FGAIFlightPlan::waypoint* const
 335 FGAIFlightPlan::getNextWaypoint( void ) const
 336 {
 337   wpt_vector_iterator i = waypoints.end();
 338   i--;  // end() points to one element after the last one.
 339   if (wpt_iterator == i) {
 340     return 0;
 341   } else {
 342     wpt_vector_iterator next = wpt_iterator;
 343     return *(++next);
 344   }
 345 }
 346
 347 void FGAIFlightPlan::IncrementWaypoint(bool eraseWaypoints )
 348 {
 349   if (eraseWaypoints)
 350     {
 351       if (wpt_iterator == waypoints.begin())
 352         wpt_iterator++;
 353       else
 354         {
 355           delete *(waypoints.begin());
 356           waypoints.erase(waypoints.begin());
 357           wpt_iterator = waypoints.begin();
 358           wpt_iterator++;
 359         }
 360     }
 361   else
 362     wpt_iterator++;
 363
 364 }
 365
 366 void FGAIFlightPlan::DecrementWaypoint(bool eraseWaypoints )
 367 {
 368     if (eraseWaypoints)
 369     {
 370         if (wpt_iterator == waypoints.end())
 371             wpt_iterator--;
 372         else
 373         {
 374             delete *(waypoints.end());
 375             waypoints.erase(waypoints.end());
 376             wpt_iterator = waypoints.end();
 377             wpt_iterator--;
 378         }
 379     }
 380     else
 381         wpt_iterator--;
 382
 383 }
 384
 385
 386 // gives distance in feet from a position to a waypoint
 387 double FGAIFlightPlan::getDistanceToGo(double lat, double lon, waypoint* wp) const{
 388   return SGGeodesy::distanceM(SGGeod::fromDeg(lon, lat),
 389       SGGeod::fromDeg(wp->longitude, wp->latitude));
 390 }
 391
 392 // sets distance in feet from a lead point to the current waypoint
 393 void FGAIFlightPlan::setLeadDistance(double speed, double bearing,
 394                                      waypoint* current, waypoint* next){
 395   double turn_radius;
 396   // Handle Ground steering
 397   // At a turn rate of 30 degrees per second, it takes 12 seconds to do a full 360 degree turn
 398   // So, to get an estimate of the turn radius, calculate the cicumference of the circle
 399   // we travel on. Get the turn radius by dividing by PI (*2).
 400   if (speed < 0.5) {
 401         lead_distance = 0.5;
 402         return;
 403   }
 404   if (speed < 25) {
 405        turn_radius = ((360/30)*fabs(speed)) / (2*M_PI);
 406   } else
 407       turn_radius = 0.1911 * speed * speed; // an estimate for 25 degrees bank
 408
 409   double inbound = bearing;
 410   double outbound = getBearing(current, next);
 411   leadInAngle = fabs(inbound - outbound);
 412   if (leadInAngle > 180.0)
 413     leadInAngle = 360.0 - leadInAngle;
 414   //if (leadInAngle < 30.0) // To prevent lead_dist from getting so small it is skipped
 415   //  leadInAngle = 30.0;
 416
 417   //lead_distance = turn_radius * sin(leadInAngle * SG_DEGREES_TO_RADIANS);
 418   lead_distance = turn_radius * tan((leadInAngle * SG_DEGREES_TO_RADIANS)/2);
 419   /*
 420   if ((lead_distance > (3*turn_radius)) && (current->on_ground == false)) {
 421       // cerr << "Warning: Lead-in distance is large. Inbound = " << inbound
 422       //      << ". Outbound = " << outbound << ". Lead in angle = " << leadInAngle  << ". Turn radius = " << turn_radius << endl;
 423        lead_distance = 3 * turn_radius;
 424        return;
 425   }
 426   if ((leadInAngle > 90) && (current->on_ground == true)) {
 427       lead_distance = turn_radius * tan((90 * SG_DEGREES_TO_RADIANS)/2);
 428       return;
 429   }*/
 430 }
 431
 432 void FGAIFlightPlan::setLeadDistance(double distance_ft){
 433   lead_distance = distance_ft;
 434 }
 435
 436
 437 double FGAIFlightPlan::getBearing(waypoint* first, waypoint* second) const{
 438   return getBearing(first->latitude, first->longitude, second);
 439 }
 440
 441
 442 double FGAIFlightPlan::getBearing(double lat, double lon, waypoint* wp) const{
 443   return SGGeodesy::courseDeg(SGGeod::fromDeg(lon, lat),
 444       SGGeod::fromDeg(wp->longitude, wp->latitude));
 445 }
 446
 447 void FGAIFlightPlan::deleteWaypoints()
 448 {
 449   for (wpt_vector_iterator i = waypoints.begin(); i != waypoints.end();i++)
 450     delete (*i);
 451   waypoints.clear();
 452 }
 453
 454 // Delete all waypoints except the last,
 455 // which we will recycle as the first waypoint in the next leg;
 456 void FGAIFlightPlan::resetWaypoints()
 457 {
 458   if (waypoints.begin() == waypoints.end())
 459     return;
 460   else
 461     {
 462       waypoint *wpt = new waypoint;
 463       wpt_vector_iterator i = waypoints.end();
 464       i--;
 465       wpt->name      = (*i)->name;
 466       wpt->latitude  = (*i)->latitude;
 467       wpt->longitude =  (*i)->longitude;
 468       wpt->altitude  =  (*i)->altitude;
 469       wpt->speed     =  (*i)->speed;
 470       wpt->crossat   =  (*i)->crossat;
 471       wpt->gear_down =  (*i)->gear_down;
 472       wpt->flaps_down=  (*i)->flaps_down;
 473       wpt->finished  = false;
 474       wpt->on_ground =  (*i)->on_ground;
 475       //cerr << "Recycling waypoint " << wpt->name << endl;
 476       deleteWaypoints();
 477       waypoints.push_back(wpt);
 478     }
 479 }
 480
 481 // Start flightplan over from the beginning
 482 void FGAIFlightPlan::restart()
 483 {
 484   wpt_iterator = waypoints.begin();
 485 }
 486
 487
 488 void FGAIFlightPlan::deleteTaxiRoute()
 489 {
 490   delete taxiRoute;
 491   taxiRoute = 0;
 492 }
 493
 494
 495 int FGAIFlightPlan::getRouteIndex(int i) {
 496   if ((i > 0) && (i < (int)waypoints.size())) {
 497     return waypoints[i]->routeIndex;
 498   }
 499   else
 500     return 0;
 501 }
 502
 503
 504 double FGAIFlightPlan::checkTrackLength(string wptName) {
 505     // skip the first two waypoints: first one is behind, second one is partially done;
 506     double trackDistance = 0;
 507     wpt_vector_iterator wptvec = waypoints.begin();
 508     wptvec++;
 509     wptvec++;
 510     while ((wptvec != waypoints.end()) && ((*wptvec)->name != wptName)) {
 511            trackDistance += (*wptvec)->trackLength;
 512            wptvec++;
 513     }
 514     if (wptvec == waypoints.end()) {
 515         trackDistance = 0; // name not found
 516     }
 517     return trackDistance;
 518 }