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