src/AIModel/AIFlightPlanCreatePushBack.cxx

   1 /******************************************************************************
   2  * AIFlightPlanCreatePushBack.cxx
   3  * Written by Durk Talsma, started August 1, 2007.
   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  **************************************************************************/
  20
  21 #ifdef HAVE_CONFIG_H
  22 #  include <config.h>
  23 #endif
  24
  25 #include <simgear/math/sg_geodesy.hxx>
  26
  27 #include <Airports/simple.hxx>
  28 #include <Airports/runways.hxx>
  29 #include <Airports/dynamics.hxx>
  30
  31 #include <Environment/environment_mgr.hxx>
  32 #include <Environment/environment.hxx>
  33
  34 #include "AIFlightPlan.hxx"
  35 #include "AIAircraft.hxx"
  36 #include "performancedata.hxx"
  37
  38
  39 // TODO: Use James Turner's createOnGround functions.
  40 bool FGAIFlightPlan::createPushBack(FGAIAircraft *ac,
  41                                     bool firstFlight, FGAirport *dep,
  42                                     double latitude,
  43                                     double longitude,
  44                                     double radius,
  45                                     const string& fltType,
  46                                     const string& aircraftType,
  47                                     const string& airline)
  48 {
  49     double lat, lon, heading;
  50     double vTaxi = ac->getPerformance()->vTaxi();
  51     double vTaxiBackward = vTaxi * (-2.0/3.0);
  52     double vTaxiReduced  = vTaxi * (2.0/3.0);
  53     FGTaxiRoute *pushBackRoute;
  54     // Active runway can be conditionally set by ATC, so at the start of a new flight, this
  55     // must be reset.
  56     activeRunway.clear();
  57
  58     if (!(dep->getDynamics()->getGroundNetwork()->exists())) {
  59         //cerr << "Push Back fallback" << endl;
  60         createPushBackFallBack(ac, firstFlight, dep, latitude, longitude,
  61                                radius, fltType, aircraftType, airline);
  62     } else {
  63         if (firstFlight) {
  64
  65              if (!(dep->getDynamics()->getAvailableParking(&lat, &lon,
  66                                                            &heading, &gateId,
  67                                                            radius, fltType,
  68                                                            aircraftType, airline))) {
  69                     SG_LOG(SG_INPUT, SG_WARN, "Warning: Could not find parking for a " <<
  70                                               aircraftType <<
  71                                               " of flight type " << fltType <<
  72                                               " of airline     " << airline <<
  73                                               " at airport     " << dep->getId());
  74                     return false;
  75                     char buffer[10];
  76                     snprintf (buffer, 10, "%d", gateId);
  77                     //FGTaxiNode *tn = dep->getDynamics()->getGroundNetwork()->findNode(node);
  78                     waypoint *wpt;
  79                     wpt = new waypoint;
  80                     wpt->name      = string(buffer); // fixme: should be the name of the taxiway
  81                     wpt->latitude  = lat;
  82                     wpt->longitude = lon;
  83                     // Elevation is currently disregarded when on_ground is true
  84                     // because the AIModel obtains a periodic ground elevation estimate.
  85                    wpt->altitude  = dep->getElevation();
  86                    wpt->speed = vTaxiBackward;
  87                    wpt->crossat   = -10000;
  88                    wpt->gear_down = true;
  89                    wpt->flaps_down= true;
  90                    wpt->finished  = false;
  91                    wpt->on_ground = true;
  92                    wpt->routeIndex = -1;
  93                    waypoints.push_back(wpt);
  94             }
  95            //cerr << "Success : GateId = " << gateId << endl;
  96            SG_LOG(SG_INPUT, SG_WARN, "Warning: Succesfully found a parking for a " <<
  97                                               aircraftType <<
  98                                               " of flight type " << fltType <<
  99                                               " of airline     " << airline <<
 100                                               " at airport     " << dep->getId());
 101         } else {
 102             //cerr << "Push Back follow-up Flight" << endl;
 103             dep->getDynamics()->getParking(gateId, &lat, &lon, &heading);
 104         }
 105         if (gateId < 0) {
 106              createPushBackFallBack(ac, firstFlight, dep, latitude, longitude,
 107                                     radius, fltType, aircraftType, airline);
 108              return true;
 109
 110         }
 111         //cerr << "getting parking " << gateId;
 112         //cerr << " for a " <<
 113         //                                      aircraftType <<
 114         //                                      " of flight type " << fltType <<
 115         //                                      " of airline     " << airline <<
 116         //                                      " at airport     " << dep->getId() << endl;
 117         FGParking *parking = dep->getDynamics()->getParking(gateId);
 118         int pushBackNode = parking->getPushBackPoint();
 119
 120
 121         pushBackRoute = parking->getPushBackRoute();
 122         if ((pushBackNode > 0) && (pushBackRoute == 0)) {
 123             int node, rte;
 124             FGTaxiRoute route;
 125             //cerr << "Creating push-back for " << gateId << " (" << parking->getName() << ") using push-back point " << pushBackNode << endl;
 126             route = dep->getDynamics()->getGroundNetwork()->findShortestRoute(gateId, pushBackNode, false);
 127             parking->setPushBackRoute(new FGTaxiRoute(route));
 128
 129
 130             pushBackRoute = parking->getPushBackRoute();
 131             int size = pushBackRoute->size();
 132             if (size < 2) {
 133                 SG_LOG(SG_GENERAL, SG_WARN, "Push back route from gate " << gateId << " has only " << size << " nodes.");
 134                 SG_LOG(SG_GENERAL, SG_WARN, "Using  " << pushBackNode);
 135             }
 136             pushBackRoute->first();
 137             waypoint *wpt;
 138             while(pushBackRoute->next(&node, &rte))
 139               {
 140                 //FGTaxiNode *tn = apt->getDynamics()->getGroundNetwork()->findSegment(node)->getEnd();
 141                 char buffer[10];
 142                 snprintf (buffer, 10, "%d", node);
 143                 FGTaxiNode *tn = dep->getDynamics()->getGroundNetwork()->findNode(node);
 144                 //ids.pop_back();
 145                 wpt = new waypoint;
 146                 wpt->name      = string(buffer); // fixme: should be the name of the taxiway
 147                 wpt->latitude  = tn->getLatitude();
 148                 wpt->longitude = tn->getLongitude();
 149                 // Elevation is currently disregarded when on_ground is true
 150                 // because the AIModel obtains a periodic ground elevation estimate.
 151                 wpt->altitude  = dep->getElevation();
 152                 wpt->speed = vTaxiBackward;
 153                 wpt->crossat   = -10000;
 154                 wpt->gear_down = true;
 155                 wpt->flaps_down= true;
 156                 wpt->finished  = false;
 157                 wpt->on_ground = true;
 158                 wpt->routeIndex = rte;
 159                 waypoints.push_back(wpt);
 160               }
 161               // some special considerations for the last point:
 162               wpt->name = string("PushBackPoint");
 163               wpt->speed = vTaxi;
 164               //for (wpt_vector_iterator i = waypoints.begin(); i != waypoints.end(); i++) {
 165               //    cerr << "Waypoint Name: " << (*i)->name << endl;
 166               //}
 167         } else {
 168            /*
 169            string rwyClass = getRunwayClassFromTrafficType(fltType);
 170
 171            // Only set this if it hasn't been set by ATC already.
 172           if (activeRunway.empty()) {
 173                //cerr << "Getting runway for " << ac->getTrafficRef()->getCallSign() << " at " << apt->getId() << endl;
 174               double depHeading = ac->getTrafficRef()->getCourse();
 175              dep->getDynamics()->getActiveRunway(rwyClass, 1, activeRunway,
 176                                                  depHeading);
 177            }
 178            rwy = dep->getRunwayByIdent(activeRunway);
 179            SGGeod runwayTakeoff = rwy->pointOnCenterline(5.0);
 180
 181           FGGroundNetwork *gn = dep->getDynamics()->getGroundNetwork();
 182           if (!gn->exists()) {
 183               createDefaultTakeoffTaxi(ac, dep, rwy);
 184               return true;
 185           }
 186           int runwayId = gn->findNearestNode(runwayTakeoff);
 187           int node = 0;
 188            // Find out which node to start from
 189           FGParking *park = dep->getDynamics()->getParking(gateId);
 190         if (park) {
 191             node = park->getPushBackPoint();
 192         }
 193
 194         if (node == -1) {
 195             node = gateId;
 196         }
 197         // HAndle case where parking doens't have a node
 198         if ((node == 0) && park) {
 199             if (firstFlight) {
 200                 node = gateId;
 201             } else {
 202                 node = gateId;
 203             }
 204         }
 205         //delete taxiRoute;
 206         //taxiRoute = new FGTaxiRoute;
 207         FGTaxiRoute tr = gn->findShortestRoute(node, runwayId);
 208         int route;
 209         FGTaxiNode *tn;
 210         waypoint *wpt;
 211         int nr = 0;
 212         cerr << "Creating taxiroute from gate: " << gateId << " at " << dep->getId() << endl;
 213         while (tr.next(&node, &route) && (nr++ < 3)) {
 214             char buffer[10];
 215             snprintf(buffer, 10, "%d", node);
 216              tn = dep->getDynamics()->getGroundNetwork()->findNode(node);
 217              wpt = createOnGround(ac, buffer, tn->getGeod(), dep->getElevation(),
 218                                vTaxiReduced);
 219             wpt->routeIndex = route;
 220             waypoints.push_back(wpt);
 221         }
 222         wpt->name      = "PushBackPoint";
 223         lastNodeVisited = tn->getIndex();
 224            //FGTaxiNode *firstNode = findNode(gateId);
 225            //FGTaxiNode *lastNode =  findNode(runwayId);
 226            //cerr << "Creating direct forward departure route fragment" << endl;
 227            */
 228            double lat2 = 0.0, lon2 = 0.0, az2 = 0.0;
 229            waypoint *wpt;
 230            geo_direct_wgs_84 ( 0, lat, lon, heading,
 231                                2, &lat2, &lon2, &az2 );
 232            wpt = new waypoint;
 233            wpt->name      = "park2";
 234            wpt->latitude  = lat2;
 235            wpt->longitude = lon2;
 236            wpt->altitude  = dep->getElevation();
 237            wpt->speed     = vTaxiReduced;
 238            wpt->crossat   = -10000;
 239            wpt->gear_down = true;
 240            wpt->flaps_down= true;
 241            wpt->finished  = false;
 242            wpt->on_ground = true;
 243            wpt->routeIndex = 0;
 244            waypoints.push_back(wpt);
 245
 246            geo_direct_wgs_84 ( 0, lat, lon, heading,
 247                                4, &lat2, &lon2, &az2 );
 248            wpt = new waypoint;
 249            wpt->name      = "name";
 250            wpt->latitude  = lat2;
 251            wpt->longitude = lon2;
 252            wpt->altitude  = dep->getElevation();
 253            wpt->speed     = vTaxiReduced;
 254            wpt->crossat   = -10000;
 255            wpt->gear_down = true;
 256            wpt->flaps_down= true;
 257            wpt->finished  = false;
 258            wpt->on_ground = true;
 259            wpt->routeIndex = 0;
 260            waypoints.push_back(wpt);
 261
 262            //cerr << "Creating final push forward point for gate " << gateId << endl;
 263            FGTaxiNode *tn = dep->getDynamics()->getGroundNetwork()->findNode(gateId);
 264            FGTaxiSegmentVectorIterator ts = tn->getBeginRoute();
 265            FGTaxiSegmentVectorIterator te = tn->getEndRoute();
 266            if (ts == te) {
 267                SG_LOG(SG_GENERAL, SG_ALERT, "Gate " << gateId << "doesn't seem to have routes associated with it.");
 268                //exit(1);
 269            }
 270            tn = (*ts)->getEnd();
 271            lastNodeVisited = tn->getIndex();
 272            if (tn == NULL) {
 273                SG_LOG(SG_GENERAL, SG_ALERT, "No valid taxinode found");
 274                exit(1);
 275            }
 276            wpt = new waypoint;
 277            wpt->name      = "PushBackPoint";
 278            wpt->latitude  = tn->getLatitude();
 279            wpt->longitude = tn->getLongitude();
 280            wpt->altitude  = dep->getElevation();
 281            wpt->speed     = vTaxiReduced;
 282            wpt->crossat   = -10000;
 283            wpt->gear_down = true;
 284            wpt->flaps_down= true;
 285            wpt->finished  = false;
 286            wpt->on_ground = true;
 287            wpt->routeIndex = (*ts)->getIndex();
 288            waypoints.push_back(wpt);
 289         }
 290
 291     }
 292     return true;
 293 }
 294 /*******************************************************************
 295  * createPushBackFallBack
 296  * This is the backup function for airports that don't have a
 297  * network yet.
 298  ******************************************************************/
 299 void FGAIFlightPlan::createPushBackFallBack(FGAIAircraft *ac, bool firstFlight, FGAirport *dep,
 300                                     double latitude,
 301                                     double longitude,
 302                                     double radius,
 303                                     const string& fltType,
 304                                     const string& aircraftType,
 305                                     const string& airline)
 306 {
 307   double heading;
 308   double lat;
 309   double lon;
 310   double lat2 = 0.0;
 311   double lon2 = 0.0;
 312   double az2 = 0.0;
 313
 314   double vTaxi = ac->getPerformance()->vTaxi();
 315   double vTaxiBackward = vTaxi * (-2.0/3.0);
 316   double vTaxiReduced  = vTaxi * (2.0/3.0);
 317
 318
 319
 320   dep->getDynamics()->getParking(-1, &lat, &lon, &heading);
 321
 322   heading += 180.0;
 323   if (heading > 360)
 324     heading -= 360;
 325   waypoint *wpt = new waypoint;
 326   wpt->name      = "park";
 327   wpt->latitude  = lat;
 328   wpt->longitude = lon;
 329   wpt->altitude  = dep->getElevation();
 330   wpt->speed     = vTaxiBackward;
 331   wpt->crossat   = -10000;
 332   wpt->gear_down = true;
 333   wpt->flaps_down= true;
 334   wpt->finished  = false;
 335   wpt->on_ground = true;
 336
 337   waypoints.push_back(wpt);
 338
 339   geo_direct_wgs_84 ( 0, lat, lon, heading,
 340                       10,
 341                       &lat2, &lon2, &az2 );
 342   wpt = new waypoint;
 343   wpt->name      = "park2";
 344   wpt->latitude  = lat2;
 345   wpt->longitude = lon2;
 346   wpt->altitude  = dep->getElevation();
 347   wpt->speed     = vTaxiBackward;
 348   wpt->crossat   = -10000;
 349   wpt->gear_down = true;
 350   wpt->flaps_down= true;
 351   wpt->finished  = false;
 352   wpt->on_ground = true;
 353   wpt->routeIndex = 0;
 354   waypoints.push_back(wpt);
 355   geo_direct_wgs_84 ( 0, lat, lon, heading,
 356                       2.2*radius,
 357                       &lat2, &lon2, &az2 );
 358   wpt = new waypoint;
 359   wpt->name      = "taxiStart";
 360   wpt->latitude  = lat2;
 361   wpt->longitude = lon2;
 362   wpt->altitude  = dep->getElevation();
 363   wpt->speed     = vTaxiReduced;
 364   wpt->crossat   = -10000;
 365   wpt->gear_down = true;
 366   wpt->flaps_down= true;
 367   wpt->finished  = false;
 368   wpt->on_ground = true;
 369   wpt->routeIndex = 0;
 370   waypoints.push_back(wpt);
 371 }