1 /******************************************************************************
2 * AIFlightPlanCreate.cxx
3 * Written by Durk Talsma, started May, 2004.
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., 675 Mass Ave, Cambridge, MA 02139, USA.
20 **************************************************************************/
21 #include "AIFlightPlan.hxx"
22 #include <simgear/math/sg_geodesy.hxx>
23 #include <Airports/runways.hxx>
25 #include <Environment/environment_mgr.hxx>
26 #include <Environment/environment.hxx>
29 /* FGAIFlightPlan::create()
30 * dynamically create a flight plan for AI traffic, based on data provided by the
31 * Traffic Manager, when reading a filed flightplan failes. (DT, 2004/07/10)
33 * This is the top-level function, and the only one that publicly available.
38 // Check lat/lon values during initialization;
39 void FGAIFlightPlan::create(FGAirport *dep, FGAirport *arr, int legNr, double alt, double speed,
40 double latitude, double longitude, bool firstFlight,
41 double radius, string fltType, string aircraftType, string airline)
43 int currWpt = wpt_iterator - waypoints.begin();
47 //cerr << "Creating Push_Back" << endl;
48 createPushBack(firstFlight,dep, latitude, longitude, radius, fltType, aircraftType, airline);
49 //cerr << "Done" << endl;
52 //cerr << "Creating Taxi" << endl;
53 createTaxi(firstFlight, 1, dep, radius, fltType, aircraftType, airline);
56 //cerr << "Creating TAkeoff" << endl;
57 createTakeOff(firstFlight, dep, speed);
60 //cerr << "Creating Climb" << endl;
61 createClimb(firstFlight, dep, speed, alt);
64 //cerr << "Creating Cruise" << endl;
65 createCruise(firstFlight, dep,arr, latitude, longitude, speed, alt);
68 //cerr << "Creating Decent" << endl;
72 //cerr << "Creating Landing" << endl;
76 //cerr << "Creating Taxi 2" << endl;
77 createTaxi(false, 2, arr, radius, fltType, aircraftType, airline);
80 //cerr << "Creating Parking" << endl;
85 cerr << "Unknown case: " << legNr << endl;
87 wpt_iterator = waypoints.begin()+currWpt;
91 /*******************************************************************
93 * initialize the Aircraft at the parking location
94 ******************************************************************/
95 void FGAIFlightPlan::createPushBack(bool firstFlight, FGAirport *dep,
110 //int currWpt = wpt_iterator - waypoints.begin();
111 // Erase all existing waypoints.
114 // We only need to get a valid parking if this is the first leg.
115 // Otherwise use the current aircraft position.
118 if (!(dep->getAvailableParking(&lat, &lon, &heading, &gateId, radius, fltType, aircraftType, airline)))
120 cerr << "Could not find parking " << endl;
125 dep->getParking(gateId, &lat, &lon, &heading);
128 //heading = getHeading();
133 waypoint *wpt = new waypoint;
136 wpt->longitude = lon;
137 wpt->altitude = dep->getElevation();
139 wpt->crossat = -10000;
140 wpt->gear_down = true;
141 wpt->flaps_down= true;
142 wpt->finished = false;
143 wpt->on_ground = true;
144 waypoints.push_back(wpt);
146 // Add park twice, because it uses park once for initialization and once
147 // to trigger the departure ATC message
148 geo_direct_wgs_84 ( 0, lat, lon, heading,
150 &lat2, &lon2, &az2 );
153 wpt->latitude = lat2;
154 wpt->longitude = lon2;
155 wpt->altitude = dep->getElevation();
157 wpt->crossat = -10000;
158 wpt->gear_down = true;
159 wpt->flaps_down= true;
160 wpt->finished = false;
161 wpt->on_ground = true;
162 waypoints.push_back(wpt);
163 geo_direct_wgs_84 ( 0, lat, lon, heading,
165 &lat2, &lon2, &az2 );
167 wpt->name = "taxiStart";
168 wpt->latitude = lat2;
169 wpt->longitude = lon2;
170 wpt->altitude = dep->getElevation();
172 wpt->crossat = -10000;
173 wpt->gear_down = true;
174 wpt->flaps_down= true;
175 wpt->finished = false;
176 wpt->on_ground = true;
177 waypoints.push_back(wpt);
179 //wpt = new waypoint;
181 //wpt->finished = false;
182 //waypoints.push_back(wpt); // add one more to prevent a segfault.
183 //waypoints.push_back(wpt); // add one more to prevent a segfault.
184 //wpt_iterator = waypoints.begin();
187 //wpt_iterator = waypoints.begin()+currWpt;
190 /*******************************************************************
192 * initialize the Aircraft at the parking location
193 ******************************************************************/
194 void FGAIFlightPlan::createTaxi(bool firstFlight, int direction, FGAirport *apt, double radius, string fltType, string acType, string airline)
201 double lat2, lon2, az2;
205 // Erase all existing waypoints.
206 // wpt_vector_iterator i= waypoints.begin();
208 //int currWpt = wpt_iterator - waypoints.begin();
215 // Get the current active runway, based on code from David Luff
216 // This should actually be unified and extended to include
217 // Preferential runway use schema's
219 //stationweather = ((FGEnvironmentMgr *) globals->get_subsystem("environment"))
220 //->getEnvironment(apt->getLatitude(), apt->getLongitude(), apt->getElevation());
222 //wind_speed = stationweather.get_wind_speed_kt();
223 //wind_heading = stationweather.get_wind_from_heading_deg();
224 //if (wind_speed == 0) {
225 //wind_heading = 270; // This forces West-facing rwys to be used in no-wind situations
226 // which is consistent with Flightgear's initial setup.
229 //string rwy_no = globals->get_runways()->search(apt->getId(), int(wind_heading));
231 apt->getActiveRunway("com", 1, &name);
232 if (!(globals->get_runways()->search(apt->getId(),
236 cout << "Failed to find runway for " << apt->getId() << endl;
237 // Hmm, how do we handle a potential error like this?
241 //apt->getActiveRunway(string("com"), 1, &test);
244 heading = rwy._heading;
245 double azimuth = heading + 180.0;
246 while ( azimuth >= 360.0 ) { azimuth -= 360.0; }
247 geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, azimuth,
248 rwy._length * SG_FEET_TO_METER * 0.5 - 5.0,
249 &lat2, &lon2, &az2 );
251 //Add the runway startpoint;
253 wpt->name = "Airport Center";
254 wpt->latitude = apt->getLatitude();
255 wpt->longitude = apt->getLongitude();
256 wpt->altitude = apt->getElevation();
258 wpt->crossat = -10000;
259 wpt->gear_down = true;
260 wpt->flaps_down= true;
261 wpt->finished = false;
262 wpt->on_ground = true;
263 waypoints.push_back(wpt);
265 //Add the runway startpoint;
267 wpt->name = "Runway Takeoff";
268 wpt->latitude = lat2;
269 wpt->longitude = lon2;
270 wpt->altitude = apt->getElevation();
272 wpt->crossat = -10000;
273 wpt->gear_down = true;
274 wpt->flaps_down= true;
275 wpt->finished = false;
276 wpt->on_ground = true;
277 waypoints.push_back(wpt);
278 //wpt = new waypoint;
279 //wpt->finished = false;
280 //waypoints.push_back(wpt); // add one more to prevent a segfault.
284 //direction = (rand() % 360);
285 //geo_direct_wgs_84 ( 0, arr->getLatitude(), arr->getLongitude(), direction,
287 //&lat2, &lon2, &az2 );
289 // This next statement really requires the flight plan to be
290 // split up into smaller sections, because
291 // gate assignments will typically not be known until minutes before
292 // landing, and certainly not at the start of a 10 hour flight.
293 apt->getAvailableParking(&lat, &lon, &heading, &gateId, radius, fltType, acType, airline);
297 geo_direct_wgs_84 ( 0, lat, lon, heading,
299 &lat2, &lon2, &az2 );
300 //Add the runway center
302 wpt->name = "Airport Center";
303 wpt->latitude = apt->getLatitude();
304 wpt->longitude = apt->getLongitude();
305 wpt->altitude = apt->getElevation();
307 wpt->crossat = -10000;
308 wpt->gear_down = true;
309 wpt->flaps_down= true;
310 wpt->finished = false;
311 wpt->on_ground = true;
312 waypoints.push_back(wpt);
314 // Add the final destination waypoint
316 wpt->name = "Begin Parkingg"; //apt->getId(); //wpt_node->getStringValue("name", "END");
317 wpt->latitude = lat2;
318 wpt->longitude = lon2;
319 wpt->altitude = apt->getElevation();
321 wpt->crossat = -10000;
322 wpt->gear_down = true;
323 wpt->flaps_down= true;
324 wpt->finished = false;
325 wpt->on_ground = true;
326 waypoints.push_back(wpt);
330 // wpt_iterator = waypoints.begin();
333 //wpt_iterator = waypoints.begin()+currWpt;
336 /*******************************************************************
338 * initialize the Aircraft at the parking location
339 ******************************************************************/
340 void FGAIFlightPlan::createTakeOff(bool firstFlight, FGAirport *apt, double speed)
347 double lat2, lon2, az2;
352 // Erase all existing waypoints.
353 // wpt_vector_iterator i= waypoints.begin();
354 //while(waypoints.begin() != waypoints.end())
357 // waypoints.erase(i);
362 // Get the current active runway, based on code from David Luff
363 // This should actually be unified and extended to include
364 // Preferential runway use schema's
368 apt->getActiveRunway("com", 1, &name);
369 if (!(globals->get_runways()->search(apt->getId(),
373 cout << "Failed to find runway for " << apt->getId() << endl;
374 // Hmm, how do we handle a potential error like this?
378 //apt->getActiveRunway(string("com"), 1, &test);
382 heading = rwy._heading;
383 double azimuth = heading + 180.0;
384 while ( azimuth >= 360.0 ) { azimuth -= 360.0; }
385 geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, azimuth,
386 rwy._length * SG_FEET_TO_METER * 0.5 - 105.0,
387 &lat2, &lon2, &az2 );
390 wpt->latitude = lat2;
391 wpt->longitude = lon2;
392 wpt->altitude = apt->getElevation();
394 wpt->crossat = -10000;
395 wpt->gear_down = true;
396 wpt->flaps_down= true;
397 wpt->finished = false;
398 wpt->on_ground = true;
399 waypoints.push_back(wpt);
405 //Next: the Start of Climb
406 geo_direct_wgs_84 ( 0, lat, lon, heading,
407 2560 * SG_FEET_TO_METER,
408 &lat2, &lon2, &az2 );
412 wpt->latitude = lat2;
413 wpt->longitude = lon2;
414 wpt->altitude = apt->getElevation()+3000;
416 wpt->crossat = -10000;
417 wpt->gear_down = true;
418 wpt->flaps_down= true;
419 wpt->finished = false;
420 wpt->on_ground = false;
421 waypoints.push_back(wpt);
422 // waypoints.push_back(wpt);
423 //waypoints.push_back(wpt); // add one more to prevent a segfault.
424 // wpt_iterator = waypoints.begin();
429 /*******************************************************************
431 * initialize the Aircraft at the parking location
432 ******************************************************************/
433 void FGAIFlightPlan::createClimb(bool firstFlight, FGAirport *apt, double speed, double alt)
440 double lat2, lon2, az2;
444 // Erase all existing waypoints.
445 // wpt_vector_iterator i= waypoints.begin();
446 //while(waypoints.begin() != waypoints.end())
449 // waypoints.erase(i);
454 // Get the current active runway, based on code from David Luff
455 // This should actually be unified and extended to include
456 // Preferential runway use schema's
460 apt->getActiveRunway("com", 1, &name);
461 if (!(globals->get_runways()->search(apt->getId(),
465 cout << "Failed to find runway for " << apt->getId() << endl;
466 // Hmm, how do we handle a potential error like this?
470 //apt->getActiveRunway(string("com"), 1, &test);
475 heading = rwy._heading;
476 double azimuth = heading + 180.0;
477 while ( azimuth >= 360.0 ) { azimuth -= 360.0; }
478 geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, heading,
480 &lat2, &lon2, &az2 );
482 wpt->name = "10000ft climb";
483 wpt->latitude = lat2;
484 wpt->longitude = lon2;
485 wpt->altitude = 10000;
487 wpt->crossat = -10000;
488 wpt->gear_down = true;
489 wpt->flaps_down= true;
490 wpt->finished = false;
491 wpt->on_ground = false;
492 waypoints.push_back(wpt);
495 geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, heading,
497 &lat2, &lon2, &az2 );
499 wpt->name = "18000ft climb";
500 wpt->latitude = lat2;
501 wpt->longitude = lon2;
502 wpt->altitude = 18000;
504 wpt->crossat = -10000;
505 wpt->gear_down = true;
506 wpt->flaps_down= true;
507 wpt->finished = false;
508 wpt->on_ground = false;
509 waypoints.push_back(wpt);
510 //waypoints.push_back(wpt);
511 //waypoints.push_back(wpt); // add one more to prevent a segfault.
512 // wpt_iterator = waypoints.begin();
518 /*******************************************************************
520 * initialize the Aircraft at the parking location
521 ******************************************************************/
522 void FGAIFlightPlan::createCruise(bool firstFlight, FGAirport *dep, FGAirport *arr, double latitude, double longitude, double speed, double alt)
529 double lat2, lon2, az2;
534 // Erase all existing waypoints.
535 // wpt_vector_iterator i= waypoints.begin();
536 //while(waypoints.begin() != waypoints.end())
539 // waypoints.erase(i);
544 wpt->name = "Cruise"; //wpt_node->getStringValue("name", "END");
545 wpt->latitude = latitude;
546 wpt->longitude = longitude;
549 wpt->crossat = -10000;
550 wpt->gear_down = false;
551 wpt->flaps_down= false;
552 wpt->finished = false;
553 wpt->on_ground = false;
554 waypoints.push_back(wpt);
555 //Beginning of Decent
558 arr->getActiveRunway("com", 2, &name);
559 if (!(globals->get_runways()->search(arr->getId(),
563 cout << "Failed to find runway for " << arr->getId() << endl;
564 // Hmm, how do we handle a potential error like this?
568 //arr->getActiveRunway(string("com"), 1, &test);
571 //cerr << "Altitude = " << alt << endl;
572 //cerr << "Done" << endl;
573 //if (arr->getId() == "EHAM")
575 // cerr << "Creating cruise to EHAM " << latitude << " " << longitude << endl;
577 heading = rwy._heading;
578 azimuth = heading + 180.0;
579 while ( azimuth >= 360.0 ) { azimuth -= 360.0; }
582 // Note: This places us at the location of the active
583 // runway during initial cruise. This needs to be
585 geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, azimuth,
587 &lat2, &lon2, &az2 );
589 wpt->name = "BOD"; //wpt_node->getStringValue("name", "END");
590 wpt->latitude = lat2;
591 wpt->longitude = lon2;
595 wpt->gear_down = false;
596 wpt->flaps_down= false;
597 wpt->finished = false;
598 wpt->on_ground = false;
599 waypoints.push_back(wpt);
600 //waypoints.push_back(wpt);
601 //waypoints.push_back(wpt); // add one more to prevent a segfault.
602 //wpt_iterator = waypoints.begin();
607 /*******************************************************************
609 * initialize the Aircraft at the parking location
610 ******************************************************************/
611 void FGAIFlightPlan::createDecent(FGAirport *apt)
614 // Ten thousand ft. Slowing down to 240 kts
620 double lat2, lon2, az2;
625 //// Erase all existing waypoints.
626 // wpt_vector_iterator i= waypoints.begin();
627 //while(waypoints.begin() != waypoints.end())
630 // waypoints.erase(i);
634 //Beginning of Decent
636 apt->getActiveRunway("com", 2, &name);
637 if (!(globals->get_runways()->search(apt->getId(),
641 cout << "Failed to find runway for " << apt->getId() << endl;
642 // Hmm, how do we handle a potential error like this?
646 //apt->getActiveRunway(string("com"), 1, &test);
649 //cerr << "Done" << endl;
650 heading = rwy._heading;
651 azimuth = heading + 180.0;
652 while ( azimuth >= 360.0 ) { azimuth -= 360.0; }
656 geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, azimuth,
658 &lat2, &lon2, &az2 );
661 wpt->name = "Dec 10000ft"; //wpt_node->getStringValue("name", "END");
662 wpt->latitude = lat2;
663 wpt->longitude = lon2;
664 wpt->altitude = apt->getElevation();
666 wpt->crossat = 10000;
667 wpt->gear_down = false;
668 wpt->flaps_down= false;
669 wpt->finished = false;
670 wpt->on_ground = false;
671 waypoints.push_back(wpt);
673 // Three thousand ft. Slowing down to 160 kts
674 geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, azimuth,
676 &lat2, &lon2, &az2 );
678 wpt->name = "DEC 3000ft"; //wpt_node->getStringValue("name", "END");
679 wpt->latitude = lat2;
680 wpt->longitude = lon2;
681 wpt->altitude = apt->getElevation();
684 wpt->gear_down = true;
685 wpt->flaps_down= true;
686 wpt->finished = false;
687 wpt->on_ground = false;
688 waypoints.push_back(wpt);
689 //waypoints.push_back(wpt);
690 //waypoints.push_back(wpt); // add one more to prevent a segfault.
691 //wpt_iterator = waypoints.begin();
693 //if (apt->getId() == "EHAM")
695 // cerr << "Created Decend to EHAM " << lat2 << " " << lon2 << ": Runway = " << rwy._rwy_no
696 // << "heading " << heading << endl;
699 /*******************************************************************
701 * initialize the Aircraft at the parking location
702 ******************************************************************/
703 void FGAIFlightPlan::createLanding(FGAirport *apt)
705 // Ten thousand ft. Slowing down to 240 kts
711 double lat2, lon2, az2;
717 heading = rwy._heading;
718 azimuth = heading + 180.0;
719 while ( azimuth >= 360.0 ) { azimuth -= 360.0; }
722 geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, azimuth,
723 rwy._length*0.45 * SG_FEET_TO_METER,
724 &lat2, &lon2, &az2 );
726 wpt->name = "Threshold"; //wpt_node->getStringValue("name", "END");
727 wpt->latitude = lat2;
728 wpt->longitude = lon2;
729 wpt->altitude = apt->getElevation();
731 wpt->crossat = apt->getElevation();
732 wpt->gear_down = true;
733 wpt->flaps_down= true;
734 wpt->finished = false;
735 wpt->on_ground = true;
736 waypoints.push_back(wpt);
738 //Full stop at the runway centerpoint
739 geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, azimuth,
741 &lat2, &lon2, &az2 );
743 wpt->name = "Center"; //wpt_node->getStringValue("name", "END");
744 wpt->latitude = rwy._lat;
745 wpt->longitude = rwy._lon;
746 wpt->altitude = apt->getElevation();
748 wpt->crossat = -10000;
749 wpt->gear_down = true;
750 wpt->flaps_down= true;
751 wpt->finished = false;
752 wpt->on_ground = true;
753 waypoints.push_back(wpt);
755 geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, heading,
756 rwy._length*0.45 * SG_FEET_TO_METER,
757 &lat2, &lon2, &az2 );
759 wpt->name = "Threshold"; //wpt_node->getStringValue("name", "END");
760 wpt->latitude = lat2;
761 wpt->longitude = lon2;
762 wpt->altitude = apt->getElevation();
764 wpt->crossat = apt->getElevation();
765 wpt->gear_down = true;
766 wpt->flaps_down= true;
767 wpt->finished = false;
768 wpt->on_ground = true;
769 waypoints.push_back(wpt);
770 //waypoints.push_back(wpt);
771 //waypoints.push_back(wpt); // add one more to prevent a segfault.
772 //wpt_iterator = waypoints.begin();
775 //if (apt->getId() == "EHAM")
777 // cerr << "Created Landing to EHAM " << lat2 << " " << lon2 << ": Runway = " << rwy._rwy_no
778 // << "heading " << heading << endl;
782 /*******************************************************************
784 * initialize the Aircraft at the parking location
785 ******************************************************************/
786 void FGAIFlightPlan::createParking(FGAirport *apt)
792 apt->getParking(gateId, &lat, &lon, &heading);
797 // Erase all existing waypoints.
798 // wpt_vector_iterator i= waypoints.begin();
799 //while(waypoints.begin() != waypoints.end())
802 // waypoints.erase(i);
805 // And finally one more named "END"
807 wpt->name = "END"; //wpt_node->getStringValue("name", "END");
809 wpt->longitude = lon;
812 wpt->crossat = -10000;
813 wpt->gear_down = true;
814 wpt->flaps_down= true;
815 wpt->finished = false;
816 wpt->on_ground = true;
817 waypoints.push_back(wpt);
818 //waypoints.push_back(wpt);
819 //waypoints.push_back(wpt); // add one more to prevent a segfault.
820 //wpt_iterator = waypoints.begin();