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