X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FAIModel%2FAIFlightPlanCreate.cxx;h=ccb677bb80c60e05b0d1f6316674defc201e6180;hb=666910a7937712d02e62a38a83ced564f6227f52;hp=9dcfb99439f1ecffd3aabea95e28ff0f0bf7a54f;hpb=c9813d1b5d79b4aad13c263690a0223086af25ac;p=flightgear.git diff --git a/src/AIModel/AIFlightPlanCreate.cxx b/src/AIModel/AIFlightPlanCreate.cxx index 9dcfb9943..ccb677bb8 100644 --- a/src/AIModel/AIFlightPlanCreate.cxx +++ b/src/AIModel/AIFlightPlanCreate.cxx @@ -29,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, const string& fltType, const string& aircraftType, const 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, latitude, longitude, 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, latitude, longitude, 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++; @@ -92,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, - const string& fltType, - const string& aircraftType, - const string& airline) + double latitude, + double longitude, + double radius, + const string& fltType, + const string& aircraftType, + const string& airline) { double heading; double lat; @@ -115,19 +111,20 @@ void FGAIFlightPlan::createPushBack(bool firstFlight, FGAirport *dep, if (firstFlight) { if (!(dep->getDynamics()->getAvailableParking(&lat, &lon, - &heading, &gateId, - radius, fltType, - aircraftType, airline))) - { - cerr << "Could not find parking " << endl; - } + &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->getDynamics()->getParking(gateId, &lat, &lon, &heading); - //lat = latitude; - //lon = longitude; - //heading = getHeading(); } heading += 180.0; if (heading > 360) @@ -143,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; @@ -161,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, - radius, // push back one entire aircraft radius - &lat2, &lon2, &az2 ); + 2.2*radius, + &lat2, &lon2, &az2 ); wpt = new waypoint; wpt->name = "taxiStart"; wpt->latitude = lat2; @@ -176,33 +173,57 @@ void FGAIFlightPlan::createPushBack(bool firstFlight, FGAirport *dep, wpt->flaps_down= true; wpt->finished = false; wpt->on_ground = true; - waypoints.push_back(wpt); - - + 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 latitude, double longitude, double radius, const string& fltType, const string& acType, const 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; + + if (direction == 1) { - //string name; + // 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 @@ -211,39 +232,43 @@ void FGAIFlightPlan::createTaxi(bool firstFlight, int direction, FGAirport *apt, 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 ); + if (apt->getDynamics()->getGroundNetwork()->exists()) { intVec ids; - int runwayId = apt->getDynamics()->getGroundNetwork()->findNearestNode(lat2, lon2); - //int currId = apt->getGroundNetwork()->findNearestNode(latitude,longitude); - //exit(1); + 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 is a bit of a hack... - FGTaxiRoute route; + // 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) - route = apt->getDynamics()->getGroundNetwork()->findShortestRoute(gateId, runwayId); + *taxiRoute = apt->getDynamics()->getGroundNetwork()->findShortestRoute(gateId, + runwayId); else - route = apt->getDynamics()->getGroundNetwork()->findShortestRoute(0, runwayId); + *taxiRoute = apt->getDynamics()->getGroundNetwork()->findShortestRoute(0, runwayId); intVecIterator i; - //cerr << "creating route : "; - // No route found: go from gate directly to runway - if (route.empty()) { + + if (taxiRoute->empty()) { //Add the runway startpoint; wpt = new waypoint; wpt->name = "Airport Center"; @@ -256,6 +281,7 @@ void FGAIFlightPlan::createTaxi(bool firstFlight, int direction, FGAirport *apt, wpt->flaps_down= true; wpt->finished = false; wpt->on_ground = true; + wpt->routeIndex = 0; waypoints.push_back(wpt); //Add the runway startpoint; @@ -270,35 +296,87 @@ void FGAIFlightPlan::createTaxi(bool firstFlight, int direction, FGAirport *apt, wpt->flaps_down= true; wpt->finished = false; wpt->on_ground = true; + wpt->routeIndex = 0; waypoints.push_back(wpt); } else { int node; - route.first(); - while(route.next(&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)) { - //i = ids.end()-1; - //cerr << "Creating Node: " << node << endl; + //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 = "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(); // should maybe be tn->elev too - wpt->speed = 15; + // 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; - } - //exit(1); + //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"; @@ -311,6 +389,7 @@ void FGAIFlightPlan::createTaxi(bool firstFlight, int direction, FGAirport *apt, wpt->flaps_down= true; wpt->finished = false; wpt->on_ground = true; + wpt->routeIndex = 0; waypoints.push_back(wpt); //Add the runway startpoint; @@ -325,66 +404,42 @@ void FGAIFlightPlan::createTaxi(bool firstFlight, int direction, FGAirport *apt, wpt->flaps_down= true; wpt->finished = false; wpt->on_ground = true; + wpt->routeIndex = 0; waypoints.push_back(wpt); - //wpt = new waypoint; - //wpt->finished = false; - //waypoints.push_back(wpt); // add one more to prevent a segfault. } } else // Landing taxi { - //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->getActiveRunway("com", 1, 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); - - //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 ); - apt->getDynamics()->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 ); + 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; + //cerr << (*(waypoints.end()-1))->name << endl; + + // Find a route from runway end to parking/gate. if (apt->getDynamics()->getGroundNetwork()->exists()) { intVec ids; - int runwayId = apt->getDynamics()->getGroundNetwork()->findNearestNode(lat3, lon3); - //int currId = apt->getGroundNetwork()->findNearestNode(latitude,longitude); - //exit(1); - + 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; + //FGTaxiRoute route; + if (taxiRoute) + delete taxiRoute; + taxiRoute = new FGTaxiRoute; if (gateId >= 0) - route = apt->getDynamics()->getGroundNetwork()->findShortestRoute(runwayId, gateId); + *taxiRoute = apt->getDynamics()->getGroundNetwork()->findShortestRoute(runwayId, + gateId); else - route = apt->getDynamics()->getGroundNetwork()->findShortestRoute(runwayId, 0); + *taxiRoute = apt->getDynamics()->getGroundNetwork()->findShortestRoute(runwayId, 0); intVecIterator i; - //cerr << "creating route : "; + // No route found: go from gate directly to runway - if (route.empty()) { + if (taxiRoute->empty()) { //Add the runway startpoint; wpt = new waypoint; wpt->name = "Airport Center"; @@ -397,13 +452,14 @@ void FGAIFlightPlan::createTaxi(bool firstFlight, int direction, FGAirport *apt, 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->latitude = lat3; + wpt->longitude = lon3; wpt->altitude = apt->getElevation(); wpt->speed = 15; wpt->crossat = -10000; @@ -411,36 +467,51 @@ void FGAIFlightPlan::createTaxi(bool firstFlight, int direction, FGAirport *apt, wpt->flaps_down= true; wpt->finished = false; wpt->on_ground = true; + wpt->routeIndex = 0; waypoints.push_back(wpt); } else { int node; - route.first(); - while(route.next(&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++) { - //i = ids.end()-1; - //cerr << "Creating Node: " << node << endl; + 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); - //ids.pop_back(); wpt = new waypoint; - wpt->name = "taxiway"; // fixme: should be the name of the taxiway + //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(); // should maybe be tn->elev too + 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); } - cerr << endl; + //taxiRoute->first(); + //taxiRoute->next(&node); } - //exit(1); } else { - //Add the runway startpoint; + // 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(); @@ -452,33 +523,48 @@ void FGAIFlightPlan::createTaxi(bool firstFlight, int direction, FGAirport *apt, 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); } - - - - - // Add the final destination waypoint - 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; - waypoints.push_back(wpt); - - + } - // wpt_iterator = waypoints.begin(); - //if (!firstFlight) - // wpt_iterator++; - //wpt_iterator = waypoints.begin()+currWpt; } /******************************************************************* @@ -487,26 +573,11 @@ 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 @@ -521,15 +592,13 @@ void FGAIFlightPlan::createTakeOff(bool firstFlight, FGAirport *apt, double spee 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; } @@ -547,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; @@ -569,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); } /******************************************************************* @@ -583,28 +710,13 @@ 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; @@ -616,19 +728,18 @@ void FGAIFlightPlan::createClimb(bool firstFlight, FGAirport *apt, double speed, 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 ); @@ -643,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); @@ -660,104 +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; +// /******************************************************************* +// * 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; - // Erase all existing waypoints. - // wpt_vector_iterator i= waypoints.begin(); - //while(waypoints.begin() != waypoints.end()) - // { - // delete *(i); - // waypoints.erase(i); - // } - //resetWaypoints(); - - 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; - // should be changed dynamically to allow "gen" and "mil" - arr->getDynamics()->getActiveRunway("com", 2, activeRunway); - if (!(globals->get_runways()->search(arr->getId(), - activeRunway, - &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 @@ -767,48 +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; // allow "mil" and "gen" as well apt->getDynamics()->getActiveRunway("com", 2, activeRunway); - if (!(globals->get_runways()->search(apt->getId(), - 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); - - //cerr << "Done" << endl; + 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 ); @@ -824,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; @@ -841,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 @@ -858,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; @@ -889,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 @@ -906,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, @@ -922,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->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++; }