X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FAIModel%2FAIFlightPlanCreate.cxx;h=ccb677bb80c60e05b0d1f6316674defc201e6180;hb=666910a7937712d02e62a38a83ced564f6227f52;hp=f5455effe64f5c84aa6b0efcbdfbff69e4b9ce7f;hpb=02a757f9e670e8036169145ff289b24081f703e8;p=flightgear.git diff --git a/src/AIModel/AIFlightPlanCreate.cxx b/src/AIModel/AIFlightPlanCreate.cxx index f5455effe..ccb677bb8 100644 --- a/src/AIModel/AIFlightPlanCreate.cxx +++ b/src/AIModel/AIFlightPlanCreate.cxx @@ -14,8 +14,7 @@ * * 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., 675 Mass Ave, Cambridge, MA 02139, USA. - * + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. * **************************************************************************/ #include "AIFlightPlan.hxx" @@ -30,59 +29,55 @@ * dynamically create a flight plan for AI traffic, based on data provided by the * Traffic Manager, when reading a filed flightplan failes. (DT, 2004/07/10) * - * This is the top-level function, and the only one that publicly available. + * This is the top-level function, and the only one that is publicly available. * */ // Check lat/lon values during initialization; -void FGAIFlightPlan::create(FGAirport *dep, FGAirport *arr, int legNr, double alt, double speed, - double latitude, double longitude, bool firstFlight, - double radius, string fltType, string aircraftType, string airline) +void FGAIFlightPlan::create(FGAirport *dep, FGAirport *arr, int legNr, + double alt, double speed, double latitude, + double longitude, bool firstFlight,double radius, + const string& fltType, const string& aircraftType, + const string& airline) { int currWpt = wpt_iterator - waypoints.begin(); switch(legNr) { - case 1: - //cerr << "Creating Push_Back" << endl; - createPushBack(firstFlight,dep, latitude, longitude, radius, fltType, aircraftType, airline); - //cerr << "Done" << endl; + case 1: + createPushBack(firstFlight,dep, latitude, longitude, + radius, fltType, aircraftType, airline); break; case 2: - //cerr << "Creating Taxi" << endl; - createTaxi(firstFlight, 1, dep, radius, fltType, aircraftType, airline); + createTaxi(firstFlight, 1, dep, latitude, longitude, + radius, fltType, aircraftType, airline); break; case 3: - //cerr << "Creating TAkeoff" << endl; createTakeOff(firstFlight, dep, speed); break; case 4: - //cerr << "Creating Climb" << endl; createClimb(firstFlight, dep, speed, alt); break; case 5: - //cerr << "Creating Cruise" << endl; createCruise(firstFlight, dep,arr, latitude, longitude, speed, alt); break; case 6: - //cerr << "Creating Decent" << endl; createDecent(arr); break; case 7: - //cerr << "Creating Landing" << endl; createLanding(arr); break; case 8: - //cerr << "Creating Taxi 2" << endl; - createTaxi(false, 2, arr, radius, fltType, aircraftType, airline); + createTaxi(false, 2, arr, latitude, longitude, radius, + fltType, aircraftType, airline); break; - case 9: - //cerr << "Creating Parking" << endl; - createParking(arr); + case 9: + createParking(arr, radius); break; default: //exit(1); - cerr << "Unknown case: " << legNr << endl; + SG_LOG(SG_INPUT, SG_ALERT, "AIFlightPlan::create() attempting to create unknown leg" + " this is probably an internal program error"); } wpt_iterator = waypoints.begin()+currWpt; leg++; @@ -93,12 +88,12 @@ void FGAIFlightPlan::create(FGAirport *dep, FGAirport *arr, int legNr, double al * initialize the Aircraft at the parking location ******************************************************************/ void FGAIFlightPlan::createPushBack(bool firstFlight, FGAirport *dep, - double latitude, - double longitude, - double radius, - string fltType, - string aircraftType, - string airline) + double latitude, + double longitude, + double radius, + const string& fltType, + const string& aircraftType, + const string& airline) { double heading; double lat; @@ -115,17 +110,21 @@ void FGAIFlightPlan::createPushBack(bool firstFlight, FGAirport *dep, // Otherwise use the current aircraft position. if (firstFlight) { - if (!(dep->getAvailableParking(&lat, &lon, &heading, &gateId, radius, fltType, aircraftType, airline))) - { - cerr << "Could not find parking " << endl; - } + if (!(dep->getDynamics()->getAvailableParking(&lat, &lon, + &heading, &gateId, + radius, fltType, + aircraftType, airline))) + { + SG_LOG(SG_INPUT, SG_WARN, "Could not find parking for a " << + aircraftType << + " of flight type " << fltType << + " of airline " << airline << + " at airport " << dep->getId()); + } } else { - dep->getParking(gateId, &lat, &lon, &heading); - //lat = latitude; - //lon = longitude; - //heading = getHeading(); + dep->getDynamics()->getParking(gateId, &lat, &lon, &heading); } heading += 180.0; if (heading > 360) @@ -141,13 +140,12 @@ void FGAIFlightPlan::createPushBack(bool firstFlight, FGAirport *dep, wpt->flaps_down= true; wpt->finished = false; wpt->on_ground = true; + waypoints.push_back(wpt); - // Add park twice, because it uses park once for initialization and once - // to trigger the departure ATC message geo_direct_wgs_84 ( 0, lat, lon, heading, - 10, - &lat2, &lon2, &az2 ); + 10, + &lat2, &lon2, &az2 ); wpt = new waypoint; wpt->name = "park2"; wpt->latitude = lat2; @@ -159,10 +157,11 @@ void FGAIFlightPlan::createPushBack(bool firstFlight, FGAirport *dep, 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, - 100, - &lat2, &lon2, &az2 ); + 2.2*radius, + &lat2, &lon2, &az2 ); wpt = new waypoint; wpt->name = "taxiStart"; wpt->latitude = lat2; @@ -174,163 +173,398 @@ void FGAIFlightPlan::createPushBack(bool firstFlight, FGAirport *dep, wpt->flaps_down= true; wpt->finished = false; wpt->on_ground = true; - waypoints.push_back(wpt); - - //wpt = new waypoint; - //wpt->name = "END"; - //wpt->finished = false; - //waypoints.push_back(wpt); // add one more to prevent a segfault. - //waypoints.push_back(wpt); // add one more to prevent a segfault. - //wpt_iterator = waypoints.begin(); - //if (!firstFlight) - // wpt_iterator++; - //wpt_iterator = waypoints.begin()+currWpt; + wpt->routeIndex = 0; + waypoints.push_back(wpt); } /******************************************************************* * createCreate Taxi. * initialize the Aircraft at the parking location ******************************************************************/ -void FGAIFlightPlan::createTaxi(bool firstFlight, int direction, FGAirport *apt, double radius, string fltType, string acType, string airline) +void FGAIFlightPlan::createTaxi(bool firstFlight, int direction, + FGAirport *apt, double latitude, double longitude, + double radius, const string& fltType, + const string& acType, const string& airline) { - double wind_speed; - double wind_heading; double heading; - //FGRunway rwy; - double lat, lon, az; + double lat, lon; double lat2, lon2, az2; - //int direction; waypoint *wpt; - // Erase all existing waypoints. - // wpt_vector_iterator i= waypoints.begin(); - //resetWaypoints(); - //int currWpt = wpt_iterator - waypoints.begin(); - if (direction == 1) - { - - + int nrWaypointsToSkip; - - // Get the current active runway, based on code from David Luff - // This should actually be unified and extended to include - // Preferential runway use schema's - //FGEnvironment - //stationweather = ((FGEnvironmentMgr *) globals->get_subsystem("environment")) - //->getEnvironment(apt->getLatitude(), apt->getLongitude(), apt->getElevation()); - - //wind_speed = stationweather.get_wind_speed_kt(); - //wind_heading = stationweather.get_wind_from_heading_deg(); - //if (wind_speed == 0) { - //wind_heading = 270; // This forces West-facing rwys to be used in no-wind situations - // which is consistent with Flightgear's initial setup. - //} - - //string rwy_no = globals->get_runways()->search(apt->getId(), int(wind_heading)); - string name; - apt->getActiveRunway("com", 1, &name); + if (direction == 1) + { + // If this function is called during initialization, + // make sure we obtain a valid gate ID first + // and place the model at the location of the gate. + if (firstFlight) + { + if (!(apt->getDynamics()->getAvailableParking(&lat, &lon, + &heading, &gateId, + radius, fltType, + acType, airline))) + { + SG_LOG(SG_INPUT, SG_WARN, "Could not find parking for a " << + acType << + " of flight type " << fltType << + " of airline " << airline << + " at airport " << apt->getId()); + } + //waypoint *wpt = new waypoint; + //wpt->name = "park"; + //wpt->latitude = lat; + //wpt->longitude = lon; + //wpt->altitude = apt->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); + } + // "NOTE: this is currently fixed to "com" for commercial traffic + // Should be changed to be used dynamically to allow "gen" and "mil" + // as well + apt->getDynamics()->getActiveRunway("com", 1, activeRunway); if (!(globals->get_runways()->search(apt->getId(), - name, + activeRunway, &rwy))) { - cout << "Failed to find runway for " << apt->getId() << endl; - // Hmm, how do we handle a potential error like this? - exit(1); - } - //string test; - //apt->getActiveRunway(string("com"), 1, &test); - //exit(1); - + SG_LOG(SG_INPUT, SG_ALERT, "Failed to find runway " << + activeRunway << + " at airport " << apt->getId()); + exit(1); + } + + // Determine the beginning of he runway heading = rwy._heading; double azimuth = heading + 180.0; while ( azimuth >= 360.0 ) { azimuth -= 360.0; } geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, azimuth, rwy._length * SG_FEET_TO_METER * 0.5 - 5.0, &lat2, &lon2, &az2 ); - - //Add the runway startpoint; - wpt = new waypoint; - wpt->name = "Airport Center"; - wpt->latitude = apt->getLatitude(); - wpt->longitude = apt->getLongitude(); - wpt->altitude = apt->getElevation(); - wpt->speed = 15; - wpt->crossat = -10000; - wpt->gear_down = true; - wpt->flaps_down= true; - wpt->finished = false; - wpt->on_ground = true; - waypoints.push_back(wpt); - - //Add the runway startpoint; - wpt = new waypoint; - wpt->name = "Runway Takeoff"; - wpt->latitude = lat2; - wpt->longitude = lon2; - wpt->altitude = apt->getElevation(); - wpt->speed = 15; - wpt->crossat = -10000; - wpt->gear_down = true; - wpt->flaps_down= true; - wpt->finished = false; - wpt->on_ground = true; - waypoints.push_back(wpt); - //wpt = new waypoint; - //wpt->finished = false; - //waypoints.push_back(wpt); // add one more to prevent a segfault. + + if (apt->getDynamics()->getGroundNetwork()->exists()) + { + intVec ids; + int runwayId = apt->getDynamics()->getGroundNetwork()->findNearestNode(lat2, + lon2); + + + // A negative gateId indicates an overflow parking, use a + // fallback mechanism for this. + // Starting from gate 0 in this case is a bit of a hack + // which requires a more proper solution later on. + //FGTaxiRoute route; + if (taxiRoute) + delete taxiRoute; + taxiRoute = new FGTaxiRoute; + if (gateId >= 0) + *taxiRoute = apt->getDynamics()->getGroundNetwork()->findShortestRoute(gateId, + runwayId); + else + *taxiRoute = apt->getDynamics()->getGroundNetwork()->findShortestRoute(0, runwayId); + intVecIterator i; + + if (taxiRoute->empty()) { + //Add the runway startpoint; + wpt = new waypoint; + wpt->name = "Airport Center"; + wpt->latitude = latitude; + wpt->longitude = longitude; + wpt->altitude = apt->getElevation(); + wpt->speed = 15; + 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); + + //Add the runway startpoint; + wpt = new waypoint; + wpt->name = "Runway Takeoff"; + wpt->latitude = lat2; + wpt->longitude = lon2; + wpt->altitude = apt->getElevation(); + wpt->speed = 15; + 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); + } else { + int node; + taxiRoute->first(); + bool isPushBackPoint = false; + if (firstFlight) { + // If this is called during initialization, randomly + // skip a number of waypoints to get a more realistic + // taxi situation. + isPushBackPoint = true; + int nrWaypoints = taxiRoute->size(); + nrWaypointsToSkip = rand() % nrWaypoints; + // but make sure we always keep two active waypoints + // to prevent a segmentation fault + for (int i = 0; i < nrWaypointsToSkip-2; i++) { + isPushBackPoint = false; + taxiRoute->next(&node); + } + } else { + //chop off the first two waypoints, because + // those have already been created + // by create pushback + int size = taxiRoute->size(); + if (size > 2) { + taxiRoute->next(&node); + taxiRoute->next(&node); + } + } + int route; + while(taxiRoute->next(&node, &route)) + { + //FGTaxiNode *tn = apt->getDynamics()->getGroundNetwork()->findSegment(node)->getEnd(); + char buffer[10]; + snprintf (buffer, 10, "%d", node); + FGTaxiNode *tn = apt->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 = apt->getElevation(); + if (isPushBackPoint) { + wpt->speed = -10; + isPushBackPoint = false; + } + else { + wpt->speed = 15; + } + wpt->crossat = -10000; + wpt->gear_down = true; + wpt->flaps_down= true; + wpt->finished = false; + wpt->on_ground = true; + wpt->routeIndex = route; + waypoints.push_back(wpt); + } + //cerr << endl; + // finally, rewind the taxiRoute object to the point where we started + // generating the Flightplan, for AI use. + // This is a bit tricky, because the + taxiRoute->first(); + if (firstFlight) { + for (int i = 0; i < nrWaypointsToSkip-1; i++) { + taxiRoute->next(&node); + } + } else { + int size = taxiRoute->size(); + if (size > 2) { + //taxiRoute->next(&node); + //taxiRoute->next(&node); + //taxiRoute->next(&node); + } + } + } // taxiRoute not empty + } + else + { + // This is the fallback mechanism, in case no ground network is available + //Add the runway startpoint; + wpt = new waypoint; + wpt->name = "Airport Center"; + wpt->latitude = apt->getLatitude(); + wpt->longitude = apt->getLongitude(); + wpt->altitude = apt->getElevation(); + wpt->speed = 15; + 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); + + //Add the runway startpoint; + wpt = new waypoint; + wpt->name = "Runway Takeoff"; + wpt->latitude = lat2; + wpt->longitude = lon2; + wpt->altitude = apt->getElevation(); + wpt->speed = 15; + 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); + } } - else + else // Landing taxi { - //direction = (rand() % 360); - //geo_direct_wgs_84 ( 0, arr->getLatitude(), arr->getLongitude(), direction, - //100, - //&lat2, &lon2, &az2 ); + apt->getDynamics()->getAvailableParking(&lat, &lon, &heading, + &gateId, radius, fltType, + acType, airline); + + double lat3 = (*(waypoints.end()-1))->latitude; + double lon3 = (*(waypoints.end()-1))->longitude; + //cerr << (*(waypoints.end()-1))->name << endl; - // This next statement really requires the flight plan to be - // split up into smaller sections, because - // gate assignments will typically not be known until minutes before - // landing, and certainly not at the start of a 10 hour flight. - apt->getAvailableParking(&lat, &lon, &heading, &gateId, radius, fltType, acType, airline); - heading += 180.0; - if (heading > 360) - heading -= 360; - geo_direct_wgs_84 ( 0, lat, lon, heading, - 100, - &lat2, &lon2, &az2 ); - //Add the runway center - wpt = new waypoint; - wpt->name = "Airport Center"; - wpt->latitude = apt->getLatitude(); - wpt->longitude = apt->getLongitude(); - wpt->altitude = apt->getElevation(); - wpt->speed = 15; - wpt->crossat = -10000; - wpt->gear_down = true; - wpt->flaps_down= true; - wpt->finished = false; - wpt->on_ground = true; - waypoints.push_back(wpt); - - // Add the final destination waypoint - wpt = new waypoint; - wpt->name = "Begin Parkingg"; //apt->getId(); //wpt_node->getStringValue("name", "END"); - wpt->latitude = lat2; - wpt->longitude = lon2; - wpt->altitude = apt->getElevation(); - wpt->speed = 15; - wpt->crossat = -10000; - wpt->gear_down = true; - wpt->flaps_down= true; - wpt->finished = false; - wpt->on_ground = true; - waypoints.push_back(wpt); + // Find a route from runway end to parking/gate. + if (apt->getDynamics()->getGroundNetwork()->exists()) + { + intVec ids; + int runwayId = apt->getDynamics()->getGroundNetwork()->findNearestNode(lat3, + lon3); + // A negative gateId indicates an overflow parking, use a + // fallback mechanism for this. + // Starting from gate 0 is a bit of a hack... + //FGTaxiRoute route; + if (taxiRoute) + delete taxiRoute; + taxiRoute = new FGTaxiRoute; + if (gateId >= 0) + *taxiRoute = apt->getDynamics()->getGroundNetwork()->findShortestRoute(runwayId, + gateId); + else + *taxiRoute = apt->getDynamics()->getGroundNetwork()->findShortestRoute(runwayId, 0); + intVecIterator i; + + // No route found: go from gate directly to runway + if (taxiRoute->empty()) { + //Add the runway startpoint; + wpt = new waypoint; + wpt->name = "Airport Center"; + wpt->latitude = latitude; + wpt->longitude = longitude; + wpt->altitude = apt->getElevation(); + wpt->speed = 15; + 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); + + //Add the runway startpoint; + wpt = new waypoint; + wpt->name = "Runway Takeoff"; + wpt->latitude = lat3; + wpt->longitude = lon3; + wpt->altitude = apt->getElevation(); + wpt->speed = 15; + 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); + } else { + int node; + taxiRoute->first(); + int size = taxiRoute->size(); + // Omit the last two waypoints, as + // those are created by createParking() + int route; + for (int i = 0; i < size-2; i++) + { + taxiRoute->next(&node, &route); + char buffer[10]; + snprintf (buffer, 10, "%d", node); + //FGTaxiNode *tn = apt->getDynamics()->getGroundNetwork()->findNode(node); + FGTaxiNode *tn = apt->getDynamics()->getGroundNetwork()->findNode(node); + wpt = new waypoint; + //wpt->name = "taxiway"; // fixme: should be the name of the taxiway + wpt->name = string(buffer);// fixme: should be the name of the taxiway + wpt->latitude = tn->getLatitude(); + wpt->longitude = tn->getLongitude(); + wpt->altitude = apt->getElevation(); + wpt->speed = 15; + wpt->crossat = -10000; + wpt->gear_down = true; + wpt->flaps_down= true; + wpt->finished = false; + wpt->on_ground = true; + wpt->routeIndex = route; + waypoints.push_back(wpt); + } + //taxiRoute->first(); + //taxiRoute->next(&node); + } + } + else + { + // Use a fallback mechanism in case no ground network is available + // obtain the location of the gate entrance point + heading += 180.0; + if (heading > 360) + heading -= 360; + geo_direct_wgs_84 ( 0, lat, lon, heading, + 100, + &lat2, &lon2, &az2 ); + wpt = new waypoint; + wpt->name = "Airport Center"; + wpt->latitude = apt->getLatitude(); + wpt->longitude = apt->getLongitude(); + wpt->altitude = apt->getElevation(); + wpt->speed = 15; + 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); + + wpt = new waypoint; + wpt->name = "Begin Parking"; //apt->getId(); //wpt_node->getStringValue("name", "END"); + wpt->latitude = lat2; + wpt->longitude = lon2; + wpt->altitude = apt->getElevation(); + wpt->speed = 15; + 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); - + //waypoint* wpt; + //double lat; + //double lon; + //double heading; + apt->getDynamics()->getParking(gateId, &lat, &lon, &heading); + heading += 180.0; + if (heading > 360) + heading -= 360; + + wpt = new waypoint; + wpt->name = "END"; //wpt_node->getStringValue("name", "END"); + wpt->latitude = lat; + wpt->longitude = lon; + wpt->altitude = 19; + wpt->speed = 15; + 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); + } + } - // wpt_iterator = waypoints.begin(); - //if (!firstFlight) - // wpt_iterator++; - //wpt_iterator = waypoints.begin()+currWpt; } /******************************************************************* @@ -339,46 +573,32 @@ void FGAIFlightPlan::createTaxi(bool firstFlight, int direction, FGAirport *apt, ******************************************************************/ void FGAIFlightPlan::createTakeOff(bool firstFlight, FGAirport *apt, double speed) { - double wind_speed; - double wind_heading; double heading; - //FGRunway rwy; double lat, lon, az; double lat2, lon2, az2; - //int direction; waypoint *wpt; - - // Erase all existing waypoints. - // wpt_vector_iterator i= waypoints.begin(); - //while(waypoints.begin() != waypoints.end()) - // { - // delete *(i); - // waypoints.erase(i); - // } - //resetWaypoints(); - - // Get the current active runway, based on code from David Luff // This should actually be unified and extended to include // Preferential runway use schema's if (firstFlight) { - string name; - apt->getActiveRunway("com", 1, &name); + //string name; + // "NOTE: this is currently fixed to "com" for commercial traffic + // Should be changed to be used dynamically to allow "gen" and "mil" + // as well + apt->getDynamics()->getActiveRunway("com", 1, activeRunway); if (!(globals->get_runways()->search(apt->getId(), - name, + activeRunway, &rwy))) { - cout << "Failed to find runway for " << apt->getId() << endl; - // Hmm, how do we handle a potential error like this? + SG_LOG(SG_INPUT, SG_ALERT, "Failed to find runway " << + activeRunway << + " at airport " << apt->getId()); exit(1); } - //string test; - //apt->getActiveRunway(string("com"), 1, &test); - //exit(1); } - + // Acceleration point, 105 meters into the runway, heading = rwy._heading; double azimuth = heading + 180.0; while ( azimuth >= 360.0 ) { azimuth -= 360.0; } @@ -396,19 +616,41 @@ void FGAIFlightPlan::createTakeOff(bool firstFlight, FGAirport *apt, double spee wpt->flaps_down= true; wpt->finished = false; wpt->on_ground = true; + wpt->routeIndex = 0; waypoints.push_back(wpt); lat = lat2; lon = lon2; az = az2; - //Next: the Start of Climb + //Start Climbing to 3000 ft. Let's do this + // at the center of the runway for now: + // geo_direct_wgs_84 ( 0, lat, lon, heading, 2560 * SG_FEET_TO_METER, &lat2, &lon2, &az2 ); wpt = new waypoint; wpt->name = "SOC"; + wpt->latitude = rwy._lat; + wpt->longitude = rwy._lon; + wpt->altitude = apt->getElevation()+1000; + wpt->speed = speed; + wpt->crossat = -10000; + wpt->gear_down = true; + wpt->flaps_down= true; + wpt->finished = false; + wpt->on_ground = false; + wpt->routeIndex = 0; + waypoints.push_back(wpt); + + + geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, heading, + rwy._length * SG_FEET_TO_METER, + &lat2, &lon2, &az2 ); + + wpt = new waypoint; + wpt->name = "3000 ft"; wpt->latitude = lat2; wpt->longitude = lon2; wpt->altitude = apt->getElevation()+3000; @@ -418,12 +660,48 @@ void FGAIFlightPlan::createTakeOff(bool firstFlight, FGAirport *apt, double spee wpt->flaps_down= true; wpt->finished = false; wpt->on_ground = false; + wpt->routeIndex = 0; waypoints.push_back(wpt); - // waypoints.push_back(wpt); - //waypoints.push_back(wpt); // add one more to prevent a segfault. - // wpt_iterator = waypoints.begin(); - //if (!firstFlight) - // wpt_iterator++; + +// Finally, add two more waypoints, so that aircraft will remain under + // Tower control until they have reached the 3000 ft climb point + + + geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, heading, + 5000, + &lat2, &lon2, &az2 ); + + + wpt = new waypoint; + wpt->name = "5000 ft"; + wpt->latitude = lat2; + wpt->longitude = lon2; + wpt->altitude = apt->getElevation()+5000; + wpt->speed = speed; + wpt->crossat = -10000; + wpt->gear_down = true; + wpt->flaps_down= true; + wpt->finished = false; + wpt->on_ground = false; + wpt->routeIndex = 0; + waypoints.push_back(wpt); + + // geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, heading, +// 100000, +// &lat2, &lon2, &az2 ); +// wpt = new waypoint; +// wpt->name = "5100 ft"; +// wpt->latitude = lat2; +// wpt->longitude = lon2; +// wpt->altitude = apt->getElevation()+5100; +// wpt->speed = speed; +// wpt->crossat = -10000; +// wpt->gear_down = true; +// wpt->flaps_down= true; +// wpt->finished = false; +// wpt->on_ground = false; +// wpt->routeIndex = 0; +// waypoints.push_back(wpt); } /******************************************************************* @@ -432,49 +710,36 @@ void FGAIFlightPlan::createTakeOff(bool firstFlight, FGAirport *apt, double spee ******************************************************************/ void FGAIFlightPlan::createClimb(bool firstFlight, FGAirport *apt, double speed, double alt) { - double wind_speed; - double wind_heading; double heading; //FGRunway rwy; - double lat, lon, az; double lat2, lon2, az2; //int direction; waypoint *wpt; - // Erase all existing waypoints. - // wpt_vector_iterator i= waypoints.begin(); - //while(waypoints.begin() != waypoints.end()) - // { - // delete *(i); - // waypoints.erase(i); - // } - //resetWaypoints(); - - - // Get the current active runway, based on code from David Luff - // This should actually be unified and extended to include - // Preferential runway use schema's + if (firstFlight) { - string name; - apt->getActiveRunway("com", 1, &name); + //string name; + // "NOTE: this is currently fixed to "com" for commercial traffic + // Should be changed to be used dynamically to allow "gen" and "mil" + // as well + apt->getDynamics()->getActiveRunway("com", 1, activeRunway); if (!(globals->get_runways()->search(apt->getId(), - name, + activeRunway, &rwy))) { - cout << "Failed to find runway for " << apt->getId() << endl; - // Hmm, how do we handle a potential error like this? + SG_LOG(SG_INPUT, SG_ALERT, "Failed to find runway " << + activeRunway << + " at airport " << apt->getId()); exit(1); } - //string test; - //apt->getActiveRunway(string("com"), 1, &test); - //exit(1); } heading = rwy._heading; double azimuth = heading + 180.0; while ( azimuth >= 360.0 ) { azimuth -= 360.0; } + //cerr << "Creating climb at : " << rwy._id << " " << rwy._rwy_no << endl; geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, heading, 10*SG_NM_TO_METER, &lat2, &lon2, &az2 ); @@ -489,6 +754,7 @@ void FGAIFlightPlan::createClimb(bool firstFlight, FGAirport *apt, double speed, wpt->flaps_down= true; wpt->finished = false; wpt->on_ground = false; + wpt->routeIndex = 0; waypoints.push_back(wpt); @@ -506,103 +772,74 @@ void FGAIFlightPlan::createClimb(bool firstFlight, FGAirport *apt, double speed, wpt->flaps_down= true; wpt->finished = false; wpt->on_ground = false; + wpt->routeIndex = 0; waypoints.push_back(wpt); - //waypoints.push_back(wpt); - //waypoints.push_back(wpt); // add one more to prevent a segfault. - // wpt_iterator = waypoints.begin(); - //if (!firstFlight) - // wpt_iterator++; } -/******************************************************************* - * CreateCruise - * initialize the Aircraft at the parking location - ******************************************************************/ -void FGAIFlightPlan::createCruise(bool firstFlight, FGAirport *dep, FGAirport *arr, double latitude, double longitude, double speed, double alt) -{ - double wind_speed; - double wind_heading; - double heading; - //FGRunway rwy; - double lat, lon, az; - double lat2, lon2, az2; - double azimuth; - //int direction; - waypoint *wpt; - - // Erase all existing waypoints. - // wpt_vector_iterator i= waypoints.begin(); - //while(waypoints.begin() != waypoints.end()) - // { - // delete *(i); - // waypoints.erase(i); - // } - //resetWaypoints(); +// /******************************************************************* +// * CreateCruise +// * initialize the Aircraft at the parking location +// ******************************************************************/ +// void FGAIFlightPlan::createCruise(bool firstFlight, FGAirport *dep, +// FGAirport *arr, double latitude, +// double longitude, double speed, +// double alt) +// { +// double wind_speed; +// double wind_heading; +// double heading; +// double lat, lon, az; +// double lat2, lon2, az2; +// double azimuth; +// waypoint *wpt; - wpt = new waypoint; - wpt->name = "Cruise"; //wpt_node->getStringValue("name", "END"); - wpt->latitude = latitude; - wpt->longitude = longitude; - wpt->altitude = alt; - wpt->speed = speed; - wpt->crossat = -10000; - wpt->gear_down = false; - wpt->flaps_down= false; - wpt->finished = false; - wpt->on_ground = false; - waypoints.push_back(wpt); - //Beginning of Decent - - string name; - arr->getActiveRunway("com", 2, &name); - if (!(globals->get_runways()->search(arr->getId(), - name, - &rwy))) - { - cout << "Failed to find runway for " << arr->getId() << endl; - // Hmm, how do we handle a potential error like this? - exit(1); - } - //string test; - //arr->getActiveRunway(string("com"), 1, &test); - //exit(1); +// wpt = new waypoint; +// wpt->name = "Cruise"; //wpt_node->getStringValue("name", "END"); +// wpt->latitude = latitude; +// wpt->longitude = longitude; +// wpt->altitude = alt; +// wpt->speed = speed; +// wpt->crossat = -10000; +// wpt->gear_down = false; +// wpt->flaps_down= false; +// wpt->finished = false; +// wpt->on_ground = false; +// waypoints.push_back(wpt); - //cerr << "Altitude = " << alt << endl; - //cerr << "Done" << endl; - //if (arr->getId() == "EHAM") - // { - // cerr << "Creating cruise to EHAM " << latitude << " " << longitude << endl; - // } - heading = rwy._heading; - azimuth = heading + 180.0; - while ( azimuth >= 360.0 ) { azimuth -= 360.0; } + +// // should be changed dynamically to allow "gen" and "mil" +// arr->getDynamics()->getActiveRunway("com", 2, activeRunway); +// if (!(globals->get_runways()->search(arr->getId(), +// activeRunway, +// &rwy))) +// { +// SG_LOG(SG_INPUT, SG_ALERT, "Failed to find runway " << +// activeRunway << +// " at airport " << arr->getId()); +// exit(1); +// } +// heading = rwy._heading; +// azimuth = heading + 180.0; +// while ( azimuth >= 360.0 ) { azimuth -= 360.0; } - // Note: This places us at the location of the active - // runway during initial cruise. This needs to be - // fixed later. - geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, azimuth, - 110000, - &lat2, &lon2, &az2 ); - wpt = new waypoint; - wpt->name = "BOD"; //wpt_node->getStringValue("name", "END"); - wpt->latitude = lat2; - wpt->longitude = lon2; - wpt->altitude = alt; - wpt->speed = speed; - wpt->crossat = alt; - wpt->gear_down = false; - wpt->flaps_down= false; - wpt->finished = false; - wpt->on_ground = false; - waypoints.push_back(wpt); - //waypoints.push_back(wpt); - //waypoints.push_back(wpt); // add one more to prevent a segfault. - //wpt_iterator = waypoints.begin(); - //if (!firstFlight) - // wpt_iterator++; -} +// geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, azimuth, +// 110000, +// &lat2, &lon2, &az2 ); +// wpt = new waypoint; +// wpt->name = "BOD"; +// wpt->latitude = lat2; +// wpt->longitude = lon2; +// wpt->altitude = alt; +// wpt->speed = speed; +// wpt->crossat = alt; +// wpt->gear_down = false; +// wpt->flaps_down= false; +// wpt->finished = false; +// wpt->on_ground = false; +// waypoints.push_back(wpt); +// } /******************************************************************* * CreateDecent @@ -612,47 +849,30 @@ void FGAIFlightPlan::createDecent(FGAirport *apt) { // Ten thousand ft. Slowing down to 240 kts - double wind_speed; - double wind_heading; double heading; //FGRunway rwy; - double lat, lon, az; double lat2, lon2, az2; double azimuth; //int direction; waypoint *wpt; - //// Erase all existing waypoints. - // wpt_vector_iterator i= waypoints.begin(); - //while(waypoints.begin() != waypoints.end()) - // { - // delete *(i); - // waypoints.erase(i); - // } - //resetWaypoints(); - //Beginning of Decent - string name; - apt->getActiveRunway("com", 2, &name); - if (!(globals->get_runways()->search(apt->getId(), - name, - &rwy))) - { - cout << "Failed to find runway for " << apt->getId() << endl; - // Hmm, how do we handle a potential error like this? - exit(1); - } - //string test; - //apt->getActiveRunway(string("com"), 1, &test); - //exit(1); - - //cerr << "Done" << endl; + //string name; + // allow "mil" and "gen" as well + apt->getDynamics()->getActiveRunway("com", 2, activeRunway); + if (!(globals->get_runways()->search(apt->getId(), + activeRunway, + &rwy))) + { + SG_LOG(SG_INPUT, SG_ALERT, "Failed to find runway " << + activeRunway << + " at airport " << apt->getId()); + exit(1); + } + heading = rwy._heading; azimuth = heading + 180.0; while ( azimuth >= 360.0 ) { azimuth -= 360.0; } - - - geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, azimuth, 100000, &lat2, &lon2, &az2 ); @@ -668,12 +888,13 @@ void FGAIFlightPlan::createDecent(FGAirport *apt) wpt->flaps_down= false; wpt->finished = false; wpt->on_ground = false; + wpt->routeIndex = 0; waypoints.push_back(wpt); - + // Three thousand ft. Slowing down to 160 kts geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, azimuth, - 8*SG_NM_TO_METER, - &lat2, &lon2, &az2 ); + 8*SG_NM_TO_METER, + &lat2, &lon2, &az2 ); wpt = new waypoint; wpt->name = "DEC 3000ft"; //wpt_node->getStringValue("name", "END"); wpt->latitude = lat2; @@ -685,16 +906,8 @@ void FGAIFlightPlan::createDecent(FGAirport *apt) wpt->flaps_down= true; wpt->finished = false; wpt->on_ground = false; + wpt->routeIndex = 0; waypoints.push_back(wpt); - //waypoints.push_back(wpt); - //waypoints.push_back(wpt); // add one more to prevent a segfault. - //wpt_iterator = waypoints.begin(); - //wpt_iterator++; - //if (apt->getId() == "EHAM") - // { - // cerr << "Created Decend to EHAM " << lat2 << " " << lon2 << ": Runway = " << rwy._rwy_no - // << "heading " << heading << endl; - // } } /******************************************************************* * CreateLanding @@ -702,12 +915,9 @@ void FGAIFlightPlan::createDecent(FGAirport *apt) ******************************************************************/ void FGAIFlightPlan::createLanding(FGAirport *apt) { - // Ten thousand ft. Slowing down to 240 kts - double wind_speed; - double wind_heading; + // Ten thousand ft. Slowing down to 150 kts double heading; //FGRunway rwy; - double lat, lon, az; double lat2, lon2, az2; double azimuth; //int direction; @@ -733,6 +943,7 @@ void FGAIFlightPlan::createLanding(FGAirport *apt) wpt->flaps_down= true; wpt->finished = false; wpt->on_ground = true; + wpt->routeIndex = 0; waypoints.push_back(wpt); //Full stop at the runway centerpoint @@ -750,6 +961,7 @@ void FGAIFlightPlan::createLanding(FGAirport *apt) wpt->flaps_down= true; wpt->finished = false; wpt->on_ground = true; + wpt->routeIndex = 0; waypoints.push_back(wpt); geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, heading, @@ -766,57 +978,69 @@ void FGAIFlightPlan::createLanding(FGAirport *apt) wpt->flaps_down= true; wpt->finished = false; wpt->on_ground = true; + wpt->routeIndex = 0; waypoints.push_back(wpt); - //waypoints.push_back(wpt); - //waypoints.push_back(wpt); // add one more to prevent a segfault. - //wpt_iterator = waypoints.begin(); - //wpt_iterator++; - - //if (apt->getId() == "EHAM") - //{ - // cerr << "Created Landing to EHAM " << lat2 << " " << lon2 << ": Runway = " << rwy._rwy_no - // << "heading " << heading << endl; - //} } /******************************************************************* * CreateParking * initialize the Aircraft at the parking location ******************************************************************/ -void FGAIFlightPlan::createParking(FGAirport *apt) +void FGAIFlightPlan::createParking(FGAirport *apt, double radius) { waypoint* wpt; - double lat; - double lon; + double lat, lat2; + double lon, lon2; + double az2; double heading; - apt->getParking(gateId, &lat, &lon, &heading); + apt->getDynamics()->getParking(gateId, &lat, &lon, &heading); heading += 180.0; if (heading > 360) heading -= 360; + 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 = apt->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, + 0.1 *radius, + &lat2, &lon2, &az2 ); + wpt = new waypoint; + wpt->name = "taxiStart"; + wpt->latitude = lat2; + wpt->longitude = lon2; + wpt->altitude = apt->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); - // Erase all existing waypoints. - // wpt_vector_iterator i= waypoints.begin(); - //while(waypoints.begin() != waypoints.end()) - // { - // delete *(i); - // waypoints.erase(i); - // } - //resetWaypoints(); - // And finally one more named "END" wpt = new waypoint; wpt->name = "END"; //wpt_node->getStringValue("name", "END"); wpt->latitude = lat; wpt->longitude = lon; - wpt->altitude = 19; + wpt->altitude = apt->getElevation(); wpt->speed = 15; 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); - //waypoints.push_back(wpt); - //waypoints.push_back(wpt); // add one more to prevent a segfault. - //wpt_iterator = waypoints.begin(); - //wpt_iterator++; }