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