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 #include "AIFlightPlan.hxx"
  21 #include <simgear/math/sg_geodesy.hxx>
  22 #include <Airports/runways.hxx>
  23
  24 #include <Environment/environment_mgr.hxx>
  25 #include <Environment/environment.hxx>
  26
  27
  28 void FGAIFlightPlan::createPushBack(bool firstFlight, FGAirport *dep,
  29                                     double latitude,
  30                                     double longitude,
  31                                     double radius,
  32                                     const string& fltType,
  33                                     const string& aircraftType,
  34                                     const string& airline)
  35 {
  36     double lat, lon, heading;
  37     FGTaxiRoute *pushBackRoute;
  38     if (!(dep->getDynamics()->getGroundNetwork()->exists())) {
  39         //cerr << "Push Back fallback" << endl;
  40         createPushBackFallBack(firstFlight, dep, latitude, longitude,
  41                                radius, fltType, aircraftType, airline);
  42     } else {
  43         if (firstFlight) {
  44              if (!(dep->getDynamics()->getAvailableParking(&lat, &lon,
  45                                                            &heading, &gateId,
  46                                                            radius, fltType,
  47                                                            aircraftType, airline))) {
  48                     SG_LOG(SG_INPUT, SG_WARN, "Warning: Could not find parking for a " <<
  49                                               aircraftType <<
  50                                               " of flight type " << fltType <<
  51                                               " of airline     " << airline <<
  52                                               " at airport     " << dep->getId());
  53                     char buffer[10];
  54                     snprintf (buffer, 10, "%d", gateId);
  55                     //FGTaxiNode *tn = dep->getDynamics()->getGroundNetwork()->findNode(node);
  56                     waypoint *wpt;
  57                     wpt = new waypoint;
  58                     wpt->name      = string(buffer); // fixme: should be the name of the taxiway
  59                     wpt->latitude  = lat;
  60                     wpt->longitude = lon;
  61                     // Elevation is currently disregarded when on_ground is true
  62                     // because the AIModel obtains a periodic ground elevation estimate.
  63                    wpt->altitude  = dep->getElevation();
  64                    wpt->speed = -10;
  65                    wpt->crossat   = -10000;
  66                    wpt->gear_down = true;
  67                    wpt->flaps_down= true;
  68                    wpt->finished  = false;
  69                    wpt->on_ground = true;
  70                    wpt->routeIndex = -1;
  71                    waypoints.push_back(wpt);
  72             }
  73         } else {
  74             //cerr << "Push Back follow-up Flight" << endl;
  75             dep->getDynamics()->getParking(gateId, &lat, &lon, &heading);
  76         }
  77         if (gateId < 0) {
  78              createPushBackFallBack(firstFlight, dep, latitude, longitude,
  79                                     radius, fltType, aircraftType, airline);
  80              return;
  81
  82         }
  83         //cerr << "getting parking " << gateId;
  84         //cerr << " for a " <<
  85         //                                      aircraftType <<
  86         //                                      " of flight type " << fltType <<
  87         //                                      " of airline     " << airline <<
  88         //                                      " at airport     " << dep->getId() << endl;
  89         FGParking *parking = dep->getDynamics()->getParking(gateId);
  90         int pushBackNode = parking->getPushBackPoint();
  91
  92         // initialize the pushback route. Note that parts
  93         // of this procedure should probably be done inside
  94         // taxidraw. This code is likely to change once this
  95         // this is fully implemented in taxidraw. Until that time,
  96         // however, the full initialization procedure looks like this:
  97         // 1) Build a list of all the nodes that are classified as
  98         //    pushBack hold points
  99         // 2) For each hold point, use the dykstra algorithm to find a route
 100         //    between the gate and the pushBack hold nodes, however use only
 101         //    segments that are classified as "pushback" routes.
 102         // 3) return the TaxiRoute class that is non empty.
 103         // 4) store refer this route in the parking object, for future use
 104
 105         if (pushBackNode < 0) {
 106             //cerr << "Initializing PushBackRoute " << endl;
 107             intVec pushBackNodes;
 108             int nAINodes = dep->getDynamics()->getGroundNetwork()->getNrOfNodes();
 109             int hits = 0;
 110             parking->setPushBackPoint(0); // default in case no network was found.
 111
 112             // Collect all the nodes that are classified as having pushBack hold status
 113             for (int i = 0; i < nAINodes; i++) {
 114                 if (dep->getDynamics()->getGroundNetwork()->findNode(i)->getHoldPointType() == 3) {
 115                     pushBackNodes.push_back(i);
 116                 }
 117             }
 118
 119             // For each node found in step 1, check if it can be reached
 120             FGTaxiRoute route;
 121             for (intVecIterator nodes = pushBackNodes.begin();
 122                                  nodes != pushBackNodes.end();
 123                                  nodes++) {
 124                 route = dep->getDynamics()->getGroundNetwork()->findShortestRoute(gateId, *nodes, false);
 125                 if (!(route.empty())) {
 126                     //cerr << "Found Pushback route of size " << route.size() << endl;
 127                     hits++;
 128                     parking->setPushBackRoute(new FGTaxiRoute(route));
 129                     parking->setPushBackPoint(*nodes);
 130                     pushBackNode = *nodes;
 131                 }
 132              }
 133              if (hits == 0) {
 134                  SG_LOG(SG_GENERAL, SG_INFO, "No pushback route found for gate " << gateId << " at " << dep->getId());
 135              }
 136              if (hits > 1) {
 137                  SG_LOG(SG_GENERAL, SG_WARN, hits << " pushback routes found for gate " << gateId << " at " << dep->getId());
 138              }
 139         }
 140         if (pushBackNode > 0) {
 141             int node, rte;
 142             //cerr << "Found valid pusback node " << pushBackNode << "for gate " << gateId << endl;
 143             pushBackRoute = parking->getPushBackRoute();
 144             int size = pushBackRoute->size();
 145             if (size < 2) {
 146                 SG_LOG(SG_GENERAL, SG_WARN, "Push back route from gate " << gateId << " has only " << size << " nodes.");
 147             }
 148             pushBackRoute->first();
 149             waypoint *wpt;
 150             while(pushBackRoute->next(&node, &rte))
 151               {
 152                 //FGTaxiNode *tn = apt->getDynamics()->getGroundNetwork()->findSegment(node)->getEnd();
 153                 char buffer[10];
 154                 snprintf (buffer, 10, "%d", node);
 155                 FGTaxiNode *tn = dep->getDynamics()->getGroundNetwork()->findNode(node);
 156                 //ids.pop_back();
 157                 wpt = new waypoint;
 158                 wpt->name      = string(buffer); // fixme: should be the name of the taxiway
 159                 wpt->latitude  = tn->getLatitude();
 160                 wpt->longitude = tn->getLongitude();
 161                 // Elevation is currently disregarded when on_ground is true
 162                 // because the AIModel obtains a periodic ground elevation estimate.
 163                 wpt->altitude  = dep->getElevation();
 164                 wpt->speed = -10;
 165                 wpt->crossat   = -10000;
 166                 wpt->gear_down = true;
 167                 wpt->flaps_down= true;
 168                 wpt->finished  = false;
 169                 wpt->on_ground = true;
 170                 wpt->routeIndex = rte;
 171                 waypoints.push_back(wpt);
 172               }
 173               // some special considerations for the last point:
 174               wpt->name = string("PushBackPoint");
 175               wpt->speed = 15;
 176         } else {
 177            //cerr << "Creating direct forward departure route fragment" << endl;
 178            double lat2, lon2, az2;
 179            waypoint *wpt;
 180            geo_direct_wgs_84 ( 0, lat, lon, heading,
 181                                2, &lat2, &lon2, &az2 );
 182            wpt = new waypoint;
 183            wpt->name      = "park2";
 184            wpt->latitude  = lat2;
 185            wpt->longitude = lon2;
 186            wpt->altitude  = dep->getElevation();
 187            wpt->speed     = 10;
 188            wpt->crossat   = -10000;
 189            wpt->gear_down = true;
 190            wpt->flaps_down= true;
 191            wpt->finished  = false;
 192            wpt->on_ground = true;
 193            wpt->routeIndex = 0;
 194            waypoints.push_back(wpt);
 195
 196            geo_direct_wgs_84 ( 0, lat, lon, heading,
 197                                4, &lat2, &lon2, &az2 );
 198            wpt = new waypoint;
 199            wpt->name      = "name";
 200            wpt->latitude  = lat2;
 201            wpt->longitude = lon2;
 202            wpt->altitude  = dep->getElevation();
 203            wpt->speed     = 10;
 204            wpt->crossat   = -10000;
 205            wpt->gear_down = true;
 206            wpt->flaps_down= true;
 207            wpt->finished  = false;
 208            wpt->on_ground = true;
 209            wpt->routeIndex = 0;
 210            waypoints.push_back(wpt);
 211
 212            //cerr << "Creating final push forward point for gate " << gateId << endl;
 213            FGTaxiNode *tn = dep->getDynamics()->getGroundNetwork()->findNode(gateId);
 214            FGTaxiSegmentVectorIterator ts = tn->getBeginRoute();
 215            FGTaxiSegmentVectorIterator te = tn->getEndRoute();
 216            if (ts == te) {
 217                SG_LOG(SG_GENERAL, SG_ALERT, "Gate " << gateId << "doesn't seem to have routes associated with it.");
 218                //exit(1);
 219            }
 220            tn = (*ts)->getEnd();
 221            lastNodeVisited = tn->getIndex();
 222            if (tn == NULL) {
 223                SG_LOG(SG_GENERAL, SG_ALERT, "No valid taxinode found");
 224                exit(1);
 225            }
 226            wpt = new waypoint;
 227            wpt->name      = "PushBackPoint";
 228            wpt->latitude  = tn->getLatitude();
 229            wpt->longitude = tn->getLongitude();
 230            wpt->altitude  = dep->getElevation();
 231            wpt->speed     = 10;
 232            wpt->crossat   = -10000;
 233            wpt->gear_down = true;
 234            wpt->flaps_down= true;
 235            wpt->finished  = false;
 236            wpt->on_ground = true;
 237            wpt->routeIndex = (*ts)->getIndex();
 238            waypoints.push_back(wpt);
 239
 240
 241         }
 242
 243     }
 244 }
 245 /*******************************************************************
 246  * createPushBackFallBack
 247  * This is the backup function for airports that don't have a
 248  * network yet.
 249  ******************************************************************/
 250 void FGAIFlightPlan::createPushBackFallBack(bool firstFlight, FGAirport *dep,
 251                                     double latitude,
 252                                     double longitude,
 253                                     double radius,
 254                                     const string& fltType,
 255                                     const string& aircraftType,
 256                                     const string& airline)
 257 {
 258   double heading;
 259   double lat;
 260   double lon;
 261   double lat2;
 262   double lon2;
 263   double az2;
 264
 265
 266
 267   dep->getDynamics()->getParking(-1, &lat, &lon, &heading);
 268
 269   heading += 180.0;
 270   if (heading > 360)
 271     heading -= 360;
 272   waypoint *wpt = new waypoint;
 273   wpt->name      = "park";
 274   wpt->latitude  = lat;
 275   wpt->longitude = lon;
 276   wpt->altitude  = dep->getElevation();
 277   wpt->speed     = -10;
 278   wpt->crossat   = -10000;
 279   wpt->gear_down = true;
 280   wpt->flaps_down= true;
 281   wpt->finished  = false;
 282   wpt->on_ground = true;
 283
 284   waypoints.push_back(wpt);
 285
 286   geo_direct_wgs_84 ( 0, lat, lon, heading,
 287                       10,
 288                       &lat2, &lon2, &az2 );
 289   wpt = new waypoint;
 290   wpt->name      = "park2";
 291   wpt->latitude  = lat2;
 292   wpt->longitude = lon2;
 293   wpt->altitude  = dep->getElevation();
 294   wpt->speed     = -10;
 295   wpt->crossat   = -10000;
 296   wpt->gear_down = true;
 297   wpt->flaps_down= true;
 298   wpt->finished  = false;
 299   wpt->on_ground = true;
 300   wpt->routeIndex = 0;
 301   waypoints.push_back(wpt);
 302   geo_direct_wgs_84 ( 0, lat, lon, heading,
 303                       2.2*radius,
 304                       &lat2, &lon2, &az2 );
 305   wpt = new waypoint;
 306   wpt->name      = "taxiStart";
 307   wpt->latitude  = lat2;
 308   wpt->longitude = lon2;
 309   wpt->altitude  = dep->getElevation();
 310   wpt->speed     = 10;
 311   wpt->crossat   = -10000;
 312   wpt->gear_down = true;
 313   wpt->flaps_down= true;
 314   wpt->finished  = false;
 315   wpt->on_ground = true;
 316   wpt->routeIndex = 0;
 317   waypoints.push_back(wpt);
 318 }