+/******************************************************************************
+ * AIFlightPlanCreatePushBack.cxx
+ * Written by Durk Talsma, started August 1, 2007.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of the
+ * License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+ *
+ **************************************************************************/
+#include "AIFlightPlan.hxx"
+#include <simgear/math/sg_geodesy.hxx>
+#include <Airports/runways.hxx>
+
+#include <Environment/environment_mgr.hxx>
+#include <Environment/environment.hxx>
+
+
+void FGAIFlightPlan::createPushBack(bool firstFlight, FGAirport *dep,
+ double latitude,
+ double longitude,
+ double radius,
+ const string& fltType,
+ const string& aircraftType,
+ const string& airline)
+{
+ double lat, lon, heading;
+ FGTaxiRoute *pushBackRoute;
+ if (!(dep->getDynamics()->getGroundNetwork()->exists())) {
+ //cerr << "Push Back fallback" << endl;
+ createPushBackFallBack(firstFlight, dep, latitude, longitude,
+ radius, fltType, aircraftType, airline);
+ } else {
+ if (firstFlight) {
+ if (!(dep->getDynamics()->getAvailableParking(&lat, &lon,
+ &heading, &gateId,
+ radius, fltType,
+ aircraftType, airline))) {
+ SG_LOG(SG_INPUT, SG_WARN, "Warning: Could not find parking for a " <<
+ aircraftType <<
+ " of flight type " << fltType <<
+ " of airline " << airline <<
+ " at airport " << dep->getId());
+ char buffer[10];
+ snprintf (buffer, 10, "%d", gateId);
+ //FGTaxiNode *tn = dep->getDynamics()->getGroundNetwork()->findNode(node);
+ waypoint *wpt;
+ wpt = new waypoint;
+ wpt->name = string(buffer); // fixme: should be the name of the taxiway
+ wpt->latitude = lat;
+ wpt->longitude = lon;
+ // Elevation is currently disregarded when on_ground is true
+ // because the AIModel obtains a periodic ground elevation estimate.
+ wpt->altitude = dep->getElevation();
+ wpt->speed = -10;
+ wpt->crossat = -10000;
+ wpt->gear_down = true;
+ wpt->flaps_down= true;
+ wpt->finished = false;
+ wpt->on_ground = true;
+ wpt->routeIndex = -1;
+ waypoints.push_back(wpt);
+ }
+ } else {
+ //cerr << "Push Back follow-up Flight" << endl;
+ dep->getDynamics()->getParking(gateId, &lat, &lon, &heading);
+ }
+ if (gateId < 0) {
+ createPushBackFallBack(firstFlight, dep, latitude, longitude,
+ radius, fltType, aircraftType, airline);
+ return;
+
+ }
+ //cerr << "getting parking " << gateId;
+ //cerr << " for a " <<
+ // aircraftType <<
+ // " of flight type " << fltType <<
+ // " of airline " << airline <<
+ // " at airport " << dep->getId() << endl;
+ FGParking *parking = dep->getDynamics()->getParking(gateId);
+ int pushBackNode = parking->getPushBackPoint();
+
+ // initialize the pushback route. Note that parts
+ // of this procedure should probably be done inside
+ // taxidraw. This code is likely to change once this
+ // this is fully implemented in taxidraw. Until that time,
+ // however, the full initialization procedure looks like this:
+ // 1) Build a list of all the nodes that are classified as
+ // pushBack hold points
+ // 2) For each hold point, use the dykstra algorithm to find a route
+ // between the gate and the pushBack hold nodes, however use only
+ // segments that are classified as "pushback" routes.
+ // 3) return the TaxiRoute class that is non empty.
+ // 4) store refer this route in the parking object, for future use
+
+ if (pushBackNode < 0) {
+ //cerr << "Initializing PushBackRoute " << endl;
+ intVec pushBackNodes;
+ int nAINodes = dep->getDynamics()->getGroundNetwork()->getNrOfNodes();
+ int hits = 0;
+ parking->setPushBackPoint(0); // default in case no network was found.
+
+ // Collect all the nodes that are classified as having pushBack hold status
+ for (int i = 0; i < nAINodes; i++) {
+ if (dep->getDynamics()->getGroundNetwork()->findNode(i)->getHoldPointType() == 3) {
+ pushBackNodes.push_back(i);
+ }
+ }
+
+ // For each node found in step 1, check if it can be reached
+ FGTaxiRoute route;
+ for (intVecIterator nodes = pushBackNodes.begin();
+ nodes != pushBackNodes.end();
+ nodes++) {
+ route = dep->getDynamics()->getGroundNetwork()->findShortestRoute(gateId, *nodes, false);
+ if (!(route.empty())) {
+ //cerr << "Found Pushback route of size " << route.size() << endl;
+ hits++;
+ parking->setPushBackRoute(new FGTaxiRoute(route));
+ parking->setPushBackPoint(*nodes);
+ pushBackNode = *nodes;
+ }
+ }
+ if (hits == 0) {
+ SG_LOG(SG_GENERAL, SG_INFO, "No pushback route found for gate " << gateId << " at " << dep->getId());
+ }
+ if (hits > 1) {
+ SG_LOG(SG_GENERAL, SG_WARN, hits << " pushback routes found for gate " << gateId << " at " << dep->getId());
+ }
+ }
+ if (pushBackNode > 0) {
+ int node, rte;
+ //cerr << "Found valid pusback node " << pushBackNode << "for gate " << gateId << endl;
+ pushBackRoute = parking->getPushBackRoute();
+ int size = pushBackRoute->size();
+ if (size < 2) {
+ SG_LOG(SG_GENERAL, SG_WARN, "Push back route from gate " << gateId << " has only " << size << " nodes.");
+ }
+ pushBackRoute->first();
+ waypoint *wpt;
+ while(pushBackRoute->next(&node, &rte))
+ {
+ //FGTaxiNode *tn = apt->getDynamics()->getGroundNetwork()->findSegment(node)->getEnd();
+ char buffer[10];
+ snprintf (buffer, 10, "%d", node);
+ FGTaxiNode *tn = dep->getDynamics()->getGroundNetwork()->findNode(node);
+ //ids.pop_back();
+ wpt = new waypoint;
+ wpt->name = string(buffer); // fixme: should be the name of the taxiway
+ wpt->latitude = tn->getLatitude();
+ wpt->longitude = tn->getLongitude();
+ // Elevation is currently disregarded when on_ground is true
+ // because the AIModel obtains a periodic ground elevation estimate.
+ wpt->altitude = dep->getElevation();
+ wpt->speed = -10;
+ wpt->crossat = -10000;
+ wpt->gear_down = true;
+ wpt->flaps_down= true;
+ wpt->finished = false;
+ wpt->on_ground = true;
+ wpt->routeIndex = rte;
+ waypoints.push_back(wpt);
+ }
+ // some special considerations for the last point:
+ wpt->name = string("PushBackPoint");
+ wpt->speed = 15;
+ } else {
+ //cerr << "Creating direct forward departure route fragment" << endl;
+ double lat2, lon2, az2;
+ waypoint *wpt;
+ geo_direct_wgs_84 ( 0, lat, lon, heading,
+ 2, &lat2, &lon2, &az2 );
+ wpt = new waypoint;
+ wpt->name = "park2";
+ wpt->latitude = lat2;
+ wpt->longitude = lon2;
+ wpt->altitude = dep->getElevation();
+ wpt->speed = 10;
+ wpt->crossat = -10000;
+ wpt->gear_down = true;
+ wpt->flaps_down= true;
+ wpt->finished = false;
+ wpt->on_ground = true;
+ wpt->routeIndex = 0;
+ waypoints.push_back(wpt);
+
+ geo_direct_wgs_84 ( 0, lat, lon, heading,
+ 4, &lat2, &lon2, &az2 );
+ wpt = new waypoint;
+ wpt->name = "name";
+ wpt->latitude = lat2;
+ wpt->longitude = lon2;
+ wpt->altitude = dep->getElevation();
+ wpt->speed = 10;
+ wpt->crossat = -10000;
+ wpt->gear_down = true;
+ wpt->flaps_down= true;
+ wpt->finished = false;
+ wpt->on_ground = true;
+ wpt->routeIndex = 0;
+ waypoints.push_back(wpt);
+
+ //cerr << "Creating final push forward point for gate " << gateId << endl;
+ FGTaxiNode *tn = dep->getDynamics()->getGroundNetwork()->findNode(gateId);
+ FGTaxiSegmentVectorIterator ts = tn->getBeginRoute();
+ FGTaxiSegmentVectorIterator te = tn->getEndRoute();
+ if (ts == te) {
+ SG_LOG(SG_GENERAL, SG_ALERT, "Gate " << gateId << "doesn't seem to have routes associated with it.");
+ //exit(1);
+ }
+ tn = (*ts)->getEnd();
+ lastNodeVisited = tn->getIndex();
+ if (tn == NULL) {
+ SG_LOG(SG_GENERAL, SG_ALERT, "No valid taxinode found");
+ exit(1);
+ }
+ wpt = new waypoint;
+ wpt->name = "PushBackPoint";
+ wpt->latitude = tn->getLatitude();
+ wpt->longitude = tn->getLongitude();
+ wpt->altitude = dep->getElevation();
+ wpt->speed = 10;
+ wpt->crossat = -10000;
+ wpt->gear_down = true;
+ wpt->flaps_down= true;
+ wpt->finished = false;
+ wpt->on_ground = true;
+ wpt->routeIndex = (*ts)->getIndex();
+ waypoints.push_back(wpt);
+
+
+ }
+
+ }
+}
+/*******************************************************************
+ * createPushBackFallBack
+ * This is the backup function for airports that don't have a
+ * network yet.
+ ******************************************************************/
+void FGAIFlightPlan::createPushBackFallBack(bool firstFlight, FGAirport *dep,
+ double latitude,
+ double longitude,
+ double radius,
+ const string& fltType,
+ const string& aircraftType,
+ const string& airline)
+{
+ double heading;
+ double lat;
+ double lon;
+ double lat2;
+ double lon2;
+ double az2;
+
+
+
+ dep->getDynamics()->getParking(-1, &lat, &lon, &heading);
+
+ heading += 180.0;
+ if (heading > 360)
+ heading -= 360;
+ waypoint *wpt = new waypoint;
+ wpt->name = "park";
+ wpt->latitude = lat;
+ wpt->longitude = lon;
+ wpt->altitude = dep->getElevation();
+ wpt->speed = -10;
+ wpt->crossat = -10000;
+ wpt->gear_down = true;
+ wpt->flaps_down= true;
+ wpt->finished = false;
+ wpt->on_ground = true;
+
+ waypoints.push_back(wpt);
+
+ geo_direct_wgs_84 ( 0, lat, lon, heading,
+ 10,
+ &lat2, &lon2, &az2 );
+ wpt = new waypoint;
+ wpt->name = "park2";
+ wpt->latitude = lat2;
+ wpt->longitude = lon2;
+ wpt->altitude = dep->getElevation();
+ wpt->speed = -10;
+ wpt->crossat = -10000;
+ wpt->gear_down = true;
+ wpt->flaps_down= true;
+ wpt->finished = false;
+ wpt->on_ground = true;
+ wpt->routeIndex = 0;
+ waypoints.push_back(wpt);
+ geo_direct_wgs_84 ( 0, lat, lon, heading,
+ 2.2*radius,
+ &lat2, &lon2, &az2 );
+ wpt = new waypoint;
+ wpt->name = "taxiStart";
+ wpt->latitude = lat2;
+ wpt->longitude = lon2;
+ wpt->altitude = dep->getElevation();
+ wpt->speed = 10;
+ wpt->crossat = -10000;
+ wpt->gear_down = true;
+ wpt->flaps_down= true;
+ wpt->finished = false;
+ wpt->on_ground = true;
+ wpt->routeIndex = 0;
+ waypoints.push_back(wpt);
+}