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               //for (wpt_vector_iterator i = waypoints.begin(); i != waypoints.end(); i++) {
 145               //    cerr << "Waypoint Name: " << (*i)->name << endl;
 146               //}
 147         } else {
 148            //cerr << "Creating direct forward departure route fragment" << endl;
 149            double lat2, lon2, az2;
 150            waypoint *wpt;
 151            geo_direct_wgs_84 ( 0, lat, lon, heading,
 152                                2, &lat2, &lon2, &az2 );
 153            wpt = new waypoint;
 154            wpt->name      = "park2";
 155            wpt->latitude  = lat2;
 156            wpt->longitude = lon2;
 157            wpt->altitude  = dep->getElevation();
 158            wpt->speed     = 10;
 159            wpt->crossat   = -10000;
 160            wpt->gear_down = true;
 161            wpt->flaps_down= true;
 162            wpt->finished  = false;
 163            wpt->on_ground = true;
 164            wpt->routeIndex = 0;
 165            waypoints.push_back(wpt);
 166
 167            geo_direct_wgs_84 ( 0, lat, lon, heading,
 168                                4, &lat2, &lon2, &az2 );
 169            wpt = new waypoint;
 170            wpt->name      = "name";
 171            wpt->latitude  = lat2;
 172            wpt->longitude = lon2;
 173            wpt->altitude  = dep->getElevation();
 174            wpt->speed     = 10;
 175            wpt->crossat   = -10000;
 176            wpt->gear_down = true;
 177            wpt->flaps_down= true;
 178            wpt->finished  = false;
 179            wpt->on_ground = true;
 180            wpt->routeIndex = 0;
 181            waypoints.push_back(wpt);
 182
 183            //cerr << "Creating final push forward point for gate " << gateId << endl;
 184            FGTaxiNode *tn = dep->getDynamics()->getGroundNetwork()->findNode(gateId);
 185            FGTaxiSegmentVectorIterator ts = tn->getBeginRoute();
 186            FGTaxiSegmentVectorIterator te = tn->getEndRoute();
 187            if (ts == te) {
 188                SG_LOG(SG_GENERAL, SG_ALERT, "Gate " << gateId << "doesn't seem to have routes associated with it.");
 189                //exit(1);
 190            }
 191            tn = (*ts)->getEnd();
 192            lastNodeVisited = tn->getIndex();
 193            if (tn == NULL) {
 194                SG_LOG(SG_GENERAL, SG_ALERT, "No valid taxinode found");
 195                exit(1);
 196            }
 197            wpt = new waypoint;
 198            wpt->name      = "PushBackPoint";
 199            wpt->latitude  = tn->getLatitude();
 200            wpt->longitude = tn->getLongitude();
 201            wpt->altitude  = dep->getElevation();
 202            wpt->speed     = 10;
 203            wpt->crossat   = -10000;
 204            wpt->gear_down = true;
 205            wpt->flaps_down= true;
 206            wpt->finished  = false;
 207            wpt->on_ground = true;
 208            wpt->routeIndex = (*ts)->getIndex();
 209            waypoints.push_back(wpt);
 210
 211
 212         }
 213
 214     }
 215 }
 216 /*******************************************************************
 217  * createPushBackFallBack
 218  * This is the backup function for airports that don't have a
 219  * network yet.
 220  ******************************************************************/
 221 void FGAIFlightPlan::createPushBackFallBack(bool firstFlight, FGAirport *dep,
 222                                     double latitude,
 223                                     double longitude,
 224                                     double radius,
 225                                     const string& fltType,
 226                                     const string& aircraftType,
 227                                     const string& airline)
 228 {
 229   double heading;
 230   double lat;
 231   double lon;
 232   double lat2;
 233   double lon2;
 234   double az2;
 235
 236
 237
 238   dep->getDynamics()->getParking(-1, &lat, &lon, &heading);
 239
 240   heading += 180.0;
 241   if (heading > 360)
 242     heading -= 360;
 243   waypoint *wpt = new waypoint;
 244   wpt->name      = "park";
 245   wpt->latitude  = lat;
 246   wpt->longitude = lon;
 247   wpt->altitude  = dep->getElevation();
 248   wpt->speed     = -10;
 249   wpt->crossat   = -10000;
 250   wpt->gear_down = true;
 251   wpt->flaps_down= true;
 252   wpt->finished  = false;
 253   wpt->on_ground = true;
 254
 255   waypoints.push_back(wpt);
 256
 257   geo_direct_wgs_84 ( 0, lat, lon, heading,
 258                       10,
 259                       &lat2, &lon2, &az2 );
 260   wpt = new waypoint;
 261   wpt->name      = "park2";
 262   wpt->latitude  = lat2;
 263   wpt->longitude = lon2;
 264   wpt->altitude  = dep->getElevation();
 265   wpt->speed     = -10;
 266   wpt->crossat   = -10000;
 267   wpt->gear_down = true;
 268   wpt->flaps_down= true;
 269   wpt->finished  = false;
 270   wpt->on_ground = true;
 271   wpt->routeIndex = 0;
 272   waypoints.push_back(wpt);
 273   geo_direct_wgs_84 ( 0, lat, lon, heading,
 274                       2.2*radius,
 275                       &lat2, &lon2, &az2 );
 276   wpt = new waypoint;
 277   wpt->name      = "taxiStart";
 278   wpt->latitude  = lat2;
 279   wpt->longitude = lon2;
 280   wpt->altitude  = dep->getElevation();
 281   wpt->speed     = 10;
 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 = 0;
 288   waypoints.push_back(wpt);
 289 }