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