1 /******************************************************************************
2 * AIFlightPlanCreatePushBack.cxx
3 * Written by Durk Talsma, started August 1, 2007.
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.
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.
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.
19 **************************************************************************/
20 #include "AIFlightPlan.hxx"
21 #include <simgear/math/sg_geodesy.hxx>
22 #include <Airports/runways.hxx>
24 #include <Environment/environment_mgr.hxx>
25 #include <Environment/environment.hxx>
28 void FGAIFlightPlan::createPushBack(bool firstFlight, FGAirport *dep,
32 const string& fltType,
33 const string& aircraftType,
34 const string& airline)
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);
44 if (!(dep->getDynamics()->getAvailableParking(&lat, &lon,
47 aircraftType, airline))) {
48 SG_LOG(SG_INPUT, SG_WARN, "Warning: Could not find parking for a " <<
50 " of flight type " << fltType <<
51 " of airline " << airline <<
52 " at airport " << dep->getId());
54 snprintf (buffer, 10, "%d", gateId);
55 //FGTaxiNode *tn = dep->getDynamics()->getGroundNetwork()->findNode(node);
58 wpt->name = string(buffer); // fixme: should be the name of the taxiway
61 // Elevation is currently disregarded when on_ground is true
62 // because the AIModel obtains a periodic ground elevation estimate.
63 wpt->altitude = dep->getElevation();
65 wpt->crossat = -10000;
66 wpt->gear_down = true;
67 wpt->flaps_down= true;
68 wpt->finished = false;
69 wpt->on_ground = true;
71 waypoints.push_back(wpt);
74 //cerr << "Push Back follow-up Flight" << endl;
75 dep->getDynamics()->getParking(gateId, &lat, &lon, &heading);
78 createPushBackFallBack(firstFlight, dep, latitude, longitude,
79 radius, fltType, aircraftType, airline);
83 //cerr << "getting parking " << gateId;
84 //cerr << " for a " <<
86 // " of flight type " << fltType <<
87 // " of airline " << airline <<
88 // " at airport " << dep->getId() << endl;
89 FGParking *parking = dep->getDynamics()->getParking(gateId);
90 int pushBackNode = parking->getPushBackPoint();
92 // initialize the pushback route. Note that parts
93 // of this procedure should probably be done inside
94 // taxidraw. This code is likely to change once this
95 // this is fully implemented in taxidraw. Until that time,
96 // however, the full initialization procedure looks like this:
97 // 1) Build a list of all the nodes that are classified as
98 // pushBack hold points
99 // 2) For each hold point, use the dykstra algorithm to find a route
100 // between the gate and the pushBack hold nodes, however use only
101 // segments that are classified as "pushback" routes.
102 // 3) return the TaxiRoute class that is non empty.
103 // 4) store refer this route in the parking object, for future use
105 if (pushBackNode < 0) {
106 //cerr << "Initializing PushBackRoute " << endl;
107 intVec pushBackNodes;
108 int nAINodes = dep->getDynamics()->getGroundNetwork()->getNrOfNodes();
110 parking->setPushBackPoint(0); // default in case no network was found.
112 // Collect all the nodes that are classified as having pushBack hold status
113 for (int i = 0; i < nAINodes; i++) {
114 if (dep->getDynamics()->getGroundNetwork()->findNode(i)->getHoldPointType() == 3) {
115 pushBackNodes.push_back(i);
119 // For each node found in step 1, check if it can be reached
121 for (intVecIterator nodes = pushBackNodes.begin();
122 nodes != pushBackNodes.end();
124 route = dep->getDynamics()->getGroundNetwork()->findShortestRoute(gateId, *nodes, false);
125 if (!(route.empty())) {
126 //cerr << "Found Pushback route of size " << route.size() << endl;
128 parking->setPushBackRoute(new FGTaxiRoute(route));
129 parking->setPushBackPoint(*nodes);
130 pushBackNode = *nodes;
134 SG_LOG(SG_GENERAL, SG_INFO, "No pushback route found for gate " << gateId << " at " << dep->getId());
137 SG_LOG(SG_GENERAL, SG_WARN, hits << " pushback routes found for gate " << gateId << " at " << dep->getId());
140 if (pushBackNode > 0) {
142 //cerr << "Found valid pusback node " << pushBackNode << "for gate " << gateId << endl;
143 pushBackRoute = parking->getPushBackRoute();
144 int size = pushBackRoute->size();
146 SG_LOG(SG_GENERAL, SG_WARN, "Push back route from gate " << gateId << " has only " << size << " nodes.");
148 pushBackRoute->first();
150 while(pushBackRoute->next(&node, &rte))
152 //FGTaxiNode *tn = apt->getDynamics()->getGroundNetwork()->findSegment(node)->getEnd();
154 snprintf (buffer, 10, "%d", node);
155 FGTaxiNode *tn = dep->getDynamics()->getGroundNetwork()->findNode(node);
158 wpt->name = string(buffer); // fixme: should be the name of the taxiway
159 wpt->latitude = tn->getLatitude();
160 wpt->longitude = tn->getLongitude();
161 // Elevation is currently disregarded when on_ground is true
162 // because the AIModel obtains a periodic ground elevation estimate.
163 wpt->altitude = dep->getElevation();
165 wpt->crossat = -10000;
166 wpt->gear_down = true;
167 wpt->flaps_down= true;
168 wpt->finished = false;
169 wpt->on_ground = true;
170 wpt->routeIndex = rte;
171 waypoints.push_back(wpt);
173 // some special considerations for the last point:
174 wpt->name = string("PushBackPoint");
177 //cerr << "Creating direct forward departure route fragment" << endl;
178 double lat2, lon2, az2;
180 geo_direct_wgs_84 ( 0, lat, lon, heading,
181 2, &lat2, &lon2, &az2 );
184 wpt->latitude = lat2;
185 wpt->longitude = lon2;
186 wpt->altitude = dep->getElevation();
188 wpt->crossat = -10000;
189 wpt->gear_down = true;
190 wpt->flaps_down= true;
191 wpt->finished = false;
192 wpt->on_ground = true;
194 waypoints.push_back(wpt);
196 geo_direct_wgs_84 ( 0, lat, lon, heading,
197 4, &lat2, &lon2, &az2 );
200 wpt->latitude = lat2;
201 wpt->longitude = lon2;
202 wpt->altitude = dep->getElevation();
204 wpt->crossat = -10000;
205 wpt->gear_down = true;
206 wpt->flaps_down= true;
207 wpt->finished = false;
208 wpt->on_ground = true;
210 waypoints.push_back(wpt);
212 //cerr << "Creating final push forward point for gate " << gateId << endl;
213 FGTaxiNode *tn = dep->getDynamics()->getGroundNetwork()->findNode(gateId);
214 FGTaxiSegmentVectorIterator ts = tn->getBeginRoute();
215 FGTaxiSegmentVectorIterator te = tn->getEndRoute();
217 SG_LOG(SG_GENERAL, SG_ALERT, "Gate " << gateId << "doesn't seem to have routes associated with it.");
220 tn = (*ts)->getEnd();
221 lastNodeVisited = tn->getIndex();
223 SG_LOG(SG_GENERAL, SG_ALERT, "No valid taxinode found");
227 wpt->name = "PushBackPoint";
228 wpt->latitude = tn->getLatitude();
229 wpt->longitude = tn->getLongitude();
230 wpt->altitude = dep->getElevation();
232 wpt->crossat = -10000;
233 wpt->gear_down = true;
234 wpt->flaps_down= true;
235 wpt->finished = false;
236 wpt->on_ground = true;
237 wpt->routeIndex = (*ts)->getIndex();
238 waypoints.push_back(wpt);
245 /*******************************************************************
246 * createPushBackFallBack
247 * This is the backup function for airports that don't have a
249 ******************************************************************/
250 void FGAIFlightPlan::createPushBackFallBack(bool firstFlight, FGAirport *dep,
254 const string& fltType,
255 const string& aircraftType,
256 const string& airline)
267 dep->getDynamics()->getParking(-1, &lat, &lon, &heading);
272 waypoint *wpt = new waypoint;
275 wpt->longitude = lon;
276 wpt->altitude = dep->getElevation();
278 wpt->crossat = -10000;
279 wpt->gear_down = true;
280 wpt->flaps_down= true;
281 wpt->finished = false;
282 wpt->on_ground = true;
284 waypoints.push_back(wpt);
286 geo_direct_wgs_84 ( 0, lat, lon, heading,
288 &lat2, &lon2, &az2 );
291 wpt->latitude = lat2;
292 wpt->longitude = lon2;
293 wpt->altitude = dep->getElevation();
295 wpt->crossat = -10000;
296 wpt->gear_down = true;
297 wpt->flaps_down= true;
298 wpt->finished = false;
299 wpt->on_ground = true;
301 waypoints.push_back(wpt);
302 geo_direct_wgs_84 ( 0, lat, lon, heading,
304 &lat2, &lon2, &az2 );
306 wpt->name = "taxiStart";
307 wpt->latitude = lat2;
308 wpt->longitude = lon2;
309 wpt->altitude = dep->getElevation();
311 wpt->crossat = -10000;
312 wpt->gear_down = true;
313 wpt->flaps_down= true;
314 wpt->finished = false;
315 wpt->on_ground = true;
317 waypoints.push_back(wpt);